From 0e2db0beb9228720a40bd19a7bd8891e5a8fdaba Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 16 Feb 2013 13:40:09 -0800 Subject: [PATCH 001/486] Checkpoint: implement new state machine, compiling, WIP --- apps/commander/commander.c | 396 +++++++------ apps/commander/state_machine_helper.c | 518 +++++++++++------- apps/commander/state_machine_helper.h | 157 +----- apps/controllib/fixedwing.cpp | 33 +- apps/drivers/blinkm/blinkm.cpp | 37 +- .../fixedwing_att_control_main.c | 163 +++--- apps/mavlink/mavlink.c | 122 ++--- apps/mavlink/orb_listener.c | 2 +- apps/mavlink_onboard/mavlink.c | 110 ++-- .../multirotor_att_control_main.c | 84 +-- apps/sensors/sensors.cpp | 108 ++-- apps/uORB/topics/manual_control_setpoint.h | 12 +- apps/uORB/topics/rc_channels.h | 22 +- apps/uORB/topics/vehicle_status.h | 92 ++-- 14 files changed, 920 insertions(+), 936 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 6b1bc0f9b5..8716caef72 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -124,19 +124,25 @@ extern struct system_load_s system_load; static int leds; static int buzzer; static int mavlink_fd; + +/* flags */ static bool commander_initialized = false; -static struct vehicle_status_s current_status; /**< Main state machine */ -static orb_advert_t stat_pub; - -// static uint16_t nofix_counter = 0; -// static uint16_t gotfix_counter = 0; - -static unsigned int failsafe_lowlevel_timeout_ms; - static bool thread_should_exit = false; /**< daemon exit flag */ static bool thread_running = false; /**< daemon status flag */ static int daemon_task; /**< Handle of daemon task / thread */ +/* Main state machine */ +static struct vehicle_status_s current_status; +static orb_advert_t stat_pub; + +/* XXX ? */ +// static uint16_t nofix_counter = 0; +// static uint16_t gotfix_counter = 0; + +/* XXX ? */ +static unsigned int failsafe_lowlevel_timeout_ms; + + /* pthread loops */ static void *orb_receive_loop(void *arg); @@ -252,18 +258,19 @@ enum AUDIO_PATTERN { int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state) { +#warning add alarms for state transitions /* Trigger alarm if going into any error state */ - if (((new_state == SYSTEM_STATE_GROUND_ERROR) && (old_state != SYSTEM_STATE_GROUND_ERROR)) || - ((new_state == SYSTEM_STATE_MISSION_ABORT) && (old_state != SYSTEM_STATE_MISSION_ABORT))) { - ioctl(buzzer, TONE_SET_ALARM, 0); - ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_ERROR); - } +// if (((new_state == SYSTEM_STATE_GROUND_ERROR) && (old_state != SYSTEM_STATE_GROUND_ERROR)) || +// ((new_state == SYSTEM_STATE_MISSION_ABORT) && (old_state != SYSTEM_STATE_MISSION_ABORT))) { +// ioctl(buzzer, TONE_SET_ALARM, 0); +// ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_ERROR); +// } /* Trigger neutral on arming / disarming */ - if (((new_state == SYSTEM_STATE_GROUND_READY) && (old_state != SYSTEM_STATE_GROUND_READY))) { - ioctl(buzzer, TONE_SET_ALARM, 0); - ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_NOTIFY_NEUTRAL); - } +// if (((new_state == SYSTEM_STATE_GROUND_READY) && (old_state != SYSTEM_STATE_GROUND_READY))) { +// ioctl(buzzer, TONE_SET_ALARM, 0); +// ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_NOTIFY_NEUTRAL); +// } /* Trigger Tetris on being bored */ @@ -277,6 +284,7 @@ void tune_confirm(void) void tune_error(void) { + /* XXX change alarm to 2 tones*/ ioctl(buzzer, TONE_SET_ALARM, 4); } @@ -795,19 +803,21 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request to set different system mode */ switch (cmd->command) { case VEHICLE_CMD_DO_SET_MODE: { - if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, (uint8_t)cmd->param1)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; + // XXX implement this - } else { +// if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, (uint8_t)cmd->param1)) { +// result = VEHICLE_CMD_RESULT_ACCEPTED; +// +// } else { result = VEHICLE_CMD_RESULT_DENIED; - } +// } } break; case VEHICLE_CMD_COMPONENT_ARM_DISARM: { /* request to arm */ if ((int)cmd->param1 == 1) { - if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) { + if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { @@ -815,9 +825,9 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } /* request to disarm */ - + // XXX this arms it instad of disarming } else if ((int)cmd->param1 == 0) { - if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) { + if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { @@ -830,7 +840,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request for an autopilot reboot */ case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { if ((int)cmd->param1 == 1) { - if (OK == do_state_update(status_pub, current_vehicle_status, mavlink_fd, SYSTEM_STATE_REBOOT)) { + if (OK == do_navigation_state_update(status_pub, current_vehicle_status, mavlink_fd, NAVIGATION_STATE_REBOOT)) { /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -871,16 +881,21 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* gyro calibration */ if ((int)(cmd->param1) == 1) { - /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { + /* transition to init state */ + if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) + && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { + mavlink_log_info(mavlink_fd, "starting gyro cal"); tune_confirm(); do_gyro_calibration(status_pub, ¤t_status); mavlink_log_info(mavlink_fd, "finished gyro cal"); tune_confirm(); - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + + /* back to standby state */ + do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { @@ -893,16 +908,20 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* magnetometer calibration */ if ((int)(cmd->param2) == 1) { - /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { + /* transition to init state */ + if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) + && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { mavlink_log_info(mavlink_fd, "starting mag cal"); tune_confirm(); do_mag_calibration(status_pub, ¤t_status); mavlink_log_info(mavlink_fd, "finished mag cal"); tune_confirm(); - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + + /* back to standby state */ + do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { @@ -916,12 +935,17 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* zero-altitude pressure calibration */ if ((int)(cmd->param3) == 1) { /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); + if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) + && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { + // XXX implement this mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented"); tune_confirm(); + /* back to standby state */ + do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); + } else { mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration"); result = VEHICLE_CMD_RESULT_DENIED; @@ -933,15 +957,18 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* trim calibration */ if ((int)(cmd->param4) == 1) { /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); - - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { + if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) + && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { mavlink_log_info(mavlink_fd, "starting trim cal"); tune_confirm(); do_rc_calibration(status_pub, ¤t_status); mavlink_log_info(mavlink_fd, "finished trim cal"); tune_confirm(); - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + + /* back to standby state */ + do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { @@ -954,16 +981,19 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* accel calibration */ if ((int)(cmd->param5) == 1) { - /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); + if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) + && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { mavlink_log_info(mavlink_fd, "CMD starting accel cal"); tune_confirm(); do_accel_calibration(status_pub, ¤t_status); tune_confirm(); mavlink_log_info(mavlink_fd, "CMD finished accel cal"); - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + + /* back to standby state */ + do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { @@ -1262,7 +1292,7 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_sys_type = param_find("MAV_TYPE"); /* welcome user */ - warnx("I am in command now!\n"); + warnx("[commander] starting"); /* pthreads for command and subsystem info handling */ // pthread_t command_handling_thread; @@ -1270,33 +1300,39 @@ int commander_thread_main(int argc, char *argv[]) /* initialize */ if (led_init() != 0) { - warnx("ERROR: Failed to initialize leds\n"); + warnx("ERROR: Failed to initialize leds"); } if (buzzer_init() != 0) { - warnx("ERROR: Failed to initialize buzzer\n"); + warnx("ERROR: Failed to initialize buzzer"); } mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); if (mavlink_fd < 0) { - warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first.\n"); + warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first."); } /* make sure we are in preflight state */ memset(¤t_status, 0, sizeof(current_status)); - current_status.state_machine = SYSTEM_STATE_PREFLIGHT; + current_status.navigation_state = NAVIGATION_STATE_INIT; + current_status.arming_state = ARMING_STATE_INIT; current_status.flag_system_armed = false; + /* neither manual nor offboard control commands have been received */ current_status.offboard_control_signal_found_once = false; current_status.rc_signal_found_once = false; + /* mark all signals lost as long as they haven't been found */ current_status.rc_signal_lost = true; current_status.offboard_control_signal_lost = true; + /* allow manual override initially */ current_status.flag_external_manual_override_ok = true; + /* flag position info as bad, do not allow auto mode */ current_status.flag_vector_flight_mode_ok = false; + /* set battery warning flag */ current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; @@ -1315,6 +1351,7 @@ int commander_thread_main(int argc, char *argv[]) exit(ERROR); } + // XXX needed? mavlink_log_info(mavlink_fd, "system is running"); /* create pthreads */ @@ -1325,7 +1362,6 @@ int commander_thread_main(int argc, char *argv[]) /* Start monitoring loop */ uint16_t counter = 0; - uint8_t flight_env; /* Initialize to 0.0V */ float battery_voltage = 0.0f; @@ -1350,11 +1386,13 @@ int commander_thread_main(int argc, char *argv[]) struct offboard_control_setpoint_s sp_offboard; memset(&sp_offboard, 0, sizeof(sp_offboard)); + /* Subscribe to global position */ int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); struct vehicle_global_position_s global_position; memset(&global_position, 0, sizeof(global_position)); uint64_t last_global_position_time = 0; + /* Subscribe to local position data */ int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); struct vehicle_local_position_s local_position; memset(&local_position, 0, sizeof(local_position)); @@ -1365,10 +1403,13 @@ int commander_thread_main(int argc, char *argv[]) * position estimator and commander. RAW GPS is more than good enough for a * non-flying vehicle. */ + + /* Subscribe to GPS topic */ int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); struct vehicle_gps_position_s gps_position; memset(&gps_position, 0, sizeof(gps_position)); + /* Subscribe to sensor topic */ int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); struct sensor_combined_s sensors; memset(&sensors, 0, sizeof(sensors)); @@ -1502,45 +1543,36 @@ int commander_thread_main(int argc, char *argv[]) /* Slow but important 8 Hz checks */ if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) { /* toggle activity (blue) led at 1 Hz in standby, 10 Hz in armed mode */ - if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY || - current_status.state_machine == SYSTEM_STATE_AUTO || - current_status.state_machine == SYSTEM_STATE_MANUAL)) { - /* armed */ - led_toggle(LED_BLUE); +// if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY || +// current_status.state_machine == SYSTEM_STATE_AUTO || +// current_status.state_machine == SYSTEM_STATE_MANUAL)) { +// /* armed */ +// led_toggle(LED_BLUE); - } else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - /* not armed */ - led_toggle(LED_BLUE); - } + } else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { + /* not armed */ + led_toggle(LED_BLUE); + } - /* toggle error led at 5 Hz in HIL mode */ - if (current_status.flag_hil_enabled) { - /* hil enabled */ - led_toggle(LED_AMBER); + /* toggle error led at 5 Hz in HIL mode */ + if (current_status.flag_hil_enabled) { + /* hil enabled */ + led_toggle(LED_AMBER); - } else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) { - /* toggle error (red) at 5 Hz on low battery or error */ - led_toggle(LED_AMBER); + } else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) { + /* toggle error (red) at 5 Hz on low battery or error */ + led_toggle(LED_AMBER); - } else { - // /* Constant error indication in standby mode without GPS */ - // if (!current_status.gps_valid) { - // led_on(LED_AMBER); + } - // } else { - // led_off(LED_AMBER); - // } - } + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { + /* compute system load */ + uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - /* compute system load */ - uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; + if (last_idle_time > 0) + current_status.load = 1000 - (interval_runtime / 1000); //system load is time spent in non-idle - if (last_idle_time > 0) - current_status.load = 1000 - (interval_runtime / 1000); //system load is time spent in non-idle - - last_idle_time = system_load.tasks[0].total_runtime; - } + last_idle_time = system_load.tasks[0].total_runtime; } // // XXX Export patterns and threshold to parameters @@ -1583,13 +1615,14 @@ int commander_thread_main(int argc, char *argv[]) low_voltage_counter++; } - /* Critical, this is rather an emergency, kill signal to sdlog and change state machine */ + /* Critical, this is rather an emergency, change state machine */ else if (battery_voltage_valid && (bat_remain < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) { if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { critical_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!"); current_status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; - state_machine_emergency(stat_pub, ¤t_status, mavlink_fd); + // XXX implement this +// state_machine_emergency(stat_pub, ¤t_status, mavlink_fd); } critical_voltage_counter++; @@ -1698,7 +1731,7 @@ int commander_thread_main(int argc, char *argv[]) if (gps_position.fix_type == GPS_FIX_TYPE_3D && (hdop_m < hdop_threshold_m) - && (vdop_m < vdop_threshold_m) + && (vdop_m < vdop_threshold_m) // XXX note that vdop is 0 for mtk && !home_position_set && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) && !current_status.flag_system_armed) { @@ -1734,85 +1767,112 @@ int commander_thread_main(int argc, char *argv[]) if ((hrt_absolute_time() - sp_man.timestamp) < 100000) { - // /* - // * Check if manual control modes have to be switched - // */ - // if (!isfinite(sp_man.manual_mode_switch)) { - // warnx("man mode sw not finite\n"); + /* + * Check if manual control modes have to be switched + */ + if (!isfinite(sp_man.mode_switch)) { + warnx("mode sw not finite"); - // /* this switch is not properly mapped, set default */ - // if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - // (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - // (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { + /* this switch is not properly mapped, set attitude stabilized */ + if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { - // /* assuming a rotary wing, fall back to SAS */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - // current_status.flag_control_attitude_enabled = true; - // current_status.flag_control_rates_enabled = true; - // } else { + current_status.flag_control_attitude_enabled = true; + current_status.flag_control_rates_enabled = true; + } else { - // /* assuming a fixed wing, fall back to direct pass-through */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; - // current_status.flag_control_attitude_enabled = false; - // current_status.flag_control_rates_enabled = false; - // } + /* assuming a fixed wing, fall back to direct pass-through */ + current_status.flag_control_attitude_enabled = false; + current_status.flag_control_rates_enabled = false; + } - // } else if (sp_man.manual_mode_switch < -STICK_ON_OFF_LIMIT) { + } else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) { - // /* bottom stick position, set direct mode for vehicles supporting it */ - // if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - // (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - // (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { + /* this switch is not properly mapped, set attitude stabilized */ + if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { - // /* assuming a rotary wing, fall back to SAS */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - // current_status.flag_control_attitude_enabled = true; - // current_status.flag_control_rates_enabled = true; - // } else { + /* assuming a rotary wing, fall back to m */ + current_status.flag_control_attitude_enabled = true; + current_status.flag_control_rates_enabled = true; + } else { - // /* assuming a fixed wing, set to direct pass-through as requested */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; - // current_status.flag_control_attitude_enabled = false; - // current_status.flag_control_rates_enabled = false; - // } + /* assuming a fixed wing, set to direct pass-through as requested */ + current_status.flag_control_attitude_enabled = false; + current_status.flag_control_rates_enabled = false; + } - // } else if (sp_man.manual_mode_switch > STICK_ON_OFF_LIMIT) { + } else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) { - // /* top stick position, set SAS for all vehicle types */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - // current_status.flag_control_attitude_enabled = true; - // current_status.flag_control_rates_enabled = true; + /* top stick position, set auto/mission for all vehicle types */ + current_status.flag_control_position_enabled = true; + current_status.flag_control_velocity_enabled = true; + current_status.flag_control_attitude_enabled = true; + current_status.flag_control_rates_enabled = true; - // } else { - // /* center stick position, set rate control */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_RATES; - // current_status.flag_control_attitude_enabled = false; - // current_status.flag_control_rates_enabled = true; - // } + } else { + /* center stick position, set seatbelt/simple control */ + current_status.flag_control_velocity_enabled = true; + current_status.flag_control_attitude_enabled = true; + current_status.flag_control_rates_enabled = true; + } // warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode); /* - * Check if manual stability control modes have to be switched + * Check if land/RTL is requested */ - if (!isfinite(sp_man.manual_sas_switch)) { + if (!isfinite(sp_man.return_switch)) { /* this switch is not properly mapped, set default */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS; + current_status.flag_land_requested = false; // XXX default? - } else if (sp_man.manual_sas_switch < -STICK_ON_OFF_LIMIT) { + } else if (sp_man.return_switch < -STICK_ON_OFF_LIMIT) { /* bottom stick position, set altitude hold */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ALTITUDE; + current_status.flag_land_requested = false; - } else if (sp_man.manual_sas_switch > STICK_ON_OFF_LIMIT) { + } else if (sp_man.return_switch > STICK_ON_OFF_LIMIT) { /* top stick position */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_SIMPLE; + current_status.flag_land_requested = true; } else { /* center stick position, set default */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS; + current_status.flag_land_requested = false; // XXX default? + } + + /* check mission switch */ + if (!isfinite(sp_man.mission_switch)) { + + /* this switch is not properly mapped, set default */ + current_status.flag_mission_activated = false; + + } else if (sp_man.mission_switch > STICK_ON_OFF_LIMIT) { + + /* top switch position */ + current_status.flag_mission_activated = true; + + } else if (sp_man.mission_switch < -STICK_ON_OFF_LIMIT) { + + /* bottom switch position */ + current_status.flag_mission_activated = false; + + } else { + + /* center switch position, set default */ + current_status.flag_mission_activated = false; // XXX default? + } + + /* handle the case where RC signal was regained */ + if (!current_status.rc_signal_found_once) { + current_status.rc_signal_found_once = true; + mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME."); + + } else { + if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); } /* @@ -1826,7 +1886,7 @@ int commander_thread_main(int argc, char *argv[]) ((sp_man.yaw < -STICK_ON_OFF_LIMIT)) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); + current_status.flag_system_armed = false; stick_on_counter = 0; } else { @@ -1838,7 +1898,7 @@ int commander_thread_main(int argc, char *argv[]) /* check if left stick is in lower right position --> arm */ if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.2f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); + current_status.flag_system_armed = true; stick_on_counter = 0; } else { @@ -1847,37 +1907,6 @@ int commander_thread_main(int argc, char *argv[]) } } - /* check manual override switch - switch to manual or auto mode */ - if (sp_man.manual_override_switch > STICK_ON_OFF_LIMIT) { - /* enable manual override */ - update_state_machine_mode_manual(stat_pub, ¤t_status, mavlink_fd); - - } else if (sp_man.manual_override_switch < -STICK_ON_OFF_LIMIT) { - // /* check auto mode switch for correct mode */ - // if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) { - // /* enable guided mode */ - // update_state_machine_mode_guided(stat_pub, ¤t_status, mavlink_fd); - - // } else if (sp_man.auto_mode_switch < -STICK_ON_OFF_LIMIT) { - // XXX hardcode to auto for now - update_state_machine_mode_auto(stat_pub, ¤t_status, mavlink_fd); - - // } - - } else { - /* center stick position, set SAS for all vehicle types */ - update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); - } - - /* handle the case where RC signal was regained */ - if (!current_status.rc_signal_found_once) { - current_status.rc_signal_found_once = true; - mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME."); - - } else { - if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); - } - current_status.rc_signal_cutting_off = false; current_status.rc_signal_lost = false; current_status.rc_signal_lost_interval = 0; @@ -1965,12 +1994,13 @@ int commander_thread_main(int argc, char *argv[]) /* arm / disarm on request */ if (sp_offboard.armed && !current_status.flag_system_armed) { - update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); +#warning fix this +// update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); /* switch to stabilized mode = takeoff */ - update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); +// update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); } else if (!sp_offboard.armed && current_status.flag_system_armed) { - update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); +// update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); } } else { @@ -2006,18 +2036,20 @@ int commander_thread_main(int argc, char *argv[]) current_status.timestamp = hrt_absolute_time(); + // XXX what is this? /* If full run came back clean, transition to standby */ - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT && - current_status.flag_preflight_gyro_calibration == false && - current_status.flag_preflight_mag_calibration == false && - current_status.flag_preflight_accel_calibration == false) { - /* All ok, no calibration going on, go to standby */ - do_state_update(stat_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - } +// if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT && +// current_status.flag_preflight_gyro_calibration == false && +// current_status.flag_preflight_mag_calibration == false && +// current_status.flag_preflight_accel_calibration == false) { +// /* All ok, no calibration going on, go to standby */ +// do_state_update(stat_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); +// } /* publish at least with 1 Hz */ if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) { - publish_armed_status(¤t_status); +#warning fix this +// publish_armed_status(¤t_status); orb_publish(ORB_ID(vehicle_status), stat_pub, ¤t_status); state_changed = false; } @@ -2043,7 +2075,7 @@ int commander_thread_main(int argc, char *argv[]) close(sensor_sub); close(cmd_sub); - warnx("exiting..\n"); + warnx("exiting"); fflush(stdout); thread_running = false; diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index bea388a101..e1ec73110c 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -40,182 +40,297 @@ #include #include +#include #include #include #include #include +#include #include #include #include "state_machine_helper.h" -static const char *system_state_txt[] = { - "SYSTEM_STATE_PREFLIGHT", - "SYSTEM_STATE_STANDBY", - "SYSTEM_STATE_GROUND_READY", - "SYSTEM_STATE_MANUAL", - "SYSTEM_STATE_STABILIZED", - "SYSTEM_STATE_AUTO", - "SYSTEM_STATE_MISSION_ABORT", - "SYSTEM_STATE_EMCY_LANDING", - "SYSTEM_STATE_EMCY_CUTOFF", - "SYSTEM_STATE_GROUND_ERROR", - "SYSTEM_STATE_REBOOT", - -}; /** - * Transition from one state to another + * Transition from one navigation state to another */ -int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state) +int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state) { - int invalid_state = false; + bool valid_transition = false; int ret = ERROR; - commander_state_machine_t old_state = current_status->state_machine; + if (current_status->navigation_state == new_state) { + warnx("Navigation state not changed"); - switch (new_state) { - case SYSTEM_STATE_MISSION_ABORT: { - /* Indoor or outdoor */ - // if (flight_environment_parameter == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { - ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING); + } else { - // } else { - // ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF); - // } + switch (new_state) { + case NAVIGATION_STATE_INIT: + + if (current_status->navigation_state == NAVIGATION_STATE_STANDBY) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO INIT STATE"); + valid_transition = true; + } + break; + + case NAVIGATION_STATE_STANDBY: + + if (current_status->navigation_state == NAVIGATION_STATE_INIT + || current_status->navigation_state == NAVIGATION_STATE_MANUAL + || current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO STANDBY STATE"); + valid_transition = true; + } + break; + + case NAVIGATION_STATE_MANUAL: + + if ( + ( current_status->navigation_state == NAVIGATION_STATE_STANDBY + || current_status->navigation_state == NAVIGATION_STATE_SEATBELT + || current_status->navigation_state == NAVIGATION_STATE_LOITER + || current_status->navigation_state == NAVIGATION_STATE_MISSION + || current_status->navigation_state == NAVIGATION_STATE_RTL + || current_status->navigation_state == NAVIGATION_STATE_LAND + || current_status->navigation_state == NAVIGATION_STATE_TAKEOFF + || current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) + && current_status->arming_state == ARMING_STATE_ARMED) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO MANUAL STATE"); + valid_transition = true; + } + break; + + case NAVIGATION_STATE_SEATBELT: + + if (current_status->navigation_state == NAVIGATION_STATE_MANUAL + || current_status->navigation_state == NAVIGATION_STATE_LOITER + || current_status->navigation_state == NAVIGATION_STATE_MISSION) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO SEATBELT STATE"); + valid_transition = true; + } + break; + + case NAVIGATION_STATE_LOITER: + + if ( ((current_status->navigation_state == NAVIGATION_STATE_MANUAL || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) + && current_status->flag_global_position_valid) + || current_status->navigation_state == NAVIGATION_STATE_MISSION) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO LOITER STATE"); + valid_transition = true; + } + break; + + case NAVIGATION_STATE_AUTO_READY: + + if ( + (current_status->navigation_state == NAVIGATION_STATE_STANDBY + && current_status->flag_global_position_valid + && current_status->flag_valid_launch_position) + || current_status->navigation_state == NAVIGATION_STATE_LAND) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO AUTO READY STATE"); + valid_transition = true; + } + break; + + case NAVIGATION_STATE_MISSION: + + if ( + current_status->navigation_state == NAVIGATION_STATE_TAKEOFF + || current_status->navigation_state == NAVIGATION_STATE_RTL + || ( + (current_status->navigation_state == NAVIGATION_STATE_MANUAL + || current_status->navigation_state == NAVIGATION_STATE_SEATBELT + || current_status->navigation_state == NAVIGATION_STATE_LOITER) + && current_status->flag_global_position_valid + && current_status->flag_valid_launch_position) + ) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO MISSION STATE"); + valid_transition = true; + } + break; + + case NAVIGATION_STATE_RTL: + + if ( + current_status->navigation_state == NAVIGATION_STATE_MISSION + || ( + (current_status->navigation_state == NAVIGATION_STATE_MANUAL + || current_status->navigation_state == NAVIGATION_STATE_SEATBELT + || current_status->navigation_state == NAVIGATION_STATE_LOITER) + && current_status->flag_global_position_valid + && current_status->flag_valid_launch_position) + ) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO RTL STATE"); + valid_transition = true; + } + break; + + case NAVIGATION_STATE_TAKEOFF: + + if (current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO TAKEOFF STATE"); + valid_transition = true; + } + break; + + case NAVIGATION_STATE_LAND: + if (current_status->navigation_state == NAVIGATION_STATE_RTL + || current_status->navigation_state == NAVIGATION_STATE_LOITER) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO LAND STATE"); + valid_transition = true; + } + break; + + case NAVIGATION_STATE_REBOOT: + if (current_status->navigation_state == NAVIGATION_STATE_STANDBY + || current_status->navigation_state == NAVIGATION_STATE_INIT + || current_status->flag_hil_enabled) { + valid_transition = true; + /* set system flags according to state */ + current_status->flag_system_armed = false; + mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); + usleep(500000); + up_systemreset(); + /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ + + } else { + mavlink_log_critical(mavlink_fd, "REFUSED to REBOOT"); + } + + break; + + default: + warnx("Unknown navigation state"); + break; } - break; - - case SYSTEM_STATE_EMCY_LANDING: - /* Tell the controller to land */ - - /* set system flags according to state */ - current_status->flag_system_armed = true; - - warnx("EMERGENCY LANDING!\n"); - mavlink_log_critical(mavlink_fd, "EMERGENCY LANDING!"); - break; - - case SYSTEM_STATE_EMCY_CUTOFF: - /* Tell the controller to cutoff the motors (thrust = 0) */ - - /* set system flags according to state */ - current_status->flag_system_armed = false; - - warnx("EMERGENCY MOTOR CUTOFF!\n"); - mavlink_log_critical(mavlink_fd, "EMERGENCY MOTOR CUTOFF!"); - break; - - case SYSTEM_STATE_GROUND_ERROR: - - /* set system flags according to state */ - - /* prevent actuators from arming */ - current_status->flag_system_armed = false; - - warnx("GROUND ERROR, locking down propulsion system\n"); - mavlink_log_critical(mavlink_fd, "GROUND ERROR, locking down system"); - break; - - case SYSTEM_STATE_PREFLIGHT: - if (current_status->state_machine == SYSTEM_STATE_STANDBY - || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - /* set system flags according to state */ - current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "Switched to PREFLIGHT state"); - - } else { - invalid_state = true; - mavlink_log_critical(mavlink_fd, "REFUSED to switch to PREFLIGHT state"); - } - - break; - - case SYSTEM_STATE_REBOOT: - if (current_status->state_machine == SYSTEM_STATE_STANDBY - || current_status->state_machine == SYSTEM_STATE_PREFLIGHT - || current_status->flag_hil_enabled) { - invalid_state = false; - /* set system flags according to state */ - current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); - usleep(500000); - up_systemreset(); - /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ - - } else { - invalid_state = true; - mavlink_log_critical(mavlink_fd, "REFUSED to REBOOT"); - } - - break; - - case SYSTEM_STATE_STANDBY: - /* set system flags according to state */ - - /* standby enforces disarmed */ - current_status->flag_system_armed = false; - - mavlink_log_critical(mavlink_fd, "Switched to STANDBY state"); - break; - - case SYSTEM_STATE_GROUND_READY: - - /* set system flags according to state */ - - /* ground ready has motors / actuators armed */ - current_status->flag_system_armed = true; - - mavlink_log_critical(mavlink_fd, "Switched to GROUND READY state"); - break; - - case SYSTEM_STATE_AUTO: - - /* set system flags according to state */ - - /* auto is airborne and in auto mode, motors armed */ - current_status->flag_system_armed = true; - - mavlink_log_critical(mavlink_fd, "Switched to FLYING / AUTO mode"); - break; - - case SYSTEM_STATE_STABILIZED: - - /* set system flags according to state */ - current_status->flag_system_armed = true; - - mavlink_log_critical(mavlink_fd, "Switched to FLYING / STABILIZED mode"); - break; - - case SYSTEM_STATE_MANUAL: - - /* set system flags according to state */ - current_status->flag_system_armed = true; - - mavlink_log_critical(mavlink_fd, "Switched to FLYING / MANUAL mode"); - break; - - default: - invalid_state = true; - break; } - if (invalid_state == false || old_state != new_state) { - current_status->state_machine = new_state; + if (valid_transition) { + current_status->navigation_state = new_state; state_machine_publish(status_pub, current_status, mavlink_fd); - publish_armed_status(current_status); +// publish_armed_status(current_status); ret = OK; - } - - if (invalid_state) { - mavlink_log_critical(mavlink_fd, "REJECTING invalid state transition"); - ret = ERROR; + } else { + mavlink_log_critical(mavlink_fd, "REJECTING invalid navigation state transition"); } return ret; } +/** + * Transition from one arming state to another + */ +int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state) +{ + bool valid_transition = false; + int ret = ERROR; + + if (current_status->arming_state == new_state) { + warnx("Arming state not changed"); + + } else { + + switch (new_state) { + + case ARMING_STATE_INIT: + + if (current_status->arming_state == ARMING_STATE_STANDBY) { + mavlink_log_critical(mavlink_fd, "SWITCHED TO INIT ARMING STATE"); + valid_transition = true; + } + break; + + case ARMING_STATE_STANDBY: + + // TODO check for sensors + // XXX check if coming from reboot? + if (current_status->arming_state == ARMING_STATE_INIT) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO STANDBY ARMING STATE"); + valid_transition = true; + } + break; + + case ARMING_STATE_ARMED: + + if (current_status->arming_state == ARMING_STATE_STANDBY + || current_status->arming_state == ARMING_STATE_IN_AIR_RESTORE) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO ARMED ARMING STATE"); + valid_transition = true; + } + break; + + case ARMING_STATE_MISSION_ABORT: + + if (current_status->arming_state == ARMING_STATE_ARMED) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO MISSION ABORT ARMING STATE"); + valid_transition = true; + } + break; + + case ARMING_STATE_ERROR: + + if (current_status->arming_state == ARMING_STATE_ARMED + || current_status->arming_state == ARMING_STATE_MISSION_ABORT + || current_status->arming_state == ARMING_STATE_INIT) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO ERROR ARMING STATE"); + valid_transition = true; + } + break; + + case ARMING_STATE_REBOOT: + + if (current_status->arming_state == ARMING_STATE_STANDBY + || current_status->arming_state == ARMING_STATE_ERROR) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO REBOOT ARMING STATE"); + valid_transition = true; + // XXX reboot here? + } + break; + + case ARMING_STATE_IN_AIR_RESTORE: + + if (current_status->arming_state == ARMING_STATE_INIT) { + mavlink_log_critical(mavlink_fd, "SWITCHED TO IN-AIR-RESTORE ARMING STATE"); + valid_transition = true; + } + break; + default: + warnx("Unknown arming state"); + break; + } + } + + if (valid_transition) { + current_status->arming_state = new_state; + state_machine_publish(status_pub, current_status, mavlink_fd); +// publish_armed_status(current_status); + ret = OK; + } else { + mavlink_log_critical(mavlink_fd, "REJECTING invalid arming state transition"); + } + + return ret; +} + + + void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { /* publish the new state */ @@ -223,70 +338,69 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat current_status->timestamp = hrt_absolute_time(); /* assemble state vector based on flag values */ - if (current_status->flag_control_rates_enabled) { - current_status->onboard_control_sensors_present |= 0x400; +// if (current_status->flag_control_rates_enabled) { +// current_status->onboard_control_sensors_present |= 0x400; +// +// } else { +// current_status->onboard_control_sensors_present &= ~0x400; +// } - } else { - current_status->onboard_control_sensors_present &= ~0x400; - } - - current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; - current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; - current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; - current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; - - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_rates_enabled) ? 0x400 : 0; - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; +// current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; +// current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; +// current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; +// current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; +// +// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_rates_enabled) ? 0x400 : 0; +// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; +// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; +// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; +// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - printf("[cmd] new state: %s\n", system_state_txt[current_status->state_machine]); } -void publish_armed_status(const struct vehicle_status_s *current_status) -{ - struct actuator_armed_s armed; - armed.armed = current_status->flag_system_armed; - /* lock down actuators if required, only in HIL */ - armed.lockdown = (current_status->flag_hil_enabled) ? true : false; - orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); - orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); -} +//void publish_armed_status(const struct vehicle_status_s *current_status) +//{ +// struct actuator_armed_s armed; +// armed.armed = current_status->flag_system_armed; +// /* lock down actuators if required, only in HIL */ +// armed.lockdown = (current_status->flag_hil_enabled) ? true : false; +// orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); +// orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); +//} /* * Private functions, update the state machine */ -void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - warnx("EMERGENCY HANDLER\n"); - /* Depending on the current state go to one of the error states */ +//void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +//{ +// warnx("EMERGENCY HANDLER\n"); +// /* Depending on the current state go to one of the error states */ +// +// if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY) { +// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_ERROR); +// +// } else if (current_status->state_machine == SYSTEM_STATE_AUTO || current_status->state_machine == SYSTEM_STATE_MANUAL) { +// +// // DO NOT abort mission +// //do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT); +// +// } else { +// warnx("Unknown system state: #%d\n", current_status->state_machine); +// } +//} - if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_ERROR); - - } else if (current_status->state_machine == SYSTEM_STATE_AUTO || current_status->state_machine == SYSTEM_STATE_MANUAL) { - - // DO NOT abort mission - //do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT); - - } else { - warnx("Unknown system state: #%d\n", current_status->state_machine); - } -} - -void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) //do not call state_machine_emergency_always_critical if we are in manual mode for these errors -{ - if (current_status->state_machine != SYSTEM_STATE_MANUAL) { //if we are in manual: user can react to errors themself - state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd); - - } else { - //global_data_send_mavlink_statustext_message_out("[cmd] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL); - } - -} +//void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) //do not call state_machine_emergency_always_critical if we are in manual mode for these errors +//{ +// if (current_status->state_machine != SYSTEM_STATE_MANUAL) { //if we are in manual: user can react to errors themself +// state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd); +// +// } else { +// //global_data_send_mavlink_statustext_message_out("[cmd] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL); +// } +// +//} @@ -488,6 +602,7 @@ void state_machine_emergency(int status_pub, struct vehicle_status_s *current_st /* END SUBSYSTEM/EMERGENCY FUNCTIONS*/ +#if 0 void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { @@ -750,3 +865,4 @@ uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_ return ret; } +#endif diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h index 2f2ccc7298..bf9296caf0 100644 --- a/apps/commander/state_machine_helper.h +++ b/apps/commander/state_machine_helper.h @@ -47,163 +47,10 @@ #include #include -/** - * Switch to new state with no checking. - * - * do_state_update: this is the functions that all other functions have to call in order to update the state. - * the function does not question the state change, this must be done before - * The function performs actions that are connected with the new state (buzzer, reboot, ...) - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - * - * @return 0 (macro OK) or 1 on error (macro ERROR) - */ -int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state); +int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state); -/* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR */ -// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); -// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); +int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state); -// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); -// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); - -// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); -// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); - - -/** - * Handle state machine if got position fix - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if position fix lost - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if user wants to arm - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if user wants to disarm - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if mode switch is manual - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if mode switch is stabilized - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if mode switch is guided - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if mode switch is auto - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Publish current state information - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/* - * Functions that handle incoming requests to change the state machine or a parameter (probably from the mavlink app). - * If the request is obeyed the functions return 0 - * - */ - -/** - * Handles *incoming request* to switch to a specific state, if state change is successful returns 0 - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode); - -/** - * Handles *incoming request* to switch to a specific custom state, if state change is successful returns 0 - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode); - -/** - * Always switches to critical mode under any circumstances. - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Switches to emergency if required. - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Publish the armed state depending on the current system state - * - * @param current_status the current system status - */ -void publish_armed_status(const struct vehicle_status_s *current_status); - - - #endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/apps/controllib/fixedwing.cpp b/apps/controllib/fixedwing.cpp index d9637b4a7a..9a69195352 100644 --- a/apps/controllib/fixedwing.cpp +++ b/apps/controllib/fixedwing.cpp @@ -39,6 +39,7 @@ #include "fixedwing.hpp" + namespace control { @@ -294,7 +295,7 @@ void BlockMultiModeBacksideAutopilot::update() _actuators.control[i] = 0.0f; // only update guidance in auto mode - if (_status.state_machine == SYSTEM_STATE_AUTO) { + if (_status.navigation_state == NAVIGATION_STATE_MISSION) { // update guidance _guide.update(_pos, _att, _posCmd, _lastPosCmd); } @@ -304,8 +305,8 @@ void BlockMultiModeBacksideAutopilot::update() // the setpoint should update to loitering around this position // handle autopilot modes - if (_status.state_machine == SYSTEM_STATE_AUTO || - _status.state_machine == SYSTEM_STATE_STABILIZED) { + if (_status.navigation_state == NAVIGATION_STATE_MISSION || + _status.navigation_state == NAVIGATION_STATE_MANUAL) { // update guidance _guide.update(_pos, _att, _posCmd, _lastPosCmd); @@ -340,25 +341,25 @@ void BlockMultiModeBacksideAutopilot::update() } - } else if (_status.state_machine == SYSTEM_STATE_MANUAL) { - - if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { + } else if (_status.navigation_state == NAVIGATION_STATE_MANUAL) { +// if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { +#warning fix the different manual modes _actuators.control[CH_AIL] = _manual.roll; _actuators.control[CH_ELV] = _manual.pitch; _actuators.control[CH_RDR] = _manual.yaw; _actuators.control[CH_THR] = _manual.throttle; - } else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { - - _stabilization.update(_manual.roll, _manual.pitch, _manual.yaw, - _att.rollspeed, _att.pitchspeed, _att.yawspeed); - - _actuators.control[CH_AIL] = _stabilization.getAileron(); - _actuators.control[CH_ELV] = - _stabilization.getElevator(); - _actuators.control[CH_RDR] = _stabilization.getRudder(); - _actuators.control[CH_THR] = _manual.throttle; - } +// } else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { +// +// _stabilization.update(_manual.roll, _manual.pitch, _manual.yaw, +// _att.rollspeed, _att.pitchspeed, _att.yawspeed); +// +// _actuators.control[CH_AIL] = _stabilization.getAileron(); +// _actuators.control[CH_ELV] = - _stabilization.getElevator(); +// _actuators.control[CH_RDR] = _stabilization.getRudder(); +// _actuators.control[CH_THR] = _manual.throttle; +// } } // update all publications diff --git a/apps/drivers/blinkm/blinkm.cpp b/apps/drivers/blinkm/blinkm.cpp index fc929284c4..6aff27e4ca 100644 --- a/apps/drivers/blinkm/blinkm.cpp +++ b/apps/drivers/blinkm/blinkm.cpp @@ -554,26 +554,27 @@ BlinkM::led() led_blink = LED_BLINK; /* handle 4th led - flightmode indicator */ - switch((int)vehicle_status_raw.flight_mode) { - case VEHICLE_FLIGHT_MODE_MANUAL: - led_color_4 = LED_AMBER; - break; - - case VEHICLE_FLIGHT_MODE_STAB: - led_color_4 = LED_YELLOW; - break; - - case VEHICLE_FLIGHT_MODE_HOLD: - led_color_4 = LED_BLUE; - break; - - case VEHICLE_FLIGHT_MODE_AUTO: - led_color_4 = LED_GREEN; - break; - } +#warning fix this +// switch((int)vehicle_status_raw.flight_mode) { +// case VEHICLE_FLIGHT_MODE_MANUAL: +// led_color_4 = LED_AMBER; +// break; +// +// case VEHICLE_FLIGHT_MODE_STAB: +// led_color_4 = LED_YELLOW; +// break; +// +// case VEHICLE_FLIGHT_MODE_HOLD: +// led_color_4 = LED_BLUE; +// break; +// +// case VEHICLE_FLIGHT_MODE_AUTO: +// led_color_4 = LED_GREEN; +// break; +// } if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) { - /* handling used sat´s */ + /* handling used sat�s */ if(num_of_used_sats >= 7) { led_color_1 = LED_OFF; led_color_2 = LED_OFF; diff --git a/apps/fixedwing_att_control/fixedwing_att_control_main.c b/apps/fixedwing_att_control/fixedwing_att_control_main.c index aa9db6d528..12f06cdd29 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_main.c +++ b/apps/fixedwing_att_control/fixedwing_att_control_main.c @@ -185,87 +185,88 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) /* control */ - if (vstatus.state_machine == SYSTEM_STATE_AUTO || - vstatus.state_machine == SYSTEM_STATE_STABILIZED) { - /* attitude control */ - fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); - - /* angular rate control */ - fixedwing_att_control_rates(&rates_sp, gyro, &actuators); - - /* pass through throttle */ - actuators.control[3] = att_sp.thrust; - - /* set flaps to zero */ - actuators.control[4] = 0.0f; - - } else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) { - if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { - - /* if the RC signal is lost, try to stay level and go slowly back down to ground */ - if (vstatus.rc_signal_lost) { - - /* put plane into loiter */ - att_sp.roll_body = 0.3f; - att_sp.pitch_body = 0.0f; - - /* limit throttle to 60 % of last value if sane */ - if (isfinite(manual_sp.throttle) && - (manual_sp.throttle >= 0.0f) && - (manual_sp.throttle <= 1.0f)) { - att_sp.thrust = 0.6f * manual_sp.throttle; - - } else { - att_sp.thrust = 0.0f; - } - - att_sp.yaw_body = 0; - - // XXX disable yaw control, loiter - - } else { - - att_sp.roll_body = manual_sp.roll; - att_sp.pitch_body = manual_sp.pitch; - att_sp.yaw_body = 0; - att_sp.thrust = manual_sp.throttle; - } - - att_sp.timestamp = hrt_absolute_time(); - - /* attitude control */ - fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); - - /* angular rate control */ - fixedwing_att_control_rates(&rates_sp, gyro, &actuators); - - /* pass through throttle */ - actuators.control[3] = att_sp.thrust; - - /* pass through flaps */ - if (isfinite(manual_sp.flaps)) { - actuators.control[4] = manual_sp.flaps; - - } else { - actuators.control[4] = 0.0f; - } - - } else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { - /* directly pass through values */ - actuators.control[0] = manual_sp.roll; - /* positive pitch means negative actuator -> pull up */ - actuators.control[1] = manual_sp.pitch; - actuators.control[2] = manual_sp.yaw; - actuators.control[3] = manual_sp.throttle; - - if (isfinite(manual_sp.flaps)) { - actuators.control[4] = manual_sp.flaps; - - } else { - actuators.control[4] = 0.0f; - } - } - } +#warning fix this +// if (vstatus.state_machine == SYSTEM_STATE_AUTO || +// vstatus.state_machine == SYSTEM_STATE_STABILIZED) { +// /* attitude control */ +// fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); +// +// /* angular rate control */ +// fixedwing_att_control_rates(&rates_sp, gyro, &actuators); +// +// /* pass through throttle */ +// actuators.control[3] = att_sp.thrust; +// +// /* set flaps to zero */ +// actuators.control[4] = 0.0f; +// +// } else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) { +// if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { +// +// /* if the RC signal is lost, try to stay level and go slowly back down to ground */ +// if (vstatus.rc_signal_lost) { +// +// /* put plane into loiter */ +// att_sp.roll_body = 0.3f; +// att_sp.pitch_body = 0.0f; +// +// /* limit throttle to 60 % of last value if sane */ +// if (isfinite(manual_sp.throttle) && +// (manual_sp.throttle >= 0.0f) && +// (manual_sp.throttle <= 1.0f)) { +// att_sp.thrust = 0.6f * manual_sp.throttle; +// +// } else { +// att_sp.thrust = 0.0f; +// } +// +// att_sp.yaw_body = 0; +// +// // XXX disable yaw control, loiter +// +// } else { +// +// att_sp.roll_body = manual_sp.roll; +// att_sp.pitch_body = manual_sp.pitch; +// att_sp.yaw_body = 0; +// att_sp.thrust = manual_sp.throttle; +// } +// +// att_sp.timestamp = hrt_absolute_time(); +// +// /* attitude control */ +// fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); +// +// /* angular rate control */ +// fixedwing_att_control_rates(&rates_sp, gyro, &actuators); +// +// /* pass through throttle */ +// actuators.control[3] = att_sp.thrust; +// +// /* pass through flaps */ +// if (isfinite(manual_sp.flaps)) { +// actuators.control[4] = manual_sp.flaps; +// +// } else { +// actuators.control[4] = 0.0f; +// } +// +// } else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { +// /* directly pass through values */ +// actuators.control[0] = manual_sp.roll; +// /* positive pitch means negative actuator -> pull up */ +// actuators.control[1] = manual_sp.pitch; +// actuators.control[2] = manual_sp.yaw; +// actuators.control[3] = manual_sp.throttle; +// +// if (isfinite(manual_sp.flaps)) { +// actuators.control[4] = manual_sp.flaps; +// +// } else { +// actuators.control[4] = 0.0f; +// } +// } +// } /* publish rates */ orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp); diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index b958d5f96c..4636ee36ec 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -195,95 +195,85 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) /* reset MAVLink mode bitfield */ *mavlink_mode = 0; - /* set mode flags independent of system state */ + /** + * Set mode flags + **/ /* HIL */ if (v_status.flag_hil_enabled) { *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; } - /* manual input */ - if (v_status.flag_control_manual_enabled) { - *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; - } - - /* attitude or rate control */ - if (v_status.flag_control_attitude_enabled || - v_status.flag_control_rates_enabled) { - *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; - } - - /* vector control */ - if (v_status.flag_control_velocity_enabled || - v_status.flag_control_position_enabled) { - *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; - } - /* autonomous mode */ - if (v_status.state_machine == SYSTEM_STATE_AUTO) { + if (v_status.navigation_state == NAVIGATION_STATE_MISSION + || v_status.navigation_state == NAVIGATION_STATE_TAKEOFF + || v_status.navigation_state == NAVIGATION_STATE_RTL + || v_status.navigation_state == NAVIGATION_STATE_LAND + || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) { *mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED; } /* set arming state */ - if (armed.armed) { + if (v_status.flag_system_armed) { *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } else { *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; } - switch (v_status.state_machine) { - case SYSTEM_STATE_PREFLIGHT: - if (v_status.flag_preflight_gyro_calibration || - v_status.flag_preflight_mag_calibration || - v_status.flag_preflight_accel_calibration) { - *mavlink_state = MAV_STATE_CALIBRATING; + if (v_status.navigation_state == NAVIGATION_STATE_MANUAL) { - } else { - *mavlink_state = MAV_STATE_UNINIT; - } + *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + } - break; + if (v_status.navigation_state == NAVIGATION_STATE_SEATBELT) { + + *mavlink_mode |= MAV_MODE_FLAG_DECODE_POSITION_GUIDED; + } + + if (v_status.navigation_state == NAVIGATION_STATE_SEATBELT) { + + *mavlink_mode |= MAV_MODE_FLAG_DECODE_POSITION_GUIDED; + } + + + /** + * Set mavlink state + **/ + + /* set calibration state */ + if (v_status.flag_preflight_gyro_calibration || + v_status.flag_preflight_mag_calibration || + v_status.flag_preflight_accel_calibration) { + + *mavlink_state = MAV_STATE_CALIBRATING; + + } else if (v_status.flag_system_emergency) { + + *mavlink_state = MAV_STATE_EMERGENCY; + + } else if (v_status.navigation_state == NAVIGATION_STATE_MANUAL + || v_status.navigation_state == NAVIGATION_STATE_SEATBELT + || v_status.navigation_state == NAVIGATION_STATE_LOITER + || v_status.navigation_state == NAVIGATION_STATE_MISSION + || v_status.navigation_state == NAVIGATION_STATE_RTL + || v_status.navigation_state == NAVIGATION_STATE_LAND + || v_status.navigation_state == NAVIGATION_STATE_TAKEOFF + || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) { + + *mavlink_state = MAV_STATE_ACTIVE; + + } else if (v_status.navigation_state == NAVIGATION_STATE_STANDBY) { - case SYSTEM_STATE_STANDBY: *mavlink_state = MAV_STATE_STANDBY; - break; - case SYSTEM_STATE_GROUND_READY: - *mavlink_state = MAV_STATE_ACTIVE; - break; + } else if (v_status.navigation_state == NAVIGATION_STATE_INIT) { - case SYSTEM_STATE_MANUAL: - *mavlink_state = MAV_STATE_ACTIVE; - break; + *mavlink_state = MAV_STATE_UNINIT; + } else { - case SYSTEM_STATE_STABILIZED: - *mavlink_state = MAV_STATE_ACTIVE; - break; - - case SYSTEM_STATE_AUTO: - *mavlink_state = MAV_STATE_ACTIVE; - break; - - case SYSTEM_STATE_MISSION_ABORT: - *mavlink_state = MAV_STATE_EMERGENCY; - break; - - case SYSTEM_STATE_EMCY_LANDING: - *mavlink_state = MAV_STATE_EMERGENCY; - break; - - case SYSTEM_STATE_EMCY_CUTOFF: - *mavlink_state = MAV_STATE_EMERGENCY; - break; - - case SYSTEM_STATE_GROUND_ERROR: - *mavlink_state = MAV_STATE_EMERGENCY; - break; - - case SYSTEM_STATE_REBOOT: - *mavlink_state = MAV_STATE_POWEROFF; - break; + warnx("Unknown mavlink state"); + *mavlink_state = MAV_STATE_CRITICAL; } } @@ -688,7 +678,7 @@ int mavlink_thread_main(int argc, char *argv[]) get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); /* send heartbeat */ - mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state); + mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state); /* switch HIL mode if required */ set_hil_on_off(v_status.flag_hil_enabled); diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index 9f85d5801f..15066010c6 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -275,7 +275,7 @@ l_vehicle_status(struct listener *l) mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, - v_status.state_machine, + v_status.navigation_state, mavlink_state); } diff --git a/apps/mavlink_onboard/mavlink.c b/apps/mavlink_onboard/mavlink.c index 33ebe8600a..6babe14ae2 100644 --- a/apps/mavlink_onboard/mavlink.c +++ b/apps/mavlink_onboard/mavlink.c @@ -296,60 +296,60 @@ get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; } - switch (v_status->state_machine) { - case SYSTEM_STATE_PREFLIGHT: - if (v_status->flag_preflight_gyro_calibration || - v_status->flag_preflight_mag_calibration || - v_status->flag_preflight_accel_calibration) { - *mavlink_state = MAV_STATE_CALIBRATING; - } else { - *mavlink_state = MAV_STATE_UNINIT; - } - break; - - case SYSTEM_STATE_STANDBY: - *mavlink_state = MAV_STATE_STANDBY; - break; - - case SYSTEM_STATE_GROUND_READY: - *mavlink_state = MAV_STATE_ACTIVE; - break; - - case SYSTEM_STATE_MANUAL: - *mavlink_state = MAV_STATE_ACTIVE; - *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; - break; - - case SYSTEM_STATE_STABILIZED: - *mavlink_state = MAV_STATE_ACTIVE; - *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; - break; - - case SYSTEM_STATE_AUTO: - *mavlink_state = MAV_STATE_ACTIVE; - *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; - break; - - case SYSTEM_STATE_MISSION_ABORT: - *mavlink_state = MAV_STATE_EMERGENCY; - break; - - case SYSTEM_STATE_EMCY_LANDING: - *mavlink_state = MAV_STATE_EMERGENCY; - break; - - case SYSTEM_STATE_EMCY_CUTOFF: - *mavlink_state = MAV_STATE_EMERGENCY; - break; - - case SYSTEM_STATE_GROUND_ERROR: - *mavlink_state = MAV_STATE_EMERGENCY; - break; - - case SYSTEM_STATE_REBOOT: - *mavlink_state = MAV_STATE_POWEROFF; - break; - } +// switch (v_status->state_machine) { +// case SYSTEM_STATE_PREFLIGHT: +// if (v_status->flag_preflight_gyro_calibration || +// v_status->flag_preflight_mag_calibration || +// v_status->flag_preflight_accel_calibration) { +// *mavlink_state = MAV_STATE_CALIBRATING; +// } else { +// *mavlink_state = MAV_STATE_UNINIT; +// } +// break; +// +// case SYSTEM_STATE_STANDBY: +// *mavlink_state = MAV_STATE_STANDBY; +// break; +// +// case SYSTEM_STATE_GROUND_READY: +// *mavlink_state = MAV_STATE_ACTIVE; +// break; +// +// case SYSTEM_STATE_MANUAL: +// *mavlink_state = MAV_STATE_ACTIVE; +// *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; +// break; +// +// case SYSTEM_STATE_STABILIZED: +// *mavlink_state = MAV_STATE_ACTIVE; +// *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; +// break; +// +// case SYSTEM_STATE_AUTO: +// *mavlink_state = MAV_STATE_ACTIVE; +// *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; +// break; +// +// case SYSTEM_STATE_MISSION_ABORT: +// *mavlink_state = MAV_STATE_EMERGENCY; +// break; +// +// case SYSTEM_STATE_EMCY_LANDING: +// *mavlink_state = MAV_STATE_EMERGENCY; +// break; +// +// case SYSTEM_STATE_EMCY_CUTOFF: +// *mavlink_state = MAV_STATE_EMERGENCY; +// break; +// +// case SYSTEM_STATE_GROUND_ERROR: +// *mavlink_state = MAV_STATE_EMERGENCY; +// break; +// +// case SYSTEM_STATE_REBOOT: +// *mavlink_state = MAV_STATE_POWEROFF; +// break; +// } } @@ -434,7 +434,7 @@ int mavlink_thread_main(int argc, char *argv[]) get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode); /* send heartbeat */ - mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state); + mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state); /* send status (values already copied in the section above) */ mavlink_msg_sys_status_send(chan, diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index d94c0a69c9..da7550f797 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -296,38 +296,39 @@ mc_thread_main(int argc, char *argv[]) } /* act if stabilization is active or if the (nonsense) direct pass through mode is set */ - if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS || - state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { - - if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) { - rates_sp.yaw = manual.yaw; - control_yaw_position = false; - - } else { - /* - * This mode SHOULD be the default mode, which is: - * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS - * - * However, we fall back to this setting for all other (nonsense) - * settings as well. - */ - - /* only move setpoint if manual input is != 0 */ - if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { - rates_sp.yaw = manual.yaw; - control_yaw_position = false; - first_time_after_yaw_speed_control = true; - - } else { - if (first_time_after_yaw_speed_control) { - att_sp.yaw_body = att.yaw; - first_time_after_yaw_speed_control = false; - } - - control_yaw_position = true; - } - } - } +#warning fix this +// if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS || +// state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { +// +// if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) { +// rates_sp.yaw = manual.yaw; +// control_yaw_position = false; +// +// } else { +// /* +// * This mode SHOULD be the default mode, which is: +// * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS +// * +// * However, we fall back to this setting for all other (nonsense) +// * settings as well. +// */ +// +// /* only move setpoint if manual input is != 0 */ +// if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { +// rates_sp.yaw = manual.yaw; +// control_yaw_position = false; +// first_time_after_yaw_speed_control = true; +// +// } else { +// if (first_time_after_yaw_speed_control) { +// att_sp.yaw_body = att.yaw; +// first_time_after_yaw_speed_control = false; +// } +// +// control_yaw_position = true; +// } +// } +// } att_sp.thrust = manual.throttle; att_sp.timestamp = hrt_absolute_time(); @@ -348,16 +349,17 @@ mc_thread_main(int argc, char *argv[]) } } else { +#warning fix this /* manual rate inputs, from RC control or joystick */ - if (state.flag_control_rates_enabled && - state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) { - rates_sp.roll = manual.roll; - - rates_sp.pitch = manual.pitch; - rates_sp.yaw = manual.yaw; - rates_sp.thrust = manual.throttle; - rates_sp.timestamp = hrt_absolute_time(); - } +// if (state.flag_control_rates_enabled && +// state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) { +// rates_sp.roll = manual.roll; +// +// rates_sp.pitch = manual.pitch; +// rates_sp.yaw = manual.yaw; +// rates_sp.thrust = manual.throttle; +// rates_sp.timestamp = hrt_absolute_time(); +// } } } diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index d8d200ea9d..28579bc703 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -195,13 +195,11 @@ private: int rc_map_yaw; int rc_map_throttle; - int rc_map_manual_override_sw; - int rc_map_auto_mode_sw; + int rc_map_mode_sw; + int rc_map_return_sw; + int rc_map_mission_sw; - int rc_map_manual_mode_sw; - int rc_map_sas_mode_sw; - int rc_map_rtl_sw; - int rc_map_offboard_ctrl_mode_sw; +// int rc_map_offboard_ctrl_mode_sw; int rc_map_flaps; @@ -241,13 +239,11 @@ private: param_t rc_map_yaw; param_t rc_map_throttle; - param_t rc_map_manual_override_sw; - param_t rc_map_auto_mode_sw; + param_t rc_map_mode_sw; + param_t rc_map_return_sw; + param_t rc_map_mission_sw; - param_t rc_map_manual_mode_sw; - param_t rc_map_sas_mode_sw; - param_t rc_map_rtl_sw; - param_t rc_map_offboard_ctrl_mode_sw; +// param_t rc_map_offboard_ctrl_mode_sw; param_t rc_map_flaps; @@ -436,16 +432,15 @@ Sensors::Sensors() : _parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE"); /* mandatory mode switches, mapped to channel 5 and 6 per default */ - _parameter_handles.rc_map_manual_override_sw = param_find("RC_MAP_OVER_SW"); - _parameter_handles.rc_map_auto_mode_sw = param_find("RC_MAP_MODE_SW"); + _parameter_handles.rc_map_mode_sw = param_find("RC_MAP_OVER_SW"); + _parameter_handles.rc_map_return_sw = param_find("RC_MAP_RETURN_SW"); _parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS"); /* optional mode switches, not mapped per default */ - _parameter_handles.rc_map_manual_mode_sw = param_find("RC_MAP_MAN_SW"); - _parameter_handles.rc_map_sas_mode_sw = param_find("RC_MAP_SAS_SW"); - _parameter_handles.rc_map_rtl_sw = param_find("RC_MAP_RTL_SW"); - _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); + _parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSION_SW"); + +// _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); _parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1"); _parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2"); @@ -579,33 +574,25 @@ Sensors::parameters_update() warnx("Failed getting throttle chan index"); } - if (param_get(_parameter_handles.rc_map_manual_override_sw, &(_parameters.rc_map_manual_override_sw)) != OK) { - warnx("Failed getting override sw chan index"); + if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) { + warnx("Failed getting mode sw chan index"); } - if (param_get(_parameter_handles.rc_map_auto_mode_sw, &(_parameters.rc_map_auto_mode_sw)) != OK) { - warnx("Failed getting auto mode sw chan index"); + if (param_get(_parameter_handles.rc_map_return_sw, &(_parameters.rc_map_return_sw)) != OK) { + warnx("Failed getting return sw chan index"); + } + + if (param_get(_parameter_handles.rc_map_mission_sw, &(_parameters.rc_map_mission_sw)) != OK) { + warnx("Failed getting mission sw chan index"); } if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { warnx("Failed getting flaps chan index"); } - if (param_get(_parameter_handles.rc_map_manual_mode_sw, &(_parameters.rc_map_manual_mode_sw)) != OK) { - warnx("Failed getting manual mode sw chan index"); - } - - if (param_get(_parameter_handles.rc_map_rtl_sw, &(_parameters.rc_map_rtl_sw)) != OK) { - warnx("Failed getting rtl sw chan index"); - } - - if (param_get(_parameter_handles.rc_map_sas_mode_sw, &(_parameters.rc_map_sas_mode_sw)) != OK) { - warnx("Failed getting sas mode sw chan index"); - } - - if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) { - warnx("Failed getting offboard control mode sw chan index"); - } +// if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) { +// warnx("Failed getting offboard control mode sw chan index"); +// } if (param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)) != OK) { warnx("Failed getting mode aux 1 index"); @@ -649,15 +636,13 @@ Sensors::parameters_update() _rc.function[PITCH] = _parameters.rc_map_pitch - 1; _rc.function[YAW] = _parameters.rc_map_yaw - 1; - _rc.function[OVERRIDE] = _parameters.rc_map_manual_override_sw - 1; - _rc.function[AUTO_MODE] = _parameters.rc_map_auto_mode_sw - 1; + _rc.function[MODE] = _parameters.rc_map_mode_sw - 1; + _rc.function[RETURN] = _parameters.rc_map_return_sw - 1; + _rc.function[MISSION] = _parameters.rc_map_mission_sw - 1; _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; - _rc.function[MANUAL_MODE] = _parameters.rc_map_manual_mode_sw - 1; - _rc.function[RTL] = _parameters.rc_map_rtl_sw - 1; - _rc.function[SAS_MODE] = _parameters.rc_map_sas_mode_sw - 1; - _rc.function[OFFBOARD_MODE] = _parameters.rc_map_offboard_ctrl_mode_sw - 1; +// _rc.function[OFFBOARD_MODE] = _parameters.rc_map_offboard_ctrl_mode_sw - 1; _rc.function[AUX_1] = _parameters.rc_map_aux1 - 1; _rc.function[AUX_2] = _parameters.rc_map_aux2 - 1; @@ -1122,10 +1107,10 @@ Sensors::ppm_poll() manual_control.yaw = NAN; manual_control.throttle = NAN; - manual_control.manual_mode_switch = NAN; - manual_control.manual_sas_switch = NAN; - manual_control.return_to_launch_switch = NAN; - manual_control.auto_offboard_input_switch = NAN; + manual_control.mode_switch = NAN; + manual_control.return_switch = NAN; + manual_control.mission_switch = NAN; +// manual_control.auto_offboard_input_switch = NAN; manual_control.flaps = NAN; manual_control.aux1 = NAN; @@ -1211,11 +1196,14 @@ Sensors::ppm_poll() manual_control.yaw *= _parameters.rc_scale_yaw; } - /* override switch input */ - manual_control.manual_override_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OVERRIDE]].scaled); - /* mode switch input */ - manual_control.auto_mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[AUTO_MODE]].scaled); + manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled); + + /* land switch input */ + manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); + + /* land switch input */ + manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); /* flaps */ if (_rc.function[FLAPS] >= 0) { @@ -1227,21 +1215,17 @@ Sensors::ppm_poll() } } - if (_rc.function[MANUAL_MODE] >= 0) { - manual_control.manual_mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MANUAL_MODE]].scaled); + if (_rc.function[MODE] >= 0) { + manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled); } - if (_rc.function[SAS_MODE] >= 0) { - manual_control.manual_sas_switch = limit_minus_one_to_one(_rc.chan[_rc.function[SAS_MODE]].scaled); + if (_rc.function[MISSION] >= 0) { + manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); } - if (_rc.function[RTL] >= 0) { - manual_control.return_to_launch_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RTL]].scaled); - } - - if (_rc.function[OFFBOARD_MODE] >= 0) { - manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); - } +// if (_rc.function[OFFBOARD_MODE] >= 0) { +// manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); +// } /* aux functions, only assign if valid mapping is present */ if (_rc.function[AUX_1] >= 0) { diff --git a/apps/uORB/topics/manual_control_setpoint.h b/apps/uORB/topics/manual_control_setpoint.h index 261a8a4ad3..cfee81ea2b 100644 --- a/apps/uORB/topics/manual_control_setpoint.h +++ b/apps/uORB/topics/manual_control_setpoint.h @@ -56,17 +56,17 @@ struct manual_control_setpoint_s { float yaw; /**< rudder / yaw rate / yaw */ float throttle; /**< throttle / collective thrust / altitude */ - float manual_override_switch; /**< manual override mode (mandatory) */ - float auto_mode_switch; /**< auto mode switch (mandatory) */ + float mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */ + float return_switch; /**< land 2 position switch (mandatory): land, no effect */ + float mission_switch; /**< mission 2 position switch (optional): mission, loiter */ /** * Any of the channels below may not be available and be set to NaN * to indicate that it does not contain valid data. */ - float manual_mode_switch; /**< manual mode (man, sas, alt) switch (optional) */ - float manual_sas_switch; /**< sas mode (rates / attitude) switch (optional) */ - float return_to_launch_switch; /**< return to launch switch (0 = disabled, 1 = enabled) */ - float auto_offboard_input_switch; /**< controller setpoint source (0 = onboard, 1 = offboard) */ + + // XXX needed or parameter? + //float auto_offboard_input_switch; /**< controller setpoint source (0 = onboard, 1 = offboard) */ float flaps; /**< flap position */ diff --git a/apps/uORB/topics/rc_channels.h b/apps/uORB/topics/rc_channels.h index 9dd54df915..a0bb25af46 100644 --- a/apps/uORB/topics/rc_channels.h +++ b/apps/uORB/topics/rc_channels.h @@ -68,18 +68,16 @@ enum RC_CHANNELS_FUNCTION ROLL = 1, PITCH = 2, YAW = 3, - OVERRIDE = 4, - AUTO_MODE = 5, - MANUAL_MODE = 6, - SAS_MODE = 7, - RTL = 8, - OFFBOARD_MODE = 9, - FLAPS = 10, - AUX_1 = 11, - AUX_2 = 12, - AUX_3 = 13, - AUX_4 = 14, - AUX_5 = 15, + MODE = 4, + RETURN = 5, + MISSION = 6, + OFFBOARD_MODE = 7, + FLAPS = 8, + AUX_1 = 9, + AUX_2 = 10, + AUX_3 = 11, + AUX_4 = 12, + AUX_5 = 13, RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */ }; diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index 06b4c5ca5c..f9c4a5fff9 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -59,21 +59,30 @@ */ /* State Machine */ -typedef enum -{ - SYSTEM_STATE_PREFLIGHT = 0, - SYSTEM_STATE_STANDBY = 1, - SYSTEM_STATE_GROUND_READY = 2, - SYSTEM_STATE_MANUAL = 3, - SYSTEM_STATE_STABILIZED = 4, - SYSTEM_STATE_AUTO = 5, - SYSTEM_STATE_MISSION_ABORT = 6, - SYSTEM_STATE_EMCY_LANDING = 7, - SYSTEM_STATE_EMCY_CUTOFF = 8, - SYSTEM_STATE_GROUND_ERROR = 9, - SYSTEM_STATE_REBOOT= 10, +typedef enum { + NAVIGATION_STATE_INIT = 0, + NAVIGATION_STATE_STANDBY, + NAVIGATION_STATE_MANUAL, + NAVIGATION_STATE_SEATBELT, + NAVIGATION_STATE_LOITER, + NAVIGATION_STATE_AUTO_READY, + NAVIGATION_STATE_MISSION, + NAVIGATION_STATE_RTL, + NAVIGATION_STATE_TAKEOFF, + NAVIGATION_STATE_LAND, + NAVIGATION_STATE_GROUND_ERROR, + NAVIGATION_STATE_REBOOT +} navigation_state_t; -} commander_state_machine_t; +typedef enum { + ARMING_STATE_INIT = 0, + ARMING_STATE_STANDBY, + ARMING_STATE_ARMED, + ARMING_STATE_MISSION_ABORT, + ARMING_STATE_ERROR, + ARMING_STATE_REBOOT, + ARMING_STATE_IN_AIR_RESTORE +} arming_state_t; enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, @@ -86,25 +95,25 @@ enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1 }; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */ -enum VEHICLE_FLIGHT_MODE { - VEHICLE_FLIGHT_MODE_MANUAL = 0, /**< direct manual control, exact mode determined by VEHICLE_MANUAL_CONTROL_MODE */ - VEHICLE_FLIGHT_MODE_STAB, /**< attitude or rate stabilization plus velocity or position stabilization */ - VEHICLE_FLIGHT_MODE_HOLD, /**< hold current position (hover or loiter around position when switched) */ - VEHICLE_FLIGHT_MODE_AUTO /**< attitude or rate stabilization plus absolute position control and waypoints */ -}; - -enum VEHICLE_MANUAL_CONTROL_MODE { - VEHICLE_MANUAL_CONTROL_MODE_DIRECT = 0, /**< no attitude control, direct stick input mixing (only fixed wing) */ - VEHICLE_MANUAL_CONTROL_MODE_RATES, /**< body rates control mode */ - VEHICLE_MANUAL_CONTROL_MODE_SAS /**< stability augmented system (SAS) mode */ -}; - -enum VEHICLE_MANUAL_SAS_MODE { - VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS = 0, /**< roll, pitch and yaw absolute */ - VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE, /**< roll and pitch absolute, yaw rate */ - VEHICLE_MANUAL_SAS_MODE_SIMPLE, /**< simple mode (includes altitude hold) */ - VEHICLE_MANUAL_SAS_MODE_ALTITUDE /**< altitude hold */ -}; +//enum VEHICLE_FLIGHT_MODE { +// VEHICLE_FLIGHT_MODE_MANUAL = 0, /**< direct manual control, exact mode determined by VEHICLE_MANUAL_CONTROL_MODE */ +// VEHICLE_FLIGHT_MODE_STAB, /**< attitude or rate stabilization plus velocity or position stabilization */ +// VEHICLE_FLIGHT_MODE_HOLD, /**< hold current position (hover or loiter around position when switched) */ +// VEHICLE_FLIGHT_MODE_AUTO /**< attitude or rate stabilization plus absolute position control and waypoints */ +//}; +// +//enum VEHICLE_MANUAL_CONTROL_MODE { +// VEHICLE_MANUAL_CONTROL_MODE_DIRECT = 0, /**< no attitude control, direct stick input mixing (only fixed wing) */ +// VEHICLE_MANUAL_CONTROL_MODE_RATES, /**< body rates control mode */ +// VEHICLE_MANUAL_CONTROL_MODE_SAS /**< stability augmented system (SAS) mode */ +//}; +// +//enum VEHICLE_MANUAL_SAS_MODE { +// VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS = 0, /**< roll, pitch and yaw absolute */ +// VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE, /**< roll and pitch absolute, yaw rate */ +// VEHICLE_MANUAL_SAS_MODE_SIMPLE, /**< simple mode (includes altitude hold) */ +// VEHICLE_MANUAL_SAS_MODE_ALTITUDE /**< altitude hold */ +//}; /** * Should match 1:1 MAVLink's MAV_TYPE ENUM @@ -134,7 +143,7 @@ enum VEHICLE_TYPE { enum VEHICLE_BATTERY_WARNING { VEHICLE_BATTERY_WARNING_NONE = 0, /**< no battery low voltage warning active */ VEHICLE_BATTERY_WARNING_WARNING, /**< warning of low voltage 1. stage */ - VEHICLE_BATTERY_WARNING_ALERT /**< aleting of low voltage 2. stage */ + VEHICLE_BATTERY_WARNING_ALERT /**< alerting of low voltage 2. stage */ }; @@ -150,17 +159,17 @@ struct vehicle_status_s uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ uint64_t failsave_lowlevel_start_time; /**< time when the lowlevel failsafe flag was set */ - //uint64_t failsave_highlevel_begin; TO BE COMPLETED +// uint64_t failsave_highlevel_begin; TO BE COMPLETED + + navigation_state_t navigation_state; /**< current navigation state */ + arming_state_t arming_state; /**< current arming state */ - commander_state_machine_t state_machine; /**< current flight state, main state machine */ - enum VEHICLE_FLIGHT_MODE flight_mode; /**< current flight mode, as defined by mode switch */ - enum VEHICLE_MANUAL_CONTROL_MODE manual_control_mode; /**< current attitude control mode, as defined by VEHICLE_ATTITUDE_MODE enum */ - enum VEHICLE_MANUAL_SAS_MODE manual_sas_mode; /**< current stabilization mode */ int32_t system_type; /**< system type, inspired by MAVLinks VEHICLE_TYPE enum */ /* system flags - these represent the state predicates */ bool flag_system_armed; /**< true is motors / actuators are armed */ + bool flag_system_emergency; bool flag_control_manual_enabled; /**< true if manual input is mixed in */ bool flag_control_offboard_enabled; /**< true if offboard control input is on */ bool flag_hil_enabled; /**< true if hardware in the loop simulation is enabled */ @@ -170,6 +179,9 @@ struct vehicle_status_s bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ bool flag_control_position_enabled; /**< true if position is controlled */ + bool flag_land_requested; /**< true if land requested */ + bool flag_mission_activated; /**< true if in mission mode */ + bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */ bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */ bool flag_preflight_accel_calibration; @@ -185,7 +197,7 @@ struct vehicle_status_s uint64_t offboard_control_signal_lost_interval; /**< interval in microseconds without an offboard control message */ bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ - //bool failsave_highlevel; + bool failsave_highlevel; /* see SYS_STATUS mavlink message for the following */ uint32_t onboard_control_sensors_present; From 3bc385c789f2b39cda066551ff1d5b767ab26aec Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 17 Feb 2013 15:04:01 -0800 Subject: [PATCH 002/486] Checkpoint: Added arming state check --- apps/commander/commander.c | 6 +++++- apps/commander/state_machine_helper.c | 22 ++++++++++++++++++++++ apps/commander/state_machine_helper.h | 2 ++ apps/sensors/sensor_params.c | 10 ++++------ apps/sensors/sensors.cpp | 2 +- apps/uORB/topics/vehicle_status.h | 2 ++ 6 files changed, 36 insertions(+), 8 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 8716caef72..1bfdd56600 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -2054,9 +2054,13 @@ int commander_thread_main(int argc, char *argv[]) state_changed = false; } + /* make changes in state machine if needed */ + update_state_machine(stat_pub, ¤t_status, mavlink_fd); + + + /* Store old modes to detect and act on state transitions */ voltage_previous = current_status.voltage_battery; - fflush(stdout); counter++; usleep(COMMANDER_MONITORING_INTERVAL); diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index e1ec73110c..d89be781a8 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -53,6 +53,28 @@ #include "state_machine_helper.h" +void update_state_machine(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +{ + /* check arming first */ + if (current_status->flag_system_armed && current_status->flag_system_emergency) { + do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_MISSION_ABORT); + } else if(current_status->flag_system_armed && !current_status->flag_system_emergency) { + do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_ARMED); + } else if(!current_status->flag_system_armed && current_status->flag_system_emergency) { + do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_ERROR); + } else if(!current_status->flag_system_armed && !current_status->flag_system_emergency) { + do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_STANDBY); + } else if (current_status->flag_system_sensors_ok) { + do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_STANDBY); + } else if (!current_status->flag_system_sensors_ok && current_status->flag_system_armed) { + do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_MISSION_ABORT); + } else if (!current_status->flag_system_sensors_ok && !current_status->flag_system_armed) { + do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_ERROR); + } + + /* now determine the navigation state */ +} + /** * Transition from one navigation state to another */ diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h index bf9296caf0..ed18bfbd24 100644 --- a/apps/commander/state_machine_helper.h +++ b/apps/commander/state_machine_helper.h @@ -47,6 +47,8 @@ #include #include +void update_state_machine(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); + int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state); int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state); diff --git a/apps/sensors/sensor_params.c b/apps/sensors/sensor_params.c index 9f23ebbbab..df7f661161 100644 --- a/apps/sensors/sensor_params.c +++ b/apps/sensors/sensor_params.c @@ -158,13 +158,11 @@ PARAM_DEFINE_INT32(RC_MAP_PITCH, 2); PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3); PARAM_DEFINE_INT32(RC_MAP_YAW, 4); -PARAM_DEFINE_INT32(RC_MAP_OVER_SW, 5); -PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 6); +PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5); +PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 6); +PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); -PARAM_DEFINE_INT32(RC_MAP_MAN_SW, 0); -PARAM_DEFINE_INT32(RC_MAP_SAS_SW, 0); -PARAM_DEFINE_INT32(RC_MAP_RTL_SW, 0); -PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); +//PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0); diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index 28579bc703..ce404ee7e3 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -438,7 +438,7 @@ Sensors::Sensors() : _parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS"); /* optional mode switches, not mapped per default */ - _parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSION_SW"); + _parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSIO_SW"); // _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index f9c4a5fff9..ba9b9793b0 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -170,6 +170,8 @@ struct vehicle_status_s bool flag_system_armed; /**< true is motors / actuators are armed */ bool flag_system_emergency; + bool flag_system_sensors_ok; + bool flag_control_manual_enabled; /**< true if manual input is mixed in */ bool flag_control_offboard_enabled; /**< true if offboard control input is on */ bool flag_hil_enabled; /**< true if hardware in the loop simulation is enabled */ From 47b05eeb87191fd0b380de008299f85262bc8953 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 17 Feb 2013 23:07:07 -0800 Subject: [PATCH 003/486] Checkpoint, arming/disarming still has a bug --- apps/commander/commander.c | 225 ++++++++++----------- apps/commander/state_machine_helper.c | 270 +++++++++++++++----------- apps/commander/state_machine_helper.h | 2 - apps/mavlink/mavlink.c | 15 +- apps/uORB/topics/vehicle_status.h | 34 +++- 5 files changed, 303 insertions(+), 243 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 1bfdd56600..4e470f1d92 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -817,22 +817,24 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta case VEHICLE_CMD_COMPONENT_ARM_DISARM: { /* request to arm */ if ((int)cmd->param1 == 1) { - if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } + // XXX add this again +// if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { +// result = VEHICLE_CMD_RESULT_ACCEPTED; +// +// } else { +// result = VEHICLE_CMD_RESULT_DENIED; +// } /* request to disarm */ // XXX this arms it instad of disarming } else if ((int)cmd->param1 == 0) { - if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } + // XXX add this again +// if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { +// result = VEHICLE_CMD_RESULT_ACCEPTED; +// +// } else { +// result = VEHICLE_CMD_RESULT_DENIED; +// } } } break; @@ -840,7 +842,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request for an autopilot reboot */ case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { if ((int)cmd->param1 == 1) { - if (OK == do_navigation_state_update(status_pub, current_vehicle_status, mavlink_fd, NAVIGATION_STATE_REBOOT)) { + if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_REBOOT)) { /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -883,25 +885,26 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if ((int)(cmd->param1) == 1) { /* transition to init state */ - if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) - && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { - - mavlink_log_info(mavlink_fd, "starting gyro cal"); - tune_confirm(); - do_gyro_calibration(status_pub, ¤t_status); - mavlink_log_info(mavlink_fd, "finished gyro cal"); - tune_confirm(); - - /* back to standby state */ - do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); - do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); - - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING gyro cal"); - result = VEHICLE_CMD_RESULT_DENIED; - } + // XXX add this again +// if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) +// && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { +// +// mavlink_log_info(mavlink_fd, "starting gyro cal"); +// tune_confirm(); +// do_gyro_calibration(status_pub, ¤t_status); +// mavlink_log_info(mavlink_fd, "finished gyro cal"); +// tune_confirm(); +// +// /* back to standby state */ +// do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); +// do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); +// +// result = VEHICLE_CMD_RESULT_ACCEPTED; +// +// } else { +// mavlink_log_critical(mavlink_fd, "REJECTING gyro cal"); +// result = VEHICLE_CMD_RESULT_DENIED; +// } handled = true; } @@ -910,24 +913,25 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if ((int)(cmd->param2) == 1) { /* transition to init state */ - if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) - && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { - mavlink_log_info(mavlink_fd, "starting mag cal"); - tune_confirm(); - do_mag_calibration(status_pub, ¤t_status); - mavlink_log_info(mavlink_fd, "finished mag cal"); - tune_confirm(); - - /* back to standby state */ - do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); - do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); - - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING mag cal"); - result = VEHICLE_CMD_RESULT_DENIED; - } + // XXX add this again +// if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) +// && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { +// mavlink_log_info(mavlink_fd, "starting mag cal"); +// tune_confirm(); +// do_mag_calibration(status_pub, ¤t_status); +// mavlink_log_info(mavlink_fd, "finished mag cal"); +// tune_confirm(); +// +// /* back to standby state */ +// do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); +// do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); +// +// result = VEHICLE_CMD_RESULT_ACCEPTED; +// +// } else { +// mavlink_log_critical(mavlink_fd, "REJECTING mag cal"); +// result = VEHICLE_CMD_RESULT_DENIED; +// } handled = true; } @@ -935,21 +939,22 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* zero-altitude pressure calibration */ if ((int)(cmd->param3) == 1) { /* transition to calibration state */ - if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) - && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { - - // XXX implement this - mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented"); - tune_confirm(); - - /* back to standby state */ - do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); - do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration"); - result = VEHICLE_CMD_RESULT_DENIED; - } + // XXX add this again +// if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) +// && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { +// +// // XXX implement this +// mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented"); +// tune_confirm(); +// +// /* back to standby state */ +// do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); +// do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); +// +// } else { +// mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration"); +// result = VEHICLE_CMD_RESULT_DENIED; +// } handled = true; } @@ -957,49 +962,51 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* trim calibration */ if ((int)(cmd->param4) == 1) { /* transition to calibration state */ - if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) - && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { - mavlink_log_info(mavlink_fd, "starting trim cal"); - tune_confirm(); - do_rc_calibration(status_pub, ¤t_status); - mavlink_log_info(mavlink_fd, "finished trim cal"); - tune_confirm(); - - /* back to standby state */ - do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); - do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); - - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING trim cal"); - result = VEHICLE_CMD_RESULT_DENIED; - } + // XXX add this again +// if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) +// && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { +// mavlink_log_info(mavlink_fd, "starting trim cal"); +// tune_confirm(); +// do_rc_calibration(status_pub, ¤t_status); +// mavlink_log_info(mavlink_fd, "finished trim cal"); +// tune_confirm(); +// +// /* back to standby state */ +// do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); +// do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); +// +// result = VEHICLE_CMD_RESULT_ACCEPTED; +// +// } else { +// mavlink_log_critical(mavlink_fd, "REJECTING trim cal"); +// result = VEHICLE_CMD_RESULT_DENIED; +// } handled = true; } /* accel calibration */ if ((int)(cmd->param5) == 1) { - if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) - && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { - - mavlink_log_info(mavlink_fd, "CMD starting accel cal"); - tune_confirm(); - do_accel_calibration(status_pub, ¤t_status); - tune_confirm(); - mavlink_log_info(mavlink_fd, "CMD finished accel cal"); - - /* back to standby state */ - do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); - do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); - - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING accel cal"); - result = VEHICLE_CMD_RESULT_DENIED; - } + // XXX add this again +// if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) +// && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { +// +// mavlink_log_info(mavlink_fd, "CMD starting accel cal"); +// tune_confirm(); +// do_accel_calibration(status_pub, ¤t_status); +// tune_confirm(); +// mavlink_log_info(mavlink_fd, "CMD finished accel cal"); +// +// /* back to standby state */ +// do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); +// do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); +// +// result = VEHICLE_CMD_RESULT_ACCEPTED; +// +// } else { +// mavlink_log_critical(mavlink_fd, "REJECTING accel cal"); +// result = VEHICLE_CMD_RESULT_DENIED; +// } handled = true; } @@ -1315,7 +1322,7 @@ int commander_thread_main(int argc, char *argv[]) /* make sure we are in preflight state */ memset(¤t_status, 0, sizeof(current_status)); - current_status.navigation_state = NAVIGATION_STATE_INIT; + current_status.navigation_state = NAVIGATION_STATE_STANDBY; current_status.arming_state = ARMING_STATE_INIT; current_status.flag_system_armed = false; @@ -1336,6 +1343,9 @@ int commander_thread_main(int argc, char *argv[]) /* set battery warning flag */ current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; + // XXX for now just set sensors as initialized + current_status.flag_system_sensors_initialized = true; + /* advertise to ORB */ stat_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); /* publish current state machine */ @@ -1886,8 +1896,8 @@ int commander_thread_main(int argc, char *argv[]) ((sp_man.yaw < -STICK_ON_OFF_LIMIT)) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - current_status.flag_system_armed = false; - stick_on_counter = 0; + do_arming_state_update(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + stick_off_counter = 0; } else { stick_off_counter++; @@ -1898,7 +1908,7 @@ int commander_thread_main(int argc, char *argv[]) /* check if left stick is in lower right position --> arm */ if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.2f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - current_status.flag_system_armed = true; + do_arming_state_update(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_ARMED); stick_on_counter = 0; } else { @@ -2054,9 +2064,6 @@ int commander_thread_main(int argc, char *argv[]) state_changed = false; } - /* make changes in state machine if needed */ - update_state_machine(stat_pub, ¤t_status, mavlink_fd); - /* Store old modes to detect and act on state transitions */ diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index d89be781a8..a0b786aabb 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -53,33 +53,12 @@ #include "state_machine_helper.h" -void update_state_machine(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - /* check arming first */ - if (current_status->flag_system_armed && current_status->flag_system_emergency) { - do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_MISSION_ABORT); - } else if(current_status->flag_system_armed && !current_status->flag_system_emergency) { - do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_ARMED); - } else if(!current_status->flag_system_armed && current_status->flag_system_emergency) { - do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_ERROR); - } else if(!current_status->flag_system_armed && !current_status->flag_system_emergency) { - do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_STANDBY); - } else if (current_status->flag_system_sensors_ok) { - do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_STANDBY); - } else if (!current_status->flag_system_sensors_ok && current_status->flag_system_armed) { - do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_MISSION_ABORT); - } else if (!current_status->flag_system_sensors_ok && !current_status->flag_system_armed) { - do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_ERROR); - } - - /* now determine the navigation state */ -} - /** * Transition from one navigation state to another */ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state) { + bool valid_path = false; bool valid_transition = false; int ret = ERROR; @@ -89,41 +68,43 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_ } else { switch (new_state) { - case NAVIGATION_STATE_INIT: - - if (current_status->navigation_state == NAVIGATION_STATE_STANDBY) { - - mavlink_log_critical(mavlink_fd, "SWITCHED TO INIT STATE"); - valid_transition = true; - } - break; - case NAVIGATION_STATE_STANDBY: - if (current_status->navigation_state == NAVIGATION_STATE_INIT - || current_status->navigation_state == NAVIGATION_STATE_MANUAL + if (current_status->navigation_state == NAVIGATION_STATE_MANUAL || current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO STANDBY STATE"); - valid_transition = true; + if (!current_status->flag_system_armed) { + mavlink_log_critical(mavlink_fd, "Switched to STANDBY state"); + valid_transition = true; + } else { + mavlink_log_critical(mavlink_fd, "Rejected STANDBY state: armed"); + } + valid_path = true; } break; case NAVIGATION_STATE_MANUAL: - if ( - ( current_status->navigation_state == NAVIGATION_STATE_STANDBY - || current_status->navigation_state == NAVIGATION_STATE_SEATBELT - || current_status->navigation_state == NAVIGATION_STATE_LOITER - || current_status->navigation_state == NAVIGATION_STATE_MISSION - || current_status->navigation_state == NAVIGATION_STATE_RTL - || current_status->navigation_state == NAVIGATION_STATE_LAND - || current_status->navigation_state == NAVIGATION_STATE_TAKEOFF - || current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) - && current_status->arming_state == ARMING_STATE_ARMED) { + if (current_status->navigation_state == NAVIGATION_STATE_STANDBY) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO MANUAL STATE"); + /* only check for armed flag when coming from STANDBY XXX does that make sense? */ + if (current_status->flag_system_armed) { + mavlink_log_critical(mavlink_fd, "Switched to MANUAL state"); + valid_transition = true; + } else { + mavlink_log_critical(mavlink_fd, "Rejected MANUAL state: disarmed"); + } + valid_path = true; + } else if (current_status->navigation_state == NAVIGATION_STATE_SEATBELT + || current_status->navigation_state == NAVIGATION_STATE_LOITER + || current_status->navigation_state == NAVIGATION_STATE_MISSION + || current_status->navigation_state == NAVIGATION_STATE_RTL + || current_status->navigation_state == NAVIGATION_STATE_LAND + || current_status->navigation_state == NAVIGATION_STATE_TAKEOFF + || current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) { + mavlink_log_critical(mavlink_fd, "Switched to MANUAL state"); valid_transition = true; + valid_path = true; } break; @@ -133,67 +114,132 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_ || current_status->navigation_state == NAVIGATION_STATE_LOITER || current_status->navigation_state == NAVIGATION_STATE_MISSION) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO SEATBELT STATE"); + mavlink_log_critical(mavlink_fd, "Switched to SEATBELT state"); valid_transition = true; + valid_path = true; } break; case NAVIGATION_STATE_LOITER: - if ( ((current_status->navigation_state == NAVIGATION_STATE_MANUAL || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) - && current_status->flag_global_position_valid) - || current_status->navigation_state == NAVIGATION_STATE_MISSION) { - - mavlink_log_critical(mavlink_fd, "SWITCHED TO LOITER STATE"); + /* Check for position lock when coming from MANUAL or SEATBELT */ + if (current_status->navigation_state == NAVIGATION_STATE_MANUAL || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) { + mavlink_log_critical(mavlink_fd, "Switched to LOITER state"); valid_transition = true; - } + if (current_status->flag_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Switched to LOITER state"); + valid_transition = true; + + } else { + mavlink_log_critical(mavlink_fd, "Rejected LOITER state: no pos lock"); + } + valid_path = true; + } else if (current_status->navigation_state == NAVIGATION_STATE_MISSION) { + mavlink_log_critical(mavlink_fd, "Switched to LOITER state"); + valid_transition = true; + valid_path = true; + } break; case NAVIGATION_STATE_AUTO_READY: - if ( - (current_status->navigation_state == NAVIGATION_STATE_STANDBY - && current_status->flag_global_position_valid - && current_status->flag_valid_launch_position) - || current_status->navigation_state == NAVIGATION_STATE_LAND) { + /* coming from STANDBY pos and home lock are needed */ + if (current_status->navigation_state == NAVIGATION_STATE_STANDBY) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO AUTO READY STATE"); - valid_transition = true; + if (current_status->flag_global_position_valid && current_status->flag_valid_launch_position) { + mavlink_log_critical(mavlink_fd, "Switched to AUTO READY state"); + valid_transition = true; + } else if (!current_status->flag_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no pos lock"); + } else if (!current_status->flag_valid_launch_position) { + mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no home lock"); + } else { + mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no pos and home lock"); + } + valid_path = true; + /* coming from LAND home lock is needed */ + } else if (current_status->navigation_state == NAVIGATION_STATE_LAND) { + + if (current_status->flag_valid_launch_position) { + mavlink_log_critical(mavlink_fd, "Switched to AUTO READY state"); + valid_transition = true; + } else { + mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no home lock"); + } + valid_path = true; } break; case NAVIGATION_STATE_MISSION: - if ( - current_status->navigation_state == NAVIGATION_STATE_TAKEOFF - || current_status->navigation_state == NAVIGATION_STATE_RTL - || ( - (current_status->navigation_state == NAVIGATION_STATE_MANUAL - || current_status->navigation_state == NAVIGATION_STATE_SEATBELT - || current_status->navigation_state == NAVIGATION_STATE_LOITER) - && current_status->flag_global_position_valid - && current_status->flag_valid_launch_position) - ) { - - mavlink_log_critical(mavlink_fd, "SWITCHED TO MISSION STATE"); + /* coming from TAKEOFF or RTL is always possible */ + if (current_status->navigation_state == NAVIGATION_STATE_TAKEOFF + || current_status->navigation_state == NAVIGATION_STATE_RTL) { + mavlink_log_critical(mavlink_fd, "Switched to MISSION state"); valid_transition = true; + valid_path = true; + + /* coming from MANUAL or SEATBELT requires home and pos lock */ + } else if (current_status->navigation_state == NAVIGATION_STATE_MANUAL + || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) { + + if (current_status->flag_global_position_valid && current_status->flag_valid_launch_position) { + mavlink_log_critical(mavlink_fd, "Switched to MISSION state"); + valid_transition = true; + } else if (!current_status->flag_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no pos lock"); + } else if (!current_status->flag_valid_launch_position) { + mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no home lock"); + } else { + mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no pos and home lock"); + } + valid_path = true; + + /* coming from loiter a home lock is needed */ + } else if (current_status->navigation_state == NAVIGATION_STATE_LOITER) { + if (current_status->flag_valid_launch_position) { + mavlink_log_critical(mavlink_fd, "Switched to MISSION state"); + valid_transition = true; + } else { + mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no home lock"); + } + valid_path = true; } break; case NAVIGATION_STATE_RTL: - if ( - current_status->navigation_state == NAVIGATION_STATE_MISSION - || ( - (current_status->navigation_state == NAVIGATION_STATE_MANUAL - || current_status->navigation_state == NAVIGATION_STATE_SEATBELT - || current_status->navigation_state == NAVIGATION_STATE_LOITER) - && current_status->flag_global_position_valid - && current_status->flag_valid_launch_position) - ) { - - mavlink_log_critical(mavlink_fd, "SWITCHED TO RTL STATE"); + /* coming from MISSION is always possible */ + if (current_status->navigation_state == NAVIGATION_STATE_MISSION) { + mavlink_log_critical(mavlink_fd, "Switched to RTL state"); valid_transition = true; + valid_path = true; + + /* coming from MANUAL or SEATBELT requires home and pos lock */ + } else if (current_status->navigation_state == NAVIGATION_STATE_MANUAL + || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) { + + if (current_status->flag_global_position_valid && current_status->flag_valid_launch_position) { + mavlink_log_critical(mavlink_fd, "Switched to RTL state"); + valid_transition = true; + } else if (!current_status->flag_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rejected RTL state: no pos lock"); + } else if (!current_status->flag_valid_launch_position) { + mavlink_log_critical(mavlink_fd, "Rejected RTL state: no home lock"); + } else { + mavlink_log_critical(mavlink_fd, "Rejected RTL state: no pos and home lock"); + } + valid_path = true; + + /* coming from loiter a home lock is needed */ + } else if (current_status->navigation_state == NAVIGATION_STATE_LOITER) { + if (current_status->flag_valid_launch_position) { + mavlink_log_critical(mavlink_fd, "Switched to RTL state"); + valid_transition = true; + } else { + mavlink_log_critical(mavlink_fd, "Rejected RTL state: no home lock"); + } + valid_path = true; } break; @@ -201,8 +247,10 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_ if (current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO TAKEOFF STATE"); + /* TAKEOFF is straight forward from AUTO READY */ + mavlink_log_critical(mavlink_fd, "Switched to TAKEOFF state"); valid_transition = true; + valid_path = true; } break; @@ -210,29 +258,12 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_ if (current_status->navigation_state == NAVIGATION_STATE_RTL || current_status->navigation_state == NAVIGATION_STATE_LOITER) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO LAND STATE"); + mavlink_log_critical(mavlink_fd, "Switched to LAND state"); valid_transition = true; + valid_path = true; } break; - case NAVIGATION_STATE_REBOOT: - if (current_status->navigation_state == NAVIGATION_STATE_STANDBY - || current_status->navigation_state == NAVIGATION_STATE_INIT - || current_status->flag_hil_enabled) { - valid_transition = true; - /* set system flags according to state */ - current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); - usleep(500000); - up_systemreset(); - /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ - - } else { - mavlink_log_critical(mavlink_fd, "REFUSED to REBOOT"); - } - - break; - default: warnx("Unknown navigation state"); break; @@ -244,7 +275,9 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_ state_machine_publish(status_pub, current_status, mavlink_fd); // publish_armed_status(current_status); ret = OK; - } else { + } + + if (!valid_path){ mavlink_log_critical(mavlink_fd, "REJECTING invalid navigation state transition"); } @@ -269,7 +302,8 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat case ARMING_STATE_INIT: if (current_status->arming_state == ARMING_STATE_STANDBY) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO INIT ARMING STATE"); + + mavlink_log_critical(mavlink_fd, "Switched to INIT ARMING STATE"); valid_transition = true; } break; @@ -280,8 +314,12 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat // XXX check if coming from reboot? if (current_status->arming_state == ARMING_STATE_INIT) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO STANDBY ARMING STATE"); - valid_transition = true; + if (current_status->flag_system_sensors_initialized) { + mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state"); + valid_transition = true; + } else { + mavlink_log_critical(mavlink_fd, "REJ. STANDBY arming state, sensors not init."); + } } break; @@ -290,16 +328,16 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat if (current_status->arming_state == ARMING_STATE_STANDBY || current_status->arming_state == ARMING_STATE_IN_AIR_RESTORE) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO ARMED ARMING STATE"); + mavlink_log_critical(mavlink_fd, "Switched to ARMED arming state"); valid_transition = true; } break; - case ARMING_STATE_MISSION_ABORT: + case ARMING_STATE_ABORT: if (current_status->arming_state == ARMING_STATE_ARMED) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO MISSION ABORT ARMING STATE"); + mavlink_log_critical(mavlink_fd, "Switched to MISSION ABORT arming state"); valid_transition = true; } break; @@ -307,10 +345,10 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat case ARMING_STATE_ERROR: if (current_status->arming_state == ARMING_STATE_ARMED - || current_status->arming_state == ARMING_STATE_MISSION_ABORT + || current_status->arming_state == ARMING_STATE_ABORT || current_status->arming_state == ARMING_STATE_INIT) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO ERROR ARMING STATE"); + mavlink_log_critical(mavlink_fd, "Switched to ERROR arming state"); valid_transition = true; } break; @@ -320,16 +358,18 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat if (current_status->arming_state == ARMING_STATE_STANDBY || current_status->arming_state == ARMING_STATE_ERROR) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO REBOOT ARMING STATE"); valid_transition = true; - // XXX reboot here? + mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); + usleep(500000); + up_systemreset(); + /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ } break; case ARMING_STATE_IN_AIR_RESTORE: if (current_status->arming_state == ARMING_STATE_INIT) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO IN-AIR-RESTORE ARMING STATE"); + mavlink_log_critical(mavlink_fd, "Switched to IN-AIR-RESTORE arming state"); valid_transition = true; } break; diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h index ed18bfbd24..bf9296caf0 100644 --- a/apps/commander/state_machine_helper.h +++ b/apps/commander/state_machine_helper.h @@ -47,8 +47,6 @@ #include #include -void update_state_machine(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state); int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state); diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 4636ee36ec..e25f1be27f 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -248,26 +248,19 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) *mavlink_state = MAV_STATE_CALIBRATING; - } else if (v_status.flag_system_emergency) { + } else if (v_status.arming_state == ARMING_STATE_ERROR || v_status.arming_state == ARMING_STATE_ABORT) { *mavlink_state = MAV_STATE_EMERGENCY; - } else if (v_status.navigation_state == NAVIGATION_STATE_MANUAL - || v_status.navigation_state == NAVIGATION_STATE_SEATBELT - || v_status.navigation_state == NAVIGATION_STATE_LOITER - || v_status.navigation_state == NAVIGATION_STATE_MISSION - || v_status.navigation_state == NAVIGATION_STATE_RTL - || v_status.navigation_state == NAVIGATION_STATE_LAND - || v_status.navigation_state == NAVIGATION_STATE_TAKEOFF - || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) { + } else if (v_status.arming_state == ARMING_STATE_ARMED || v_status.arming_state == ARMING_STATE_IN_AIR_RESTORE) { *mavlink_state = MAV_STATE_ACTIVE; - } else if (v_status.navigation_state == NAVIGATION_STATE_STANDBY) { + } else if (v_status.arming_state == ARMING_STATE_STANDBY) { *mavlink_state = MAV_STATE_STANDBY; - } else if (v_status.navigation_state == NAVIGATION_STATE_INIT) { + } else if (v_status.arming_state == ARMING_STATE_INIT) { *mavlink_state = MAV_STATE_UNINIT; } else { diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index ba9b9793b0..874cf256c3 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -60,8 +60,7 @@ /* State Machine */ typedef enum { - NAVIGATION_STATE_INIT = 0, - NAVIGATION_STATE_STANDBY, + NAVIGATION_STATE_STANDBY=0, NAVIGATION_STATE_MANUAL, NAVIGATION_STATE_SEATBELT, NAVIGATION_STATE_LOITER, @@ -70,15 +69,13 @@ typedef enum { NAVIGATION_STATE_RTL, NAVIGATION_STATE_TAKEOFF, NAVIGATION_STATE_LAND, - NAVIGATION_STATE_GROUND_ERROR, - NAVIGATION_STATE_REBOOT } navigation_state_t; typedef enum { ARMING_STATE_INIT = 0, ARMING_STATE_STANDBY, ARMING_STATE_ARMED, - ARMING_STATE_MISSION_ABORT, + ARMING_STATE_ABORT, ARMING_STATE_ERROR, ARMING_STATE_REBOOT, ARMING_STATE_IN_AIR_RESTORE @@ -95,6 +92,22 @@ enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1 }; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */ +typedef enum { + MODE_SWITCH_MANUAL = 0, + MODE_SWITCH_ASSISTED, + MODE_SWITCH_AUTO +} mode_switch_pos_t; + +typedef enum { + RETURN_SWITCH_NONE = 0, + RETURN_SWITCH_RETURN +} return_switch_pos_t; + +typedef enum { + MISSION_SWITCH_NONE = 0, + MISSION_SWITCH_MISSION +} mission_switch_pos_t; + //enum VEHICLE_FLIGHT_MODE { // VEHICLE_FLIGHT_MODE_MANUAL = 0, /**< direct manual control, exact mode determined by VEHICLE_MANUAL_CONTROL_MODE */ // VEHICLE_FLIGHT_MODE_STAB, /**< attitude or rate stabilization plus velocity or position stabilization */ @@ -168,9 +181,18 @@ struct vehicle_status_s /* system flags - these represent the state predicates */ + mode_switch_pos_t mode_switch; + return_switch_pos_t return_switch; + mission_switch_pos_t mission_switch; + bool flag_system_armed; /**< true is motors / actuators are armed */ bool flag_system_emergency; - bool flag_system_sensors_ok; + bool flag_system_in_air_restore; /**< true if we can restore in mid air */ + bool flag_system_sensors_initialized; + bool flag_system_arming_requested; + bool flag_system_disarming_requested; + bool flag_system_reboot_requested; + bool flag_system_on_ground; bool flag_control_manual_enabled; /**< true if manual input is mixed in */ bool flag_control_offboard_enabled; /**< true if offboard control input is on */ From 5eac78d7645becc486bc6a43852b9631e62465b4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 18 Feb 2013 13:52:18 -0800 Subject: [PATCH 004/486] Checkpoint: Added DESCENT state --- apps/commander/state_machine_helper.c | 51 +++++++++++++++++---------- apps/uORB/topics/vehicle_status.h | 3 +- 2 files changed, 34 insertions(+), 20 deletions(-) diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index a0b786aabb..68b4bbe30d 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -119,6 +119,16 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_ valid_path = true; } break; + case NAVIGATION_STATE_DESCENT: + + if (current_status->navigation_state == NAVIGATION_STATE_MANUAL + || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) { + + mavlink_log_critical(mavlink_fd, "Switched to DESCENT state"); + valid_transition = true; + valid_path = true; + } + break; case NAVIGATION_STATE_LOITER: @@ -146,25 +156,28 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_ /* coming from STANDBY pos and home lock are needed */ if (current_status->navigation_state == NAVIGATION_STATE_STANDBY) { - if (current_status->flag_global_position_valid && current_status->flag_valid_launch_position) { - mavlink_log_critical(mavlink_fd, "Switched to AUTO READY state"); - valid_transition = true; + if (!current_status->flag_system_armed) { + mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: disarmed"); + } else if (!current_status->flag_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no pos lock"); + } else if (!current_status->flag_valid_launch_position) { mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no home lock"); + } else { - mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no pos and home lock"); + mavlink_log_critical(mavlink_fd, "Switched to AUTO READY state"); + valid_transition = true; } valid_path = true; /* coming from LAND home lock is needed */ } else if (current_status->navigation_state == NAVIGATION_STATE_LAND) { - if (current_status->flag_valid_launch_position) { + if (!current_status->flag_valid_launch_position) { + mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no home lock"); + } else { mavlink_log_critical(mavlink_fd, "Switched to AUTO READY state"); valid_transition = true; - } else { - mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no home lock"); } valid_path = true; } @@ -183,15 +196,15 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_ } else if (current_status->navigation_state == NAVIGATION_STATE_MANUAL || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) { - if (current_status->flag_global_position_valid && current_status->flag_valid_launch_position) { - mavlink_log_critical(mavlink_fd, "Switched to MISSION state"); - valid_transition = true; - } else if (!current_status->flag_global_position_valid) { + if (!current_status->flag_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no pos lock"); + } else if (!current_status->flag_valid_launch_position) { mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no home lock"); + mavlink_log_critical(mavlink_fd, "Switched to MISSION state"); + valid_transition = true; } else { - mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no pos and home lock"); + } valid_path = true; @@ -219,15 +232,13 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_ } else if (current_status->navigation_state == NAVIGATION_STATE_MANUAL || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) { - if (current_status->flag_global_position_valid && current_status->flag_valid_launch_position) { - mavlink_log_critical(mavlink_fd, "Switched to RTL state"); - valid_transition = true; - } else if (!current_status->flag_global_position_valid) { + if (!current_status->flag_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rejected RTL state: no pos lock"); } else if (!current_status->flag_valid_launch_position) { mavlink_log_critical(mavlink_fd, "Rejected RTL state: no home lock"); } else { - mavlink_log_critical(mavlink_fd, "Rejected RTL state: no pos and home lock"); + mavlink_log_critical(mavlink_fd, "Switched to RTL state"); + valid_transition = true; } valid_path = true; @@ -256,7 +267,8 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_ case NAVIGATION_STATE_LAND: if (current_status->navigation_state == NAVIGATION_STATE_RTL - || current_status->navigation_state == NAVIGATION_STATE_LOITER) { + || current_status->navigation_state == NAVIGATION_STATE_LOITER + || current_status->navigation_state == NAVIGATION_STATE_MISSION) { mavlink_log_critical(mavlink_fd, "Switched to LAND state"); valid_transition = true; @@ -315,6 +327,7 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat if (current_status->arming_state == ARMING_STATE_INIT) { if (current_status->flag_system_sensors_initialized) { + current_status->flag_system_armed = false; mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state"); valid_transition = true; } else { @@ -327,7 +340,7 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat if (current_status->arming_state == ARMING_STATE_STANDBY || current_status->arming_state == ARMING_STATE_IN_AIR_RESTORE) { - + current_status->flag_system_armed = true; mavlink_log_critical(mavlink_fd, "Switched to ARMED arming state"); valid_transition = true; } diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index 874cf256c3..dde978caee 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -63,12 +63,13 @@ typedef enum { NAVIGATION_STATE_STANDBY=0, NAVIGATION_STATE_MANUAL, NAVIGATION_STATE_SEATBELT, + NAVIGATION_STATE_DESCENT, NAVIGATION_STATE_LOITER, NAVIGATION_STATE_AUTO_READY, NAVIGATION_STATE_MISSION, NAVIGATION_STATE_RTL, - NAVIGATION_STATE_TAKEOFF, NAVIGATION_STATE_LAND, + NAVIGATION_STATE_TAKEOFF } navigation_state_t; typedef enum { From b7faaca435551064e6fdb070a9e762b4146ae4e8 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 18 Feb 2013 16:35:34 -0800 Subject: [PATCH 005/486] Checkpoint: Arming/Disarming works --- apps/commander/commander.c | 18 ++++++++++++------ apps/commander/state_machine_helper.c | 9 +++++++++ apps/mavlink_onboard/mavlink.c | 2 +- 3 files changed, 22 insertions(+), 7 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 27c5f19893..ac535dd9ae 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1649,6 +1649,13 @@ int commander_thread_main(int argc, char *argv[]) /* End battery voltage check */ + /* If in INIT state, try to proceed to STANDBY state */ + if (current_status.arming_state == ARMING_STATE_INIT) { + do_arming_state_update(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + } else { + // XXX: Add emergency stuff if sensors are lost + } + /* * Check for valid position information. @@ -1894,12 +1901,11 @@ int commander_thread_main(int argc, char *argv[]) * Check if left stick is in lower left position --> switch to standby state. * Do this only for multirotors, not for fixed wing aircraft. */ - if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) - ) && - ((sp_man.yaw < -STICK_ON_OFF_LIMIT)) && - (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { +// if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || +// (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || +// (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) +// ) && + if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { do_arming_state_update(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); stick_off_counter = 0; diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 68b4bbe30d..f1de99e4d6 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -304,8 +304,11 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat bool valid_transition = false; int ret = ERROR; + warnx("Current state: %d, requested state: %d", current_status->arming_state, new_state); + if (current_status->arming_state == new_state) { warnx("Arming state not changed"); + valid_transition = true; } else { @@ -333,6 +336,12 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat } else { mavlink_log_critical(mavlink_fd, "REJ. STANDBY arming state, sensors not init."); } + + } else if (current_status->arming_state == ARMING_STATE_ARMED) { + + current_status->flag_system_armed = false; + mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state"); + valid_transition = true; } break; diff --git a/apps/mavlink_onboard/mavlink.c b/apps/mavlink_onboard/mavlink.c index 6babe14ae2..c5a1e82a88 100644 --- a/apps/mavlink_onboard/mavlink.c +++ b/apps/mavlink_onboard/mavlink.c @@ -290,7 +290,7 @@ get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct } /* set arming state */ - if (armed->armed) { + if (v_status->flag_system_armed) { *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } else { *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; From aab6214cdcc630dce1f64ba9220bc1f5b10b6af1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 19 Feb 2013 12:32:47 -0800 Subject: [PATCH 006/486] Checkpoint: Added HIL state, arming/disarming works now, also from GQC --- apps/commander/commander.c | 560 +++++++++++++------------- apps/commander/state_machine_helper.c | 199 +++++---- apps/uORB/topics/vehicle_status.h | 6 + 3 files changed, 413 insertions(+), 352 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index ac535dd9ae..6b026f287e 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -797,302 +797,297 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* announce command handling */ tune_confirm(); - - /* supported command handling start */ - /* request to set different system mode */ switch (cmd->command) { - case VEHICLE_CMD_DO_SET_MODE: { - // XXX implement this + case VEHICLE_CMD_DO_SET_MODE: -// if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, (uint8_t)cmd->param1)) { -// result = VEHICLE_CMD_RESULT_ACCEPTED; -// -// } else { - result = VEHICLE_CMD_RESULT_DENIED; -// } - } - break; + /* request to activate HIL */ + if ((int)cmd->param1 & VEHICLE_MODE_FLAG_HIL_ENABLED) { - case VEHICLE_CMD_COMPONENT_ARM_DISARM: { - /* request to arm */ - if ((int)cmd->param1 == 1) { - // XXX add this again -// if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { -// result = VEHICLE_CMD_RESULT_ACCEPTED; -// -// } else { -// result = VEHICLE_CMD_RESULT_DENIED; -// } - - /* request to disarm */ - // XXX this arms it instad of disarming - } else if ((int)cmd->param1 == 0) { - // XXX add this again -// if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { -// result = VEHICLE_CMD_RESULT_ACCEPTED; -// -// } else { -// result = VEHICLE_CMD_RESULT_DENIED; -// } - } - } - break; - - /* request for an autopilot reboot */ - case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { - if ((int)cmd->param1 == 1) { - if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_REBOOT)) { - /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ + if (OK == do_hil_state_update(status_pub, current_vehicle_status, mavlink_fd, HIL_STATE_ON)) { result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - /* system may return here */ result = VEHICLE_CMD_RESULT_DENIED; } } - } - break; - -// /* request to land */ -// case VEHICLE_CMD_NAV_LAND: -// { -// //TODO: add check if landing possible -// //TODO: add landing maneuver -// -// if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED)) { -// result = VEHICLE_CMD_RESULT_ACCEPTED; -// } } -// break; -// -// /* request to takeoff */ -// case VEHICLE_CMD_NAV_TAKEOFF: -// { -// //TODO: add check if takeoff possible -// //TODO: add takeoff maneuver -// -// if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_AUTO)) { -// result = VEHICLE_CMD_RESULT_ACCEPTED; -// } -// } -// break; -// - /* preflight calibration */ - case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { - bool handled = false; - - /* gyro calibration */ - if ((int)(cmd->param1) == 1) { - - /* transition to init state */ - // XXX add this again -// if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) -// && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { -// -// mavlink_log_info(mavlink_fd, "starting gyro cal"); -// tune_confirm(); -// do_gyro_calibration(status_pub, ¤t_status); -// mavlink_log_info(mavlink_fd, "finished gyro cal"); -// tune_confirm(); -// -// /* back to standby state */ -// do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); -// do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); -// -// result = VEHICLE_CMD_RESULT_ACCEPTED; -// -// } else { -// mavlink_log_critical(mavlink_fd, "REJECTING gyro cal"); -// result = VEHICLE_CMD_RESULT_DENIED; -// } - - handled = true; - } - - /* magnetometer calibration */ - if ((int)(cmd->param2) == 1) { - - /* transition to init state */ - // XXX add this again -// if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) -// && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { -// mavlink_log_info(mavlink_fd, "starting mag cal"); -// tune_confirm(); -// do_mag_calibration(status_pub, ¤t_status); -// mavlink_log_info(mavlink_fd, "finished mag cal"); -// tune_confirm(); -// -// /* back to standby state */ -// do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); -// do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); -// -// result = VEHICLE_CMD_RESULT_ACCEPTED; -// -// } else { -// mavlink_log_critical(mavlink_fd, "REJECTING mag cal"); -// result = VEHICLE_CMD_RESULT_DENIED; -// } - - handled = true; - } - - /* zero-altitude pressure calibration */ - if ((int)(cmd->param3) == 1) { - /* transition to calibration state */ - // XXX add this again -// if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) -// && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { -// -// // XXX implement this -// mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented"); -// tune_confirm(); -// -// /* back to standby state */ -// do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); -// do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); -// -// } else { -// mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration"); -// result = VEHICLE_CMD_RESULT_DENIED; -// } - - handled = true; - } - - /* trim calibration */ - if ((int)(cmd->param4) == 1) { - /* transition to calibration state */ - // XXX add this again -// if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) -// && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { -// mavlink_log_info(mavlink_fd, "starting trim cal"); -// tune_confirm(); -// do_rc_calibration(status_pub, ¤t_status); -// mavlink_log_info(mavlink_fd, "finished trim cal"); -// tune_confirm(); -// -// /* back to standby state */ -// do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); -// do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); -// -// result = VEHICLE_CMD_RESULT_ACCEPTED; -// -// } else { -// mavlink_log_critical(mavlink_fd, "REJECTING trim cal"); -// result = VEHICLE_CMD_RESULT_DENIED; -// } - - handled = true; - } - - /* accel calibration */ - if ((int)(cmd->param5) == 1) { - // XXX add this again -// if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) -// && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { -// -// mavlink_log_info(mavlink_fd, "CMD starting accel cal"); -// tune_confirm(); -// do_accel_calibration(status_pub, ¤t_status); -// tune_confirm(); -// mavlink_log_info(mavlink_fd, "CMD finished accel cal"); -// -// /* back to standby state */ -// do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); -// do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); -// -// result = VEHICLE_CMD_RESULT_ACCEPTED; -// -// } else { -// mavlink_log_critical(mavlink_fd, "REJECTING accel cal"); -// result = VEHICLE_CMD_RESULT_DENIED; -// } - - handled = true; - } - - /* none found */ - if (!handled) { - //warnx("refusing unsupported calibration request\n"); - mavlink_log_critical(mavlink_fd, "CMD refusing unsup. calib. request"); - result = VEHICLE_CMD_RESULT_UNSUPPORTED; - } - } - break; - - case VEHICLE_CMD_PREFLIGHT_STORAGE: { - if (current_status.flag_system_armed && - ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) { - /* do not perform expensive memory tasks on multirotors in flight */ - // XXX this is over-safe, as soon as cmd is in low prio thread this can be allowed - mavlink_log_info(mavlink_fd, "REJECTING save cmd while multicopter armed"); - - } else { - - // XXX move this to LOW PRIO THREAD of commander app - /* Read all parameters from EEPROM to RAM */ - - if (((int)(cmd->param1)) == 0) { - - /* read all parameters from EEPROM to RAM */ - int read_ret = param_load_default(); - - if (read_ret == OK) { - //warnx("[mavlink pm] Loaded EEPROM params in RAM\n"); - mavlink_log_info(mavlink_fd, "OK loading params from"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else if (read_ret == 1) { - mavlink_log_info(mavlink_fd, "OK no changes in"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - if (read_ret < -1) { - mavlink_log_info(mavlink_fd, "ERR loading params from"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - - } else { - mavlink_log_info(mavlink_fd, "ERR no param file named"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - } - - result = VEHICLE_CMD_RESULT_FAILED; - } - - } else if (((int)(cmd->param1)) == 1) { - - /* write all parameters from RAM to EEPROM */ - int write_ret = param_save_default(); - - if (write_ret == OK) { - mavlink_log_info(mavlink_fd, "OK saved param file"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - if (write_ret < -1) { - mavlink_log_info(mavlink_fd, "ERR params file does not exit:"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - - } else { - mavlink_log_info(mavlink_fd, "ERR writing params to"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - } - - result = VEHICLE_CMD_RESULT_FAILED; - } + if ((int)cmd->param1 & VEHICLE_MODE_FLAG_SAFETY_ARMED) { + if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { - mavlink_log_info(mavlink_fd, "[pm] refusing unsupp. STOR request"); + result = VEHICLE_CMD_RESULT_DENIED; + } + } else { + if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } + + break; + + case VEHICLE_CMD_COMPONENT_ARM_DISARM: + + /* request to arm */ + if ((int)cmd->param1 == 1) { + if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + /* request to disarm */ + } else if ((int)cmd->param1 == 0) { + if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } + + break; + + case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { + + /* request for an autopilot reboot */ + if ((int)cmd->param1 == 1) { + if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_REBOOT)) { + /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + /* system may return here */ + result = VEHICLE_CMD_RESULT_DENIED; + } + } + } + break; + + // /* request to land */ + // case VEHICLE_CMD_NAV_LAND: + // { + // //TODO: add check if landing possible + // //TODO: add landing maneuver + // + // if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED)) { + // result = VEHICLE_CMD_RESULT_ACCEPTED; + // } } + // break; + // + // /* request to takeoff */ + // case VEHICLE_CMD_NAV_TAKEOFF: + // { + // //TODO: add check if takeoff possible + // //TODO: add takeoff maneuver + // + // if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_AUTO)) { + // result = VEHICLE_CMD_RESULT_ACCEPTED; + // } + // } + // break; + // + /* preflight calibration */ + case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { + bool handled = false; + + /* gyro calibration */ + if ((int)(cmd->param1) == 1) { + + if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { + + result = VEHICLE_CMD_RESULT_ACCEPTED; + + mavlink_log_info(mavlink_fd, "starting gyro cal"); + tune_confirm(); + do_gyro_calibration(status_pub, ¤t_status); + mavlink_log_info(mavlink_fd, "finished gyro cal"); + tune_confirm(); + + do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + + handled = true; + } + + /* magnetometer calibration */ + if ((int)(cmd->param2) == 1) { + + if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { + + result = VEHICLE_CMD_RESULT_ACCEPTED; + + mavlink_log_info(mavlink_fd, "starting mag cal"); + tune_confirm(); + do_mag_calibration(status_pub, ¤t_status); + mavlink_log_info(mavlink_fd, "finished mag cal"); + tune_confirm(); + + do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + + handled = true; + } + + /* zero-altitude pressure calibration */ + if ((int)(cmd->param3) == 1) { + /* transition to calibration state */ + // XXX add this again + // if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) + // && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { + // + // // XXX implement this + // mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented"); + // tune_confirm(); + // + // /* back to standby state */ + // do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + // do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); + // + // } else { + // mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration"); + // result = VEHICLE_CMD_RESULT_DENIED; + // } + + handled = true; + } + + /* trim calibration */ + if ((int)(cmd->param4) == 1) { + /* transition to calibration state */ + // XXX add this again + // if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) + // && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { + // mavlink_log_info(mavlink_fd, "starting trim cal"); + // tune_confirm(); + // do_rc_calibration(status_pub, ¤t_status); + // mavlink_log_info(mavlink_fd, "finished trim cal"); + // tune_confirm(); + // + // /* back to standby state */ + // do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + // do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); + // + // result = VEHICLE_CMD_RESULT_ACCEPTED; + // + // } else { + // mavlink_log_critical(mavlink_fd, "REJECTING trim cal"); + // result = VEHICLE_CMD_RESULT_DENIED; + // } + + handled = true; + } + + /* accel calibration */ + if ((int)(cmd->param5) == 1) { + + if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { + + result = VEHICLE_CMD_RESULT_ACCEPTED; + + mavlink_log_info(mavlink_fd, "starting acc cal"); + tune_confirm(); + do_accel_calibration(status_pub, ¤t_status); + mavlink_log_info(mavlink_fd, "finished acc cal"); + tune_confirm(); + + do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + + + handled = true; + } + + /* none found */ + if (!handled) { + //warnx("refusing unsupported calibration request\n"); + mavlink_log_critical(mavlink_fd, "CMD refusing unsup. calib. request"); result = VEHICLE_CMD_RESULT_UNSUPPORTED; } } - } - break; + break; + + case VEHICLE_CMD_PREFLIGHT_STORAGE: { + if (current_status.flag_system_armed && + ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) { + /* do not perform expensive memory tasks on multirotors in flight */ + // XXX this is over-safe, as soon as cmd is in low prio thread this can be allowed + mavlink_log_info(mavlink_fd, "REJECTING save cmd while multicopter armed"); + + } else { + + // XXX move this to LOW PRIO THREAD of commander app + /* Read all parameters from EEPROM to RAM */ + + if (((int)(cmd->param1)) == 0) { + + /* read all parameters from EEPROM to RAM */ + int read_ret = param_load_default(); + + if (read_ret == OK) { + //warnx("[mavlink pm] Loaded EEPROM params in RAM\n"); + mavlink_log_info(mavlink_fd, "OK loading params from"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else if (read_ret == 1) { + mavlink_log_info(mavlink_fd, "OK no changes in"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + if (read_ret < -1) { + mavlink_log_info(mavlink_fd, "ERR loading params from"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + + } else { + mavlink_log_info(mavlink_fd, "ERR no param file named"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + } + + result = VEHICLE_CMD_RESULT_FAILED; + } + + } else if (((int)(cmd->param1)) == 1) { + + /* write all parameters from RAM to EEPROM */ + int write_ret = param_save_default(); + + if (write_ret == OK) { + mavlink_log_info(mavlink_fd, "OK saved param file"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + if (write_ret < -1) { + mavlink_log_info(mavlink_fd, "ERR params file does not exit:"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + + } else { + mavlink_log_info(mavlink_fd, "ERR writing params to"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + } + + result = VEHICLE_CMD_RESULT_FAILED; + } + + } else { + mavlink_log_info(mavlink_fd, "[pm] refusing unsupp. STOR request"); + result = VEHICLE_CMD_RESULT_UNSUPPORTED; + } + } + } + break; default: { mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command"); @@ -1326,6 +1321,7 @@ int commander_thread_main(int argc, char *argv[]) memset(¤t_status, 0, sizeof(current_status)); current_status.navigation_state = NAVIGATION_STATE_STANDBY; current_status.arming_state = ARMING_STATE_INIT; + current_status.hil_state = HIL_STATE_OFF; current_status.flag_system_armed = false; /* neither manual nor offboard control commands have been received */ diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index f1de99e4d6..aae119d35a 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -413,6 +413,63 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat return ret; } +/** + * Transition from one hil state to another + */ +int do_hil_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state) +{ + bool valid_transition = false; + int ret = ERROR; + + warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state); + + if (current_status->hil_state == new_state) { + warnx("Hil state not changed"); + valid_transition = true; + + } else { + + switch (new_state) { + + case HIL_STATE_OFF: + + if (current_status->arming_state == ARMING_STATE_INIT + || current_status->arming_state == ARMING_STATE_STANDBY) { + + current_status->flag_hil_enabled = false; + mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); + valid_transition = true; + } + break; + + case HIL_STATE_ON: + + if (current_status->arming_state == ARMING_STATE_INIT + || current_status->arming_state == ARMING_STATE_STANDBY) { + + current_status->flag_hil_enabled = true; + mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); + valid_transition = true; + } + break; + + default: + warnx("Unknown hil state"); + break; + } + } + + if (valid_transition) { + current_status->hil_state = new_state; + state_machine_publish(status_pub, current_status, mavlink_fd); + ret = OK; + } else { + mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); + } + + return ret; +} + void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) @@ -684,7 +741,78 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat // } -/* END SUBSYSTEM/EMERGENCY FUNCTIONS*/ +///* END SUBSYSTEM/EMERGENCY FUNCTIONS*/ +// +//int update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode) +//{ +// int ret = 1; +// +//// /* Switch on HIL if in standby and not already in HIL mode */ +//// if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED) +//// && !current_status->flag_hil_enabled) { +//// if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) { +//// /* Enable HIL on request */ +//// current_status->flag_hil_enabled = true; +//// ret = OK; +//// state_machine_publish(status_pub, current_status, mavlink_fd); +//// publish_armed_status(current_status); +//// printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); +//// +//// } else if (current_status->state_machine != SYSTEM_STATE_STANDBY && +//// current_status->flag_system_armed) { +//// +//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!") +//// +//// } else { +//// +//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.") +//// } +//// } +// +// /* switch manual / auto */ +// if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { +// update_state_machine_mode_auto(status_pub, current_status, mavlink_fd); +// +// } else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) { +// update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd); +// +// } else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) { +// update_state_machine_mode_guided(status_pub, current_status, mavlink_fd); +// +// } else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { +// update_state_machine_mode_manual(status_pub, current_status, mavlink_fd); +// } +// +// /* vehicle is disarmed, mode requests arming */ +// if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { +// /* only arm in standby state */ +// // XXX REMOVE +// if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { +// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); +// ret = OK; +// printf("[cmd] arming due to command request\n"); +// } +// } +// +// /* vehicle is armed, mode requests disarming */ +// if (current_status->flag_system_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { +// /* only disarm in ground ready */ +// if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { +// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); +// ret = OK; +// printf("[cmd] disarming due to command request\n"); +// } +// } +// +// /* NEVER actually switch off HIL without reboot */ +// if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { +// warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n"); +// mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL"); +// ret = ERROR; +// } +// +// return ret; +//} #if 0 @@ -821,76 +949,7 @@ void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *cur } -uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode) -{ - uint8_t ret = 1; - /* Switch on HIL if in standby and not already in HIL mode */ - if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED) - && !current_status->flag_hil_enabled) { - if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) { - /* Enable HIL on request */ - current_status->flag_hil_enabled = true; - ret = OK; - state_machine_publish(status_pub, current_status, mavlink_fd); - publish_armed_status(current_status); - printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); - - } else if (current_status->state_machine != SYSTEM_STATE_STANDBY && - current_status->flag_system_armed) { - - mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!") - - } else { - - mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.") - } - } - - /* switch manual / auto */ - if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { - update_state_machine_mode_auto(status_pub, current_status, mavlink_fd); - - } else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) { - update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd); - - } else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) { - update_state_machine_mode_guided(status_pub, current_status, mavlink_fd); - - } else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { - update_state_machine_mode_manual(status_pub, current_status, mavlink_fd); - } - - /* vehicle is disarmed, mode requests arming */ - if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { - /* only arm in standby state */ - // XXX REMOVE - if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); - ret = OK; - printf("[cmd] arming due to command request\n"); - } - } - - /* vehicle is armed, mode requests disarming */ - if (current_status->flag_system_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { - /* only disarm in ground ready */ - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); - ret = OK; - printf("[cmd] disarming due to command request\n"); - } - } - - /* NEVER actually switch off HIL without reboot */ - if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { - warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n"); - mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL"); - ret = ERROR; - } - - return ret; -} uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode) //TODO: add more checks to avoid state switching in critical situations { diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index f5ab91bad9..27a471f131 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -82,6 +82,11 @@ typedef enum { ARMING_STATE_IN_AIR_RESTORE } arming_state_t; +typedef enum { + HIL_STATE_OFF = 0, + HIL_STATE_ON +} hil_state_t; + enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, @@ -177,6 +182,7 @@ struct vehicle_status_s navigation_state_t navigation_state; /**< current navigation state */ arming_state_t arming_state; /**< current arming state */ + hil_state_t hil_state; /**< current hil state */ int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */ int32_t system_id; /**< system id, inspired by MAVLink's system ID field */ From 0e29f2505a599d473244b0bb7e4b309d392ebb3c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 20 Feb 2013 10:38:06 -0800 Subject: [PATCH 007/486] Checkpoint: New hierarchical states --- Documentation/flight_mode_state_machine.odg | Bin 18105 -> 24059 bytes apps/commander/commander.c | 117 ++-- apps/commander/state_machine_helper.c | 678 +++++++++----------- apps/commander/state_machine_helper.h | 4 +- apps/uORB/topics/vehicle_status.h | 89 ++- 5 files changed, 392 insertions(+), 496 deletions(-) diff --git a/Documentation/flight_mode_state_machine.odg b/Documentation/flight_mode_state_machine.odg index b630ecb40242a9a52a796c30ff0dbc283a4de7e7..a50366bcb3e51c2c6571597de89d7b00d746b28d 100644 GIT binary patch delta 20506 zcmaHSV{qV2@MesSZQIVqwry-|o4?rH*mgFyv9WF2*^Pbs-hH`$)z!_HQCC;>)HBaa z_w=;Kg1&WvA}Gm%L!g6zz<_|vpr|Dx$N(@fF#k10K>n}tZx+PP%+-j|)6OJC87d?QvU*WeU0v6%#Jpsi)YwP0by%WBd%m~UPDIANNSz#!`EI-G`vJ?t zM?KaY+(H=L8c1?6vGGsp@uLDz#f_OiP@Anp)(`KKu!Cxe^%lVVFn^=|Vfp+n_fmke zY&FjyLaP1@d<#TX;8|?+{Z?Y+=tG793q{qo`HaW^Be4p=RR=&GY|9xQX9!0OZ|7}a7wxgsY zYuhXz-5Wka!KP7X5x!|ctcb(n_u)iQ~A5bI~TCzAKXU%vsa9HL1T^XnMNQO zNse_yqr-34XNpj?h*@#G6Pj`4;QFL=Gex{f<{IjkKbx_UK`DR78@sdcf%pNyy<=^; z-3<^x-k9Ex%2_(Y&LB=}ZawF4rY2GUUfQl|>&7dw6VfmRY2oPNg7v&|nEM1*l7)sT z!0x%42L(VtY#=~D{tqnvUs!S1c`A=&ojFjo0<{?=S($ZaQY=A`VdQ59XaSEXv{{r<|^ zl3vJ~x=^J*9L0Wmtr#Ie@$_CP`RH`&CE{7WDWgsg(4y_qVa$7*=v^r?6I(0LGqXbn{*Y<&rQIoi=WRB5 z6pyNojFZQs&Ch9L?U`BkDP#0Gwb51WwKMY~Qo^Do3xZ>G`OC0UyxLHurpvtIQTAnh zXXphPa7Ut~MSHLG)}V>`eQ(5S!&>xJgYF&nSV%`qgj%~lcHM?E6x5^kG< zyt|x|IkEggj}}EA?837jOO~-zyv-bDqDT0s$~V%ixvYm2lrR-)TggVIEP(NsB;INH z!!q0HJSxp6GWZj^R!jxiPpI@(=y$U7S=?`5;6@p-9bJ-n;ozAGMoscGd|Df-94Rqt z2wscy1{Mit6f-L+RP>3XrV41sFy&d4jOAX-<&_{Cj|>8B5Vvm4_0QJ{W1?eLXBT{CgE?G&2YfWlWODZog@7{VDkzM=yO~)!Mnz&9rhNMhWEpT+{pgf2|qUh zWX~X&-RlGWY{>@oBZm&QR0o8*vYame$k%;Vqy2HO%r2Of9QnHmA9$7kRRB5z)lZHy*9^kpoUyoFyA4KOW;Pc^Ae2gPn;!r9Fkue)5ac)7bmRBE zP1aBA#&iN~+8nirRrehBV;!Dp+>Ur_fXWRYe5F`ej&)*XKjrD`Zd!?f(f=!_K!{>n zf|UHQvNqWc?Mra?Wj(sU!}Y2|(Uaw{yYi|)zCSbSvnWrDI6`DE;Q*7XbVw6%0)?yR z6Yk>q+?+r^1G6jaI4;YZ;YmW1JlxV1+U+4PVvcO491(FY`;TKITf&p~lI&7j# zy}Ba~MGVWQFgEclC3AX9N|mrVU#`mLA_daDxK(Vrnp;#>Aw404MxIw~?MSS|+lU}V zO6q~8-e6%d?kTstZ##i)nt?xhG(Znk?Qy$V3b6w^!#Id)OZLGv-K-@wxHK^ydv*5n*TZJZQI#wqh2 zau$Mi31xvuu}y;_vI|2Tqx04FpL`wNfG>$|k=;l9#0#&HSDd-|*1|0CAR||4v0{7;JZG+=+iwXh8D+qeHlVbZFvW?`mf6`X3u! z`E5BLjQ-s2&I2oGCg{8X34N`#&dY%1UB>H4|HEC+hUavd1C;R?i+--6o61ahej4i9}vI7m4p&uKWd z>=C^bPGtZ2&Fj*CJTvN9KWcoihVEr+^KSOjjQe!FYtru4 zRcwvE>Rh@v>hWvM>edyU)_kkl4z=jL+fnan-15_R@7c`52MoHifHk*n>=}v`g&mJC zbHLg2Vaxor{bI&b2FF?h&Sq?FQJ+t-Lqp9r?~}qFlV~`SY{A z58nlx%rkp$fW0a{>jhIGP!J2UaRWMZ1^MOmzM#?7?=>O7>(Qy>jNanYl$LhCrn5Yv z^L9P_?ApZG|LTf~`4o8ndna7OiKO2PH$$N`#Th*^4VZhmdwzI#*EnD>Q4_fXon6$2 zb}u|!Z!Q;SdEcYAe9^K6Z20{+^{1_WuOP9^?((?sZ(XeheoZAGv zeA3=edq4l))i%c8g`e#m4xJ_44Ow?))gX@A5@AkEH-el{hPZd=xMD)`J)O?2z$)o4W!7-J2D~s!nG@8ITi0!jmjWMESJO z29HCu(Wr)WrYGyt&(GD7zMNwu-2@)bj^+59psF*lX0PY@tApD}ot>@kg7pRZ<{Q6O zhbV7af%c(#{1pqWz)FX2m#F@C2iq_-DbtTYi@(WhlWx)Bv~zEiE%2_|p81@o?G^L= zV{mGVtg~XfEpI^e4r7~k<>=k%)yUr^tLJ|3_;emA>+E1;$oA{;z%{ru5lBqK+GF$k zzO}h^DcIUU4nV{lXKfkxO#K)Ntlg;oNyvUODmlDjVBx zz|IF+g@O4xKm5_WPW{vU;m1koPi$NqUbI@2>7(|ND9yH$G45f{iKqTK>~FE)57agz z?|d|M-q@~(rcd3UDm3o%W_hq*jCk$TS%l0?vyM%->kEpz$2w)h>N`{Cii$t5n< z1|7W0R>+#;*XLj#kOTf34j2*&D=3-<-KQu5+u7vgg^tj?w^6J~aWI(S8{F}QWrhYH zGmJ~M6#8-gRrfkmy#xEDREKm+`MATIbwIg`v*XhwX7Jj5#Ni*4>yM=9)bo}4#pS2x z@O76*bW(M>a%QdDQ|)l%X#Q!`lj(rTrbL^EwZXuB;8dU058;<-4`3eI-+s!rMECaR z4No4#Wyd?5EL<6P$RO-Xd^&%Y;9eu{&sBQy6aVEzE((T5e$gf-{MEJsZafEN?0O4g z?1UL1S@kdxgw1EQb@RuP#4p8%PZ2wQxlFTtr1AK*wTJ>-@WWin`O6yjxwXj5)={e~ zll0@tk^>hXv-?6c576z>v!#np-2KDW39G0zEjX2yV)XK9)t-IR;EcQzKGREMm@3Lm zne>lSzdmoiD*nI;kPFQj2K%^7ocUl^*h%V3Sa%42(XIaM_4o7aHMj=#TsM}obAyhH z7Hn)V^T;YAQ!05yB1~)Py@$tthUBs(Dr0+|efjWa+X;5`17ISzk&V&(Q((pjQ5rNO z6V4Gx1QP$%*9as|DbMMjx9uwYc0Qs~1{JyP{G_fq-nbqLK0#954B`avnk|oZtPj1o zp1nP9I=$u2@4$Ofe4247fWysT=i)nc?%P-ER=t7+rv|#l;5pkPmXG^Z_VRIW6^R%t zX!O_=`Zfx*5|zc@9U%QJ0e34;{hjpHK%}kEYuu0jt|NFg&VOOeqlNK4)cS}AZM*;I z5XifaSo=dji{>lk2miK4E0>|)@7F?J$Bbp9yRG9YK_WP_&kJ0EqS2@rHhne0s?OIs z-^`Qviy`Y)aim!*zF3Tw)jWglx`)suyruf2HxCg2gjowAkE;Ea$Rea*r{nDvW_FWl zCC?8>KaPTbN$EqA?=`c{u;Ex#+(m22Jm9<|WK(%i|vJGfJov9=C-N zFoXj9ZP`+LzU_K^%dM-(y}%YQGN;%k|6vU_6495lOicx;kit1^P_P` zjP3zaYm5!FV!)w+FPzcD!6>_vy_Xl|+ap_Z9vH@{M6w+V$xjB#=|mb3L7|I9{L#DP z0IK6Y{8AyWU&Pqcpy0c=UUiTY+9ZIP7kPR@y~O|~esYr1U?6TbeN8zjY=L`{2#V2) zm64`&z8Sn_4oD_ZfH|ZlA%{@MXbuXDM(vRwFckTm{Xjc8b7$l%PsqAl z(;URbYx3PJ=Lxl!htNhpL(oaoPUN`QZ8nWna1r&gOvhv;X!#5OBKnB4)gd6FQh{g@ z9A5T2-(0#JC)(&2s#X>FGSoE+1YR#XmTEt;lnhBl>$ruAn+-~+CckqpAJMYhFf?v4by_jBRkGvlxcfwp3u}eA49hF%l79; zL*5hoNW)}~Qn`gCS+#}K1a$J-H3}hUs6BGW3lA2QoNyfV0VtcYhbs0_YiQV^eSw)R(H~C= z$cZ+Llqwv_lBiZ5x#xh?P`xck6GJSViO2+XJ*SJhmf;&c<_=Q+@1p%~=N5zeAk-kDuhMh>?JQD{)GzQQ|V9QGahA z6ZOmZcZ8JvNFRgw@+(&^#yH7h!)WsYE(52P^VqL-YkTq1k9D*5JIVSf?9%kH>b?C9 zJXFaAH92yuz5BMv;)USVh4hwO^r+B&5U=3|IJm z>glGP7nLIi!iR5e{Z5ur3M6@!9VV(HEY?jGQ+h)@Dq29+-M!Seb7D|1j)-{IK06~u zh*N9j!%4F^nrDna90Hteao|4rn`9D`Hqu&}+@swNLs?blTw5oYMGyxolaExlYf=!` zPz`k6v!VZ+7kaA=^7js+QSVjckRx4WFE%XMjqtLeE#E~42en;t6fRr_qq0E?swar8 z-%4J0703XV6#jF>*T+onduy(V|82#F*A30ZLCuf&V^$_gnYFM3yYho6ZqEz$A?X%x zDoC%dHI&aEbfH*|X}i{0ta+2zYEU{B<70mV;OvT|f3ELprLmW-#0OH#{tT@^!-i8f z8YZ98;IP{Ww_;++wHfA_3<>ku94Rk0Mdf#e4>$#0nfKBT@+TlpM4czk2CmRPPce;I zSoD6f0#SMv0z0F@?^PCxz=bW!d1v$+tyEUHsd?Cb4b8as`1 zf~)3VFBoCb?_%#*Eelae^<~uC!C_E3#f?J8PYD|;K4=JeZ$`wScY`%Cv?K9-|7{(d zP6V%@;ZKxAQy#&2du127JjRs)TAtNTPU|!PX~z7+$3%*Vj>WwFFE@Vs;9q#Cj^*)l z>OznoG5v>#(kl|S=f;J2srcBqU4fq7OmqmIzK!*w({g6YSRK{4%#ai9D5NVIsnA^! zw0hV{I57y+hl-8yq#JbE9KX0yH&^609Onj#4twljnqk~EoGIHuy4uG}5|2P#oTAr& z-Hwy6x+J%16Lbbwc<24Kf|x&EuO&=uJZemvRB1Asa+A#_h!dAFtnI`1%o;H1kbE(( z3}B`G4h_Bek;2ZSI1I=ZWpuw4C1SYDbLSIKh$hU26Xsm>B8${A2hATo16zg783oa| zhI1cX<6Kp$HS;#Z2XZEOM)x11(yf9Jq1igmedS3y>kfiQ9P8C* zLYfLXmS(rdq$9Z|Vj~E#h@tuTzhGxE8)%)N&<4*pS#$BI{P<5fp;w^eet@C_T87hw zXHa#A74Vb}AYqagMiw=ZJ+NG~jLDD1!UWa?q&LHX!elg`OeUKO)Kt=AVK88?PHvGXMcS3 z2u8tnQhP#~@QWqPi-VYoIuiK+`L&ERDCEt$B`?32COxq|AbQ#Eu19x4h~ke=CGG>P zDbFNmH+3?o<}>?-(1m4?+Q|6x$JzB`y*lJ}UB_7^SBM(gwn$_;2V0nYkMKrK)&@eo zR(*KoB;xhb&o^O_9wNEubDBkHf{vPwM}FN)r&oKRpiv4CIR5jO?H}iW8NJUE?;lB2 zz9m#4GZ%_aq;F>i17F`hSH#M{Ur-?@B22EjJ`~s!KGyZik$|eQcFQ)7^AgjIldzl)5O-2piYk?pC{o`Oib#0&Mv*1J2^GWpN3@+PlGq z6mEj&*>e1n_HIib$*f^PH^5;KSe18=A zJv&xQM{XilU4CMWT~;NGnlkOgDGt70bT#lwdY3vBgLlfvO=Z{hR1$cAYCcYr-5N#r zWgwM23t6=iC;*a3PbiNm#OzJs34<29Rm7BnMib=J>3^kc?e$@HBMc+ z_a{B%(@zVBB2VxhxoOms|gYtQQFh*oZ7`o_@fw*e9SWlRx`O zEHU~mY)lce7t@0zqw3V^qKMp#mO2S^?0eu(4!E%@_(w+(sd_K;YxYK6V~0|3wA3Z| z!L#S)bP!<3bx5gG&^oje4iFbaZ+<9R_w-UO(U>Frj?Ly!^TVdJELE~esm|Xkuzu!+ zq~T$SDg69z!myOa+ZDoxKPm$Euh1Gpg9a=+>Xfu|k{QS1DuRR>AAZq!SnGB&!4+kl z`l3?aY_g6|e_pxA+ekKLAj zb2MPcht*?E=@nsG7tx(G zMii8Xj+-XE-Z{g$71t6>Z7=I%WCo5Yl>KmD9fj-mT^XGIGit~S5$MyMiThK_iYk*6 zF##bd-?Ad_;KX_D4snA?&N43m%tlSxEZz!OnDn8sOTiZqVSvUc;QUW6K_LN+Hkdfp zO}33a&JAlD)-+mNSo|2)1jX(>60#V^w@XJ&J6KdyL`N=}*Lknl9^+D@ZszD--bns% z-311=i%HXxX_a&#RBUa@6B)|}T(s8Gb8ZoBTl8Fd@bAoi?O&}AITRI;x~_tFc%Hg8)e7p8WnkD_1eOIBElD1A^Ijjl!rBp zB?w1@zB)Q7#pPPc7n0|snu(L*1gOFX6gIl+Nr9oQj!h{`46BQ>}5NI;<~4pZ#_ zET?i60kUH@pO^hk*C+KtE~sU8{L7fOlEe24YsfJ9A<(!@~G*=2II*S87R zd%f?Z>B^Scl23{|USRiZ>y3*;W}K7w&!qEHZq?6psK}`P*irt(8uwgMit+AJew^{T z2v<>Nzv7-bN$z}i{&umFC*b-ZdB|u+HC#^}vydE0&&-_(0Ftj02m^kn^Fff!_R&7a1 zGt(hTTR$3*i4BAR&BQ#OB^us`Vjgv@lNAj1-(L($4PRt>o0$2|e?OFE|1TPEPwq$L zxF6v^7M`uVk(b+tBnK#32<$5#NTWweZW8kETT=MRV6q}sM9A(xNl&O^)}o$^q2w%I+gxVUttoCwD z3AnPy4Q)6NO#_!3*>a0WE<^O2q?GF~P`6OgY3<;XaB4VOl!21U+?-G&P-J6r!bzzBo(F;lUxoD@wvJGH8l?o0rs(qViHyvYNg(PV z1aKbmyjJ|IY?62U5lsm5{BM+ws+x zb!Kd=*G54haXKvG_?i-7G%=|WoXR5F2c0SuwSv6ze|O8IR*Hw!rD$3Fw6e4HTQ{kF zqE3VP&HkA_wcRthU1Ka)VJ42W2sHe&%kA5y?%`qb8ErtDG%>Gj??1bps&xlSobIYr z5#C0}a5#GAA3nVN5T*bT~9|fj%LAgT-*c)#hj(ppkgN}tF?*|tc zK{K8wrtwH})pj7{7HvYouiZPNA_>pL6p@wT14T}}X%tJlKm9Vf>h5@z-%O{vh>OaF zzBYR+2^u*K^2BORUKGHU*G&{CvTIkFm z!#Yk^{1M_LImP&fFEzvF0&60k^u(sBaiVZKDJ|#jb0#p&hyv;n$sYq~#|_uKg^h%c600z2~iS|=^gqCz@eAn-j%pHCbEDIqhQ0|z_? z3Qz0&ak(+!H57u0e!nZ1s&(CqJ9Ft{iB=+?X23OxJ7sGUGdl-L>|H}`S2+dtG*pN6 z5j=cbjK!A8nD?kj9*zxG3v(>gBp#H>C$%VzT1u6)Oaz7!IKSyvqOnvdR9A(MZi1c= zYHRYA&RAs%l=(>IEs!G}ieX0ZT>Xmeaus!4ekFlbZHm&(so59E5iAQLfO0U&hJkiR zZcj|MKmEHbljJN?tRRu$A}uzayI!=+R4=);CpyA^egc)wBpu(T)xpj{#$Qp#BQ@~S zghn9XS`6fPqU@bpX3*&LRlidIU@SkB&ckq9Q+bI2J4VdoQE}}F&3ns>E zpfL48KBy0Q@rF=P&eM3qUh?z-=FyN~8ugQD@*W-5(e!#!>bG)2 z8Au?#sKuD5Vbg$)S>}#JZuEmQdpw2m@LBpi zc8wh@Ylz~d($NPMk2Lf9`7QM#HMVnOAu~YpNhqze`iJ?Pm=Ct`D(b^PRG2_uODwAj zua!F!`TeYU;e5(>i}|ThHL~RVzfiKoY!@^o&LP^miF8Ma^tZ&24Y=r|4gG$F)K8JN z%{K%27H?;NlFc4zz&Q zlPS{0w(Ociv^uAL%%2BgJjWbA>v8hh*B?JU$a}WK$$PUE`N#i)n6|s^$x>=qT%NRwM~)#ZD@rQ$zhxa%RI8rmkto>76Q7 zFJY(yN3j^)#DQ~ar(46Qf9t&MSxxD}c7 zu!O7$sx1rVRLP^4wdPxMecHepWPrGPylR|P^cRyEt{r7b8NvZuwB-btAj|z&Cy8p{ z$@mz@qQ+QCVJsU$kNn9~fzG5{UQMuRWPV$d;D{Y7a4l5Hzrjq%jDzKHqH6~)(?!EF z=Oih=^262o0d|4pIOOi9aS$22j`$TC>-K!5D@cB-1p)O=pXqUUDqppcSXze|M~?W0 z7L%E->Rsx?Upau~(wY(QMN%!Pt7@Nxk8HIqP^;lPA~|4<@8sa(MDIQJWONO#{95(h zlb1)!Kx{EIoMA^eIF_ZQ7BL^pV#0Tbl}<;>Mkr`6d%CY4=A62(m<>5=+4_4WmrnU* z+n6}tlO%o*Dm7BD4`)r)ps0U0)8H9S6ozZ1R8E*dBd<0FTc!laKS@UvWzN%*RF07S|{zmSmcx?m*2!1VK2dhDMi#(v~+mJVQy5Wa2&d$jl+d($A+JjCzzt5)IyXs zUq|*s5tHw7D~$Mcnx)wE%)>)KAx)o3W5RsPNMOGS!`y0tX%0Rp|Kr%~TiQ4)NO}|q zZJ>#$9_^YKTiq+b%8fD{wos5sSDFqZvsi7F;3#M`*W56q6Z4n-LQO*C2Lr7hUFdDM z5JcPKdf>&G(kUb#dYA*G$B*z0Ij*=~lNo62)73aX5HuH0c}m^fh{pq~-P!2zK`f>7u{HivxBC0QGW5k1q(XkMuM zCbT@PH?|3jRDyp0RKM{|22?Ua(h+E@a+}fQ-fG>hJv&ZAfd-T6qot3?+Rx);_lQD< zFjr(rmR0G|N)<13u-zjbi)=W6c%OxCOrBC9w3>%ymypy6n%&2=&+M1$FeoIWJlkC4 zix_bwJDL*!6LGa57t|Vu7TS3)f`}UU#)Q**jUKD&oN!E)?<^$id4GmVn0`-62@9DB z$-Bo`jHX^TJ{q4CB@ub5Nvb9pnO~9o>Pl-y71;*?ON#QUmSfCjW~%5@mlc_!NQ3fLvGb%tf{DXF_GipnC>LL!L9bRv`k0gjxOlal~uH_r^-Q z3eI-mR-l2_8T%F*C4Y}}=V5QD!`xa2tG07-sOtD9T8qfzI8}A;VHtcvqR5#gq|XMr zjJ|k$>gO^O6AHh)jN~hF_ubeYkMF4z;*;K-PxBH*GP%2!K+vk^)6vnS!U0cnBqk%1 zPQem=9g0!cO53%`N{^!0pczRRbsl@6lGOr0ouiHc-6WetNkS)N(VjNCS>UR(2W&iB zBOq_A=v7-KwqL85`iq!23%^l4^ew-V;=1%~F%Xt^zLax>S97%IGFvBI40E=Z!YUS0 zOzD?-xE^SDg>T~3?0vE>=2)h<+#Yk6dZSm47K2skz4FtCLUX3cyaks#H{*B&SDFFG zv4>Q1lYYiN4lYPNndQz6g{NgMcKbY>DYg=mf#&`aT4oDe23XH56rg>5HFKrD7J z_Wv@Z1;ccYrpiiMV~Tp{O`Z{?C4~arQMPT_TC5|b!4Bm18%b?*{=^Rx+rMKUlqCf? zRdA1q{%yw0`YWd^rM^R1e_iwWKRtu zn)K~4;$o9)L>J*PI2OXoF-&Q?^_GVfP&CQxGdZJX`Pri^xroJ)k~_#S&YA(4KUI)? zcof>PAovc;n3*4VbI-aw_1roF#k|FrF4d!d=f3AP6uVq|p}sR=F&S}ku@+ZT&Q<%+ zAx=3?sQHPAj(f_2y2NU0#;86B3rDCvSGJ0%5tE_O^aNfOPpV7S!?T8m)NY~L8C1a! zv1_YAZ1J*;kkeAaK4iESQ>p`KD3=MiGX1y)Sjuj=USLCpmg z?*{^Yy?zB5@px@^w8sj_Cc~DQCi;Vy14?xU6xW4=w%VW;viVxTC{O@D?P_a0c(HP_ zvv#~zD@W0ec!Bv~OH6gTVx2U0Ag3lSV!0GMLudoik_}>6t(gWs`uDHI=OR8St!(Dy z_@TA36}4G=tcHGiOPcr`+r4_Wv)DDuM!2NP2eK)~Z|p4fkb@(9!d`u*L=Q4aAlPN7tWuOXm z`|K|&c@v5DY$q!hGdHh3@$B$8GNc@Ajc#tHcd-&oA8b+s;oH!_U~&%1HD?y`v2*v`MS9N#u= zdPVO^5P@EOvwHA`vhiGGJJlz||BXo^ldto-JXco2is#T?b59(pDke#5%s{K!J7vjS zlMKZjqWX99+LBb08;AASQk+T^<##pkT)c}cT-aw|IbK0r6k_^`5f`~(QvQ<0vi%#c zadub>1ubDHXL~i8v7)X?)&4Jry(_{x`7|H>YNIS&gdeyBb{r8Fqc$Q~#i)CBs)MgN zP{R|yprUmVG?Jk!cn~*K2_%sM*Z)=rq_(GHujfv4wx~)|AUGVNQ2cRVLE7)q(#Jj2 z*{l!;8sWeV70R=XguZi4?vv;{$Vv@ClV8fKPIBsIC3Qn@%ro|wrBU@fC5+RNUWs)F zn%C?4GVaG7L0p(DU7)C}h%KUuVQl%VB1w|ShuaSakveU!Jr*X3+o~;k1eq`Tqk_E4e8(yjdXW%A&kvWO&E?s=%`wUu6a(jwCUR`u{ zIYW%l@Pv}+IsQ!He~yWxT8}ofWQ4;RvIpRt%V}e-4bJX z{96AF(U;(VnbuOIr>iUUARuwipdi@)<3o~&s0_v|1P%xY1WpnKg)o3UAuT4X=9zt! z^X7ss>EGvLPUKzP^iv29J95QH=tngf3|HO^7kYUA<4bu^P!%bO`U}Oh;tTX&Ku`Tr zO~uJr)7HdAx_tm=WM~ZA1TiKajTn7WR*H{8dtRn5s7665iUo8B8K4@ z0D&lwzz!f@gaJnm!TldX@(kDkZB%P$Iju?#R?*D6V9vdt`EmMG-pj}1a+P7 z0Fu24PPkd&9@WTzqG%ac3LE(EqBksyR&RC5WM=Ro&0!}JN{<1I8XvgOyX)C*#BmQ0K*w=Vpw={ z;lD+0A)MPPui*c-w!p;^?DZ#i$p?NLP%OcWL+I0CbIo6nf^bvAe?uLMCWBrJXg5jZ=NcGqTqhufF1=<4%7i$mV-F zuNyisFD!RJGPqyAORnk}fP0$1n1`@jY*I)0+T{Q-NjHRC|>F*a5GQWq}Vz#Bd z6r;g55yA8n10gtH9tZASOPh?n-j{PS?6xAMAbdTz8(kz9%6jp5Z*?waZ!arms8lMn zM1$`Kf8p~P`4+;|df$AM-JTxT08qU{hKFbLo_YyLO{t#=sXJPQe50~E@)NvSDI07m z5}*~-N&&AbjowBwIylg$v!0RVVn!g%LIvgM-_F-u&gAhf-W(>Y&tqDf#A@HU7uHb1 zU$;Cs-i|MR5do~6tU6*;`Bk&Zz`sX^X}1>(Dzow%z%$qdANA$Tt^n(gdQ>QPmh1d%DFWSYBegjf;X^4H>}$K}k=DH(i% zp0(}vIq&#r)Yze|i#Kn(Cwn`Z$W$|gsb9;zgPu46CxT<9htpfWHa6r*${mKb@5dXi z4cxe~GO@8J<=5?9&gb_Kz`M)TITz2Kj3JakXq^4p=I$G-Q0M!HSL5HY@=a|`%u*(} zpC~l6!W=nwjTOX%pk>2k2jZSm;^9%a_4<=R5fTBBR8Os+M~QS_Z~ZMRpRenhd9JqS zfnU1V0dV=G$Bn8(?=j>P7e0?iWecJoK=zcGe}mo3Z68IO^2#a%pyoMS0N z{tL!~^5+$SKCjexP6px1r!C2Mmy3BUB@lJ>VZu$FKS|dW_2Hpqfw~P1>aY@0c0ZjAWulH_@3qx1el%kbJ)+92fO99{JC$W>e4Zzot_v2u2_?SYb z9^^`x24i=4F+N^cTNS#c$Gh!jGCWZWKNtA8NYVj!{1CzZT+{L+YpPg0uZhm<28Yg0 zd;@Pk`*YnrGD;@}SG=Rt-V99J@!e4g2>YP-pV+uaV2tIfxI*lhI~_N*pZ7yR z^=2Kx70vW9G~>IX^-^gG!AzEZXx_G_@0R9MR5U(~EOw+=!EjanCg>u?&x81zqXw&{ zKm{8qatFiDhMx9cYNKgP(61|HF%%l~3gM3yCT_80!Qj6pGzL-3Upjup-`sQmBF4Ck zxI$DM;@sB<#PNkqK^#B9zrLbBfZ*0Wz0v7VjI6f(=_w|nkS~WdKbX&*4qt?ZrX!nH z5uboNbfS5jhoXE8e{$CzL>=&r5A{3QN}tmka7oNkEAc)1F;hh{Q|XKLMD?$uk)%&EP6ke zMugU(NVtOZkxmMdyyco($h%?DhZ6y-v*PizCP)4${O>5mx2M6Plj*WXZL_Z)3yr&< z_cb?6o)?7RWzi}(hg}ygnl4_4y}i4rq2bdd!WxN>t>>D|^POo!E&+9SyL0qSu1hSc#)CkJNWU2EI{r|K>rBnqx(M&HG!PPPd~GmHYZCcXll96ldP zm`czZBc$|KrxO#~eY$ueE&A({!FhxTxF-A;dz9@8B zek@sd+Q63)5bu}2Uer~P1xW%>YdQLX15fBA)|= z*B+NxlFz1zevlRUUdg&c@CoO4I2@Msm`jr&l>$G$l=(!SAau*9r5HdEre=FKPs+Il zR`4>^6TY=IglW-q)KT9sUY>~9vfVe&hKgq__y|>Cibek@vOa%Y81i>&dgUC(C>9qQ zTJA5!bnOyaHCn4|=3&(6M}?htajMfpmC?TqsqQbjo&1Kz&(V zHKlL}U|e1kLpc|i~KE3|`N z8DhtRWLp~4g@F;`PJQCDBFkHc=$Cr>?{41o@7;ehA&$pY;29-&fWscEk0`R-PPtqwNwbK z^p_k^bQb@G-6L!B`P%dK0XYLSC{OTXO-P_*Lhat` zI`{sh>v+4&scGWu)fZ!)wjOV^T`XvUaVO~dUAyTlu+JC-p7o#nG2~`9{0T=$V3>$y zO)~-{IB~rGkX)4~Oyf~a^pI!fbcDqkAE_%@Krga&df6>~mdC%_6cB=|0$$ElFye(_!Zm^M*4S)KoYvCvT}Lpaa9la}Q)hCW_g7`|Ld)(p-(8{Sih*);D?oe7H~$ z{=L_*y~5&sPCvOtIc}IWsR&IGiVhdVtXB0lgt#Xqc5*CfR2>o1JAtpGREvomi@ND|bZt9WQ+h;9Z$uN2OF3 znL30!*L0Z9)OIEn2{+q5)i;xpn3^m;PPiq$zRoAOsH}N8J;2rbL?;oMv+3 zA-p6(y;rcuYD`V9*SXZ?4yAEPdQ*K9Rn?8CvfvLc*u!`v`-1m`q3o2t>o$Kfmv^o6 zU?j=CM@@?|Knl zy)>kO*(u$A8zmPTM5f+SZliq51S)IJWv~XE`lv{u*vzJ7mpozFib`6FvZz;(KXEG6 zC2#lgD(P%Lr}ITiEhyamtI*5;YZ*dI$~^?FvxunRfAw+QQB5vO{7ZsJi=nq55J030 z3IvpnAs|weA_5ALj&zhFr+4BRHj=bBCWD-s1TDz0S>d~c5nFl?Z2g%)75q25fjT( zf9a@Dg33cJZHf6ti?WGQjBD*+_=|Fwe#DE~O!bF*t3SduAZVN~#PUWao?=pbq=r8f zdGC0>U~HJIbD~6T^;KiV(FLsesMc`7fg15V`k;;q6nL2I3$f!{#o7crPZuOa zi{whFp`r9cKzbpk9p`QX$!Sz|m7wWu+Nu>SZc9J^Acx?TX0uB51PfS{j+Ir0cNsSW zGBCGvBc*vl{ky6KdyZSr4>!X_<*3(73Ff^P4%cQm&cG-2Wf!^q4AP);8BU&QhisX} zfRA0E<9rx9%Y=?>Kk2JLS^*^H(#zXBl}%U&_@!<6H-~$TdD)ABtALnKe!=PqHjG6;tPFD~lNi{j;o;Dx%o}%2IUOucPm}#P~&EF5Q0cId62KAqHT* zt{o*sS8L`Ze?KUTl~sw$`8*nnDH#Y%9R;nH0!3;a0lJ245@XNZ3if-`Q5+i~Rq3v5 zLEpk}uyWqHF2eK;)W|CLYpYfmg*UYFo7MA4e2+8uq8KqTEOvH3ib&APZ;()>oDBD3 zs16b}vJCI){^;w%CCyyH6#pj!Z)vea_3SCl>o+C^?uEQxu!z2iw0y`Pb4T>grkz>P zO`2+n2;mOmOI}OUA@6~_5ya;1T5G6mt`_}V>b=dIyy-5>w11rS2YA-m# zoU6{=J)m33$Vi{=iOo%mC2iArx%pH2n+M;*=H@Osw61Iwo;&lYiYbvwC0$2pZ{%ap zfN4ayonC-a;)PaDP66g2YyXV9>V;dN5*2ZQ$8pt=x>G2Lu5ZtI{yc*}T71#T*ri*O z<49J93S?fpYd%ca!Nz~T$H!rxZ5R%{2*Xa|h;1Ya=_ZFj>_^?e9glWB#@4lX|Z-G@@rId?LGuf}KNlSyW)0gwejI^ovZb zS{Z)Ki5^Cy;)~kyWi2Z+-Sb!1Pfb;J&*^P+Knb^&FT5DzP zMnol^m+~KuY$@D0=Pb(HlvO4Z%$MxH>dg7Wt%vV)-yHQ;j%+EltEx`V(M>7ho6D7? z#f}T@nuBy1mTyU5POuNfW6t;(HdPB_a#C%*`8Gm0A$9f&OLF~)U;KoLVl?u8(~f09 zGGz3~L`GQGOSKLQxo4`Vi>!RfV&Srvn1M2`gFLu%aL7Z$$48z!@YU=pmS%}{C2p)7 z=%1B4CM41Tk?sks!TQIvViDPt+~F}b(X}`cEXP=kMGZ_) zA4mG74TIHn9?f3$Ve1b(zq}tTc;R8gRe3H^nOWCw{!bg`B+j%FQqJ7``q#K`I)Sui zL7oo6&oo6R=MnM_W@@o`-}o=~s+_`jKVM-9t#gn1o?WGuV5I8!+y}()6sn6yM30j* z@sOtE@>1^)GG*pU>&4iE<*`d`>uVn;2kShKBcFi1`;sm0l7UUMWxHF?`&xN=1|wmmiBk zDh@&t+nR%SjZ9@6>(>@+ik`p{=%1Y$6X&Y)&pr556>c5iDSE83?oLN{(R!LsU4(Qj z2A%+F-s4%g;eB!?;$@k5cvj%{(`YZbMbVedRwDiY7tC;4|bG~-=r}Os% z&BJAUGBMk3=SRmb;P{x?VJfHr^!``4^VW)v@&St%UN=_Bw07O5zI7^?oO%K}!wypj z#jOuFH0L7_b{5+hQupPDA&=8t#VNu2hgwnkJj z5PNFl77Umd^m3(?CP*3x!m~DlvUnA5rx*rjqeM$2z9MtuS5HOd?&0kdbv}f5ASGpB zD4lZXO*OVpRR-YxoN_dbY;iH4-VmljQLb$jZ~rWc;uAmtRf9SJX@ar&QjWwz=o9`J zRwK{QAj?t9Q%ihUcB}JoC76P-yB`)1F3MQiKhYUw@=X9(47b_SD3!&s^A?2&b`At-)k<)M#jCJIb z{;C0QRQ-&VkG{pyFpyi5XZvlT#_#EsB>tOVwRiQ-GAB%4u`4cZE;V!#|CZ+sauSm# z?wBuPC)g44f%}3(lY%smlq~ILKAXO;{C4KF-oo{mFJ7; z7o?p$fnjXDQgH9GsxYf*^6hM6Z^cSac%^D|*f4E1!s6h&bzVY*=w#g)p7zAH@M}kl zz;$6KO{?sZ1pF}Z*_GCW_e|R=p1#xI?8MWPx^dI1ZTmp>4kx*(4yC3%#F$E4|xX zxaEq_-Sld&mHWbC9G4cS-aJ_1x8Qfay@Y zoodH`%YEy?*@fX3JM=x@)xXGc)A_SCUBvmYh7J?Y=A4{rn|Mn6y4-TTcjZ;9_Io)4 z_LIU%7r+775r0x4h0Gmo{@i&Gb{I@POcGnH+!NZ)mXSQq7)SED;UGjS6x+woFPDWz zC2eU|Q!#8`NT9J?qr%k`{#kZB7E*96OOw%e<*P+-+1Ta+&jcTa@k1bZS>QoHYI}1; z4(jXvNMqy%VO%dtAUauMDR9MG+nf%X-jy_UgMM6R=NcVUUB^dl|Vf7;zeWN4>T;{&*;waqhO0w-mGwTGl&=#L-viva2jfZrqj0-o)cNHL58s zrEYUn_>yi;YSh#!lob?qtxd8>(dJ+1XQ&T1i{I1Nz)@)GW&3pprnXyO{`PIDyIT)* zA1@1dR9)2uJ}^^Az5k&k@X^F1b6hY)!Dp?+M2uU}f2##-C-mBvk}@;%H&!>M%M8?V zGy5j2Yjy45z&dwlKsxlm)ye5=y;O+}={}t52o1_#DJjzuh&aZ59*I51qlTop^4ylf zNeVeC-|KVS3*ONC*+@429lvpdjCu+42;BrFOUjK}L)t$w9--)XYeEOdqTDTkDG7h6 z16=rVvUcMW5yDO(1KbKY?@((Zl|3SE0XSiRo)Ypv(X>@RzNB7KRRicK&YH zj}nyCId~7v|6L*i$fxK(n~OBTM_us82-DS(&|Dotox1oBa)C@v>k#T?x#2%O{x$DI z!Es$eI+~RLM^pU|p^+}(6q;M)H?7~W^0@vv0w7kGV29@Ymz`w{68iek3D^G+?CBCb zF?|1Bh{J!+=n;--{G7*QGU=d4fXfME{~avr7DLPpGE(1|6aditHi!m2a+q)VI@$aH YZ2OlU!BqnZH8ml`Xq==hzIv$lAGA*-%m4rY delta 14485 zcma*O1yo%5llcXxMpcZw7*QrxAuyKE@#a*DfaaVQQ23ba7+;_mKJDDct8?(^RF zj(_}noO9So=9+7+WG7@~ClBoqt+5a&stQoh*dP!*2!t=qlZf&f`0G}3AZjYe5E=yf z{e39~*;}}ouzK0s35&h_kb79TxLP|ph){B{vQv^YHJ|1%KB{|Ogh z`|o%K{#U%_E+(G;GWl-{4tBQR7)tU#=>`Awv~p64{{3kFZb4DuKibjE#o~`(Sj+(v zHw#ftb`DOKmk$?*2D^YD7q1|PfH2$t0{+Gco0|!~#Po*&7YjQF3+JB(oPyjh27ed+ z>B7R?+U=!_u$a5M{NYMeRfAuSn_Y>EQ~fXQKM=sbA^&M<=51zY;VSyKl)nl8%Kj5; zW#M4)!tdxJs%UNMVxjU!CuHMd<)v0KF;h|3HuB=Ud$zg86%VHwH$HU2Q#=>vO&dY25m(>4p^QYUtdn}9V?+)PVW^G3P zM|C-x{w9BU-rODBUd9S8K1w!G;lEl_@Xz}HJM|X@_Man#=%3>fJUsmGaS3b*Nd~C7 zZt$W9oYo9^-H(f+OUZHK(K<=Vy-TA)K-sFE%l&NJ7m004Mq3&D!IsLOJ`N(JfR%^V z*QBN|E*+xk%o-lJaGp|tb|g>K;05?2#d$Txd+_gkbj(~m`jS{=4GFf#lpISmmd+l3 z`UMErkQJugH$_Jyt%@3`#KOp-y!wE{Pqx03p|k+{8KQhyz-;AME!%%8nF*utrD~0C z=AgX$7a*c1^I?0N)0GKArY^;MB%mH=o%0{nP0v-!CGM@9I~Q+586#p;NIr(eDl4gA@fy;Pbo*weHlOVB8+9Hut z#dKavHAP%YeM_FL$kwt=ePaFIUi-kVX1#XZ02K)2=Vuh>=T0B)UiY4T5-)Ag*tgxq zyI#6Wj_AkvCgm0Cl!>&n?`r|fzPrVa7RK!Bz;lRXNFI+{Yl_k1C%i|K=*!WcH$w`D z847f&;h|8J#Q0S$-etcjm3O<90QL$Aqd_u^EF!-TR9ugXVA!q~#=M*_CFW#6H7*H1 z7egVKvpFyaQQ$`E4_kA%9&u#OBao~Jlj(Jw5gl0uS&*~(kTL*kUI6flNL~8 z7jLe$JUoP$;dLu_-mlVipRr7>jA79|md#~$TUi{S!~CvN(}-@m-t=opj4Z>U9@#BI zW2>7|i7eO(eiio%Ky*O5_CD9N|z;y?i zM8HH6vxNIt)j|V{&g#2bWQayCrd4){L&qm+n3J%q*04bpaxc9%y>JL<8gI0cm%%r^ zexY|Gd08pTw4ZSha)w)y>|BoQMR92-KG$RofM1=y*-m>4xL__#vO{OKzuvdkMGX!x z(i)rKjG+&9Ip!`X9&!}y;uCg5rK@g~>y;&^aA~HOs1zdS#5n8syho|**uPsC5PX9f zE8(UlO?@r1=Jiat*C>WVor40VXJl&8Rk`P)h^eq|z}tt_)GOF4RNT87!mHfa^>hmS3FsQW`|NZM`h2Yy8$?z>0QMT1O(KA#jodF#Z*boVU zPh?1)&lfwezp%pv#rBc$W67iDbM%u*wz(YfpT7Iri5+HQd~{S*fP>GLt&aNkGQ-lN z0D=Bv$di1Ta{Y55z1)HQ;E(}><)lS^kFGKG+D3UBPxTKGKrg{!^0XxrUe3{vxKB3V zOa`5TwpOahs4|cCTN;=2+hOn)aY6woDxnN_xn$;Qx=aERJs%CO=ksR)GQjKEq6t}) zZc%vjhoh`HaqN3@k>Q>F`;+6nF*1N`pz!we=_K5)PR{CUrZ=yzHVtrSz(4#$=#EJI z*!`(w$Y023u6oscN_cd&sklJ>?QQEbC*kHd=-j}D)_ zxzdG2MdQfk`4?A|B7OiqrY@DRV-+r$;m4C7GRA1Xa()iq#^R&}`_F6EXExiDr8H$W z259@gZvL2`eo`Xa*)~rrfE|<@{eJ!=`baR&&%eu-#US@on0@S|bn%CaC1QI)@6Pe7 z(9z?z%Kej-sqXVF*&%K+u>k&|8IPG89qk`MHqA8+!wDv_tO+G?>7zsY>-q9JX!TGb{Y7YgP2t z^-C&TGnbiNu)2iRE(wD82J<_ynu`W`&6RA&U8u4eyhhVNcO7s0$VJw1hT!RDE^Qj# z#yv8V>Q8C~w){d*Uz(ycw+nf9v-{d%RTry#omUs0V%U|DKar!jdGC+kbx{;v445Jo z?jShe9J;imuh0}mb{{O?lpQR4w6&^OJuGJK<7!g5V&En_DUOvc{(Se`X_4#%g-^3+ zkg)|(#edcU*xx8%ik7%$?H4-tTU91za}B2uRF!r|jdZI8ehhOrc~Gbih3nOIEmNTR z(S+2qW;SLKs2&bR^dItNj(a0no|%I+B2Lykn%ZNRrRwoIR1eBbQ<(4kymOpu4jlSAl9GRSB)p6^CWTl_FW0rj%x?_!WKC$7x;k-F$H|VZ+u(IO?_jzPU*Ho>leFUx_ zp9ZNm3EVEH-OOgnXGq%h)+gYEdf-AqfKKh2Wi5e?jU`*mc+KnN;8?C zcg5_I+|Q=mvhh+i)lq5s+Rd}=rU-_{%rbVBBbC}RgQ`A1QL!M8K7?=WudbD1G|dZa zpL_6+PVFzGMp{7+Zz-fNllD__eLW78Qwf?&^9CgvLT@BWYXoK#Lmz@IO zpMI(3)_T>c{XBnXb4Y^A#@ixFI;z^)S8&N;s;`q`ZCLRdm6U8~h_zNxly3M<;r&&u zK5cv6Q2)c(W8p^q5aNXA`tjcL(=EXGtBS2omD_zByD=-^eb#H9q)^jw)caiV;h+1~ z*9>|m5MD!S!Fpp7%JIoO}wUuVpY-VtoSSCBCRjF0~*B- z0!ed4M@)(p3dwUNN@_u=h`66Ge}$wXmZ?Q6g!~-ba^hYRt9!y9g{1d*cdfan-Juij zx@c{f+NYc73{@I9H`RH83f9-D^`2t*L?nVKI6vAX`37y@kcf$C$|!GUY5@yGu5%ki z@r|6Po?lJxYx_}R8Q6V?@<~hr#FnWCjm(8-7^w=(Vp2 z4QOzj{JE$mxQgdm^NU0gZP|0r$oiU%2#KbOG*0QHD|q<)7*R7Fp)dgO&w9qVcw1|r z+~6+ISi2OEW?zkwrrEY^sW+TvNYHb9)QPY>>K6JDn^T6GN%W?`f>mxL-NoW#i%RgH!EYr*v6dV6pgM@^Y`32Ws{L-_G- z5jLr5b97zBR(KvLHk~%vPtB9HutZ#+1bY*r-nv=Dabr{}K7CY73pU7z!;4TnQbkoC zT1`%_hl&JU0-x5d*meW*T(Sqdc+wqnr<*!{fQUU%D=rkI3dsqS{L14`YO|D3P2S@5 z`k+@q!_!t=G0yaTm+4oTYkSxl$I=MYCuiBYO_YyFs^M9ojIi@aFtNSbh3uGwOyCiO zWWxR|b*e&)=;*6Yr_mxqy%`&-((`i|s=PlIAPAXwo51o>MR4Y0 zNYuqqVJ1afQhtF4HXnnqZlp9?%vQqGL?58;m zwnr$pTtoccgXdZ_GyQc~*+DGJ-nNQV>MqLB1k#SMZ}PmZ1?J z4H`MFYdP?mKA(iD8}Uy2H|NYra}V{6MMC8fh)b(sx}O2og0i>+_hW#6T8jJKR-T zyGYdj&O1tY&b>yiZxOwb!F?p3wG-K0Z2@w35LNQ%=nBcS zIe=Y}fEDKPEIrY(9_4=-(y+eag%6zxwdBN*22X8=1&{T`XrXc$mqg)quVCTuZ9<`b>zV6y$5zWB51nTl;U z5i++5qVaY4(e@gsTTzj?tj@9%W2XDkcz_JnxgEyYHGV)2&dRJ7n@e_OdS=q&HsL$i zB;Y|ACQb-)mylt|o9WB^CWnYtbq7IBAQN?5?8@T8U+bk?^& z5sLBLQnL^;G3qNuzV%DTo0LJ*=oN`r7=S}SLB`H03RP6qW|gW)N2ZCC94JO~C*G!@ z;XrzYoh~iDNh)zv%`f9XwdJALBmvE*8u|81E4$;#(DGB*1)4Gtx65 zI`OS}D)C#Fk*ZB<07?|ks}F)Fc53#~@bH(clCGk~!&i>MJ5O!OzLdUSS!aNXGD~#2 zaQ5s;1DQ6godB6m)#5fgr8mvWIQcU~oi|qb?4$_!GH(MQT`fn3HGrKFSWOl7kjP6lUqI0+vdczub9WJ+1D`m^Xlu zxH+#)&ohBfSsa2$BS@PV0Wq2OD}$+PQ&n+^UEf6%^|op>qyQ+e<<6Hn>Nn>0j=Kri z>T~AzS&rYIyErZiPa7(MIKB+8{OG-4Id=+#v zXU{hyMDpArR@Yrz6pH$AnC55srJJ3!K2#4P6sT<|wG7(ee$&DlbkctOPz7ZVB4*#<;89{;-1wL<9-?PRWkjtZN?z z+C&Kt3(jUg?A6NSd(g3E@hBx_sX;{%4Sn`}KX~f2;@ZGC@fYBH6rCab$#kKn` zh>Tq6ru&9%+lcup|Mg%An_Kt(jf4 z$i$tI?#%E$SM=&CsCvt6=sHzUV)H8$bhU5eJcTB#r{J66-ApNvlnqJHPAPZmr}Lfu z6`|!<;pKEsYbl9$>qB2h$=%~bWWo%mwuvjVKqY%P?>kp+Pc#AM@b}_Q+^(dWewVaa@(oP6$PK%rn#)j0@~5y%nNTN;O++`rS85Hy${s9=h{m3+fZJ7`ktZd` zzsztiPitnj5+<4L%q6%EgTWMYm%AhYkD>I^E>n-cMwheGd6ws!&{3Oi)t2kGQ{Aj{ zGkaT!i3gZ2jI`|&-KKnvlZF;?H}zxAnZPi(aL_6zl{n~mb?~{nIJyC)4U2pUzC3s2 zo9ouZdk*>(XqLG2xRbemO2fz*F~2hd3A1RiGMIK60Gk0S!@URX2rr3#)!!hU%mt zK@pxypO?EtII&Cy9}N6vKaNwFA)7wzn}h^eVML$LxSABNk5wU|tS#P_i8qz@K~7;g zI6na_3S8kB*RA&RkgsVF%X-7=wB7IKHX`cE`iD!bv>(@4$ z;==-a1$6Ov`i@dAE6cTCB2zlAe~=L8qjmsU{^%{KNb^ov@Gng=KkRL9AFnKJJfZNl z**e-4zPISSKhV8wZ6R!CFa#C42#|ozLu^&>)hL8=Li~W#PBw+c< z0HF~Ru|XGxN!ijFLDX2JzE_BXnMy}~` z$jaFAQ5;7VrnRwogXc(2ijUa+^I$TOV>VW%c+7{1StKfFj}zCOoW6s^+BMl~A?QL; z|F-WiQaMGZO`$os#Xomr2Rck`b~!o6)6-r`<=t1JZVv~df?mVv zV)V1WMs55EowI8bV$BXp$1dR3_`^@1NjlJhG@X~JIrY1l)_te zoRNVxKghgn!<^I_f@g2OZS>bcb|x_Q9yaW{?5^8MNxP9zVkiPmt}tWBezU-SWK5*c z_>A^F+&#ishQ$=#PwQbzYLg-bQ`A;dMkrJ>J;clK2raXY!b@PbIMd@CSNYg0t8Cq( zuTOGi!kfq4h}0U1u!%U6NVe{|wk;j4?~}gJK<`FvM$~&}XaQ}B1~e3)Y&s0DP(|PC z`w;ie(L%HmOY?ApLez6nTRIRs6HuNn<1mA7WJHK)Zv167SrwyzMM(OgQvMo_k1+PI zpP5yywz{)K?tb-+8|0_qX_;_Q^e^LQ(1p9g7AC~$o~aq#wUW083b$8Xekj=PI@-h? z*2M@s8{P~|wPOfh>UGpB5@6Dd8&tUyLor&R%)UfH7aK57un1@~R{84xknG8x9h-S5Ie+qQhgPn=!!mca=EO=)S)mz?N_I`|UDVM@^jm)%%Rj0km zm+!(C;BkdQnfoaotRT{<3FkPRvE!X_3Tt>k>MJxDUgl7gW02tF~0OS3ATN zwOObZLpZvd6IvrsSR(;^D!~PNer78D)!~A7;i2wZQ0TjSPrB1&NP#^g(8~Bw@wR6g z;njj8ryc66OT@I%VFC&&ygtkHPu4x4;&(}(^TfcW2>?wIN!(lp&F~y@u?w-dM)uZ` zxzi~ES@BMSd~W_YYI+C>0vdNSQ&C;y$xKoRvXKs?t&`oGRYOG}`px(Aprmkn!}YHI za;dfGnC$P$2^V!P>sk-T-(ev4@A{a$EXB_SYQKQFRmtDC&?VBsER=R>kqe1nIDJ=* znGw{wzLjj%BJ%l0wck5i?BoB*KE4P)10UW_3)?AFqC^9#8UxkQEq+h~AcsjzlKw$H zbJEDL)bu5^kwhF77-b5lgOohOri9`AjzJ5vMtR1{7kMavfadTM&I?w#m(&^afF4r5 zFmYPAS7)HXnS>t!PPsP-?6gKr%#UCc16K8JpXA|e{k%N0-6PMRmd1+Jl0tejB2r@NQCMZii)kAHkt$kM6xN=Nf0?k;*w8aN@h_7(5p>yN!LMjS6b8iaXmUyF(xrGaa= z?|Tj65mtf*`(IQ>S1QPE?1YhMrM?i81NL}wnGOv0)fXe}YsJrkSs|Z`9+Zr8s(FXa z(YHjATojt6P6Q_}F5?=Z7T299&+FHtCiq1Y#vAj6s=z<68>|~JPk_+Zg>>k*7{Pk^ z&AhV|Jka4Wgpf2{DmTM8d-V0^^gcf^d!xhN zro&2z<0oom?>vvfme{gmwS79D9fFq=bmed{h5{I_sAl*0z82!X(JtkcZVn4%)TGvR z!F1b{?=jdtJ8&`PBgQ>(Y;t-~>8dg26LvmYKB`ze(4=35Yd0~#iW-Qr)x$VQx=DCI z>+3wcSr5Ds_Z@Ohib2fjI{k2s4qP-<9`}z09dp7=<};K{k-}1@X8rx1a8Hb^eClwy1K)&0boo#gr9kzO`u4KR#KRwh|SgV0arSL z`U3KhFmm|}cIC5eQWqqaX^x_2Zqhb7*OC!g^a>|M4*tOCw|KU=lWPtNz@^*Kts$s) z=famoF@nx6aumYJ5P~QTebGyX=OOzpCW`NKNZCj9(O&B)gh4-r!P<6r=sQeW=^F7i z#fURS?Fw$0R>WSY(DRQU5KZ3%p?4zM?WG}{_mg*znE4q?3PY)uDn_s|yYlkOo^B@b zm`Sb^c$7Et)U=C!QXP9}(S}b%B*4+IHfDZPHr2S3oMj&UN>E{C?W3E&XUjQ5rkKwB z{EU0|cVN0Ecl)0O$wu z(h?e8S;yJ79$WO7q2~gfk<~gft)3i^fvNsPP8{5Sa z^vn_d7XbJNq1xZq*B976w-9rF}kpt?ov+mBxs&89BkuH9n51iUzhvE1=8TbD`fKIUxyn;9pTzw zWh`WXa+nA-xe3WY@rqe%erR9qNbzOo3se>TW0G$B8j*zRBPAl9kS$G!l{{2ga+e$# z5(fNfvhO13y=EpocQk>x3iItD3k4*CgRZ-fV{icJ9$^3-HUas(ZKQ3w;p4f)1{X=j zQE0+1O-}}(<`Y^HhgNm#iVjamcLLlF5qWBxczsIb81qkkcb5}MukYmU;9Oha^o^GZ zb5-81Y~<08iylR&4|I!5@S2ot1z&VDhgDKs>Y1mh(<}ouDdnszQ~VegVB^Jrp77jw zHdM{ru5M8hz^GenSah9H>Ae6f`}L>5MWU~c%d>QF`@tv62Io1MWJ|-&9pNZ$1Nm0( zA#7&Em|%HIiH{+|KkUws?X7GA@npNgT^ppUxs zE#&U*oQ>mTNBhxB5Gg1hHez-4ljrqXDc3uN3dy8ovLiUZtO;dMMcB%j<^rWh08T{e zqi>uo%;q#)F?;!3U_TVF-(Fu$L70Q0!7&9o`ldp+;Vt_10`$bU562r5iSJ|TUIua1 z;i2f@5>zqGNKrPK6$fC@X>>j?x{^WCeuDV0X64h{t$oYnV@ydiQOx+-N-N0aI(O~P zny0*u09|D;el~kgzBaAJRR;Yg394xVhrJsE`OX<6hK$^%@{|c&K~a_K2c%1gv!9A< z9>AidT#0+b?cragNk>EI%RHX(sfroUYHYu6BG9|Q{Wi-e?mMf37*!gI$xDt)fxb6! z%MgVOGG2yD$z$7k<>@WBz8&|OMqv0_OS~2m3%>2fne1I&fL+YixP(IP93$ztDQIZI z6#&ykqZg@HlZFC|{a?kCf`*k#BcbR>zhd2b^y`#iD=|m&i%MyxY8TqL_7pPj^vj@I#i*A>VkaIx<&b-<88ltd`}y z@wbT`OBWn1W-!7!QX#$BaJ(&kq;e$8D<*2Jqol@pf}KqT5z434ZMuWGf^!U z1F6QEX%JE4wf5IGJmAHrA~ZC%TeR^~=*{slW!Xz(JgCMG6^4&Ai&{d94fj&_hPR$y zR$Blv;AKW784WS*f%EoQ-(Rnn&4BHe0~|xCbe*fRzk78Y?;CEeOjHzp=o#GHGuPq# zD`&=rGmfnB3Tlme0TybmjHr=U-Fm)F!(-Sme!&HEdaP__$)5eVxKSghG2dy`BkvoO zCLJGh2>P8!oK6}twTTy#wXI*s)!UJVc16;p{X<3Ov;hImbW zWAACicYfUF=qnI2gCxNO$);RmgEs!sAN4l2bVdU^7eL|)KQK=h7K4o^6WiaaMF8@$ zpASOCmp%^<*ze_`1d7qkGSWc!KXl*awR`@28;gV7vjB2uCmd`FMyocNRCC}k7+w%O zf+&~?a*q%HR7>zH@uL0Ka*7eX9o^W@0<@lFJ($mt?XL?1qr=pr}vk=i9vr9$T*4 zI4{cE-|XVZBn%wf?>ZKE7YcDCFAE^k!M#oV=`-U9DGWpALbqo!$O{sev-;I_%mR+{ z8cXqgoPwl4+?6*cUZYB=L<_ODg^D@d$LcmyaLo$9w~w4bsDz}!?{M_p?76hIVxn+E zkP;P|urGrv90IA!7`lk;`P|r_j#`UZOTcqBMkdEEW`s1MshLYfyLSM zz4smK!O}5a!^}qfYOmHf4RR+4WG$a3q1_PTL6+;#KBPHtv@?KjR&mKjeC>9sQ-YG` zk?ey82~PFR8Tx{h3Bmu2i&42F)2%)SOFk)o%y^C4ze^I?=(%Yb4>Ws$CEw!Ehd*f^ zD5xIB>g$E_{cJo)GO02NMB?&V!N{CLo~6g)ZiXmPM9h%$WhwJpY4U=xzBYd|3oDnS zFQf}iw&Cc`3)PPlWYTBo;7>&V5fURY+865a{>tm4U}8Qlt17X)0)=YMBUJs(8ACB0 zXQiqQi2NiPv{6S<1w36qP#F$a$-Ztt3Ze-MpJ@)v{?04zr->dcI#ahXEE_?Dqo0%& zqgP8u!m&kTD69KvzT^{10#!NM%@9U#Z=^$vEesvW$I7A4Wz~@012u0PsXbHY@Xynm zz;U|YX5#A3H+dh=*0*<$p&Ivcy1Jh)t1OHG6e#^LZ6~+r4!}>B=hURlYwh=d!1raf zl?3qi607ckwZ%71o}h&NTvX@I#KI@!KlG^To%b*{buzd*<{U48hFsbG(FFAAGvhVG z^1((Z==a0`XuY1gy#*W5@@b@S^gKkR@S-8xf*N~jB_3+4W8g$>L}@0UJ9x11)QJv} z+=6{r^)Ud%vihK*>98N-0m7IwIpB7(SD!`2l4kEJ5;W>7r)S=94l(i!Om78|Aw6^6 zacYvI^+7RJbZ2cA@mX8>6Xq|>?+{s^cJd%{G^t{cH@v$3rat|Uwv~v}Mn5_;1 zjm@9WTHka_sk8~ApU#X&6f_3)RosiTXOitcH0?XDQ+X95Y4hd`uu+*uFn(66xPxze zPYt+49P$s$D|iSCqY)qM9(!(`5HvG)?O=5nqnoXF->R{X^W~td$FFV5!z!3svW-^} zM0b6QBkR*#;H*Ut!1EALUF<~K1s-UB+<+o{fa>o*y(NoI)xXg7kqvA)jGuFv)OPtt zFHyjx&S4t8kN2t&2F(M*Xy))fd9^bNLI4GW5C0p7FphTwa6P)nsI+6u&&2u>GpAj; ztn9GoSrX*^=^_VorM2yWgb#jsy7A8&n}QRjptoFP{N=hET&kMO-@F6y;3)z7z zLC^V%c3F;f@tlzk#lYrMLS4}gb?oO4-VL3uo<6dKtG=&^KzQteK7%sfCRD0+JOQ`~ zJe;h$8F@Ja6}-v43G)L~o#X`q55=COhVL}KhBytVx>eri8ZAe!@SK(&S%6YceJfPEZGwN9ED7D!Ny*1o(tcyFrVhFAta~Gz^y1l_)ueTV4kCk(O zDwx5hp;g0t0Ys#^@@@;P5^-GLk5NSsh`WRm#`hi@vUV`cNsoX|p}FJ-#d?OwhRR;K zJ}rOufF0u5*+7t_Qt#Y`12|c$zC+lV2O7`g5evKbvU_)yFP)Z&JiIjwv@;3R{?-p- z7qxO)>{(Sa@1}}z!BI9xg5vjI9us&E8TIny)AB7Cc$v)5Y*1Y7+{5Ng1!}>n@Evby zF|+4TYgg3h$8ENDL8O$ZHGOW<%s)J~N=XuUj;K~pwaz5-pF`xI)&Ufp52f3Bt-x{Y-2cPGu3F!*1T_dL!r;-s)|W`d%|gkG#Ou*Lcw(UhTJ)!x z>$yiKA+^J<1kbrN0ccZZkV7soL%%*^C|rEU&Cb>eX@gx+W({z`NxRtGQ0Y<~I89%L ztqq98_c)}C8_5F9H~m^;R=HL|rHynr%R7g7{J|bH+d1JY5P_p@lW@4~J+$zQU*9sX zibgn6mp!=`qm{7-&b;q85}TDtZJRhC=#~wI2wVu$)lAR10<`l9UpGq{BUM%=ZFDJ@ z8_puhk-bdPWyU%mGVo&&z(fhXSd>mPk~gf6XQb!^OYV3{qIq9Hpgvg2?0Id<&*F~1 zg5Zm`ti4s{B-`)V82rUkAp(;Oh0ldH=F!uy11Pew zJBJ@LIz6B;rV|vd*4g?n2*Pk(g5ds83cQtGp4;7wdZD^72rmtk)d*x9jI{Ue^)V2- z7yDC{h_l<%u(o?v1W+wC4VZge-Zs^UOVa>~7H42IK#TOHL1=Sx+tL`?q3XLSEa>dl z%GUXsv8H=5qm!9?3U^>>CB4$QP;D~=Vm=at?e~)rTv+xP;&y!U`LAY=0t2kDPUYK{ zwEkvPP#{_#sD^9^WT^k|HHUwe99|YB*#5mL@q{+YF|w^NHF5rX6SDj(W$9{3)7V?52lu5z1dWipIE%JgL^>M)} z%J3+Es{6Y>e&ezDz|JrD2R+39azp$dZWQ=lh_S#O$|(PfWksI|GL-Md1??Xme<$1d zzyvD9jQ?2|zu6%FqNMu|>T^D@gaOAt&Gef=jvuU|Lj0GfzuVH=fQaPfEjP&3&HLrt Z(*IH4;5q{$NPG$KcLO?D`#(0k|33l|ZFv9y diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 6b026f287e..8b9e7c49cd 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1785,103 +1785,90 @@ int commander_thread_main(int argc, char *argv[]) if ((hrt_absolute_time() - sp_man.timestamp) < 100000) { - /* - * Check if manual control modes have to be switched - */ - if (!isfinite(sp_man.mode_switch)) { - warnx("mode sw not finite"); + /* + * Check if manual control modes have to be switched + */ + if (!isfinite(sp_man.mode_switch)) { + warnx("mode sw not finite"); - /* this switch is not properly mapped, set attitude stabilized */ - if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { + /* no valid stick position, go to default */ - current_status.flag_control_attitude_enabled = true; - current_status.flag_control_rates_enabled = true; - } else { - /* assuming a fixed wing, fall back to direct pass-through */ - current_status.flag_control_attitude_enabled = false; - current_status.flag_control_rates_enabled = false; - } + } else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) { - } else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) { + /* bottom stick position, go to manual mode */ + current_status.mode_switch = MODE_SWITCH_MANUAL; - /* this switch is not properly mapped, set attitude stabilized */ - if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { + } else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) { - /* assuming a rotary wing, fall back to m */ - current_status.flag_control_attitude_enabled = true; - current_status.flag_control_rates_enabled = true; - } else { + /* top stick position, set auto/mission for all vehicle types */ + current_status.mode_switch = MODE_SWITCH_AUTO; - /* assuming a fixed wing, set to direct pass-through as requested */ - current_status.flag_control_attitude_enabled = false; - current_status.flag_control_rates_enabled = false; - } - - } else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) { - - /* top stick position, set auto/mission for all vehicle types */ - current_status.flag_control_position_enabled = true; - current_status.flag_control_velocity_enabled = true; - current_status.flag_control_attitude_enabled = true; - current_status.flag_control_rates_enabled = true; - - } else { - /* center stick position, set seatbelt/simple control */ - current_status.flag_control_velocity_enabled = true; - current_status.flag_control_attitude_enabled = true; - current_status.flag_control_rates_enabled = true; - } + } else { + /* center stick position, set seatbelt/simple control */ + current_status.mode_switch = MODE_SWITCH_SEATBELT; + } // warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode); /* - * Check if land/RTL is requested - */ + * Check if land/RTL is requested + */ if (!isfinite(sp_man.return_switch)) { /* this switch is not properly mapped, set default */ - current_status.flag_land_requested = false; // XXX default? + current_status.return_switch = RETURN_SWITCH_NONE; } else if (sp_man.return_switch < -STICK_ON_OFF_LIMIT) { /* bottom stick position, set altitude hold */ - current_status.flag_land_requested = false; + current_status.return_switch = RETURN_SWITCH_NONE; } else if (sp_man.return_switch > STICK_ON_OFF_LIMIT) { /* top stick position */ - current_status.flag_land_requested = true; + current_status.return_switch = RETURN_SWITCH_RETURN; } else { /* center stick position, set default */ - current_status.flag_land_requested = false; // XXX default? + current_status.return_switch = RETURN_SWITCH_NONE; } /* check mission switch */ if (!isfinite(sp_man.mission_switch)) { /* this switch is not properly mapped, set default */ - current_status.flag_mission_activated = false; + current_status.mission_switch = MISSION_SWITCH_NONE; } else if (sp_man.mission_switch > STICK_ON_OFF_LIMIT) { /* top switch position */ - current_status.flag_mission_activated = true; + current_status.mission_switch = MISSION_SWITCH_MISSION; } else if (sp_man.mission_switch < -STICK_ON_OFF_LIMIT) { /* bottom switch position */ - current_status.flag_mission_activated = false; + current_status.mission_switch = MISSION_SWITCH_NONE; } else { /* center switch position, set default */ - current_status.flag_mission_activated = false; // XXX default? + current_status.mission_switch = MISSION_SWITCH_NONE; // XXX default? + } + + /* Now it's time to handle the stick inputs */ + + if (current_status.arming_state == ARMING_STATE_ARMED) { + + if (current_status.mode_switch == MODE_SWITCH_MANUAL) { + do_navigation_state_update(stat_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_MANUAL ); + } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT) { + do_navigation_state_update(stat_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_SEATBELT ); + } else if (current_status.mode_switch == MODE_SWITCH_AUTO) { + if (current_status.navigation_state == NAVIGATION_STATE_MANUAL) { + do_navigation_state_update(stat_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_MISSION ); + } + } } /* handle the case where RC signal was regained */ @@ -1890,17 +1877,19 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME."); } else { - if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); + if (current_status.rc_signal_lost) { + mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); + } } /* - * Check if left stick is in lower left position --> switch to standby state. - * Do this only for multirotors, not for fixed wing aircraft. - */ -// if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || -// (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || -// (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) -// ) && + * Check if left stick is in lower left position --> switch to standby state. + * Do this only for multirotors, not for fixed wing aircraft. + */ + // if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + // (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + // (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) + // ) && if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { do_arming_state_update(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); @@ -1969,8 +1958,8 @@ int commander_thread_main(int argc, char *argv[]) /* decide about attitude control flag, enable in att/pos/vel */ bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE || - sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || - sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION); + sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || + sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION); /* decide about rate control flag, enable it always XXX (for now) */ bool rates_ctrl_enabled = true; diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index aae119d35a..2e49354714 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -54,421 +54,329 @@ /** - * Transition from one navigation state to another + * Transition from one sytem state to another */ -int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state) +void state_machine_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { - bool valid_path = false; - bool valid_transition = false; + int ret = ERROR; + system_state_t new_system_state; + + + /* + * Firstly evaluate mode switch position + * */ + + /* Always accept manual mode */ + if (current_status->mode_switch == MODE_SWITCH_MANUAL) { + new_system_state = SYSTEM_STATE_MANUAL; + + /* Accept SEATBELT if there is a local position estimate */ + } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT) { + + if (current_status->flag_local_position_valid) { + new_system_state = SYSTEM_STATE_SEATBELT; + } else { + /* or just fall back to manual */ + mavlink_log_critical(mavlink_fd, "REJ SEATBELT: no local position"); + new_system_state = SYSTEM_STATE_MANUAL); + } + + /* Accept AUTO if there is a global position estimate */ + } else if (current_status->mode_switch == MODE_SWITCH_AUTO) { + if (current_status->flag_local_position_valid) { + new_system_state = SYSTEM_STATE_SEATBELT); + } else { + mavlink_log_critical(mavlink_fd, "REJ AUTO: no global position"); + + /* otherwise fallback to seatbelt or even manual */ + if (current_status->flag_local_position_valid) { + new_system_state = SYSTEM_STATE_SEATBELT; + } else { + mavlink_log_critical(mavlink_fd, "REJ SEATBELT: no local position"); + new_system_state = SYSTEM_STATE_MANUAL; + } + } + } + + /* + * Next up, check for + */ + + + + /* Update the system state in case it has changed */ + if (current_status->system_state != new_system_state) { + + /* Check if the transition is valid */ + if (system_state_update(current_status->system_state, new_system_state) == OK) { + current_status->system_state = new_system_state; + state_machine_publish(status_pub, current_status, mavlink_fd); + } else { + mavlink_log_critical(mavlink_fd, "System state transition not valid"); + } + } + + return; +} + +/* + * This functions does not evaluate any input flags but only checks if the transitions + * are valid. + */ +int system_state_update(system_state_t current_system_state, system_state_t new_system_state) { + int ret = ERROR; - if (current_status->navigation_state == new_state) { - warnx("Navigation state not changed"); + /* only check transition if the new state is actually different from the current one */ + if (new_system_state != current_system_state) { - } else { + switch (new_system_state) { + case SYSTEM_STATE_INIT: - switch (new_state) { - case NAVIGATION_STATE_STANDBY: + /* transitions back to INIT are possible for calibration */ + if (current_system_state == SYSTEM_STATE_MANUAL + || current_system_state == SYSTEM_STATE_SEATBELT + || current_system_state == SYSTEM_STATE_AUTO) { - if (current_status->navigation_state == NAVIGATION_STATE_MANUAL - || current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) { + ret = OK; + } - if (!current_status->flag_system_armed) { - mavlink_log_critical(mavlink_fd, "Switched to STANDBY state"); - valid_transition = true; - } else { - mavlink_log_critical(mavlink_fd, "Rejected STANDBY state: armed"); - } - valid_path = true; + break; + + case SYSTEM_STATE_MANUAL: + + /* transitions from INIT or from other modes */ + if (current_system_state == SYSTEM_STATE_INIT + || current_system_state == SYSTEM_STATE_SEATBELT + || current_system_state == SYSTEM_STATE_AUTO) { + + ret = OK; + } + + break; + + case SYSTEM_STATE_SEATBELT: + + /* transitions from INIT or from other modes */ + if (current_system_state == SYSTEM_STATE_INIT + || current_system_state == SYSTEM_STATE_MANUAL + || current_system_state == SYSTEM_STATE_AUTO) { + + ret = OK; + } + + break; + + case SYSTEM_STATE_AUTO: + + /* transitions from INIT or from other modes */ + if (current_system_state == SYSTEM_STATE_INIT + || current_system_state == SYSTEM_STATE_MANUAL + || current_system_state == SYSTEM_STATE_SEATBELT) { + + ret = OK; } break; - case NAVIGATION_STATE_MANUAL: + case SYSTEM_STATE_REBOOT: - if (current_status->navigation_state == NAVIGATION_STATE_STANDBY) { + /* from INIT you can go straight to REBOOT */ + if (current_system_state == SYSTEM_STATE_INIT) { - /* only check for armed flag when coming from STANDBY XXX does that make sense? */ - if (current_status->flag_system_armed) { - mavlink_log_critical(mavlink_fd, "Switched to MANUAL state"); - valid_transition = true; - } else { - mavlink_log_critical(mavlink_fd, "Rejected MANUAL state: disarmed"); - } - valid_path = true; - } else if (current_status->navigation_state == NAVIGATION_STATE_SEATBELT - || current_status->navigation_state == NAVIGATION_STATE_LOITER - || current_status->navigation_state == NAVIGATION_STATE_MISSION - || current_status->navigation_state == NAVIGATION_STATE_RTL - || current_status->navigation_state == NAVIGATION_STATE_LAND - || current_status->navigation_state == NAVIGATION_STATE_TAKEOFF - || current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) { - mavlink_log_critical(mavlink_fd, "Switched to MANUAL state"); - valid_transition = true; - valid_path = true; + ret = OK; } break; - case NAVIGATION_STATE_SEATBELT: + case SYSTEM_STATE_IN_AIR_RESTORE: - if (current_status->navigation_state == NAVIGATION_STATE_MANUAL - || current_status->navigation_state == NAVIGATION_STATE_LOITER - || current_status->navigation_state == NAVIGATION_STATE_MISSION) { + /* from INIT you can go straight to an IN AIR RESTORE */ + if (current_system_state == SYSTEM_STATE_INIT) { - mavlink_log_critical(mavlink_fd, "Switched to SEATBELT state"); - valid_transition = true; - valid_path = true; - } - break; - case NAVIGATION_STATE_DESCENT: - - if (current_status->navigation_state == NAVIGATION_STATE_MANUAL - || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) { - - mavlink_log_critical(mavlink_fd, "Switched to DESCENT state"); - valid_transition = true; - valid_path = true; - } - break; - - case NAVIGATION_STATE_LOITER: - - /* Check for position lock when coming from MANUAL or SEATBELT */ - if (current_status->navigation_state == NAVIGATION_STATE_MANUAL || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) { - mavlink_log_critical(mavlink_fd, "Switched to LOITER state"); - valid_transition = true; - if (current_status->flag_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Switched to LOITER state"); - valid_transition = true; - - } else { - mavlink_log_critical(mavlink_fd, "Rejected LOITER state: no pos lock"); - } - valid_path = true; - } else if (current_status->navigation_state == NAVIGATION_STATE_MISSION) { - mavlink_log_critical(mavlink_fd, "Switched to LOITER state"); - valid_transition = true; - valid_path = true; - } - break; - - case NAVIGATION_STATE_AUTO_READY: - - /* coming from STANDBY pos and home lock are needed */ - if (current_status->navigation_state == NAVIGATION_STATE_STANDBY) { - - if (!current_status->flag_system_armed) { - mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: disarmed"); - - } else if (!current_status->flag_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no pos lock"); - - } else if (!current_status->flag_valid_launch_position) { - mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no home lock"); - - } else { - mavlink_log_critical(mavlink_fd, "Switched to AUTO READY state"); - valid_transition = true; - } - valid_path = true; - /* coming from LAND home lock is needed */ - } else if (current_status->navigation_state == NAVIGATION_STATE_LAND) { - - if (!current_status->flag_valid_launch_position) { - mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no home lock"); - } else { - mavlink_log_critical(mavlink_fd, "Switched to AUTO READY state"); - valid_transition = true; - } - valid_path = true; - } - break; - - case NAVIGATION_STATE_MISSION: - - /* coming from TAKEOFF or RTL is always possible */ - if (current_status->navigation_state == NAVIGATION_STATE_TAKEOFF - || current_status->navigation_state == NAVIGATION_STATE_RTL) { - mavlink_log_critical(mavlink_fd, "Switched to MISSION state"); - valid_transition = true; - valid_path = true; - - /* coming from MANUAL or SEATBELT requires home and pos lock */ - } else if (current_status->navigation_state == NAVIGATION_STATE_MANUAL - || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) { - - if (!current_status->flag_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no pos lock"); - - } else if (!current_status->flag_valid_launch_position) { - mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no home lock"); - mavlink_log_critical(mavlink_fd, "Switched to MISSION state"); - valid_transition = true; - } else { - - } - valid_path = true; - - /* coming from loiter a home lock is needed */ - } else if (current_status->navigation_state == NAVIGATION_STATE_LOITER) { - if (current_status->flag_valid_launch_position) { - mavlink_log_critical(mavlink_fd, "Switched to MISSION state"); - valid_transition = true; - } else { - mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no home lock"); - } - valid_path = true; - } - break; - - case NAVIGATION_STATE_RTL: - - /* coming from MISSION is always possible */ - if (current_status->navigation_state == NAVIGATION_STATE_MISSION) { - mavlink_log_critical(mavlink_fd, "Switched to RTL state"); - valid_transition = true; - valid_path = true; - - /* coming from MANUAL or SEATBELT requires home and pos lock */ - } else if (current_status->navigation_state == NAVIGATION_STATE_MANUAL - || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) { - - if (!current_status->flag_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rejected RTL state: no pos lock"); - } else if (!current_status->flag_valid_launch_position) { - mavlink_log_critical(mavlink_fd, "Rejected RTL state: no home lock"); - } else { - mavlink_log_critical(mavlink_fd, "Switched to RTL state"); - valid_transition = true; - } - valid_path = true; - - /* coming from loiter a home lock is needed */ - } else if (current_status->navigation_state == NAVIGATION_STATE_LOITER) { - if (current_status->flag_valid_launch_position) { - mavlink_log_critical(mavlink_fd, "Switched to RTL state"); - valid_transition = true; - } else { - mavlink_log_critical(mavlink_fd, "Rejected RTL state: no home lock"); - } - valid_path = true; - } - break; - - case NAVIGATION_STATE_TAKEOFF: - - if (current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) { - - /* TAKEOFF is straight forward from AUTO READY */ - mavlink_log_critical(mavlink_fd, "Switched to TAKEOFF state"); - valid_transition = true; - valid_path = true; - } - break; - - case NAVIGATION_STATE_LAND: - if (current_status->navigation_state == NAVIGATION_STATE_RTL - || current_status->navigation_state == NAVIGATION_STATE_LOITER - || current_status->navigation_state == NAVIGATION_STATE_MISSION) { - - mavlink_log_critical(mavlink_fd, "Switched to LAND state"); - valid_transition = true; - valid_path = true; + ret = OK; } break; default: - warnx("Unknown navigation state"); break; } } - if (valid_transition) { - current_status->navigation_state = new_state; - state_machine_publish(status_pub, current_status, mavlink_fd); -// publish_armed_status(current_status); - ret = OK; - } - - if (!valid_path){ - mavlink_log_critical(mavlink_fd, "REJECTING invalid navigation state transition"); - } - return ret; } -/** - * Transition from one arming state to another - */ -int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state) -{ - bool valid_transition = false; - int ret = ERROR; +///** +// * Transition from one arming state to another +// */ +//int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state) +//{ +// bool valid_transition = false; +// int ret = ERROR; +// +// warnx("Current state: %d, requested state: %d", current_status->arming_state, new_state); +// +// if (current_status->arming_state == new_state) { +// warnx("Arming state not changed"); +// valid_transition = true; +// +// } else { +// +// switch (new_state) { +// +// case ARMING_STATE_INIT: +// +// if (current_status->arming_state == ARMING_STATE_STANDBY) { +// +// mavlink_log_critical(mavlink_fd, "Switched to INIT ARMING STATE"); +// valid_transition = true; +// } +// break; +// +// case ARMING_STATE_STANDBY: +// +// // TODO check for sensors +// // XXX check if coming from reboot? +// if (current_status->arming_state == ARMING_STATE_INIT) { +// +// if (current_status->flag_system_sensors_initialized) { +// current_status->flag_system_armed = false; +// mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state"); +// valid_transition = true; +// } else { +// mavlink_log_critical(mavlink_fd, "REJ. STANDBY arming state, sensors not init."); +// } +// +// } else if (current_status->arming_state == ARMING_STATE_ARMED) { +// +// current_status->flag_system_armed = false; +// mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state"); +// valid_transition = true; +// } +// break; +// +// case ARMING_STATE_ARMED: +// +// if (current_status->arming_state == ARMING_STATE_STANDBY +// || current_status->arming_state == ARMING_STATE_IN_AIR_RESTORE) { +// current_status->flag_system_armed = true; +// mavlink_log_critical(mavlink_fd, "Switched to ARMED arming state"); +// valid_transition = true; +// } +// break; +// +// case ARMING_STATE_ABORT: +// +// if (current_status->arming_state == ARMING_STATE_ARMED) { +// +// mavlink_log_critical(mavlink_fd, "Switched to MISSION ABORT arming state"); +// valid_transition = true; +// } +// break; +// +// case ARMING_STATE_ERROR: +// +// if (current_status->arming_state == ARMING_STATE_ARMED +// || current_status->arming_state == ARMING_STATE_ABORT +// || current_status->arming_state == ARMING_STATE_INIT) { +// +// mavlink_log_critical(mavlink_fd, "Switched to ERROR arming state"); +// valid_transition = true; +// } +// break; +// +// case ARMING_STATE_REBOOT: +// +// if (current_status->arming_state == ARMING_STATE_STANDBY +// || current_status->arming_state == ARMING_STATE_ERROR) { +// +// valid_transition = true; +// mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); +// usleep(500000); +// up_systemreset(); +// /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ +// } +// break; +// +// case ARMING_STATE_IN_AIR_RESTORE: +// +// if (current_status->arming_state == ARMING_STATE_INIT) { +// mavlink_log_critical(mavlink_fd, "Switched to IN-AIR-RESTORE arming state"); +// valid_transition = true; +// } +// break; +// default: +// warnx("Unknown arming state"); +// break; +// } +// } +// +// if (valid_transition) { +// current_status->arming_state = new_state; +// state_machine_publish(status_pub, current_status, mavlink_fd); +//// publish_armed_status(current_status); +// ret = OK; +// } else { +// mavlink_log_critical(mavlink_fd, "REJECTING invalid arming state transition"); +// } +// +// return ret; +//} - warnx("Current state: %d, requested state: %d", current_status->arming_state, new_state); - - if (current_status->arming_state == new_state) { - warnx("Arming state not changed"); - valid_transition = true; - - } else { - - switch (new_state) { - - case ARMING_STATE_INIT: - - if (current_status->arming_state == ARMING_STATE_STANDBY) { - - mavlink_log_critical(mavlink_fd, "Switched to INIT ARMING STATE"); - valid_transition = true; - } - break; - - case ARMING_STATE_STANDBY: - - // TODO check for sensors - // XXX check if coming from reboot? - if (current_status->arming_state == ARMING_STATE_INIT) { - - if (current_status->flag_system_sensors_initialized) { - current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state"); - valid_transition = true; - } else { - mavlink_log_critical(mavlink_fd, "REJ. STANDBY arming state, sensors not init."); - } - - } else if (current_status->arming_state == ARMING_STATE_ARMED) { - - current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state"); - valid_transition = true; - } - break; - - case ARMING_STATE_ARMED: - - if (current_status->arming_state == ARMING_STATE_STANDBY - || current_status->arming_state == ARMING_STATE_IN_AIR_RESTORE) { - current_status->flag_system_armed = true; - mavlink_log_critical(mavlink_fd, "Switched to ARMED arming state"); - valid_transition = true; - } - break; - - case ARMING_STATE_ABORT: - - if (current_status->arming_state == ARMING_STATE_ARMED) { - - mavlink_log_critical(mavlink_fd, "Switched to MISSION ABORT arming state"); - valid_transition = true; - } - break; - - case ARMING_STATE_ERROR: - - if (current_status->arming_state == ARMING_STATE_ARMED - || current_status->arming_state == ARMING_STATE_ABORT - || current_status->arming_state == ARMING_STATE_INIT) { - - mavlink_log_critical(mavlink_fd, "Switched to ERROR arming state"); - valid_transition = true; - } - break; - - case ARMING_STATE_REBOOT: - - if (current_status->arming_state == ARMING_STATE_STANDBY - || current_status->arming_state == ARMING_STATE_ERROR) { - - valid_transition = true; - mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); - usleep(500000); - up_systemreset(); - /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ - } - break; - - case ARMING_STATE_IN_AIR_RESTORE: - - if (current_status->arming_state == ARMING_STATE_INIT) { - mavlink_log_critical(mavlink_fd, "Switched to IN-AIR-RESTORE arming state"); - valid_transition = true; - } - break; - default: - warnx("Unknown arming state"); - break; - } - } - - if (valid_transition) { - current_status->arming_state = new_state; - state_machine_publish(status_pub, current_status, mavlink_fd); -// publish_armed_status(current_status); - ret = OK; - } else { - mavlink_log_critical(mavlink_fd, "REJECTING invalid arming state transition"); - } - - return ret; -} - -/** - * Transition from one hil state to another - */ -int do_hil_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state) -{ - bool valid_transition = false; - int ret = ERROR; - - warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state); - - if (current_status->hil_state == new_state) { - warnx("Hil state not changed"); - valid_transition = true; - - } else { - - switch (new_state) { - - case HIL_STATE_OFF: - - if (current_status->arming_state == ARMING_STATE_INIT - || current_status->arming_state == ARMING_STATE_STANDBY) { - - current_status->flag_hil_enabled = false; - mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); - valid_transition = true; - } - break; - - case HIL_STATE_ON: - - if (current_status->arming_state == ARMING_STATE_INIT - || current_status->arming_state == ARMING_STATE_STANDBY) { - - current_status->flag_hil_enabled = true; - mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); - valid_transition = true; - } - break; - - default: - warnx("Unknown hil state"); - break; - } - } - - if (valid_transition) { - current_status->hil_state = new_state; - state_machine_publish(status_pub, current_status, mavlink_fd); - ret = OK; - } else { - mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); - } - - return ret; -} +///** +// * Transition from one hil state to another +// */ +//int do_hil_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state) +//{ +// bool valid_transition = false; +// int ret = ERROR; +// +// warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state); +// +// if (current_status->hil_state == new_state) { +// warnx("Hil state not changed"); +// valid_transition = true; +// +// } else { +// +// switch (new_state) { +// +// case HIL_STATE_OFF: +// +// if (current_status->arming_state == ARMING_STATE_INIT +// || current_status->arming_state == ARMING_STATE_STANDBY) { +// +// current_status->flag_hil_enabled = false; +// mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); +// valid_transition = true; +// } +// break; +// +// case HIL_STATE_ON: +// +// if (current_status->arming_state == ARMING_STATE_INIT +// || current_status->arming_state == ARMING_STATE_STANDBY) { +// +// current_status->flag_hil_enabled = true; +// mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); +// valid_transition = true; +// } +// break; +// +// default: +// warnx("Unknown hil state"); +// break; +// } +// } +// +// if (valid_transition) { +// current_status->hil_state = new_state; +// state_machine_publish(status_pub, current_status, mavlink_fd); +// ret = OK; +// } else { +// mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); +// } +// +// return ret; +//} diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h index bf9296caf0..57b3db8f19 100644 --- a/apps/commander/state_machine_helper.h +++ b/apps/commander/state_machine_helper.h @@ -47,9 +47,9 @@ #include #include -int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state); +int navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state); -int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state); +//int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state); void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index 27a471f131..20cb25cc02 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -60,27 +60,48 @@ /* State Machine */ typedef enum { - NAVIGATION_STATE_STANDBY=0, - NAVIGATION_STATE_MANUAL, - NAVIGATION_STATE_SEATBELT, - NAVIGATION_STATE_DESCENT, - NAVIGATION_STATE_LOITER, - NAVIGATION_STATE_AUTO_READY, - NAVIGATION_STATE_MISSION, - NAVIGATION_STATE_RTL, - NAVIGATION_STATE_LAND, - NAVIGATION_STATE_TAKEOFF -} navigation_state_t; + SYSTEM_STATE_INIT=0, + SYSTEM_STATE_MANUAL, + SYSTEM_STATE_SEATBELT, + SYSTEM_STATE_AUTO, + SYSTEM_STATE_REBOOT, + SYSTEM_STATE_IN_AIR_RESTORE +} system_state_t; typedef enum { - ARMING_STATE_INIT = 0, - ARMING_STATE_STANDBY, - ARMING_STATE_ARMED, - ARMING_STATE_ABORT, - ARMING_STATE_ERROR, - ARMING_STATE_REBOOT, - ARMING_STATE_IN_AIR_RESTORE -} arming_state_t; + MANUAL_STANDBY = 0, + MANUAL_READY, + MANUAL_IN_AIR +} manual_state_t; + +typedef enum { + SEATBELT_STANDBY, + SEATBELT_READY, + SEATBELT, + SEATBELT_ASCENT, + SEATBELT_DESCENT, +} seatbelt_state_t; + +typedef enum { + AUTO_STANDBY, + AUTO_READY, + AUTO_LOITER, + AUTO_MISSION, + AUTO_RTL, + AUTO_TAKEOFF, + AUTO_LAND, +} auto_state_t; + +//typedef enum { +// ARMING_STATE_INIT = 0, +// ARMING_STATE_STANDBY, +// ARMING_STATE_ARMED_GROUND, +// ARMING_STATE_ARMED_AIRBORNE, +// ARMING_STATE_ERROR_GROUND, +// ARMING_STATE_ERROR_AIRBORNE, +// ARMING_STATE_REBOOT, +// ARMING_STATE_IN_AIR_RESTORE +//} arming_state_t; typedef enum { HIL_STATE_OFF = 0, @@ -100,7 +121,7 @@ enum VEHICLE_MODE_FLAG { typedef enum { MODE_SWITCH_MANUAL = 0, - MODE_SWITCH_ASSISTED, + MODE_SWITCH_SEATBELT, MODE_SWITCH_AUTO } mode_switch_pos_t; @@ -114,26 +135,6 @@ typedef enum { MISSION_SWITCH_MISSION } mission_switch_pos_t; -//enum VEHICLE_FLIGHT_MODE { -// VEHICLE_FLIGHT_MODE_MANUAL = 0, /**< direct manual control, exact mode determined by VEHICLE_MANUAL_CONTROL_MODE */ -// VEHICLE_FLIGHT_MODE_STAB, /**< attitude or rate stabilization plus velocity or position stabilization */ -// VEHICLE_FLIGHT_MODE_HOLD, /**< hold current position (hover or loiter around position when switched) */ -// VEHICLE_FLIGHT_MODE_AUTO /**< attitude or rate stabilization plus absolute position control and waypoints */ -//}; -// -//enum VEHICLE_MANUAL_CONTROL_MODE { -// VEHICLE_MANUAL_CONTROL_MODE_DIRECT = 0, /**< no attitude control, direct stick input mixing (only fixed wing) */ -// VEHICLE_MANUAL_CONTROL_MODE_RATES, /**< body rates control mode */ -// VEHICLE_MANUAL_CONTROL_MODE_SAS /**< stability augmented system (SAS) mode */ -//}; -// -//enum VEHICLE_MANUAL_SAS_MODE { -// VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS = 0, /**< roll, pitch and yaw absolute */ -// VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE, /**< roll and pitch absolute, yaw rate */ -// VEHICLE_MANUAL_SAS_MODE_SIMPLE, /**< simple mode (includes altitude hold) */ -// VEHICLE_MANUAL_SAS_MODE_ALTITUDE /**< altitude hold */ -//}; - /** * Should match 1:1 MAVLink's MAV_TYPE ENUM */ @@ -180,8 +181,8 @@ struct vehicle_status_s uint64_t failsave_lowlevel_start_time; /**< time when the lowlevel failsafe flag was set */ // uint64_t failsave_highlevel_begin; TO BE COMPLETED - navigation_state_t navigation_state; /**< current navigation state */ - arming_state_t arming_state; /**< current arming state */ + system_state_t system_state; /**< current system state */ +// arming_state_t arming_state; /**< current arming state */ hil_state_t hil_state; /**< current hil state */ int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */ @@ -194,6 +195,7 @@ struct vehicle_status_s return_switch_pos_t return_switch; mission_switch_pos_t mission_switch; + bool flag_system_armed; /**< true is motors / actuators are armed */ bool flag_system_emergency; bool flag_system_in_air_restore; /**< true if we can restore in mid air */ @@ -212,9 +214,6 @@ struct vehicle_status_s bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ bool flag_control_position_enabled; /**< true if position is controlled */ - bool flag_land_requested; /**< true if land requested */ - bool flag_mission_activated; /**< true if in mission mode */ - bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */ bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */ bool flag_preflight_accel_calibration; From ebe0285ce7964ac1a81a65bae417e978cf366466 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 21 Feb 2013 13:06:56 -0800 Subject: [PATCH 008/486] Checkpoint: navigation state machine as discussed with Lorenz --- Documentation/flight_mode_state_machine.odg | Bin 24059 -> 23463 bytes apps/commander/commander.c | 16 +- apps/commander/state_machine_helper.c | 397 +++++++++++++++----- apps/commander/state_machine_helper.h | 3 +- apps/controllib/fixedwing.cpp | 8 +- apps/mavlink/mavlink.c | 55 ++- apps/uORB/topics/vehicle_status.h | 86 ++--- 7 files changed, 393 insertions(+), 172 deletions(-) diff --git a/Documentation/flight_mode_state_machine.odg b/Documentation/flight_mode_state_machine.odg index a50366bcb3e51c2c6571597de89d7b00d746b28d..cbdad91c9c0e539eb5b02b700e4b9d8eab5efd50 100644 GIT binary patch delta 18537 zcmb4qRZt&K5athe*Wm6D+=IJ&aCdiy#U;2q1b6q~?u6hT+})ka|Lz{HuIe6cUwUVD zre;(V7Je)PB*>tKkB#}YY zt=rtLF5!2#vzl3Nv$PRawV$XzsB)n#o-X-jXD?J2JI5Y=`xwO8b!4x8x13(#@0j^| zQBYcq`Ce_s|B!MTY+lvDiFwEqTyt7&w`u{_$o8bo!?PG6wjClxoT%E z6d2Di=p2tMsToD6-73kRA>IsA_SXBbHp$B-VnL>0bp}=9QX-H>sS=npiPK7>^|b}b zI%;gmX?pM&bDqTKKOg+eyM42X?a52*3DRXfHdu9<`{{ZM4!}gT?oV$n6L;~y(u0>A?6Q3j^!ujqEiK1_IeZ zf%PKh&D8sIu*}}nVeYj!PrF+6Jgsbq?7;|#RN%jp0ug`-PAPL zO);`^TW9n@+lrr&ELZmp2M1^7GA#sNCdoZbof#@lbB~9&&)08zxI^=`9C%2Ae!p_@ z5aRKEl&NhQhh*7sT0!h#@G|~r$nI(2U;8kd0e`vj@Ma|7#c?gv@$~^<8htx6N%5O4aJMjCYox*KX zP`792GE0^ySW#r?1D!bbk+dl@-CNBu7V~Ee*Ofnw6l>1vp#&vNh1!;KQK|9~KdObg z?RG_fKJLXQd4_~hqUc8!QM?4mZRbYN?#~iNKA))~w`0OE&+R^ag`KaS37*!1E<;Jc zH-lzK@CX6I5lzFV#`^PL@s<_!J`n#2JV$bJ3x9Hlj`99D?pGaw33M)qTWJ0p8R@b6B}>c331&&Z~^` zKsmi_B2k!~SG4@wH}>1T)Zx)#Brqhoj=5cM zsS7_@qE-U)XQe|U?3ScWYKnKO@`;tsi(*B$GT9@f&7ZBcP(O(6ky68(ud25PyEQJz z(9i+r;#ze()lb2#=Y}-Vo`r=|fxZdS%am23)#R%38GxSx4Yojud#?>T*6KNBz!?-< z`x!)JuyOP!$!g5T(!Q;H+4<7dtRX_eLrLp168GCp^vlnP$uGfKU4fk=mF}VskZ={A zHdZ|G5$tY2(a3zHNsDJHNlheK_*cf7vRU9qZ1h)t!F!iq#DX8mmSFz+Xiv^5LvfU` z=bCPmldx8M|>pJ3>`)@>^$EPgFGlGedDi$C~M%)bLB24zejvUTyHw(gsYLd+Svg# z8pi5d!L%~PUGhqye^l<|CAvmsr|?2v$d%QZzTRp5vFz zs~O(x)-gez$Lw;g{*q5_NwPj?YPJ-|%eR(ydb^@aA}1 z+w<;X@8JJ({rBC<59mps`nyqWp@?$3z?4`0o$rY&xbpj&$NmutmtxjJ$GvOEa<`5= z@xhu$$4bZju5WYCiJs>J->2R_-`d8K+2O?d*!BX@@tmf&6m_lZe17Nm=;8&@%7Lp0XYw1 zj9VKAAW-=8Ff9+ox!LRJMbDEd(7Hv7-~A(Fe`Wjr^!``3dgo8y<@fWOg80_`)2*}L z)uq5DgZr(#<4e{3-HIzuC7OZYpiuDbGGjK7@LMo{bO{9wyk1_n*I{D&W$#??rttG+ z;&;!qz%+45coI&j;xta>x&1F*l14vG2EO0m*;%No$JT$eFt64NB`t#c<{8j zr)qM4zFtIud=Y9{UfO&#OVn{=FBISF+B>3h;akb=g>wQ~#{?M<@;)!Zsua$Hj`4zk zWA*oP**EEJ?HO+{di(2a?VNxzBJ7%}aBsUV1HtNxaW5LMY``nq0h^m$?`2{$L6L-h z+_-zaGUMgdgc!mjQ_xoUWb0U4up6PK1Y!PQTedZBfZE#6=9?(JR_)zZrVE8r>Qs}@**|fwY3)@=6KFNCXsLAovz{iT5;cc=hJ+jW%CY&%Omcr z#Cz8tUUBNXXQgi1?Z|QX^OW7oEJ{|Pte|a495GJg`&!%_Ql`X*xW0bK@4F3G7mvoz z^^?RmH0DeP_o!#iU@Vw!`^FEGTtI(4zvL@bX!f`D5c0-*XQb2PzgQnVS%{Ya-eTw! z@hy@sX%$DfP5ZFT0LIpK6!PD%saF(sOA|wSR|@ZXE-{~#5{|}OkMg(w4tdAHCi~v^ zm#%NX*mG;?xxLe8v)2ywn_uIA6$*l<n*<%9bpE(v_P|F*xs&$-^3^Lh0A^>z94 z%!&7ERA2Fzu|jM3@!@eL{V6aafhg8E&iX=mUy^Fd*MjbE`--jFGyZFi{OfzyqKNO~ z+(mbz6^=3ITp{1ok4xLlKUKtsegnB^0(=t`d~4q5zqxX!pErI-5Py43qCpe}0L^!{ zQ;&XP_y@4f!4Dq`mvu<~U7xtrQQ4l>&f&N}bC=t+(~o~|h)d&6&w-EIUEh>o&) zgZ_E>+qqOT5^~gcpEiHKS*75Cf=7$N!z?D8wecS$o^O-({TQjYinnHFww-`B(_u#y z!QkWet;2ZQzEUh)nBe+b&*v*({T74*r*yZp8LL_1BBa*i1!*v7zP_H{nwJ4WM z6W>6JI*5%`7rp8C78(yz_P4H(6>6V-yR#3nzk34>0tOzOc!N=L{?>BceuG^Kk3T{p z<=p-r8GnXG>XQOnzKObn;gte=yorkbJu>bXC69h#u7xXPFXhB)iCko7BZ4y65Qn&li>s+vZ7)gJlu1^2=#S__K0 zoExsJ2QvRE=A|CZJYkqC>a@lAzB;{d3x1H~Dd>k^N=09PeNEWEqsLb66Fe=|Ymd{C zhp7Y)dRi1npQN(?#BL+MxPm_jPPs9FTimsCi@IHqtm0{hNV5Xong7~#zueY)?)21b zv;C+V<(?D2Z~aw?GEwUF(WYPJu5$;<8J^QLih4g@K+rMJ>{2G=YX_(yK@jOzUWo{w zP7=5;)g85Qps9D~_mwn_ea!=9q=lZHKTFc@iR#2RF848eyC->SId?q`pWC75mgBtB zWQx#%)>RogFHV@06K{d zl^FGllIXx<_3NX-H3-?1_nOwo-~M|1oZHq zV4Vc*&ctVn2>fv!DTy_iw^Yq)|KV0t$9Z_1fH^M=4rz@_d>pXWuM0b~P%xS?L7;l2 z2x;zW>WTCIN0p91zT5PhsR8Rst6yt&&4tA71$;K#;%Mg?>HVxc{0?RZk?#)he153B zUL(AIrXa*~6&_iiEi6kLs| z->Vl?X*cf6YNGVTGKZFW0)nY2hDzEx*hFtOTVD{mfI%9&M8S|jEc8nfJD{`BWJg@m zHRxIM<&+r%@4|_=Ffmd3ZiWR^a5@lT5F5;dk&QCOQi{;3_*AG9F*N8X4rU?Ujw9V! zxYBnoGlLEF$1ymgk8x0!2Km%>I%lTcT%4(hv%PVM^KkNqE>@`EXj_auy>V~66L~m< zpx$t2FOI2!vLc1seC6FU4e)Zjer74gi4_D7(SUsMwJ{X@s+}FSd#FhxALP4z+fw-Y zgZY4*S~Ss>ZWLBP-Z(|Aw)fc%?_-kRGj$x7>Y4c8>bJ)$8JxTp*)~egLmwo$P`}&z z>Y32Wa^J$=PZm8hI1k0D?$RkRv5^3*nKZw7$1Vc~8~}eK;?f2MsDiZ%86T#{`{VCk zc35$f$gbYxBkx<{=dsC)liU|@9z*I^HJH9Xe(1P|Ji&&0q)lI07y%x@0-eN1p@&~z z9u;Y?>1=3u1-*0JRmHw_JvEHX!M4LE56U_9MANhX2$!v zaC`jQNOn!dizUwd-VA2#S4zh3>+SP9B{Svt!#nPnOgk9%SnBN<)A)S|#tVO3?=Yx_ zGoV@t(Yd+bN?8ZqE2zDU9P1%O6@I;w((KLm!${=J&-%Z5mHjc7hxz3^D?%U4g~Rhe zK+bD#=}H5+4_L|vw8X1NvZdo{9Hw5cb041yxAL?JI6@-)?0s>@mgfqfR5WAuf?($7 zMc{LhzmxA(cFJ#}MmkYCMfKup$p$&-!}jW8S|fPgPGT8eSy5e)m>cjR&atpj{7J)D zDGN%K4@KeRK4p5dLimc7t^fyE?nvi7QOXvh(1R@T07sK#h;ZoB$9HA9W$`{X+*8t^ zWF~MD{Gium2tPOJNRsjmx+w=gqv3%z@~4~(4@qd4Cr@@st^yb}Hfb)E?Zt8aghfcE z5?B@QUczHA^^#-j$vHFdQq@$RUmx*9=g7*dge`vC^nZnjCa{?%2R7G?SiSf9)eaRr z%M=HVfa5AG88n7c4ng~3#eUPSsC4G*e z=pVIPpP8`t=I@YX4S`>>Y6Rwyiis9IqF#r58^vv6f_ZGkwtJ@&CX0#o@-A3}Z)>kU z^DY?qr=MykK1zX}w?M%GmbK5-9*xf(X~CrtV9}uW;V#1m_VaPh6^54rGw1akrOUv9 zC^uD=!lSF*t|Nfs>(it7L)_ZX$tCbBJh%tYU1!yK8xU*)U>*En*Xq`njI=~@-^pa$6YOb+{RjaBLb zJDiI8qp_x?VpN@|MX&Z+R>?QGYDeyYdHa+w7gu4?_JSrb@y{E%Ufcw0y^S2dd-hPG z7IwDb@J*UJ-R0nC0jOVJTlaS3b$}M zm^@r8UrC+GO|DRjE>g&X(8a<`D~!R&DPM~+t0z5aCiBN*haXkma)cMMLu&8bavp^6 zC*jqvp?8%h3%?66s-u)WXeiQnP*?60h3jQO=TVSY&AxIT-wZyPdsPUqL=;&A4)Uxe z{ET&XN+tri8ZG>Z(_-Z1lj(@V&~-i=CPAWgJcoWV3zZ5cxrA# z7}FJ3)FGy%RTa)alpjuG(8T>7S1lw zuVvFvwJiu#K!q?-;6Gs$hRHMq8&r8!O zBlkPEd}z6-m_d|t=Y?t^He3B07VH|7t@H)bs!FH7*r}@HN`XX& z0iKRk5jVuJC4}I3SU_AkW+O5*CHOP;t)B)axY$;b%YRj)(#HSBC zgNb6iW;nP_6CW4w79WTz$-N>{QN#=TPJBKiDKqWRt$&XhSpqfsro~wCtQ;egw?9Ho zet0j1&oSbKa==~am7SJ!%&-)-;mh`8Z4Hx4#ec+VlUXo_YdtiMEuPD(+#cWNvb#|} z?xyPJL#+HBDaBdRuw{fk`;)qDQogDjgW_CkF%QlTtsEh+`uP0R$=v;1ywyuwIDSQp*F*WzuzgxS<+b4omA43dpa zO3A=RTK@pbcT8fVMbPB7?Rs6g15~W;D?$#!>){G32IO3|-I{*l;T)+A+0)E~LT}_} z{9SF-wLotQ)%YMTDbOBx1eD*zwTP`sAms-S{n#JeD7Z$V-zOw1^GlI@71pN1f`)^W zZ;v2yH@o6g3yW+a(0?;{T(;9iTE&;)u1Bn699sj2v?>MbHmgbbh*45pkaLnJ_al~O*0H;F8a{EV0%=%iSHY~6nCEoED^LhZ`2|er zJnevZ@qB?aC%%*;jF4R(uF<-ws}`k#gcwXO)@rnUutVkGfI{&ANV(eK9aRSjc71(g zv>9K{+R@MsW+p*sjBeG&fGFwUd#&8@Qya!XE3ft66Q>Ac-s3Gd4RWWuDjm9-vKGt_3IqQ zQ{eIOq$IyJeR;zM*MK z_BjTe<$BmupVK&FPn|Pd4p`y}{;0DVTt7#ZiB8rrI7UvDMRdW`iO76aqX(juFF|tS zWQe}hFW<0zIBjyjpenj%H3@=~FAo8bUxpcC_2@>sF52T68Hit7OIE^ThwD9cYL+wn zvXUZSjB3(Dbq2-w{(?3QuUS0;yaMSUCMd3G=thROmIL5NgfThhklF~<=YB9bV_w*A zq4}G_{e?a1Yh|vEK3toJcT|3=xr4bz5(zUoZ&-~?x>f3M3b;6TbcPPwXm$pie-(ge z_d?wpt}^K_lH#(n$*B)V$9SBbW3#hshsUfERi#Bula+q@dr3!}#n&=BJL@Y4ROR)< z3VRq0S`D%O8%XA{!U-xqE;sl$rEp#kjk*+epz{0l7%d)UzPy5hfU2sJ_1Rn}@CO3yb9BtB zILu!#g(@hu^as(ns-(g+{gPX)`O1}Qa<==g?Uy4rmC{#GH?|TpX4Q?@X-Rx zBVK5FDI%hfpb#2CbwO1vndn948XT{xtvHZj!s!qchY?nj6W2Jz@dHC7QVo1zbYvwk zPPOP%c^94{C0T$$V7g|_E}2LCt}nY^`gAEF&V}8}z<()ycKipCr5Tiw>6d5jTJbIY z1ixHt5c?DNKvjsUFObcu2ZqW8XBu9OqtUj*`@~&d?lU-CjwjWMCN`cbWThEf1OL?n ztgEE#h(qg-NZnYNM4eU$PX}C({O0J6-GE}zik#?cSsjNN6~^We>tt$n2H8{w?aR*O z*}r;RCLWoO6p|797puL=&L14b)J}y9`Tqj;AS2>xOi0GxK%lHWwm{DqVg7e77C9Yb${fG353 zmRz&5(}{$eOS$asgZr;6C3Xkt64>+L9rJu-vFu4HCB*(#R1JF|y{r#slS`~*)Zt=_ zojN~Vs&arQ0NAe(Hgi=ov!@fZ5JX~R$YIT?!=YqNED!05pI0JHNT_A`>TD-(A6?3# z@&Tfv(N2pJ(~m_eV`xo@N!nVIaHX1aCW7M5K>Ol-4W0AEH8dnGc9Z_2am?9Vn}QR` zl6^=GyOeT~d%QnVIWZbvm8j^58dGGFS6uvMEH0k?9q9JwVRed?7-(h1o8@~%qEe6_ zBc;WlRu9a1flSG|%KgrkwrDkpfD@K7nOP#mX4Ai{W3t#JEcG$(?Lx(tkGXwu8K^mz ze7CC0^Bvu$L8K&u!|7^D*+v_WXfr7%FY?4|kpE*@*Isr4a@NGxg)e7c0 zw4PLW17Pba;Z7pd7>8VUNYq!IiT%FgI#QW5!9OLBzrrz6{E=ANW#K-RTKe8e(bZRP z(?Ax<8)H2ugR&wzSC8{^X??o1U zaLHC0Dyo#e6ggrjMknhO^WBj8{CcOvvnb68 z9)RrAH_ywRY5p3DCB;^Tv(Yi1L1&tUfv&H5i0(#{C?5-5M<7Oot3WlqMnV`HGy|-Pkt|{w1P@hNAucm zIm@X2%P_)L`|vkmpx2Hcnck_mk(%gA0XXXnKe_=sCR*|0D{->U(KakHaU0CIFw^Hsgrj5#4>|cCc01oV>er$4uQ^VmU<7Ve%Y_c*$=|TaY=@S4j65K z5#}__n|mM`Hfevohs`b!s6Oc{MH-TSO{hn7Pv*d6(uUUQ=`4c~Ym>SqDzak;A#AH_ z)0-G) zIWeEDhFTd}1dmBOB6^!lq%>|C2EuG(+~m7v<{y^6Eq{c3R1yytv)t|=Md(U;`-{UV zsiQ!a@t5r$MS+@Qa)D@ggLWE^h1?%hHRVL;OX;V1ldWAK^W+2NOxf#VcBGT0Lzf!% zM4&I%9Z(O5B?#USMw`+q1zmZixhC9n& z={%t|3A?5o_72^|7M_v{qaWZo_Pgk{qo+#5Va4hsV4xdhf}n1x-eg-)*iwwee0;&k=W z)~8>~4s@E2rYNVIvMUZz>YRGf*Z0EsjyZnT-u#%c ztS)yc%^kJxg}enzUluTSK zrHJ{M)b-WvHkVcV*b-*2e?<}=Lf2Gg5k|0*wlweTfvmwmk%b3Lz&ii@{2a-9ArKM+^PJt3PaxF<1$Q_YB z<49Z{ST1H)=o;sAX#2DLn6@>AKUk&CxZG}(-$tD?OF=pW4;3SWs7VvNJPKOpN zc^oP557dUW0r1P|=n<+5+pCiYE^d-(DQI`jAg&Snv)m@A<82r6ibB{;=CJ7fG)ex~ zW>L(2BDU9*uS#?|&vn{z2{iZVOuRHUFdpe&I_W9?NEF7A*XLWzJm`kAGHs4J!kUPp zJ4kTJ#=g}4lK7nA%u^~AorF6qhOppibZcFVi*9Xd1_(iA`&uwmqRbi3f#vZW(MKw2 zWhIT4_@~9$ZP)`}yYzF9zlIU&Cyb!pMT&6JCSe5Ys4C%njAY+l7%1tq*dQT?y#BlKy!mVr4jmcc4Msv656wEbARCb2=oetZySR!Ts#qRhx8Oo2Pk>r z0o;=oL(hpM)MnL0u+=N!0H4XCJKxEFY#S05Pc2JHzZugLd0Z0~$2gxh9?dhJxqt?v2L3Hhc&=z30(|Usqy{H^>w8wSxCC{L zlDTYcm9YZ5Mpsx`)*m|Rba5h}PTj+3z-1dnjNhEHeQdBiUY#0xJ$TAZs8JGHcX3>k zDA{S29m9`zN{j&9_?10rt9n;#o5`*{SZwPEH0A3Y2-H%eqYqC?V%`BLAD&^F3R;|rbstLlizrk z(KyTgTx@0!&=(`TIpfy>ENo2#0Lp<-z}UkN>tw0;*x@bNQV)&kv#!dH!4$%E!Y2!} zQ@s*-rp~yk9|X{E!jP72qpfaU%?fpyi$kYOZr2sTsB(5pzDvVWhpDnw(*?e=VK(3Nc z4n=GQ$*P+lc7|!Za1g04@FXR>Y$Lc}x0GR;?HNao4j`QzYxF@mNot0R!hqI~JKvmv z94*okubl1*?V4P_ScL8lXJQw4A#hXX zKZ)TcEBBB9S^YYo_UD2}UP?3$IKnKlNCcfyY&(N!Iq_G>Gk;2-hkA5L;t2LzSZ3Wp zBT=35!lDk{Pp;n-mHmRKqRh{~pM;sDNn>NOtEpl_X^TjbcBDu`9gy)hC55uXwC5LB zZcB1SXEYpHS%>EJpt0F@>t=tyvUZr0u|9J^d=v&a(*fa4Sl+Pt9ZrmlKv zQu(RzP2Q068wE34zXeG9NkjzV@1X+=^b?Hj!XvKT4wCo2jDO-7Uv-!YuNSrbe7ISj z0LDnNE;n9sfhab?8Mf_2aJpF5^x?v`z&+6bn{TYVxI`zo2yk zD$_6Rsd9&Mxbvi)@e4+s_h_-Sa>32PVXvB5N0C;nejQB#t%m-&BBa@vM6R{U&Ws3$ zRy0r%?a7?20Q^|@F47r;JFq=qY4|-CODU)U&ID#G)^s9?!QP%KXj=5$bnqabQtP7= z^fd!!^1hb_WmG`Y1@hx5+lzVb-6+-a70Sf$)rBiWZ>-RZ=HH9~4SC?_0#Q*UOc{F}qpE7_zG$&yHn^yh5IrSoOnvS~Z*dS0kc2XF- zFo10!EhenynSGvf?lLctJEDU(wM?{Tg6|VBbU>N~3JXXYh~HR4&|)Gb&8n8iVp^uV za8`djaQ+gTuXBU_u8 zop!lmB;Ab5Kh@m|{xcDbcumqgT|Lm8)YW7UJ6TH6Y%2w%OU;{XVl0VmE7(I}wyyxF zb+02Rnh68&Q%qv=;|Hl<^uBb;?v97wZ6+(!LV(NqesUc-$a`6_~>l*=*ffw?c>}hq^*U z=TN55%dC*sJZBq92AJ-@wSuw$^J0LFE<9++ni&BRc?lu8XwXgq=4io%iFFiCR@%Gg zK%Zb*eH_WXGrh&)-2Zv2*pY6k!30cGCk)ILS6YNn+#({uwB)xqv>z9h4KT+QfV@kw zZzv71SmMBMw#|I}k6kn!$_Cb(ezryitrOXy))4(C4N+yVh2It zJAQI#z}gY}uHT>)jp0%2dT-l+?1X0G=Gq$8vo_PJLoDmEHeRymetP=EideSxIc7Re zNLb&O#O9+fdpuw?D!Z1o&2%?6qK_*5Y2m27Iag9(htow;0AL%Q{5iMl5n#USA?|rQ z5Y&HrXuOzuzp4!d>4K2z3DE{9q6be~ z)069*2UkV=12{xm1vj_H3yMe&f2P77TNXj!JiOKjoLxSr)5Grm_p3SeCC)7mt_XD= zJGY;?J(G?;&_(BTKKwS}H&u_z?IF-%V=-|EWUfD&zvgr(#`$riD2!xf7F90YTm?+3 z9S_724e053XT2%BlFejVOc?5?nEvcq$d^y)ZRhd&Ug_R>f1z>S1dq7!((u0;7R*t79RD>gUEtaQX+{hOl^a3%E{fiHKxL*RDLzrih3n@;3g z**@8Ny59||8iSkbnhnGNwLs($#?_PTU%~~1-uonO-xW_2bZfef}uy z^X(X;(l13)Q-h@{_4r%}=jZjd8s}@dZ@0%8YLx{vDy(0E4|2bjjl?SU<()23&VY#y zXf@fKrkWu!OLdB4Cx?FS3MCcGmow%syLgNyN0fAyEP{qL9kJvttj;lSGWYDpQLW+VK0h_mpYNVqD;*=2OA z!y`Hd>o3rSo=9n-?|FAG`pvpQ4QHBNeOA!4Dz`6n-{)ExX(v?5i-bi1dr@t5jwg%@ z6?ITG!KW_oPq1_2Y|kjohuX5D1EKHxz*jj!TNpK@QQ2k;_z}_;^4$?r&94=#l4;6fQLkwPV=JIb6@zl-e+UVc}Kl*W%SK8ee3;!L6D<-9} z4Wpi12+dANWxE~jBJVWUr%q2~5>##>SRquaGxz|FucHFdCZ>79KaPBaFRto8fndi+ zx-uf1cjB!wC=zzGhj)`w7HFnmaObuQaHwAz=7`edTR!)3Bvz2?>Yu&kAYB~_>2uiM zX4r*azZ8uI(elj&EG_=t8q2gfg<;YL;ZTVf6EJmqhy4Ky8nYe#QWO!@aMS;iQ?p{^ zd2!H$MpEy;Rf7Tb+urd7kDTnuX0Z;r zH5fiN8B^dve#dDg&1U+yfCFUq@F+f_%Edjss4aZnGy`gCRa|WXa|BBSkoV@D;5-~v z?y3Exs~DVE$Id9#6Nhj1q$L@Ft&2bs!BJx{&q#mMkGOxnr40(1v4gaXp??$61mwA~ zKtm;*RUXI~-S%*@Z5W`9wembAguF=RiSH5gXrF{;_*Vr#gZaW$F}{{;Ru>{jwl9Qa zf^2I9z~3q(GyKj^pkB0rV6&G2yY~Psgx<(~sCwF~$|LS}GyQ>iReowEKx_*t`juy) z-bG)a$&d>=LMm`pTuw;;dT-aYM3m4vVcgFL z*6dC1$xX6{Rs$PohjKFwn=3CqT+v1ZwMRbm&1`z-6zyBCC)zBayw2wvJCxz4tO!vy z{V&;SNm{Ujzm5rfoI`~3#R%{n24N+jY=7B{G#7D?ye^*vZJRl+pB}cqFpW>&JB8$4 zU!arRJc$xd~qH|VHjS%1EnnV4>0m~q#V>RLt+x8KBvp$Z9u_H4ehG}8H}u8`@D z&3Kf?=94-zf>2Na?VbR#|3Z%_T|j0Id~lTbH?DB#JRdJbH|}8lc!BcUMpf^$IBdAw zwp(DO{QBU5{jJ0QwbC|`&M=IcvG+~a{N+05^}D}rW4qw(7M1fF4gK}m-Qav-eI~f! zq~c%SJKxVuF%vc!3c0Jn4f@EE)VFm^XK*csy?9{sR1^sQdgi%%^6ao>K&OoVB1jEg zYVdc?{8o4G3ezsi2!z#n0=|D(g0Hr^FDJGDX5)2bT(wQcXRzG zCWCazGJYP-CLVp2ZicA8 zEU^H?v>7nr@aA_iE9HOY-d!o{byWvDFk+9>pvAFKoXyKNk@9^G^&e;=+W0jylbmP* ze^W*YMw=j~_r7hT*{Gepe=Mov?M9jw$DM#QmYN#75H})GhcQb1SVQ_5U)-r4B;Dr9 z^SYUTH*I|5%xGtq5;gRLxFd?ZU9{)&?GgC&4C!~d-4BMyk&IQ)@ZrEh-H8On zM-5rG#=M}iEjJzG8?1!u3g9I+IlpV1LC&t8Q}C20-|SzP>naTS%a&TmaM@#*Sd24H zv92$)lZCx!1j$f>S9E2|_4(~imK6Z_Rjr$AuK5~c)U3=%@f(*YS&{cYAQ zEMIUGl?O3ZjZ6CMkT%!ggDJnOh8#;?;{yMz{w7`<2M={o5ocVFf04%H z1B}f`3Ha6@T+rVlAqI|pA8RDuyVd$~p}5vy!#E9-hf$SxJ-$^BLWv}jZ!m^+A^0c= zS};yXk6ZmBAhpHf`t+v!=6{`wUk&9z+Mp~KjsVX-9#I2bR47lJfGBDW3fZ=6VD1`q zqP;%Q1FTKL_ZL$g1Ru;7rpbistSk{`w2<;BiW6*e&5gkTOGrq!?N$Uez4zT>jejua z@vht11HTcE9O<7>;xShDs!f9F)*9>{hYH_z?IJ)ZDJepA#w^MIv$8 z^fv5(x{uNRUr;1}K>9;sQPefLz&GSqK|8&TU6VrjDcYux$PNl8n|EWf2rvQHz%zyq zRhOr=f5@U!ZR|#(dO@CfaR8*i&jIH@y|74z9m@ zNYZw2YM{>_fpt+2Y3|5^pk|jAYiiiC(=>Ds5n>6+ABq6By!}wDI^K_U(- zfj*^Es$%3qB8HGZC0zliCfFhnB(xa$R;D3>(7GEKY2QBu`u|TMBou)fKr#yp76QqF z>@KMU6+i?Pp<^G8nw;Vx>kgw1dcuMe))P_E*fb+80~dm9l`)B0r^*kzR}q zvp^d#R5ITXxRd@`(3#rjqQXrzZ?AvU-zZy)Zd@%?Uz#zd>(m3 z|Nf7m%ZcUUkp5n~zYUgAJ{wN_Ev%#Ej`KO^LHsAj@6^6dx-lW2O3^I3Ko^&r8jbsx z&SZ6AZ7{kS|CNfIjFd$RY!9<>F&1cP&?e6q0?bgNHiI>FPu@yrxzpaw+iI?uP8RE( zYD*~}n7r?k$VE=;n&7*wvu{BNazN=%J%51OH{%+ed%66>h^(biyo2*AExM&@Q$7!E zt$Kf)+;}2YpvL*MDlP5T3$o}0Rj~Q9&o;+IoKPds#b?p~A2R*{?Ry5b{Snt-_}l5{ zm`ZU2@w9X~peYUwubI9$CqxDvYi68*jyr8-`CU5A@LzJK0HuV1qi{n8b+&M4Cp+Gt zMrSoMleZSHp^o$s(xHYWL_Z>!hHNR+@nrP*L&h4}RsExXzRz7~A)r%j;(u&$fy|=& zPo@5|K6qgoUhem)>7UVt+6lGVT}%G&-0<;?e+|RE)`x4?)UWcDX`c{npPy%tG~d~e)QzGlASZI?>@!7x|_k)JDz!e|1wAYm^uH??%Vb(P4aAG z-2RE}&i6{D3-zCVEPOONxpkSERb^R1C=a2dIE@NjzgYx5t!u3;-+goLU)p}* zeI9|$|5a{me(#wPJ!{LuH90ewbyqy7-@E14#|oc`Tgt7j_^T&P)j5-K;@4frNQNu- z^>Q-&QswS-9hE+I-0h=@#-~XtptgsidZnRi;nViV277jwr+36Yjci`#C@uA5;_1(a z#W!d0iS(8)Jr(!#^yA>2b7pyXWY~R~8ml|M#_Hp>duAY4Xf3;Y!uxXSgdR_2%SwxS zcAnw|nyfW5Cl+gYcwE}3DfzByY3SdSn7Y+>Ys)+8xE)UaD%8)o+Lk*>C)~e0@9tjt z7tfu=&EECSLWQhP3$fPahs;vaGHz2Nr@VJGRw1C^2+T*DJ92 zxm#6Fs&PgAPR_$SGFU1$*NOKsbbe(nXWC$Y)%ob`<+b*e6HRM-)*X0#f?<~3gH3Cu zZ4Ll+hZukNKbjl%D&&r&!M9%8~3G$8pkwnI!10h*<{Q(LBjCp*WJt|;(uG)Z*mmb)W`n3`~UhvRyjew*WZ7+ z2C_3PFkLGA;`c$zhtaB+UT?kY$}PZlDDY})iVY6}W0l*^ykk6UD*cx0z@pbi zz%lms%X}-UL#tBV+g~2nUL+neeV1-shj)K%u-ih`88hce+Wd?)o9$a(dT@Tc?Ek9! zv#Ym%`8FwjX0*-Qsckpv_nrN)k=xyo;k(=&{i$B;>}-89wyQL}{L9N@Uq63)I(xax z1-F%lPgcx5C&6&bHsXg9>px$$$;#8+x94rV)L^iHa(ln#5B*&S4z6ImFk58% zvsS?}Fi7_4Cgu(F!ZL=NdL_ zsBm-;yu1Koa91DL^?{G~e*Q1~|Ff*6T|DqS5C#TMS3j3^P6T3l$6K1lO zpW@{BXr9UPejHG~DO~(LP`n>1KFiM>Ebid1IQgre5S*>;4`UmH#mf9cK=kIb{?ow* zZcdK~VFb(Ui!|m1oks#csbuoMNXoTuFd8Ba;Y&2m|bF6XfA} zZiF@K_>_%gVZznOfCef{~gVRVT}xmHI_a=gM}yW@#94^ zQ8Yt^c~ zs=KTDs@hjs8`x_X7^1Qq1SAFs1PcPqpsFV#Dnk4>7~%s1aak-VJ&*JWU0N*AAwf{p z3t}1?dUhq|C0k_Qebm}UBwKZU_tn{n%DNY6P$03~Zgqd%XT5)Kz@CGj52IfNB^Q%? z|3Nc;SOBK9KJy29qm9`5{%sO&NIkK^;`fL^lfgmx?;W1S02R4v-XX+PgBgTY$g03! zp+U=*1R+Pjn2O_b8CYL6Axp?S2c=prjbA`{B^^#9eDV)=dz=#vcmq8=4tA#XRhHy+>uR2JUD=8zE2S(Llxhnr-Tr4Ek*wTkJxl>Es!HPQBSZbk z=A`bvmYC6AC%@U`D4Xy-{^MDNo+g90!PPrd{ptbWbyKshv!pX?%Pb$m8zDl`rb%}J zp?N~{W#8u9F+#j)R_+3!aS3zm`{RLSmL@kdYo)a(l$Kf$wkT0qoSNMnbP@Qm6=K>! zbw5qpmGe*6g#yu6zv$o08Ff?G!)y#98JwSLvT<1%r|0*+w-x{dRn&h{TFm zl*YRN7^czvtK-s*6p3cpE9e@3_V31qr2?I=9L^&968iwpwzcI}4?qZYZF)B*Z|Mv- zgEXzR`IN(znnd$+ajUAm2fxHlSkn~L%Gu2g=XvQc_W_|S2LoGx(|h?F3lA zL%jA{pf&Ky%M{YzHx-2_567WEb=3HgpG-uibmz)+KCz6*jM^P1t_tWJQ-JGei4dCX zoLk*b&D|&4J|D!YeEhyKmmPN7&YL+dyKPd#Q0r5zT70{^^tNOWwx%gm9SBFYpI$9S zOi((xQ%*iSnRXtn>>t1(?G$^nX>*=DPX@1Y%%c8j!6K>@cHQ?kdFpBT|%7(?Co z_P>&4EtPIEN0=E9->dSCwQ4UKpoAn%h1-|2QK$-F{iR5@TmP`mb~%s9@QV)pfT>3UkcMK~lDOBu{ zv$hK8z%=DukOE?PR&)6z$;Tsuz#1j2+j9N$b;FqHSu~i&Q$@PyJv#KOEn7$h8fP;c z@bjeG`TOR`eh%8azyJoE7tIK+G3p0>g{9#G@FCZ7f40NVOyo`>S=<`}{cOpH3?hg3 zH`NA(yR)1w{wUOcRHOfKugor(l^XrKfe?6_09^n!0}b@n3nWy~^M5<($1w1Uj^uLA zn%gmhbU0%w?$Bw6Rgj&{MHLLC*56_vcsfX!LpcEbM4xW@y1T*lVcnEYh(nj790*%C~s4i@n37~d^J8+EVlj6)U2_9=`_JWa`* z-jr4&YRQ+cvN=zIaxZQZpRVQ+lT%Dj2%%NrQ(rw4FYz`eOp%tpr)@BtUx<6mE$`n- zU|+v^!i98@k$ocB4V2YSSvAPmN_Rjp!fCf!vqRfR*cl$qF-Y5&LzIL?4{Ue8 zX&?)mlI>(%a}a2H-A?v)70GR|Mj%B+m~O!`bb%mp9?F7#HW{Ny)CSp{$@Ec%sI=HU zkX;5~V2Nv`g&esk<)HhRHmqZ7`=Aeb$xvuxgQ2F1Z+!0t;w(S@-DWVbIPNld6aUhiA?g2<;*uO-jDR_G@<4`w zkX~6+GjlWl=d!$BeugHc+bQ=3!*|Cas$m5#qp2m2=*4hi`;YrJVES@ON$8_XsOI__ zyXm>B>;B@@xOeTa>E0TqkGx?_klQACJkF)g@pBiC1qAI< zKdQS{FMWANnb)@(;kq{SURkyky0(aO@wa}To$kK-&f{jD+Iw?U0r6SSSc-u{*icRD zFrmvR&o6fcO|E_~2?1UYPMxO=79XZ`bbGa3mvd z+WO}*GVAOPzdO7IaB!mXMA#M``)1v_UC_%X?d_!RF&YsY0~Ym zbyrp`(wHqV*2HuZ=$ID0qxAU8 zG^t4{!$WOu3k>zHmzAozoQY&X$3{fQSx`s{X&;Rq2k2u_jp@vf)}7TST84qq#^!}kTPFno8FQ4i`Mr1Q+i+mr zdi7`eaUQlu`DwHasRi=a-7(u9{UPrckI+w#q+J7GsI&grq&JW`q~*wwaoN+(6P(|7 zZufNDrZpth3s?GStJU!`&t=uU^~dH+4L4xFeEn%BxbA0oAy}K| z4tsb@^}LbswtLiP3;Hc*!OiJ3l!fP&6K|lpzV!-hzoS#y7TuD!7%{@T@N9?{mRe|p~ixTyR| zz88lVtrTVYs6Qvluy1FKd)RZ~YkZ9OTg>|dbuB2{@6BD;wrip3Q+FqdO?!M<9vtVR zUfcB+Av4o#zouJrf8NHHxqMt%{EW|iPhvn20Y2+rW~#U0K9%c{Z>S!&`Lgz@c5t_S zn#B!YdJZ}LV{-kGm7ID%Q$M-=^c_BL^N5eDE|yQN^?Iuv4js)ujC(U3u-KL9^03z# zdG?(evIZdiGVQSr?QcHhTBG|0@0eyGEijbS|uy3rYBJA9^()+kkshbrmA5xD*Qxc;p8120G+JZBW_<1%sTgHvHAWguzY zDe_6b@}tk+&$G|)3e0ouyR@AfOkA{3Q=^$jRvEc+$qO=3T5I1O0>Kj$w=HoQ`_t_6 zyEprGu%jPr8{?YBlA5%!*n0Ff-W{9T*YhNeN~qI`ORrh_c+-toRV1O8EwMjrZ57QNKl|XTYA^GL!W=CZ3U$d*aSxBl-d>Et-(hl z`g4|OsG$^7xJC>MR-#Mn8gwrtRl2}c)WjDS4hzlYOlpKwZeKsH`{I5-D3sXs=uTO5 z-BVFnlhInkEn;rmUs&B*8NwmoUKMh-ys=>9M>?7UdniRx6 zTnm%;WaR(qh0$h%^0kwA%zFhTe*_b~S9@u6%!r$Mc+HSfThO;{$2reuP$_8jLrf34L2mvQAEW2s@`4f=GV$PGNgO}(ZCs^NE zS@l)=byTk;(5@}r9edE4B}6zjzL`71vtmaL@n*SDyVmL?!dEC<*)~k^G?8gZ>>{(- z7p+I(u$XR)?e;6Bu`=I4t!gcx%Mjkv`3Xh={=8rL@Dde5^9KW}gI_0X3qZb1h(Qjl zH5xP^qo?l6lU}lz=doW26ggkmf|Qc9DHrJ-{jZ%RyFBe;W67avGLS@u1QRP{D;t3>iV)nGtj9Ut>=UZ%cmNdtHM7 z=tc1h8vjH|Hs=wZbyRj!C}3V1qUTv{=d?{jnX!EHF_9*wXEpEm%R|sH^cMlTb7}mH zrV#WkX5auxW?9nq?0X@8Dgh2&cc7;?Gd-fGZ&QQVw7i)Lc4svn3)DmhD%rATDol3- zojy(yZVV#Lfl^aE**bkTXAO7i#h2gXNjwB|af)8u z={ydrPjag^!C-VnaNb)ji2397Qo_v6tIoVZohG{>KiOh}G;tBb)-iI&q6wQ0#UJy+ z2wpni(AbwBDdIea%ZOr8M*mYuGKSkc_jdv+@r2n(!kmkKWRZI2koo;bV4H|Jz$ApR zIgFEfGZ!PL@D>v|T}&`uZ{{S=}np>Y** zD|r*7?Z!d)9unzfK42FPiG=j2Zr*NmPr;19|>mFX1~4n z3Pr(n(Rf0e2#6>AmH?TGITHKi*D=weQncumJl8N!dg6FM_OahyjqQMlD3;n=v0D)j{iWj{lnah{zr-TwU;Gzn2)j;b!`YHjmf{jxW}0? z-IIK}#=~U-p`So!+YR@=L)yF2r~fm|2g0cB=kiC(ODy%fKCvDEfUVD4T#>$QE6o$0 zBq+OHJpw21-nTc+*<>B(Q+Df6)m{UA%#FYXlP2&vFF^42zQ32cx<~v`@GjiRobyr9KwLuO5ml;4oxT~-xOf%rn=*LA_d$-w;ZL(ZH{61 zGLlK1hOAf#79^1!Qyo!?+nXW~1ugWbiYo_=CCF8+h7vJBQKpxgS!ALVs7URH7jwzzc$cX(y0k$qQhGryArNul7qO=Xaav(2Ei zNDZB+ehJjbj3k#qr4m_Bj?Ry#O8w5Tq)5R{8dw3j&z$wh3NMdKj#a9Ah+GvlO|9$DHleUEFgM@U_!aQcHp za_mdkFD0x#EDzF*suQd8A__A)nk2AaUju(|!v6xQ5FQ*wr5n63t~i?XzB`mcpr;1g~8)lwK z8}ApH!_WREj7V#~ULwBxqapItgw`4vHe%b+q@G| zC_SiH?DvRtsJ6~V%TgwFCQqL_A&RAw4S^VjgMbKUOQad(*?itE9qUmpD%Y;}7k2(i zCEF*u%z0I`;0P~tZ6_AarumUGXN>S`#&=y{UxdLg)hdZcFn@vb2oGbR6V(n3;@3^- z7pGHCcqQuUpBmEPdyq%`O7$ZF@I!Y(lNtlMhUDxjj;HZtj_{_=VGY<*`bAjQMfAr_ z5e4O;r4R^;GQwaMaQ&y2pqPM87fcfC zCfCjp=Z3unXBsUbB5?#~f@=2`2~`a1+pVjv6D%essw zJg0I65vp@GpO52K&nNX^K+6pGoJ(+^Qex(2-FCv10;>weCPJ8Rd9SsQ~VH zeT1tRi(hf?oD@&KyFiC{$>Y_2^04uYTDZOjRu|>*7ptg-U=%H$uu~TV;@6^;LWcwv z7v=W`PgX1ES7j{wWlPD@`JORd7J->oO8pF)-rNv;8`AbMyWK9z*X?hJ;*m0QNXo%M z8ArV8emmTUe@N8^Y=QxXFZUfW>lNa8Bm^@ZB4Ku6ZJ}tfw#8#fR~hb$Vt^zsbpj(wE1pp82b8XkcjsRY9_MDd=zNL4#+99i1&+ z>Uz)&1$+2oipx3uc#e5Tf1E`9h8}=1wElgxT;xglP{%ZUWMzmhleX+;ziBlg{mAfM?-)@jEKc02383#$xVtp(=9aHWO-V{Jhe~ zIVI@I5jVW@v)-K}NOq41EI~oz?*{39pW;O%*7m!ovkU3a!qKR9K#2 z-+x)2sp*?)eC&N zfkkxI0LMwUAtpP!Xw0A$QP|4+EbX5)8aJ(slRl{BjUGptwvV)T>>u97ieb9QzIjMXc5&%5DLqegoL6ct6}R3wml9RI0Z zlTMt4JgpW#shH#)y+;$lKK&EYbKbpHaG*BxYuqX4@;W;kcGUU|WoPeA`l2W9(aAbH zv(5ba)n}uqm^d94adbt6IF^{y1VL>P?SnxbidI2U`9IS#>E+@P4QV=7Kka|5FoU)Y z8lR|>U;(p#u1{_CPHxqHm#;9BKwfD4=a<`;4ZVYdasL;|nS>EXg=6;@Z`Cp}&7v!VJ+#w`?APINvt;?Bjo3r0JU*vu7 z0xM+3`^Y>VNukyOgxsJ{DEf7HXH+ENo0uZ8F}|b9OEiyR>kOn{0LfLiM=Jtmy46M8 z)GiEl*_%l)C}~i~R&xqscnW$c5-@Xisy<|h(j&37$g0^`UsTL<_Nd+|45TyXjrs^q z1*F(_^)ld2^#kBf1oY6r_A-9wtYP-}dC!z8Xv)X97RW47Uzf=r!%)PA42|3cKB^Tu zGs?1!(-*&oI7v-00j~toGwd#KCK5@H>}s0FiYJpY^6pN)(Fo`AwExPpr(b3asPj*`dq@kGOwZ#x`)@FBRJ*T_DWMrElpZ~!haQf?DLNm285)gi3+ zz3LC!Sg~}Re!fq;9{j4_kEOj1feMoIMVG+N{QkB{3-qXv&Syvh53;9Y$3QBm4ClZ> zkHNx|dVf3~ECfx(VB()|Dy8b(cM{Ir2H0YiC?^^4%@R)8S|lvafs(sd&|6hbfxV5@ zVf}>nUlw9<0NIS+9<|9MvBBzLj)htzL$di~7Nt>(sZy4S!wFnp3@XuCs}!rN!pAni zj)`=%_)2H2G6l|S z@Ea*jeNgrrLY}=L6;<*y?|0_I_DbK!%=Wlai47yGWn@FBUr7eZqdBfc)<({T0^#*+ z=9Y#(fUqxrq2$JQ+#}hjDbBo}RS5;%Ceys5ud2BhG=1+2-qH6sCFN@6yLIP9d}a-e zfv+Qqe=uX$&c}aNRZ&W|VVM%^!E+cE`rNQj?7hw5;Wu(Fajt5ptQ*~bdu5lvnopAd zN=R{K-o1mHAo^WvG}gHL^Pp?W~0#>?~M5FZkBM zB}x3_O_gedW;8f$zxEHvntp=F_E3_4hr;dQt6UyV$#aS@Xi*DPJvqPmQUQC+Gi#mLp+5r!8>~(a}q!KTKq}A%SYd!x(EE@GGQw zjI?EWOX;shbzqJ(HwB%cIeo|CfJR2z9!Al?4oa$9?uM&aC@XqI4G_Bp6p5 z=(HbAkxC~78l=J;8UQ`EhD`{6;+yA@8+m#ri){ugB0;kGMBt;rv( zM5_M~=M9A7r4x^sz#F}1yI$TFs1_TAxP76gYPepOBB~QVu7pVq^+(N_4V$03q6H`z zoGR562T|#C=Ta z2k{Yqe1Vn?q{gk)ONixlp*x&JWrG_1chx#aE&yYoa<9^u$&{hrpO#)i2(&nz&7FM~ z{W@9WWAqfZm%Z_b5(8)2CC(Nr+zVt2;IESt-~_yRQ|Q;GP$oUDjc3bs zNk8jc7V1oF2l0Ctz3`#rF00*KuS{iaAbS`~Adjb0rU}Jbp{7%yB#d246BV;ZpMP@x zjT~AjIe_mO=Mp*^De0;93=Sx$gyQ?kfbm$YmPujyZHMwJlXhU9_D24<+#A4Kx?B}e z-x-Bowm;}S3_r;B@md^;hY-$L#kQ;YC#GW-E?UG1F0(LCaDwgve!f_HAh9Qg9(wFD z({Dsrmn~Cgp|dB18?Mx0_x|~b@A~OUmAu+m9mg))ZNBC!XBqTLKQxkI^hbGgFwc-S zBa`kIQMABxvyFqC_mMLkY>Bz^e7xi<*zmtPwN!t z%#m2vW;WAPyG_0SD-W<radLBE^c{IJxdvB$ zuK4aMD4=H`wHg`Ca3Jm<$nh#|$6F9`mq$6h|7Ic(7-c^rqP2E+@hMcx;{=Jk> zr~0&QO8niMByk5OJz4;hv8W%mlI{sDyaX0D^pf@l!++H{7pw% zS(5U1N!cxGjjR)MflH1;VFP=VqXZwe6iH9X(%}i0rAdX-arl-l4iBmW2VqiyaEh8r z8%fT54aE;tT%p^oFrwxpOR4#ZmzR)Ih9Q;Kgyn{b(0&7!rOg7sGKUya_;zIWC2gDy zlpY153p5ear&|?guYY0VK^+O3FUX`XO^20TsJ2RQ6f&M`X&ly#`O9&xE-CtrkxrjJ z^rlA`vi)H#@cdNy1d1Oc%mK>dTll&>cU+&z3=Gc6N}L}E-Nh3DXV@StX7x`%fjxX! z8$X;1A0=)+A~7V;J}NIe<2r$HW%WG)3peZ?>?7hRI3P4g2pS`sQz7VrydBe+f%$nX zFVuYlMgh(n$Ancn!C+u&;QLGlbTVSnAy}JAyYb}iO8t&K2X145CbQaurH|<9kE3Mw zh(g9NR}?AM6`8S0RWA(iokL!WYfUbf2_my0g`Uulo~vGRzFxrmv1dOQ-5_Ffkbghn_4j;PS0n>P6x}&>bU`5$`E|n=m0`!mPpGIMNl; zTT`W71y{$7U?ZI~&J7G|{w~?}{qAC?xwS5KUDv{J)zJ_1R?&xX>gv9OGK7Rg(Njw( zpLGmb1Bv+5k0oYiQ~?E9sTY)<+h4o9z9-U1kNR^yEsK=N6z_B0^p42|)PjWx8^)vQfMM{uM74PsVgg`CKW*%aQvqX$#ihFFLwJ=p7>qOl$^lT>ovLIbpZt-pjSLjQ#xHRGaF~k-+Cnms5aqv&n(W+ z^hlpyB(#uoA+}##X|hN@oNtt8_fSZi+?;PDXwB>7&*wxlGisseBMQ`-2ebDyy9+_L z&)`Wa{#pFPckri%&33jCv{IMgZaV5!Z4~NPI2+yp-m{DhBnF9-SVDbvdiiO1*D02m z;D0|gy3C=R8L)z*c9m%+01~7_Z4N}F%T}5=o}}NuU1{WQO)c|5bdnQ4B3EE0P<-A{ zEwn>ekKkC0YPKT_2HfVw9CIe}lk=&|OfMo|H<|qXBv-tGnr~IZAdU^DO(D;%d_sYhKgJs9v^TH?Fr zQE-rX#3KJ3#tQo-VyC>8aMm?Kvu%Wqm%m87Xs)$gJZIwr5kR0=A-fm zEk&0FYYhd&Oxsz{X11ez*5O+%V(g!{(mWVe51v_Ekwqex#m)%|hXK!ddCDT<8W0Ew zv`~qWT-d=Ez?|T=o!n1ySZuil4=Pjrit&QGVz@${8Y1Sog0gx!2aDl`reYrL%Qad2 z(P<)a;lVvE z#VbmPK~6t1;h{86DqPT7c6{Oco*mXoNkU;Jr7kTiSgwch@DTIk% zWKLp-OE>@c9;3FC{H~IZS2w+#?yM*FR8^bb!6G`Eae53o!(a}XS~P2&NxxGCQJ(GA ziIw&*rF;gfVcV>Av-t;`w_ixh8I5l(6#2{FRC$VXwZ^RUh<ou-3S*cGmX|b(MsfkNx=cU@x*sW%*NeJIaZ99K`9k%^I z+SaWWB-r|o7J`RRXQ70AyT}wCzdCSDO!q1DAE!{7>||w`0R)PB0{dUu^#3yEMa5(> zXCZMxAV}P#Tn-TcXF^6?MBOv{GUwF=L(0G3$DG)^y7`ANJWk}YvGBKQa#-%X8E%a5 zfrsbvpr9%;QjKTIX{BeFzkt5Rg}SQKcP(2J7nzPh+|l7*I3`Fj@#rKNlXBAhlsdm< zyVe%3ri-;0Tf#DORx?g>b8~Y(FeGA+8s0B+uiJ>XGoJl{qRL9}K$2?lcPwF&8+2n( zkTN-fFp6~u7_4Y43rM6K6`BN=a}WesAc+$|vH%N#5`y=CDJd}G1W?HMM=L}986j4p>Nc7WL>Pk+{i^{z1@e4ccQQQ@en~?T?)ru!WHxr%Q7)H79jhI}T2t zZZ?qMbvU1fy8%4k@SHW7G4xfw11Ng~f@q_{J*tTX4g2>OcE>LY5H~%W!R^zn3G*%M zQZ~?WObDd<{4gY>5wZ^GqHW0hKnkBvYRv0hTtG{rRXTgk-jMdssW(* zxhO{SitgvCIQx&igr7GCsyDs%9o@rKSlnAlJ1LUYk-y{Q+g>sp3|@KANq)G@9e=f2 z7%TxJA$7G59C)=tgyo7hF)F-1_ur(j5Xo&*Q1pLYo#*BZ_WF~%=!38YD3xHvA@=LC zyXMbJL%OLGyrTVzCWl!KXgR}YqRksAz}2qmMRLV6X3WL~F--O{eXmb#Px@gN*{h!L zg-c`GGqUeHenZ83=dHnVkj>X}J~s@=Eq8IRXx4pM_cciDw#|FW*EYILrJ~-tIxOO z?pi}XrqYk)^tW?L*`FiqF^38%A$>h~ znp~vj%lh#7ZgkIQZ!Riks8uVp#e(mKY6$p^eG6ghyszKOZcdJB0qDMAqk~fhPyGbs z=G2db)NSoT{xP|2g$cf_ly!DhNw5kU<$xE}CU0X|U0j%xSz=ckI z^i#I0EU!QUNSI&HAU9n21=DYHExEA#M#)oJ1*Gouc035@=7^63AGKGt z0gc2+3x9_^Go@q`mv#I2e62_IA{$kEJogyeyZ+P--$LhCf)CxW$qO zL)1)Y4xySqchVn5OM{AIj%_PcsQxeQFsjh zJ4X5Caj58cx~xgZ?6cQG^Y+JG?KQLKIT1uzwCeRi_qmIfi`PM4-ws-6_;iVgX5vHJ znHJ0MuC!s7fco2=IR^bxB7yF^pT7WBOAtCbUgG;0}Jq`J??;8b(;^0g0G!1 zaPg^^YlhVdqlBxCufU){$cGl8H{htb>Pi)1QZWTZLNdquR|Tb>{}v68GbohMDA9)N zf)=*<*Nq*aY^)=}jA_ajgGtMeB@a&<{4@sQ{qom}x(l+PNC6scM?dgjIbb0o`We}g z101CicK^8{8Vy2AM0_|U0BiNP(a9@#zo7bCE^m7-I8?J1R{KypMr(Z66)`s_Xc?2a0Qll z^tU4GvxoU%f2Zabt`W>)3E|=78tglYWU5Z?eJQ}h5#rlpPx@WjjbP8lENet#M zrShNx^00h`H~(?Y01PO0fL$8l#DcOd4eP_eiSVXA2-r{*tV0Y+J^gn!t_ODS$h8T> z$yaK?tm`iMB{;D0ac~3LbK+2@h7mG#$YRAPh^D;Kr!^^O>*81gZYzrT%6kyX7KxhL zB6zYl!E=ibJNkBXxX>&W11tTd29=y8YH)hxY(8FkKi{EdfJT+TQm~cJe|?gJsH@@< zCJ2_#fQjwuQvL}^)J*7|J3Z&VAM~BC7df@fTzv-OEYsHGO|}aKt+4Kd-9PI#oCWuo zf*`W~(?5dR=s`H<3<(SqwXAJHgn}T6HyD<$@`PMjaIvZ*vC8ttxRNY)M7vO3?IpAd`=`!`FW~fA}0- zTAJqOM;0@%42+#zSMv z%jpF!-yW7f-FMzBXy6WPnWF!SwK5pM%@E*rgMF51yyVrIN=d>$Lf`s42H3poB=@}k z*#`#yK?ep*e7xyvFHWy(f1<0rbee0CUvb)0?xT?@Vc{0Bsv=e}*T*VYrMK9no^oHW zVe1ffe)UwjAc(G(GAQ zRyL!nu_{T2!>E?K87oi%C)!DY|G+*CPQWn37}X?ObwV^autpKAEn*E`V@VTB761m8DT0+)5e!z`5i1QTV4@$e`x)=Yg1 zBgAT+072OND`M+gb+}2I9|#NYc4y+KzF!%yZ8hy@q;a8&3x&bX76hSOB8hS@ zwdUKtJ@@-cBa)KQCy<_zQ~6YrYZk?@Kpd0ChYLqzkxUOKxdi1bnns2RdW^y1 z7S15DG~Zi0y#M*Ev)AFmDcn;TS2LW0h{7z)BWwP+YDd%cgzPWrRfNKC(@j8GkSAm| zUVIF;h>_+$P;L4+9Zs8&bE1Wr*0<>szpE_}jh{03`mH(*Yzu0)Fm$R_bi#{%rKSs@b^=hmm7(Ot{;qJ*1ucI$Fld! zw1vn=H5h^e-kM)PXp67Y5Czb!bT|R=B$-y!D*93r-30#6tU7|7YQy1^SC=@~q|*)2 zJ}LH72W;(AX#sZp#&btbt?`EDfE`Vl<1Q2%S6h*u4VLrf-eWMlv7QE^%oL!Plac(0 z_9WouXbf_12i|>2)9mbxA_vnZoDX)Vu6KjD(dwi~mv`X^@qKbPfUaKWA@{z}`wW-k z`PJ%1XETBhzCp^!sPXJXU(5u>oH3 z$Ce^^v;jU|v`jL5SqoJhsUG z%Gmwg%KmDv3n6|UF!#ao(}~4@5WeN@sbRt4^E7ga_~ojP|M}z{k%r=aNNTvQH+O9= zY4#&7-l%0p%XpWSMg7k{?O|IydDhwDM|Cru;Pszu2C(+e;KZ?^pf?R&PV2tAE3?|X z|I^2nheO%5@n>edUJYYpi$=^0Wl3eS*T_&~6f?ZpC5$~u_N5v#7?kYOqE~7x*(&r- zN@PvTgpjB-_9b~sV<+1;q`tbY@A{s9&VBB4Z|8pgIQMn#bNwJGBDeSS?vklKSFrk_ zBG!!(xKsF1^_j+&;+Y#^)o~h$cvuS6_>$bXOQ_;x+=H@RvAGcoxANfnvfVW;24{q? zG+lsqHgqI_E5ydMKZSdG?JZh~aER3m%fT=B9qQ>lLPm>AL2#(I+AE)7r{*V#`l}Kl zG*aRL`>GlDnx{iiywVdOa!w4v_2xTWjK|{Oo=yvRR?E@9qpxS3-t||!_X3+*TXCr< zkm7hf&FS!KJAUp-z$5EyP=Dh+h{mHktZ+8;is)pt9a4-{-hSJFvPp^Hh;6R0) z2-=$)mYrxdGZu(}PRZ4>KsrwgmdUn2`N90<$R`EM#r<7C6F+FtZ}D#kzVu_y45Cfx z*#)OlF~cP~0`HbL+jQ&qaR91Aw5vU+2BN3-Bv{j*E|(^Uip%xl)v@#1Kt~90kPmLK zh0?I1U00h-3k)e;C90)cP|QE{@6Kt}Onpmz9J!2AY8EK>D9CJd0n8L@XgVzK15R$zHeP z`91)AU_ zOt&7OmcZPXjqYm^q>#nnul#+Zt^iJ+XzS-cjR9UI^M!dU5#%6!OU5)N^f3bP%U-T9 z0e@KtiEB4Q>zay z0_Y0|1j@%({F6$7Tw}C?rEa|gE=|6PdiwoG6npbX5>lELs$1_3`5HjS@Ax#`IFSj(8|S$ ztq@(k=A;{+GU&m&8j@?VOFV{{#?Uj@7QAvm}lFZ$Hx&ZVOU%98e1gY&FI^&SPn9vNJW;Rsg3 zomRiO6{f`P9IRHCW2e4|J^p7gbw=68*fp;-g~nmtwt1S;F1COR3?HVB_TN$@B@Iu# zoNz6KACO5jQW%2A%(2szn+Q;qCB?dH6i2LyHZkVg+u)xJbgW7vCV;A>VgwVAtqynNl zJJ`$G9^Z=HQ{duvLvK+h+ZJTV6Xo=!tjdtd%gUtd6P03-BbPjW0 zzT(h}{#(q?6Ag}Slh2--wCh+%DJrKOp}vLuagKGDPxFk4e8CtMvX)FVK5a{)&P~Cl zhVH_^1aIDdBZHW(s4edX9!x0psChuAGVxnJvZ&TNby&M}`B5nv0d(hhuCZwN$vAN6^qyY9j)qt7f};o{ZZ+=$6g69*x1R&&~ri%1`F z@wgCJp$pETO4BGdaN$urI(u1zoMlBx$g8`Z4!XIip7sFR(L}=;edcIZS=MX?nTude=qKPr*K+81Nf#S=)OA zS)`mKh2yHtp4ZHqqmi48D*2r;!P4d7tdoOAkE^g{ckVV%4@zJJi;i?`u8BRCylien z<})_SJKGYG$#y&Xx!42ty3A$-Xs+A;t|8 z&5aSom@^Syr-efxH0#J1_tQx(5ZF@bwJb9d^pc+yn#`2gHFN06D+* zfCPWjbY!U7Zk1cJ{@-p8z&kL1{d^hjwuco*#Ss;cIPqnZzL5@AxULZpK(NmDz}CBNvQTdj$A|83RL9Cj2n*BTa4%mUtH) poK!Cc0HD8xQESFaa8S69`~ONWarming_state) { - /* - * Firstly evaluate mode switch position - * */ + /* evaluate the navigation state when disarmed */ + case ARMING_STATE_STANDBY: - /* Always accept manual mode */ - if (current_status->mode_switch == MODE_SWITCH_MANUAL) { - new_system_state = SYSTEM_STATE_MANUAL; + /* Always accept manual mode */ + if (current_status->mode_switch == MODE_SWITCH_MANUAL) { + new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; - /* Accept SEATBELT if there is a local position estimate */ - } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT) { + /* Accept SEATBELT if there is a local position estimate */ + } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT) { - if (current_status->flag_local_position_valid) { - new_system_state = SYSTEM_STATE_SEATBELT; - } else { - /* or just fall back to manual */ - mavlink_log_critical(mavlink_fd, "REJ SEATBELT: no local position"); - new_system_state = SYSTEM_STATE_MANUAL); - } + if (current_status->flag_local_position_valid) { + new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; + } else { + /* or just fall back to manual */ + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); + new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; + } - /* Accept AUTO if there is a global position estimate */ - } else if (current_status->mode_switch == MODE_SWITCH_AUTO) { - if (current_status->flag_local_position_valid) { - new_system_state = SYSTEM_STATE_SEATBELT); - } else { - mavlink_log_critical(mavlink_fd, "REJ AUTO: no global position"); + /* Accept AUTO if there is a global position estimate */ + } else if (current_status->mode_switch == MODE_SWITCH_AUTO) { + if (current_status->flag_local_position_valid) { + new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; + } else { + mavlink_log_critical(mavlink_fd, "Rej. AUTO: no global position"); - /* otherwise fallback to seatbelt or even manual */ - if (current_status->flag_local_position_valid) { - new_system_state = SYSTEM_STATE_SEATBELT; - } else { - mavlink_log_critical(mavlink_fd, "REJ SEATBELT: no local position"); - new_system_state = SYSTEM_STATE_MANUAL; + /* otherwise fallback to seatbelt or even manual */ + if (current_status->flag_local_position_valid) { + new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; + } else { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); + new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; + } + } } - } - } - /* - * Next up, check for - */ + break; + + /* evaluate the navigation state when armed */ + case ARMING_STATE_ARMED: + + /* Always accept manual mode */ + if (current_status->mode_switch == MODE_SWITCH_MANUAL) { + new_navigation_state = NAVIGATION_STATE_MANUAL; + + /* Accept SEATBELT if there is a local position estimate */ + } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT + && current_status->return_switch == RETURN_SWITCH_NONE) { + + if (current_status->flag_local_position_valid) { + new_navigation_state = NAVIGATION_STATE_SEATBELT; + } else { + /* or just fall back to manual */ + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); + new_navigation_state = NAVIGATION_STATE_MANUAL; + } + + /* Accept SEATBELT_DESCENT if there is a local position estimate and the return switch is on */ + } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT + && current_status->return_switch == RETURN_SWITCH_RETURN) { + + if (current_status->flag_local_position_valid) { + new_navigation_state = NAVIGATION_STATE_SEATBELT_DESCENT; + } else { + /* descent not possible without baro information, fall back to manual */ + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no local position"); + new_navigation_state = NAVIGATION_STATE_MANUAL; + } + + /* Accept LOITER if there is a global position estimate */ + } else if (current_status->mode_switch == MODE_SWITCH_AUTO + && current_status->return_switch == RETURN_SWITCH_NONE + && current_status->mission_switch == MISSION_SWITCH_NONE) { + + if (current_status->flag_global_position_valid) { + new_navigation_state = NAVIGATION_STATE_AUTO_LOITER; + } else { + mavlink_log_critical(mavlink_fd, "Rej. LOITER: no global position"); + + /* otherwise fallback to SEATBELT or even manual */ + if (current_status->flag_local_position_valid) { + new_navigation_state = NAVIGATION_STATE_SEATBELT; + } else { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); + new_navigation_state = NAVIGATION_STATE_MANUAL; + } + } + + /* Accept MISSION if there is a global position estimate and home position */ + } else if (current_status->mode_switch == MODE_SWITCH_AUTO + && current_status->return_switch == RETURN_SWITCH_NONE + && current_status->mission_switch == MISSION_SWITCH_MISSION) { + + if (current_status->flag_global_position_valid && current_status->flag_valid_home_position) { + new_navigation_state = NAVIGATION_STATE_AUTO_MISSION; + } else { + + /* spit out what exactly is unavailable */ + if (current_status->flag_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. MISSION: no home position"); + } else if (current_status->flag_valid_home_position) { + mavlink_log_critical(mavlink_fd, "Rej. MISSION: no global position"); + } else { + mavlink_log_critical(mavlink_fd, "Rej. MISSION: no global position and no home position"); + } + + /* otherwise fallback to SEATBELT or even manual */ + if (current_status->flag_local_position_valid) { + new_navigation_state = NAVIGATION_STATE_SEATBELT; + } else { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); + new_navigation_state = NAVIGATION_STATE_MANUAL; + } + } + + /* Go to RTL or land if global position estimate and home position is available */ + } else if (current_status->mode_switch == MODE_SWITCH_AUTO + && current_status->return_switch == RETURN_SWITCH_RETURN + && (current_status->mission_switch == MISSION_SWITCH_NONE || current_status->mission_switch == MISSION_SWITCH_MISSION)) { + + if (current_status->flag_global_position_valid && current_status->flag_valid_home_position) { + + /* after RTL go to LAND */ + if (current_status->flag_system_returned_to_home) { + new_navigation_state = NAVIGATION_STATE_AUTO_LAND; + } else { + new_navigation_state = NAVIGATION_STATE_AUTO_RTL; + } + + } else { + + /* spit out what exactly is unavailable */ + if (current_status->flag_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no home position"); + } else if (current_status->flag_valid_home_position) { + mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no global position"); + } else { + mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no global position and no home position"); + } + + /* otherwise fallback to SEATBELT_DESCENT or even MANUAL */ + if (current_status->flag_local_position_valid) { + new_navigation_state = NAVIGATION_STATE_SEATBELT_DESCENT; + } else { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); + new_navigation_state = NAVIGATION_STATE_MANUAL; + } + } + } + break; + + case ARMING_STATE_ARMED_ERROR: + + // TODO work out fail-safe scenarios + break; + + case ARMING_STATE_STANDBY_ERROR: + + // TODO work out fail-safe scenarios + break; + + case ARMING_STATE_REBOOT: + + // XXX I don't think we should end up here + break; + + case ARMING_STATE_IN_AIR_RESTORE: + + // XXX not sure what to do here + break; + default: + break; + } /* Update the system state in case it has changed */ - if (current_status->system_state != new_system_state) { + if (current_status->navigation_state != new_navigation_state) { /* Check if the transition is valid */ - if (system_state_update(current_status->system_state, new_system_state) == OK) { - current_status->system_state = new_system_state; + if (check_navigation_state_transition(current_status->navigation_state, new_navigation_state) == OK) { + current_status->navigation_state = new_navigation_state; state_machine_publish(status_pub, current_status, mavlink_fd); } else { mavlink_log_critical(mavlink_fd, "System state transition not valid"); @@ -123,74 +258,164 @@ void state_machine_update(int status_pub, struct vehicle_status_s *current_statu * This functions does not evaluate any input flags but only checks if the transitions * are valid. */ -int system_state_update(system_state_t current_system_state, system_state_t new_system_state) { +int check_navigation_state_transition(navigation_state_t current_navigation_state, navigation_state_t new_navigation_state) { int ret = ERROR; /* only check transition if the new state is actually different from the current one */ - if (new_system_state != current_system_state) { + if (new_navigation_state != current_navigation_state) { - switch (new_system_state) { - case SYSTEM_STATE_INIT: + switch (new_navigation_state) { + case NAVIGATION_STATE_INIT: /* transitions back to INIT are possible for calibration */ - if (current_system_state == SYSTEM_STATE_MANUAL - || current_system_state == SYSTEM_STATE_SEATBELT - || current_system_state == SYSTEM_STATE_AUTO) { - - ret = OK; - } - - break; - - case SYSTEM_STATE_MANUAL: - - /* transitions from INIT or from other modes */ - if (current_system_state == SYSTEM_STATE_INIT - || current_system_state == SYSTEM_STATE_SEATBELT - || current_system_state == SYSTEM_STATE_AUTO) { - - ret = OK; - } - - break; - - case SYSTEM_STATE_SEATBELT: - - /* transitions from INIT or from other modes */ - if (current_system_state == SYSTEM_STATE_INIT - || current_system_state == SYSTEM_STATE_MANUAL - || current_system_state == SYSTEM_STATE_AUTO) { - - ret = OK; - } - - break; - - case SYSTEM_STATE_AUTO: - - /* transitions from INIT or from other modes */ - if (current_system_state == SYSTEM_STATE_INIT - || current_system_state == SYSTEM_STATE_MANUAL - || current_system_state == SYSTEM_STATE_SEATBELT) { + if (current_navigation_state == NAVIGATION_STATE_MANUAL_STANDBY + || current_navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY) { ret = OK; } break; - case SYSTEM_STATE_REBOOT: + case NAVIGATION_STATE_MANUAL_STANDBY: - /* from INIT you can go straight to REBOOT */ - if (current_system_state == SYSTEM_STATE_INIT) { + /* transitions from INIT and other STANDBY states as well as MANUAL are possible */ + if (current_navigation_state == NAVIGATION_STATE_INIT + || current_navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_navigation_state == NAVIGATION_STATE_MANUAL) { ret = OK; } break; - case SYSTEM_STATE_IN_AIR_RESTORE: + case NAVIGATION_STATE_MANUAL: - /* from INIT you can go straight to an IN AIR RESTORE */ - if (current_system_state == SYSTEM_STATE_INIT) { + /* all transitions possible */ + ret = OK; + break; + + case NAVIGATION_STATE_SEATBELT_STANDBY: + + /* transitions from INIT and other STANDBY states as well as SEATBELT and SEATBELT_DESCENT are possible */ + if (current_navigation_state == NAVIGATION_STATE_INIT + || current_navigation_state == NAVIGATION_STATE_MANUAL_STANDBY + || current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_navigation_state == NAVIGATION_STATE_SEATBELT + || current_navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT) { + + ret = OK; + } + break; + + case NAVIGATION_STATE_SEATBELT: + + /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/ + if (current_navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT + || current_navigation_state == NAVIGATION_STATE_MANUAL + || current_navigation_state == NAVIGATION_STATE_AUTO_LAND + || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_navigation_state == NAVIGATION_STATE_AUTO_READY + || current_navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + + ret = OK; + } + break; + + case NAVIGATION_STATE_SEATBELT_DESCENT: + + /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT and SEATBELT_STANDBY */ + if (current_navigation_state == NAVIGATION_STATE_SEATBELT + || current_navigation_state == NAVIGATION_STATE_MANUAL + || current_navigation_state == NAVIGATION_STATE_AUTO_LAND + || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_navigation_state == NAVIGATION_STATE_AUTO_READY + || current_navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + + ret = OK; + } + break; + + case NAVIGATION_STATE_AUTO_STANDBY: + + /* transitions from INIT or from other STANDBY modes or from AUTO READY */ + if (current_navigation_state == NAVIGATION_STATE_INIT + || current_navigation_state == NAVIGATION_STATE_MANUAL_STANDBY + || current_navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_navigation_state == NAVIGATION_STATE_AUTO_READY) { + + ret = OK; + } + break; + + case NAVIGATION_STATE_AUTO_READY: + + /* transitions from AUTO_STANDBY or AUTO_LAND */ + if (current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_navigation_state == NAVIGATION_STATE_AUTO_LAND) { + + ret = OK; + } + break; + + case NAVIGATION_STATE_AUTO_TAKEOFF: + + /* only transitions from AUTO_READY */ + if (current_navigation_state == NAVIGATION_STATE_AUTO_READY) { + + ret = OK; + } + break; + + case NAVIGATION_STATE_AUTO_LOITER: + + /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ + if (current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_navigation_state == NAVIGATION_STATE_SEATBELT + || current_navigation_state == NAVIGATION_STATE_MANUAL) { + + ret = OK; + } + break; + + case NAVIGATION_STATE_AUTO_MISSION: + + /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ + if (current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_navigation_state == NAVIGATION_STATE_SEATBELT + || current_navigation_state == NAVIGATION_STATE_MANUAL) { + + ret = OK; + } + break; + + case NAVIGATION_STATE_AUTO_RTL: + + /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ + if (current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_navigation_state == NAVIGATION_STATE_SEATBELT + || current_navigation_state == NAVIGATION_STATE_MANUAL) { + + ret = OK; + } + break; + + case NAVIGATION_STATE_AUTO_LAND: + /* after AUTO_RTL or when in AUTO_LOITER or AUTO_MISSION */ + if (current_navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER) { ret = OK; } diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h index 57b3db8f19..1c0564d077 100644 --- a/apps/commander/state_machine_helper.h +++ b/apps/commander/state_machine_helper.h @@ -47,10 +47,11 @@ #include #include -int navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state); +void navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); //int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state); void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); +int check_navigation_state_transition(navigation_state_t current_navigation_state, navigation_state_t new_navigation_state); #endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/apps/controllib/fixedwing.cpp b/apps/controllib/fixedwing.cpp index 9a69195352..584ca2306a 100644 --- a/apps/controllib/fixedwing.cpp +++ b/apps/controllib/fixedwing.cpp @@ -294,8 +294,9 @@ void BlockMultiModeBacksideAutopilot::update() for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++) _actuators.control[i] = 0.0f; +#warning this if is incomplete, should be based on flags // only update guidance in auto mode - if (_status.navigation_state == NAVIGATION_STATE_MISSION) { + if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) { // update guidance _guide.update(_pos, _att, _posCmd, _lastPosCmd); } @@ -304,8 +305,9 @@ void BlockMultiModeBacksideAutopilot::update() // once the system switches from manual or auto to stabilized // the setpoint should update to loitering around this position +#warning should be base on flags // handle autopilot modes - if (_status.navigation_state == NAVIGATION_STATE_MISSION || + if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION || _status.navigation_state == NAVIGATION_STATE_MANUAL) { // update guidance @@ -340,7 +342,7 @@ void BlockMultiModeBacksideAutopilot::update() _actuators.control[CH_THR] : _manual.throttle; } - +#warning should be base on flags } else if (_status.navigation_state == NAVIGATION_STATE_MANUAL) { // if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index e25f1be27f..34b267d56b 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -205,36 +205,40 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) } /* autonomous mode */ - if (v_status.navigation_state == NAVIGATION_STATE_MISSION - || v_status.navigation_state == NAVIGATION_STATE_TAKEOFF - || v_status.navigation_state == NAVIGATION_STATE_RTL - || v_status.navigation_state == NAVIGATION_STATE_LAND - || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) { + if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND + || v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER + || v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION + || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY + || v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL + || v_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { *mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED; } /* set arming state */ - if (v_status.flag_system_armed) { + if (v_status.arming_state == ARMING_STATE_ARMED + || v_status.arming_state == ARMING_STATE_ARMED_ERROR) { *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } else { *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; } - if (v_status.navigation_state == NAVIGATION_STATE_MANUAL) { + if (v_status.navigation_state == NAVIGATION_STATE_MANUAL + || v_status.navigation_state == NAVIGATION_STATE_MANUAL_STANDBY) { *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; } - if (v_status.navigation_state == NAVIGATION_STATE_SEATBELT) { - - *mavlink_mode |= MAV_MODE_FLAG_DECODE_POSITION_GUIDED; - } - - if (v_status.navigation_state == NAVIGATION_STATE_SEATBELT) { - - *mavlink_mode |= MAV_MODE_FLAG_DECODE_POSITION_GUIDED; - } +// if (v_status.system_state == NAVIGATION_STATE_SEATBELT) { +// +// *mavlink_mode |= MAV_MODE_FLAG_DECODE_POSITION_GUIDED; +// } +// +// if (v_status.system_state == NAVIGATION_STATE_SEATBELT) { +// +// *mavlink_mode |= MAV_MODE_FLAG_DECODE_POSITION_GUIDED; +// } /** @@ -248,19 +252,30 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) *mavlink_state = MAV_STATE_CALIBRATING; - } else if (v_status.arming_state == ARMING_STATE_ERROR || v_status.arming_state == ARMING_STATE_ABORT) { + } else if (v_status.flag_system_emergency) { *mavlink_state = MAV_STATE_EMERGENCY; - } else if (v_status.arming_state == ARMING_STATE_ARMED || v_status.arming_state == ARMING_STATE_IN_AIR_RESTORE) { + // XXX difference between active and armed? is AUTO_READY active? + } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND + || v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER + || v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION + || v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL + || v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || v_status.navigation_state == NAVIGATION_STATE_SEATBELT + || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT + || v_status.navigation_state == NAVIGATION_STATE_MANUAL) { *mavlink_state = MAV_STATE_ACTIVE; - } else if (v_status.arming_state == ARMING_STATE_STANDBY) { + } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY // XXX correct? + || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || v_status.navigation_state == NAVIGATION_STATE_MANUAL_STANDBY) { *mavlink_state = MAV_STATE_STANDBY; - } else if (v_status.arming_state == ARMING_STATE_INIT) { + } else if (v_status.navigation_state == NAVIGATION_STATE_INIT) { *mavlink_state = MAV_STATE_UNINIT; } else { diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index 20cb25cc02..29baff34bd 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -60,13 +60,20 @@ /* State Machine */ typedef enum { - SYSTEM_STATE_INIT=0, - SYSTEM_STATE_MANUAL, - SYSTEM_STATE_SEATBELT, - SYSTEM_STATE_AUTO, - SYSTEM_STATE_REBOOT, - SYSTEM_STATE_IN_AIR_RESTORE -} system_state_t; + NAVIGATION_STATE_INIT=0, + NAVIGATION_STATE_MANUAL_STANDBY, + NAVIGATION_STATE_MANUAL, + NAVIGATION_STATE_SEATBELT_STANDBY, + NAVIGATION_STATE_SEATBELT, + NAVIGATION_STATE_SEATBELT_DESCENT, + NAVIGATION_STATE_AUTO_STANDBY, + NAVIGATION_STATE_AUTO_READY, + NAVIGATION_STATE_AUTO_TAKEOFF, + NAVIGATION_STATE_AUTO_LOITER, + NAVIGATION_STATE_AUTO_MISSION, + NAVIGATION_STATE_AUTO_RTL, + NAVIGATION_STATE_AUTO_LAND +} navigation_state_t; typedef enum { MANUAL_STANDBY = 0, @@ -75,50 +82,20 @@ typedef enum { } manual_state_t; typedef enum { - SEATBELT_STANDBY, - SEATBELT_READY, - SEATBELT, - SEATBELT_ASCENT, - SEATBELT_DESCENT, -} seatbelt_state_t; - -typedef enum { - AUTO_STANDBY, - AUTO_READY, - AUTO_LOITER, - AUTO_MISSION, - AUTO_RTL, - AUTO_TAKEOFF, - AUTO_LAND, -} auto_state_t; - -//typedef enum { -// ARMING_STATE_INIT = 0, -// ARMING_STATE_STANDBY, -// ARMING_STATE_ARMED_GROUND, -// ARMING_STATE_ARMED_AIRBORNE, -// ARMING_STATE_ERROR_GROUND, -// ARMING_STATE_ERROR_AIRBORNE, -// ARMING_STATE_REBOOT, -// ARMING_STATE_IN_AIR_RESTORE -//} arming_state_t; + ARMING_STATE_INIT = 0, + ARMING_STATE_STANDBY, + ARMING_STATE_ARMED, + ARMING_STATE_ARMED_ERROR, + ARMING_STATE_STANDBY_ERROR, + ARMING_STATE_REBOOT, + ARMING_STATE_IN_AIR_RESTORE +} arming_state_t; typedef enum { HIL_STATE_OFF = 0, HIL_STATE_ON } hil_state_t; -enum VEHICLE_MODE_FLAG { - VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, - VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, - VEHICLE_MODE_FLAG_HIL_ENABLED = 32, - VEHICLE_MODE_FLAG_STABILIZED_ENABLED = 16, - VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8, - VEHICLE_MODE_FLAG_AUTO_ENABLED = 4, - VEHICLE_MODE_FLAG_TEST_ENABLED = 2, - VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1 -}; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */ - typedef enum { MODE_SWITCH_MANUAL = 0, MODE_SWITCH_SEATBELT, @@ -135,6 +112,17 @@ typedef enum { MISSION_SWITCH_MISSION } mission_switch_pos_t; +enum VEHICLE_MODE_FLAG { + VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, + VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, + VEHICLE_MODE_FLAG_HIL_ENABLED = 32, + VEHICLE_MODE_FLAG_STABILIZED_ENABLED = 16, + VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8, + VEHICLE_MODE_FLAG_AUTO_ENABLED = 4, + VEHICLE_MODE_FLAG_TEST_ENABLED = 2, + VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1 +}; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */ + /** * Should match 1:1 MAVLink's MAV_TYPE ENUM */ @@ -181,8 +169,8 @@ struct vehicle_status_s uint64_t failsave_lowlevel_start_time; /**< time when the lowlevel failsafe flag was set */ // uint64_t failsave_highlevel_begin; TO BE COMPLETED - system_state_t system_state; /**< current system state */ -// arming_state_t arming_state; /**< current arming state */ + navigation_state_t navigation_state; /**< current system state */ + arming_state_t arming_state; /**< current arming state */ hil_state_t hil_state; /**< current hil state */ int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */ @@ -203,7 +191,9 @@ struct vehicle_status_s bool flag_system_arming_requested; bool flag_system_disarming_requested; bool flag_system_reboot_requested; - bool flag_system_on_ground; + bool flag_system_returned_to_home; + + bool flag_auto_enabled; bool flag_control_manual_enabled; /**< true if manual input is mixed in */ bool flag_control_offboard_enabled; /**< true if offboard control input is on */ From c056410f8439e760905cb50fe08c49c3a0b344e5 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 21 Feb 2013 18:10:34 -0800 Subject: [PATCH 009/486] Checkpoint: Added arming check function --- apps/commander/state_machine_helper.c | 55 +++++++++++++++++++++++++++ apps/commander/state_machine_helper.h | 2 + 2 files changed, 57 insertions(+) diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index e6344f1a84..0611bb52c2 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -254,6 +254,61 @@ void navigation_state_update(int status_pub, struct vehicle_status_s *current_st return; } +int check_arming_state_transition(arming_state_t current_arming_state, arming_state_t new_arming_state) { + + int ret = ERROR; + + /* only check transition if the new state is actually different from the current one */ + if (new_arming_state != current_arming_state) { + + switch (new_arming_state) { + case ARMING_STATE_INIT: + + /* allow going back from INIT for calibration */ + if (current_arming_state == ARMING_STATE_STANDBY) { + ret = OK; + } + break; + case ARMING_STATE_STANDBY: + + /* allow coming from INIT and disarming from ARMED */ + if (current_arming_state == ARMING_STATE_INIT + || current_arming_state == ARMING_STATE_ARMED) { + ret = OK; + } + break; + case ARMING_STATE_ARMED: + + /* allow arming from STANDBY and IN-AIR-RESTORE */ + if (current_arming_state == ARMING_STATE_STANDBY + || current_arming_state == ARMING_STATE_IN_AIR_RESTORE) { + ret = OK; + } + break; + case ARMING_STATE_ARMED_ERROR: + + /* an armed error happens when ARMED obviously */ + if (current_arming_state == ARMING_STATE_ARMED) { + ret = OK; + } + break; + case ARMING_STATE_STANDBY_ERROR: + /* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */ + if (current_arming_state == ARMING_STATE_STANDBY + || current_arming_state == ARMING_STATE_INIT + || current_arming_state == ARMING_STATE_ARMED_ERROR) { + ret = OK; + } + break; + default: + break; + } + } + return ret; +} + + + /* * This functions does not evaluate any input flags but only checks if the transitions * are valid. diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h index 1c0564d077..cf1fa80cd5 100644 --- a/apps/commander/state_machine_helper.h +++ b/apps/commander/state_machine_helper.h @@ -54,4 +54,6 @@ void navigation_state_update(int status_pub, struct vehicle_status_s *current_st void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); int check_navigation_state_transition(navigation_state_t current_navigation_state, navigation_state_t new_navigation_state); +int check_arming_state_transition(arming_state_t current_arming_state, arming_state_t new_arming_state); + #endif /* STATE_MACHINE_HELPER_H_ */ From be7aeb754b85016e2609508d1c059797d5068ec1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 21 Feb 2013 20:01:54 -0800 Subject: [PATCH 010/486] Checkpoint: Added flag checks inside arming state update --- apps/commander/state_machine_helper.c | 35 ++++++++++++++++++--------- apps/commander/state_machine_helper.h | 2 +- 2 files changed, 24 insertions(+), 13 deletions(-) diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 0611bb52c2..e5ef00e934 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -254,49 +254,60 @@ void navigation_state_update(int status_pub, struct vehicle_status_s *current_st return; } -int check_arming_state_transition(arming_state_t current_arming_state, arming_state_t new_arming_state) { +int check_arming_state_transition(struct vehicle_status_s *current_state, arming_state_t new_arming_state, const int mavlink_fd) { int ret = ERROR; /* only check transition if the new state is actually different from the current one */ - if (new_arming_state != current_arming_state) { + if (new_arming_state != current_state->arming_state) { switch (new_arming_state) { case ARMING_STATE_INIT: /* allow going back from INIT for calibration */ - if (current_arming_state == ARMING_STATE_STANDBY) { + if (current_state->arming_state == ARMING_STATE_STANDBY) { ret = OK; } break; case ARMING_STATE_STANDBY: /* allow coming from INIT and disarming from ARMED */ - if (current_arming_state == ARMING_STATE_INIT - || current_arming_state == ARMING_STATE_ARMED) { - ret = OK; + if (current_state->arming_state == ARMING_STATE_INIT + || current_state->arming_state == ARMING_STATE_ARMED) { + + /* sensors need to be initialized for STANDBY state */ + if (current_state->flag_system_sensors_initialized) { + ret = OK; + } else { + mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized"); + } + } break; case ARMING_STATE_ARMED: /* allow arming from STANDBY and IN-AIR-RESTORE */ - if (current_arming_state == ARMING_STATE_STANDBY - || current_arming_state == ARMING_STATE_IN_AIR_RESTORE) { + if (current_state->arming_state == ARMING_STATE_STANDBY + || current_state->arming_state == ARMING_STATE_IN_AIR_RESTORE) { + + /* XXX conditions for arming? */ ret = OK; } break; case ARMING_STATE_ARMED_ERROR: /* an armed error happens when ARMED obviously */ - if (current_arming_state == ARMING_STATE_ARMED) { + if (current_state->arming_state == ARMING_STATE_ARMED) { ret = OK; + + /* XXX conditions for an error state? */ } break; case ARMING_STATE_STANDBY_ERROR: /* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */ - if (current_arming_state == ARMING_STATE_STANDBY - || current_arming_state == ARMING_STATE_INIT - || current_arming_state == ARMING_STATE_ARMED_ERROR) { + if (current_state->arming_state == ARMING_STATE_STANDBY + || current_state->arming_state == ARMING_STATE_INIT + || current_state->arming_state == ARMING_STATE_ARMED_ERROR) { ret = OK; } break; diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h index cf1fa80cd5..f2928db098 100644 --- a/apps/commander/state_machine_helper.h +++ b/apps/commander/state_machine_helper.h @@ -54,6 +54,6 @@ void navigation_state_update(int status_pub, struct vehicle_status_s *current_st void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); int check_navigation_state_transition(navigation_state_t current_navigation_state, navigation_state_t new_navigation_state); -int check_arming_state_transition(arming_state_t current_arming_state, arming_state_t new_arming_state); +int check_arming_state_transition(struct vehicle_status_s current_state, arming_state_t new_arming_state); #endif /* STATE_MACHINE_HELPER_H_ */ From 36f9f8082e1ac676994cab0a6ee2c7a8344b0216 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 22 Feb 2013 09:32:49 -0800 Subject: [PATCH 011/486] Checkpoint: Added flag checks inside navigation state update --- apps/commander/commander.c | 40 +- apps/commander/state_machine_helper.c | 838 ++++++++++++-------------- apps/commander/state_machine_helper.h | 8 +- apps/uORB/topics/vehicle_status.h | 1 + 4 files changed, 426 insertions(+), 461 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 4e2b4907ba..953ba7a1e9 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -804,7 +804,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request to activate HIL */ if ((int)cmd->param1 & VEHICLE_MODE_FLAG_HIL_ENABLED) { - if (OK == do_hil_state_update(status_pub, current_vehicle_status, mavlink_fd, HIL_STATE_ON)) { + if (OK == hil_state_transition(status_pub, current_vehicle_status, mavlink_fd, HIL_STATE_ON)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -812,13 +812,13 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } if ((int)cmd->param1 & VEHICLE_MODE_FLAG_SAFETY_ARMED) { - if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; } } else { - if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -831,14 +831,14 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request to arm */ if ((int)cmd->param1 == 1) { - if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; } /* request to disarm */ } else if ((int)cmd->param1 == 0) { - if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -851,7 +851,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request for an autopilot reboot */ if ((int)cmd->param1 == 1) { - if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_REBOOT)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_REBOOT)) { /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -893,7 +893,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* gyro calibration */ if ((int)(cmd->param1) == 1) { - if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -902,8 +902,8 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta do_gyro_calibration(status_pub, ¤t_status); mavlink_log_info(mavlink_fd, "finished gyro cal"); tune_confirm(); - - do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + // XXX if this fails, go to ERROR + arming_state_transition(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -915,7 +915,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* magnetometer calibration */ if ((int)(cmd->param2) == 1) { - if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -924,8 +924,8 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta do_mag_calibration(status_pub, ¤t_status); mavlink_log_info(mavlink_fd, "finished mag cal"); tune_confirm(); - - do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + // XXX if this fails, go to ERROR + arming_state_transition(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -986,7 +986,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* accel calibration */ if ((int)(cmd->param5) == 1) { - if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -995,8 +995,8 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta do_accel_calibration(status_pub, ¤t_status); mavlink_log_info(mavlink_fd, "finished acc cal"); tune_confirm(); - - do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + // XXX what if this fails + arming_state_transition(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -1647,7 +1647,8 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (current_status.arming_state == ARMING_STATE_INIT) { - do_arming_state_update(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + //XXX what if this fails + arming_state_transition(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); } else { // XXX: Add emergency stuff if sensors are lost } @@ -1857,7 +1858,8 @@ int commander_thread_main(int argc, char *argv[]) } /* Now it's time to handle the stick inputs */ - navigation_state_update(stat_pub, ¤t_status, mavlink_fd); + #warning do that + // navigation_state_update(stat_pub, ¤t_status, mavlink_fd); /* handle the case where RC signal was regained */ if (!current_status.rc_signal_found_once) { @@ -1880,7 +1882,7 @@ int commander_thread_main(int argc, char *argv[]) // ) && if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - do_arming_state_update(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + arming_state_transition(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); stick_off_counter = 0; } else { @@ -1892,7 +1894,7 @@ int commander_thread_main(int argc, char *argv[]) /* check if left stick is in lower right position --> arm */ if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.2f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - do_arming_state_update(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_ARMED); + arming_state_transition(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_ARMED); stick_on_counter = 0; } else { diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index e5ef00e934..6c15bd7251 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -56,206 +56,205 @@ /** * Transition from one sytem state to another */ -void navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - int ret = ERROR; - navigation_state_t new_navigation_state; - - /* do the navigation state update depending on the arming state */ - switch (current_status->arming_state) { - - /* evaluate the navigation state when disarmed */ - case ARMING_STATE_STANDBY: - - /* Always accept manual mode */ - if (current_status->mode_switch == MODE_SWITCH_MANUAL) { - new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; - - /* Accept SEATBELT if there is a local position estimate */ - } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT) { - - if (current_status->flag_local_position_valid) { - new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; - } else { - /* or just fall back to manual */ - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); - new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; - } - - /* Accept AUTO if there is a global position estimate */ - } else if (current_status->mode_switch == MODE_SWITCH_AUTO) { - if (current_status->flag_local_position_valid) { - new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; - } else { - mavlink_log_critical(mavlink_fd, "Rej. AUTO: no global position"); - - /* otherwise fallback to seatbelt or even manual */ - if (current_status->flag_local_position_valid) { - new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; - } else { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); - new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; - } - } - } - - break; - - /* evaluate the navigation state when armed */ - case ARMING_STATE_ARMED: - - /* Always accept manual mode */ - if (current_status->mode_switch == MODE_SWITCH_MANUAL) { - new_navigation_state = NAVIGATION_STATE_MANUAL; - - /* Accept SEATBELT if there is a local position estimate */ - } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT - && current_status->return_switch == RETURN_SWITCH_NONE) { - - if (current_status->flag_local_position_valid) { - new_navigation_state = NAVIGATION_STATE_SEATBELT; - } else { - /* or just fall back to manual */ - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); - new_navigation_state = NAVIGATION_STATE_MANUAL; - } - - /* Accept SEATBELT_DESCENT if there is a local position estimate and the return switch is on */ - } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT - && current_status->return_switch == RETURN_SWITCH_RETURN) { - - if (current_status->flag_local_position_valid) { - new_navigation_state = NAVIGATION_STATE_SEATBELT_DESCENT; - } else { - /* descent not possible without baro information, fall back to manual */ - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no local position"); - new_navigation_state = NAVIGATION_STATE_MANUAL; - } - - /* Accept LOITER if there is a global position estimate */ - } else if (current_status->mode_switch == MODE_SWITCH_AUTO - && current_status->return_switch == RETURN_SWITCH_NONE - && current_status->mission_switch == MISSION_SWITCH_NONE) { - - if (current_status->flag_global_position_valid) { - new_navigation_state = NAVIGATION_STATE_AUTO_LOITER; - } else { - mavlink_log_critical(mavlink_fd, "Rej. LOITER: no global position"); - - /* otherwise fallback to SEATBELT or even manual */ - if (current_status->flag_local_position_valid) { - new_navigation_state = NAVIGATION_STATE_SEATBELT; - } else { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); - new_navigation_state = NAVIGATION_STATE_MANUAL; - } - } - - /* Accept MISSION if there is a global position estimate and home position */ - } else if (current_status->mode_switch == MODE_SWITCH_AUTO - && current_status->return_switch == RETURN_SWITCH_NONE - && current_status->mission_switch == MISSION_SWITCH_MISSION) { - - if (current_status->flag_global_position_valid && current_status->flag_valid_home_position) { - new_navigation_state = NAVIGATION_STATE_AUTO_MISSION; - } else { - - /* spit out what exactly is unavailable */ - if (current_status->flag_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. MISSION: no home position"); - } else if (current_status->flag_valid_home_position) { - mavlink_log_critical(mavlink_fd, "Rej. MISSION: no global position"); - } else { - mavlink_log_critical(mavlink_fd, "Rej. MISSION: no global position and no home position"); - } - - /* otherwise fallback to SEATBELT or even manual */ - if (current_status->flag_local_position_valid) { - new_navigation_state = NAVIGATION_STATE_SEATBELT; - } else { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); - new_navigation_state = NAVIGATION_STATE_MANUAL; - } - } - - /* Go to RTL or land if global position estimate and home position is available */ - } else if (current_status->mode_switch == MODE_SWITCH_AUTO - && current_status->return_switch == RETURN_SWITCH_RETURN - && (current_status->mission_switch == MISSION_SWITCH_NONE || current_status->mission_switch == MISSION_SWITCH_MISSION)) { - - if (current_status->flag_global_position_valid && current_status->flag_valid_home_position) { - - /* after RTL go to LAND */ - if (current_status->flag_system_returned_to_home) { - new_navigation_state = NAVIGATION_STATE_AUTO_LAND; - } else { - new_navigation_state = NAVIGATION_STATE_AUTO_RTL; - } - - } else { - - /* spit out what exactly is unavailable */ - if (current_status->flag_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no home position"); - } else if (current_status->flag_valid_home_position) { - mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no global position"); - } else { - mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no global position and no home position"); - } - - /* otherwise fallback to SEATBELT_DESCENT or even MANUAL */ - if (current_status->flag_local_position_valid) { - new_navigation_state = NAVIGATION_STATE_SEATBELT_DESCENT; - } else { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); - new_navigation_state = NAVIGATION_STATE_MANUAL; - } - } - } - break; - - case ARMING_STATE_ARMED_ERROR: - - // TODO work out fail-safe scenarios - break; - - case ARMING_STATE_STANDBY_ERROR: - - // TODO work out fail-safe scenarios - break; - - case ARMING_STATE_REBOOT: - - // XXX I don't think we should end up here - break; - - case ARMING_STATE_IN_AIR_RESTORE: - - // XXX not sure what to do here - break; - default: - break; - } - - - - /* Update the system state in case it has changed */ - if (current_status->navigation_state != new_navigation_state) { - - /* Check if the transition is valid */ - if (check_navigation_state_transition(current_status->navigation_state, new_navigation_state) == OK) { - current_status->navigation_state = new_navigation_state; - state_machine_publish(status_pub, current_status, mavlink_fd); - } else { - mavlink_log_critical(mavlink_fd, "System state transition not valid"); - } - } - - return; -} - -int check_arming_state_transition(struct vehicle_status_s *current_state, arming_state_t new_arming_state, const int mavlink_fd) { +//void navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +//{ +// int ret = ERROR; +// navigation_state_t new_navigation_state; +// +// /* do the navigation state update depending on the arming state */ +// switch (current_status->arming_state) { +// +// /* evaluate the navigation state when disarmed */ +// case ARMING_STATE_STANDBY: +// +// /* Always accept manual mode */ +// if (current_status->mode_switch == MODE_SWITCH_MANUAL) { +// new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; +// +// /* Accept SEATBELT if there is a local position estimate */ +// } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT) { +// +// if (current_status->flag_local_position_valid) { +// new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; +// } else { +// /* or just fall back to manual */ +// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); +// new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; +// } +// +// /* Accept AUTO if there is a global position estimate */ +// } else if (current_status->mode_switch == MODE_SWITCH_AUTO) { +// if (current_status->flag_local_position_valid) { +// new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; +// } else { +// mavlink_log_critical(mavlink_fd, "Rej. AUTO: no global position"); +// +// /* otherwise fallback to seatbelt or even manual */ +// if (current_status->flag_local_position_valid) { +// new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; +// } else { +// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); +// new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; +// } +// } +// } +// +// break; +// +// /* evaluate the navigation state when armed */ +// case ARMING_STATE_ARMED: +// +// /* Always accept manual mode */ +// if (current_status->mode_switch == MODE_SWITCH_MANUAL) { +// new_navigation_state = NAVIGATION_STATE_MANUAL; +// +// /* Accept SEATBELT if there is a local position estimate */ +// } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT +// && current_status->return_switch == RETURN_SWITCH_NONE) { +// +// if (current_status->flag_local_position_valid) { +// new_navigation_state = NAVIGATION_STATE_SEATBELT; +// } else { +// /* or just fall back to manual */ +// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); +// new_navigation_state = NAVIGATION_STATE_MANUAL; +// } +// +// /* Accept SEATBELT_DESCENT if there is a local position estimate and the return switch is on */ +// } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT +// && current_status->return_switch == RETURN_SWITCH_RETURN) { +// +// if (current_status->flag_local_position_valid) { +// new_navigation_state = NAVIGATION_STATE_SEATBELT_DESCENT; +// } else { +// /* descent not possible without baro information, fall back to manual */ +// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no local position"); +// new_navigation_state = NAVIGATION_STATE_MANUAL; +// } +// +// /* Accept LOITER if there is a global position estimate */ +// } else if (current_status->mode_switch == MODE_SWITCH_AUTO +// && current_status->return_switch == RETURN_SWITCH_NONE +// && current_status->mission_switch == MISSION_SWITCH_NONE) { +// +// if (current_status->flag_global_position_valid) { +// new_navigation_state = NAVIGATION_STATE_AUTO_LOITER; +// } else { +// mavlink_log_critical(mavlink_fd, "Rej. LOITER: no global position"); +// +// /* otherwise fallback to SEATBELT or even manual */ +// if (current_status->flag_local_position_valid) { +// new_navigation_state = NAVIGATION_STATE_SEATBELT; +// } else { +// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); +// new_navigation_state = NAVIGATION_STATE_MANUAL; +// } +// } +// +// /* Accept MISSION if there is a global position estimate and home position */ +// } else if (current_status->mode_switch == MODE_SWITCH_AUTO +// && current_status->return_switch == RETURN_SWITCH_NONE +// && current_status->mission_switch == MISSION_SWITCH_MISSION) { +// +// if (current_status->flag_global_position_valid && current_status->flag_valid_home_position) { +// new_navigation_state = NAVIGATION_STATE_AUTO_MISSION; +// } else { +// +// /* spit out what exactly is unavailable */ +// if (current_status->flag_global_position_valid) { +// mavlink_log_critical(mavlink_fd, "Rej. MISSION: no home position"); +// } else if (current_status->flag_valid_home_position) { +// mavlink_log_critical(mavlink_fd, "Rej. MISSION: no global position"); +// } else { +// mavlink_log_critical(mavlink_fd, "Rej. MISSION: no global position and no home position"); +// } +// +// /* otherwise fallback to SEATBELT or even manual */ +// if (current_status->flag_local_position_valid) { +// new_navigation_state = NAVIGATION_STATE_SEATBELT; +// } else { +// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); +// new_navigation_state = NAVIGATION_STATE_MANUAL; +// } +// } +// +// /* Go to RTL or land if global position estimate and home position is available */ +// } else if (current_status->mode_switch == MODE_SWITCH_AUTO +// && current_status->return_switch == RETURN_SWITCH_RETURN +// && (current_status->mission_switch == MISSION_SWITCH_NONE || current_status->mission_switch == MISSION_SWITCH_MISSION)) { +// +// if (current_status->flag_global_position_valid && current_status->flag_valid_home_position) { +// +// /* after RTL go to LAND */ +// if (current_status->flag_system_returned_to_home) { +// new_navigation_state = NAVIGATION_STATE_AUTO_LAND; +// } else { +// new_navigation_state = NAVIGATION_STATE_AUTO_RTL; +// } +// +// } else { +// +// /* spit out what exactly is unavailable */ +// if (current_status->flag_global_position_valid) { +// mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no home position"); +// } else if (current_status->flag_valid_home_position) { +// mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no global position"); +// } else { +// mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no global position and no home position"); +// } +// +// /* otherwise fallback to SEATBELT_DESCENT or even MANUAL */ +// if (current_status->flag_local_position_valid) { +// new_navigation_state = NAVIGATION_STATE_SEATBELT_DESCENT; +// } else { +// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); +// new_navigation_state = NAVIGATION_STATE_MANUAL; +// } +// } +// } +// break; +// +// case ARMING_STATE_ARMED_ERROR: +// +// // TODO work out fail-safe scenarios +// break; +// +// case ARMING_STATE_STANDBY_ERROR: +// +// // TODO work out fail-safe scenarios +// break; +// +// case ARMING_STATE_REBOOT: +// +// // XXX I don't think we should end up here +// break; +// +// case ARMING_STATE_IN_AIR_RESTORE: +// +// // XXX not sure what to do here +// break; +// default: +// break; +// } +// +// +// +// /* Update the system state in case it has changed */ +// if (current_status->navigation_state != new_navigation_state) { +// +// /* Check if the transition is valid */ +// if (check_navigation_state_transition(current_status->navigation_state, new_navigation_state) == OK) { +// current_status->navigation_state = new_navigation_state; +// state_machine_publish(status_pub, current_status, mavlink_fd); +// } else { +// mavlink_log_critical(mavlink_fd, "System state transition not valid"); +// } +// } +// +// return; +//} +int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, const int mavlink_fd) { int ret = ERROR; /* only check transition if the new state is actually different from the current one */ @@ -324,20 +323,20 @@ int check_arming_state_transition(struct vehicle_status_s *current_state, arming * This functions does not evaluate any input flags but only checks if the transitions * are valid. */ -int check_navigation_state_transition(navigation_state_t current_navigation_state, navigation_state_t new_navigation_state) { +int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, const int mavlink_fd) { int ret = ERROR; /* only check transition if the new state is actually different from the current one */ - if (new_navigation_state != current_navigation_state) { + if (new_navigation_state != current_state->navigation_state) { switch (new_navigation_state) { case NAVIGATION_STATE_INIT: /* transitions back to INIT are possible for calibration */ - if (current_navigation_state == NAVIGATION_STATE_MANUAL_STANDBY - || current_navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY - || current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY) { + if (current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY) { ret = OK; } @@ -346,93 +345,139 @@ int check_navigation_state_transition(navigation_state_t current_navigation_stat case NAVIGATION_STATE_MANUAL_STANDBY: /* transitions from INIT and other STANDBY states as well as MANUAL are possible */ - if (current_navigation_state == NAVIGATION_STATE_INIT - || current_navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY - || current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || current_navigation_state == NAVIGATION_STATE_MANUAL) { + if (current_state->navigation_state == NAVIGATION_STATE_INIT + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { - ret = OK; + /* need to be disarmed first */ + if (current_state->arming_state != ARMING_STATE_STANDBY) { + mavlink_log_critical(mavlink_fd, "Rej. MANUAL_STANDBY: not disarmed"); + } else { + ret = OK; + } } break; case NAVIGATION_STATE_MANUAL: - /* all transitions possible */ - ret = OK; + /* need to be armed first */ + if (current_state->arming_state != ARMING_STATE_ARMED) { + mavlink_log_critical(mavlink_fd, "Rej. MANUAL: not armed"); + } else { + ret = OK; + } break; case NAVIGATION_STATE_SEATBELT_STANDBY: /* transitions from INIT and other STANDBY states as well as SEATBELT and SEATBELT_DESCENT are possible */ - if (current_navigation_state == NAVIGATION_STATE_INIT - || current_navigation_state == NAVIGATION_STATE_MANUAL_STANDBY - || current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || current_navigation_state == NAVIGATION_STATE_SEATBELT - || current_navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT) { + if (current_state->navigation_state == NAVIGATION_STATE_INIT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT) { - ret = OK; + /* need to be disarmed and have a position estimate */ + if (current_state->arming_state != ARMING_STATE_STANDBY) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: not disarmed"); + } else if (!current_state->flag_local_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: no position estimate"); + } else { + ret = OK; + } } break; case NAVIGATION_STATE_SEATBELT: /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/ - if (current_navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY - || current_navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT - || current_navigation_state == NAVIGATION_STATE_MANUAL - || current_navigation_state == NAVIGATION_STATE_AUTO_LAND - || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_navigation_state == NAVIGATION_STATE_AUTO_READY - || current_navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + if (current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - ret = OK; + /* need to be armed and have a position estimate */ + if (current_state->arming_state != ARMING_STATE_ARMED) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: not armed"); + } else if (!current_state->flag_local_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate"); + } else { + ret = OK; + } } break; case NAVIGATION_STATE_SEATBELT_DESCENT: /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT and SEATBELT_STANDBY */ - if (current_navigation_state == NAVIGATION_STATE_SEATBELT - || current_navigation_state == NAVIGATION_STATE_MANUAL - || current_navigation_state == NAVIGATION_STATE_AUTO_LAND - || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_navigation_state == NAVIGATION_STATE_AUTO_READY - || current_navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + if (current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - ret = OK; + /* need to be armed and have a position estimate */ + if (current_state->arming_state != ARMING_STATE_ARMED) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: not armed"); + } else if (!current_state->flag_local_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no pos estimate"); + } else { + ret = OK; + } } break; case NAVIGATION_STATE_AUTO_STANDBY: /* transitions from INIT or from other STANDBY modes or from AUTO READY */ - if (current_navigation_state == NAVIGATION_STATE_INIT - || current_navigation_state == NAVIGATION_STATE_MANUAL_STANDBY - || current_navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY - || current_navigation_state == NAVIGATION_STATE_AUTO_READY) { + if (current_state->navigation_state == NAVIGATION_STATE_INIT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) { - ret = OK; + /* need to be disarmed and have a position and home lock */ + if (current_state->arming_state != ARMING_STATE_STANDBY) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: not disarmed"); + } else if (!current_state->flag_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no pos lock"); + } else if (!current_state->flag_valid_home_position) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no home pos"); + } else { + ret = OK; + } } break; case NAVIGATION_STATE_AUTO_READY: /* transitions from AUTO_STANDBY or AUTO_LAND */ - if (current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || current_navigation_state == NAVIGATION_STATE_AUTO_LAND) { + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND) { - ret = OK; + // XXX flag test needed? + + /* need to be armed and have a position and home lock */ + if (current_state->arming_state != ARMING_STATE_ARMED) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_READY: not armed"); + } else { + ret = OK; + } } break; case NAVIGATION_STATE_AUTO_TAKEOFF: /* only transitions from AUTO_READY */ - if (current_navigation_state == NAVIGATION_STATE_AUTO_READY) { + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) { ret = OK; } @@ -441,49 +486,75 @@ int check_navigation_state_transition(navigation_state_t current_navigation_stat case NAVIGATION_STATE_AUTO_LOITER: /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ - if (current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF - || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_navigation_state == NAVIGATION_STATE_SEATBELT - || current_navigation_state == NAVIGATION_STATE_MANUAL) { + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { - ret = OK; + /* need to have a position and home lock */ + if (!current_state->flag_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no pos lock"); + } else if (!current_state->flag_valid_home_position) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no home pos"); + } else { + ret = OK; + } } break; case NAVIGATION_STATE_AUTO_MISSION: /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ - if (current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF - || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_navigation_state == NAVIGATION_STATE_SEATBELT - || current_navigation_state == NAVIGATION_STATE_MANUAL) { + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { - ret = OK; + /* need to have a mission ready */ + if (!current_state->flag_auto_mission_available) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_MISSION: no mission available"); + } else { + ret = OK; + } } break; case NAVIGATION_STATE_AUTO_RTL: /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ - if (current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF - || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_navigation_state == NAVIGATION_STATE_SEATBELT - || current_navigation_state == NAVIGATION_STATE_MANUAL) { + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { - ret = OK; + /* need to have a position and home lock */ + if (!current_state->flag_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no pos lock"); + } else if (!current_state->flag_valid_home_position) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no home pos"); + } else { + ret = OK; + } } break; case NAVIGATION_STATE_AUTO_LAND: /* after AUTO_RTL or when in AUTO_LOITER or AUTO_MISSION */ - if (current_navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER) { + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER) { - ret = OK; + /* need to have a position and home lock */ + if (!current_state->flag_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no pos lock"); + } else if (!current_state->flag_valid_home_position) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no home pos"); + } else { + ret = OK; + } } break; @@ -492,182 +563,71 @@ int check_navigation_state_transition(navigation_state_t current_navigation_stat } } + if (ret == OK) { + current_state->navigation_state = new_navigation_state; + state_machine_publish(status_pub, current_state, mavlink_fd); + } + return ret; } -///** -// * Transition from one arming state to another -// */ -//int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state) -//{ -// bool valid_transition = false; -// int ret = ERROR; -// -// warnx("Current state: %d, requested state: %d", current_status->arming_state, new_state); -// -// if (current_status->arming_state == new_state) { -// warnx("Arming state not changed"); -// valid_transition = true; -// -// } else { -// -// switch (new_state) { -// -// case ARMING_STATE_INIT: -// -// if (current_status->arming_state == ARMING_STATE_STANDBY) { -// -// mavlink_log_critical(mavlink_fd, "Switched to INIT ARMING STATE"); -// valid_transition = true; -// } -// break; -// -// case ARMING_STATE_STANDBY: -// -// // TODO check for sensors -// // XXX check if coming from reboot? -// if (current_status->arming_state == ARMING_STATE_INIT) { -// -// if (current_status->flag_system_sensors_initialized) { -// current_status->flag_system_armed = false; -// mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state"); -// valid_transition = true; -// } else { -// mavlink_log_critical(mavlink_fd, "REJ. STANDBY arming state, sensors not init."); -// } -// -// } else if (current_status->arming_state == ARMING_STATE_ARMED) { -// -// current_status->flag_system_armed = false; -// mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state"); -// valid_transition = true; -// } -// break; -// -// case ARMING_STATE_ARMED: -// -// if (current_status->arming_state == ARMING_STATE_STANDBY -// || current_status->arming_state == ARMING_STATE_IN_AIR_RESTORE) { -// current_status->flag_system_armed = true; -// mavlink_log_critical(mavlink_fd, "Switched to ARMED arming state"); -// valid_transition = true; -// } -// break; -// -// case ARMING_STATE_ABORT: -// -// if (current_status->arming_state == ARMING_STATE_ARMED) { -// -// mavlink_log_critical(mavlink_fd, "Switched to MISSION ABORT arming state"); -// valid_transition = true; -// } -// break; -// -// case ARMING_STATE_ERROR: -// -// if (current_status->arming_state == ARMING_STATE_ARMED -// || current_status->arming_state == ARMING_STATE_ABORT -// || current_status->arming_state == ARMING_STATE_INIT) { -// -// mavlink_log_critical(mavlink_fd, "Switched to ERROR arming state"); -// valid_transition = true; -// } -// break; -// -// case ARMING_STATE_REBOOT: -// -// if (current_status->arming_state == ARMING_STATE_STANDBY -// || current_status->arming_state == ARMING_STATE_ERROR) { -// -// valid_transition = true; -// mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); -// usleep(500000); -// up_systemreset(); -// /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ -// } -// break; -// -// case ARMING_STATE_IN_AIR_RESTORE: -// -// if (current_status->arming_state == ARMING_STATE_INIT) { -// mavlink_log_critical(mavlink_fd, "Switched to IN-AIR-RESTORE arming state"); -// valid_transition = true; -// } -// break; -// default: -// warnx("Unknown arming state"); -// break; -// } -// } -// -// if (valid_transition) { -// current_status->arming_state = new_state; -// state_machine_publish(status_pub, current_status, mavlink_fd); -//// publish_armed_status(current_status); -// ret = OK; -// } else { -// mavlink_log_critical(mavlink_fd, "REJECTING invalid arming state transition"); -// } -// -// return ret; -//} -///** -// * Transition from one hil state to another -// */ -//int do_hil_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state) -//{ -// bool valid_transition = false; -// int ret = ERROR; -// -// warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state); -// -// if (current_status->hil_state == new_state) { -// warnx("Hil state not changed"); -// valid_transition = true; -// -// } else { -// -// switch (new_state) { -// -// case HIL_STATE_OFF: -// -// if (current_status->arming_state == ARMING_STATE_INIT -// || current_status->arming_state == ARMING_STATE_STANDBY) { -// -// current_status->flag_hil_enabled = false; -// mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); -// valid_transition = true; -// } -// break; -// -// case HIL_STATE_ON: -// -// if (current_status->arming_state == ARMING_STATE_INIT -// || current_status->arming_state == ARMING_STATE_STANDBY) { -// -// current_status->flag_hil_enabled = true; -// mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); -// valid_transition = true; -// } -// break; -// -// default: -// warnx("Unknown hil state"); -// break; -// } -// } -// -// if (valid_transition) { -// current_status->hil_state = new_state; -// state_machine_publish(status_pub, current_status, mavlink_fd); -// ret = OK; -// } else { -// mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); -// } -// -// return ret; -//} +/** +* Transition from one hil state to another +*/ +int hil_state_transition(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state) +{ + bool valid_transition = false; + int ret = ERROR; + + warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state); + + if (current_status->hil_state == new_state) { + warnx("Hil state not changed"); + valid_transition = true; + + } else { + + switch (new_state) { + + case HIL_STATE_OFF: + + if (current_status->arming_state == ARMING_STATE_INIT + || current_status->arming_state == ARMING_STATE_STANDBY) { + + current_status->flag_hil_enabled = false; + mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); + valid_transition = true; + } + break; + + case HIL_STATE_ON: + + if (current_status->arming_state == ARMING_STATE_INIT + || current_status->arming_state == ARMING_STATE_STANDBY) { + + current_status->flag_hil_enabled = true; + mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); + valid_transition = true; + } + break; + + default: + warnx("Unknown hil state"); + break; + } + } + + if (valid_transition) { + current_status->hil_state = new_state; + state_machine_publish(status_pub, current_status, mavlink_fd); + ret = OK; + } else { + mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); + } + + return ret; +} diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h index f2928db098..5b57cffb73 100644 --- a/apps/commander/state_machine_helper.h +++ b/apps/commander/state_machine_helper.h @@ -53,7 +53,9 @@ void navigation_state_update(int status_pub, struct vehicle_status_s *current_st void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); -int check_navigation_state_transition(navigation_state_t current_navigation_state, navigation_state_t new_navigation_state); -int check_arming_state_transition(struct vehicle_status_s current_state, arming_state_t new_arming_state); +int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, const int mavlink_fd); +int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, const int mavlink_fd); -#endif /* STATE_MACHINE_HELPER_H_ */ +int hil_state_transition(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state); + +#endif /* STATE_MACHINE_HELPER_H_ */ \ No newline at end of file diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index 29baff34bd..4955422441 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -193,6 +193,7 @@ struct vehicle_status_s bool flag_system_reboot_requested; bool flag_system_returned_to_home; + bool flag_auto_mission_available; bool flag_auto_enabled; bool flag_control_manual_enabled; /**< true if manual input is mixed in */ From cbfa64b59eef8362494d0753ce3567e804f2d682 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 22 Feb 2013 11:54:41 -0800 Subject: [PATCH 012/486] Checkpoint: Added switch handling --- apps/commander/commander.c | 162 ++++++++++++++++++++++++++++++++++++- 1 file changed, 160 insertions(+), 2 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 953ba7a1e9..38d2ffc962 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1725,7 +1725,7 @@ int commander_thread_main(int argc, char *argv[]) state_changed = true; } - if (orb_check(ORB_ID(vehicle_gps_position), &new_data)) { + if (orb_check(gps_sub, &new_data)) { orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); @@ -1858,7 +1858,165 @@ int commander_thread_main(int argc, char *argv[]) } /* Now it's time to handle the stick inputs */ - #warning do that + + switch (current_status.arming_state) { + + /* evaluate the navigation state when disarmed */ + case ARMING_STATE_STANDBY: + + /* just manual, XXX this might depend on the return switch */ + if (current_status.mode_switch == MODE_SWITCH_MANUAL) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); + } + + /* Try seatbelt or fallback to manual */ + } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); + } + } + + /* Try auto or fallback to seatbelt or even manual */ + } else if (current_status.mode_switch == MODE_SWITCH_AUTO) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_STANDBY, mavlink_fd) != OK) { + // first fallback to SEATBELT_STANDY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) { + // or fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); + } + } + } + } + + break; + + /* evaluate the navigation state when armed */ + case ARMING_STATE_ARMED: + + /* Always accept manual mode */ + if (current_status.mode_switch == MODE_SWITCH_MANUAL) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + + /* SEATBELT_STANDBY (fallback: MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT + && current_status.return_switch == RETURN_SWITCH_NONE) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + + /* SEATBELT_DESCENT (fallback: MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT + && current_status.return_switch == RETURN_SWITCH_RETURN) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + + /* AUTO_LOITER (fallback: SEATBELT, MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_AUTO + && current_status.return_switch == RETURN_SWITCH_NONE + && current_status.mission_switch == MISSION_SWITCH_NONE) { + + /* we might come from the disarmed state AUTO_STANDBY */ + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_READY, mavlink_fd) != OK) { + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + /* or from some other armed state like SEATBELT or MANUAL */ + } else if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_LOITER, mavlink_fd) != OK) { + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } + + /* AUTO_MISSION (fallback: SEATBELT, MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_AUTO + && current_status.return_switch == RETURN_SWITCH_NONE + && current_status.mission_switch == MISSION_SWITCH_MISSION) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_MISSION, mavlink_fd) != OK) { + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } + + /* AUTO_RTL (fallback: SEATBELT_DESCENT, MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_AUTO + && current_status.return_switch == RETURN_SWITCH_RETURN + && (current_status.mission_switch == MISSION_SWITCH_NONE || current_status.mission_switch == MISSION_SWITCH_MISSION)) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_RTL, mavlink_fd) != OK) { + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } + } + break; + + // XXX we might be missing something that triggers a transition from RTL to LAND + + case ARMING_STATE_ARMED_ERROR: + + // TODO work out fail-safe scenarios + break; + + case ARMING_STATE_STANDBY_ERROR: + + // TODO work out fail-safe scenarios + break; + + case ARMING_STATE_REBOOT: + + // XXX I don't think we should end up here + break; + + case ARMING_STATE_IN_AIR_RESTORE: + + // XXX not sure what to do here + break; + default: + break; + } + // navigation_state_update(stat_pub, ¤t_status, mavlink_fd); /* handle the case where RC signal was regained */ From 793b482e00013ea66bb1b0cdc0366bb720648938 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 22 Feb 2013 15:52:13 -0800 Subject: [PATCH 013/486] Checkpoint: Navigation states and arming seem to work --- apps/commander/commander.c | 43 ++++++++++++++------------- apps/commander/state_machine_helper.c | 25 ++++++++++++---- apps/sensors/sensors.cpp | 2 +- 3 files changed, 43 insertions(+), 27 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 38d2ffc962..2784e77dbe 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -812,13 +812,13 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } if ((int)cmd->param1 & VEHICLE_MODE_FLAG_SAFETY_ARMED) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; } } else { - if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -831,14 +831,14 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request to arm */ if ((int)cmd->param1 == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; } /* request to disarm */ } else if ((int)cmd->param1 == 0) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -851,7 +851,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request for an autopilot reboot */ if ((int)cmd->param1 == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_REBOOT)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_REBOOT, mavlink_fd)) { /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -893,7 +893,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* gyro calibration */ if ((int)(cmd->param1) == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -903,7 +903,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta mavlink_log_info(mavlink_fd, "finished gyro cal"); tune_confirm(); // XXX if this fails, go to ERROR - arming_state_transition(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -915,7 +915,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* magnetometer calibration */ if ((int)(cmd->param2) == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -925,7 +925,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta mavlink_log_info(mavlink_fd, "finished mag cal"); tune_confirm(); // XXX if this fails, go to ERROR - arming_state_transition(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -986,7 +986,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* accel calibration */ if ((int)(cmd->param5) == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -996,7 +996,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta mavlink_log_info(mavlink_fd, "finished acc cal"); tune_confirm(); // XXX what if this fails - arming_state_transition(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -1647,8 +1647,9 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (current_status.arming_state == ARMING_STATE_INIT) { - //XXX what if this fails - arming_state_transition(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + // XXX fix for now + current_status.flag_system_sensors_initialized = true; + arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); } else { // XXX: Add emergency stuff if sensors are lost } @@ -1790,11 +1791,10 @@ int commander_thread_main(int argc, char *argv[]) * Check if manual control modes have to be switched */ if (!isfinite(sp_man.mode_switch)) { - warnx("mode sw not finite"); + warnx("mode sw not finite"); /* no valid stick position, go to default */ - } else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) { /* bottom stick position, go to manual mode */ @@ -1806,6 +1806,7 @@ int commander_thread_main(int argc, char *argv[]) current_status.mode_switch = MODE_SWITCH_AUTO; } else { + /* center stick position, set seatbelt/simple control */ current_status.mode_switch = MODE_SWITCH_SEATBELT; } @@ -1869,7 +1870,7 @@ int commander_thread_main(int argc, char *argv[]) if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); + warnx("ERROR: Navigation state MANUAL_STANDBY rejected 1"); } /* Try seatbelt or fallback to manual */ @@ -1879,7 +1880,7 @@ int commander_thread_main(int argc, char *argv[]) // fallback to MANUAL_STANDBY if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); + warnx("ERROR: Navigation state MANUAL_STANDBY rejected 2"); } } @@ -1892,7 +1893,7 @@ int commander_thread_main(int argc, char *argv[]) // or fallback to MANUAL_STANDBY if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); + warnx("ERROR: Navigation state MANUAL_STANDBY rejected 3"); } } } @@ -2040,7 +2041,8 @@ int commander_thread_main(int argc, char *argv[]) // ) && if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - arming_state_transition(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + printf("Try disarm\n"); + arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); stick_off_counter = 0; } else { @@ -2052,7 +2054,8 @@ int commander_thread_main(int argc, char *argv[]) /* check if left stick is in lower right position --> arm */ if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.2f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - arming_state_transition(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_ARMED); + printf("Try arm\n"); + arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_ARMED, mavlink_fd); stick_on_counter = 0; } else { diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 6c15bd7251..ae7f2a1c17 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -255,10 +255,13 @@ //} int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, const int mavlink_fd) { + int ret = ERROR; /* only check transition if the new state is actually different from the current one */ - if (new_arming_state != current_state->arming_state) { + if (new_arming_state == current_state->arming_state) { + ret = OK; + } else { switch (new_arming_state) { case ARMING_STATE_INIT: @@ -313,7 +316,13 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta default: break; } + + if (ret == OK) { + current_state->arming_state = new_arming_state; + state_machine_publish(status_pub, current_state, mavlink_fd); + } } + return ret; } @@ -328,7 +337,9 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current int ret = ERROR; /* only check transition if the new state is actually different from the current one */ - if (new_navigation_state != current_state->navigation_state) { + if (new_navigation_state == current_state->navigation_state) { + ret = OK; + } else { switch (new_navigation_state) { case NAVIGATION_STATE_INIT: @@ -561,12 +572,14 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current default: break; } + + if (ret == OK) { + current_state->navigation_state = new_navigation_state; + state_machine_publish(status_pub, current_state, mavlink_fd); + } } - if (ret == OK) { - current_state->navigation_state = new_navigation_state; - state_machine_publish(status_pub, current_state, mavlink_fd); - } + return ret; } diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index ce404ee7e3..b53de8c467 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -432,7 +432,7 @@ Sensors::Sensors() : _parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE"); /* mandatory mode switches, mapped to channel 5 and 6 per default */ - _parameter_handles.rc_map_mode_sw = param_find("RC_MAP_OVER_SW"); + _parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW"); _parameter_handles.rc_map_return_sw = param_find("RC_MAP_RETURN_SW"); _parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS"); From 0eca49a4f6d4a06868770c8b0c36094d889cb846 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 22 Feb 2013 19:46:47 -0800 Subject: [PATCH 014/486] Checkpoint: Separated all bools in vehicle status into conditions and flags, they should be protected --- apps/commander/commander.c | 107 +++++++++--------- apps/commander/state_machine_helper.c | 26 ++--- apps/drivers/blinkm/blinkm.cpp | 2 +- apps/drivers/px4io/px4io.cpp | 13 ++- apps/mavlink/mavlink.c | 4 +- apps/mavlink_onboard/mavlink.c | 2 +- .../multirotor_att_control_main.c | 7 +- apps/uORB/topics/vehicle_status.h | 43 +++---- 8 files changed, 102 insertions(+), 102 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 94e344da12..2fdcf4ce3f 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -323,8 +323,8 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) { /* set to mag calibration mode */ - status->flag_preflight_mag_calibration = true; - state_machine_publish(status_pub, status, mavlink_fd); + // status->flag_preflight_mag_calibration = true; + // state_machine_publish(status_pub, status, mavlink_fd); int sub_mag = orb_subscribe(ORB_ID(sensor_mag)); struct mag_report mag; @@ -559,8 +559,8 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) } /* disable calibration mode */ - status->flag_preflight_mag_calibration = false; - state_machine_publish(status_pub, status, mavlink_fd); + // status->flag_preflight_mag_calibration = false; + // state_machine_publish(status_pub, status, mavlink_fd); close(sub_mag); } @@ -568,8 +568,8 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) { /* set to gyro calibration mode */ - status->flag_preflight_gyro_calibration = true; - state_machine_publish(status_pub, status, mavlink_fd); + // status->flag_preflight_gyro_calibration = true; + // state_machine_publish(status_pub, status, mavlink_fd); const int calibration_count = 5000; @@ -621,8 +621,8 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) gyro_offset[2] = gyro_offset[2] / calibration_count; /* exit gyro calibration mode */ - status->flag_preflight_gyro_calibration = false; - state_machine_publish(status_pub, status, mavlink_fd); + // status->flag_preflight_gyro_calibration = false; + // state_machine_publish(status_pub, status, mavlink_fd); if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) { @@ -679,8 +679,8 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) mavlink_log_info(mavlink_fd, "keep it level and still"); /* set to accel calibration mode */ - status->flag_preflight_accel_calibration = true; - state_machine_publish(status_pub, status, mavlink_fd); + // status->flag_preflight_accel_calibration = true; + // state_machine_publish(status_pub, status, mavlink_fd); const int calibration_count = 2500; @@ -787,8 +787,8 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) } /* exit accel calibration mode */ - status->flag_preflight_accel_calibration = false; - state_machine_publish(status_pub, status, mavlink_fd); + // status->flag_preflight_accel_calibration = false; + // state_machine_publish(status_pub, status, mavlink_fd); close(sub_sensor_combined); } @@ -1022,7 +1022,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta break; case VEHICLE_CMD_PREFLIGHT_STORAGE: { - if (current_status.flag_system_armed && + if (current_status.flag_fmu_armed && ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || (current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) { @@ -1327,7 +1327,8 @@ int commander_thread_main(int argc, char *argv[]) current_status.navigation_state = NAVIGATION_STATE_INIT; current_status.arming_state = ARMING_STATE_INIT; current_status.hil_state = HIL_STATE_OFF; - current_status.flag_system_armed = false; + current_status.flag_fmu_armed = false; + current_status.flag_io_armed = false; // XXX read this from somewhere /* neither manual nor offboard control commands have been received */ current_status.offboard_control_signal_found_once = false; @@ -1341,13 +1342,13 @@ int commander_thread_main(int argc, char *argv[]) current_status.flag_external_manual_override_ok = true; /* flag position info as bad, do not allow auto mode */ - current_status.flag_vector_flight_mode_ok = false; + // current_status.flag_vector_flight_mode_ok = false; /* set battery warning flag */ current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; // XXX for now just set sensors as initialized - current_status.flag_system_sensors_initialized = true; + current_status.condition_system_sensors_initialized = true; /* advertise to ORB */ stat_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); @@ -1501,7 +1502,7 @@ int commander_thread_main(int argc, char *argv[]) /* update parameters */ - if (!current_status.flag_system_armed) { + if (!current_status.flag_fmu_armed) { if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { warnx("failed setting new system type"); } @@ -1654,7 +1655,7 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (current_status.arming_state == ARMING_STATE_INIT) { // XXX fix for now - current_status.flag_system_sensors_initialized = true; + current_status.condition_system_sensors_initialized = true; arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); } else { // XXX: Add emergency stuff if sensors are lost @@ -1671,45 +1672,45 @@ int commander_thread_main(int argc, char *argv[]) */ /* store current state to reason later about a state change */ - bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; - bool global_pos_valid = current_status.flag_global_position_valid; - bool local_pos_valid = current_status.flag_local_position_valid; + // bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; + bool global_pos_valid = current_status.condition_global_position_valid; + bool local_pos_valid = current_status.condition_local_position_valid; /* check for global or local position updates, set a timeout of 2s */ if (hrt_absolute_time() - last_global_position_time < 2000000) { - current_status.flag_global_position_valid = true; + current_status.condition_global_position_valid = true; // XXX check for controller status and home position as well } else { - current_status.flag_global_position_valid = false; + current_status.condition_global_position_valid = false; } if (hrt_absolute_time() - last_local_position_time < 2000000) { - current_status.flag_local_position_valid = true; + current_status.condition_local_position_valid = true; // XXX check for controller status and home position as well } else { - current_status.flag_local_position_valid = false; + current_status.condition_local_position_valid = false; } /* * Consolidate global position and local position valid flags * for vector flight mode. */ - if (current_status.flag_local_position_valid || - current_status.flag_global_position_valid) { - current_status.flag_vector_flight_mode_ok = true; + // if (current_status.condition_local_position_valid || + // current_status.condition_global_position_valid) { + // current_status.flag_vector_flight_mode_ok = true; - } else { - current_status.flag_vector_flight_mode_ok = false; - } + // } else { + // current_status.flag_vector_flight_mode_ok = false; + // } - /* consolidate state change, flag as changed if required */ - if (vector_flight_mode_ok != current_status.flag_vector_flight_mode_ok || - global_pos_valid != current_status.flag_global_position_valid || - local_pos_valid != current_status.flag_local_position_valid) { - state_changed = true; - } + // /* consolidate state change, flag as changed if required */ + // if (vector_flight_mode_ok != current_status.flag_vector_flight_mode_ok || + // global_pos_valid != current_status.flag_global_position_valid || + // local_pos_valid != current_status.flag_local_position_valid) { + // state_changed = true; + // } /* * Mark the position of the first position lock as return to launch (RTL) @@ -1721,16 +1722,16 @@ int commander_thread_main(int argc, char *argv[]) * 2) The system has not aquired position lock before * 3) The system is not armed (on the ground) */ - if (!current_status.flag_valid_launch_position && - !vector_flight_mode_ok && current_status.flag_vector_flight_mode_ok && - !current_status.flag_system_armed) { - /* first time a valid position, store it and emit it */ + // if (!current_status.flag_valid_launch_position && + // !vector_flight_mode_ok && current_status.flag_vector_flight_mode_ok && + // !current_status.flag_system_armed) { + // first time a valid position, store it and emit it - // XXX implement storage and publication of RTL position - current_status.flag_valid_launch_position = true; - current_status.flag_auto_flight_mode_ok = true; - state_changed = true; - } + // // XXX implement storage and publication of RTL position + // current_status.flag_valid_launch_position = true; + // current_status.flag_auto_flight_mode_ok = true; + // state_changed = true; + // } if (orb_check(gps_sub, &new_data)) { @@ -1760,7 +1761,7 @@ int commander_thread_main(int argc, char *argv[]) && (vdop_m < vdop_threshold_m) // XXX note that vdop is 0 for mtk && !home_position_set && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) - && !current_status.flag_system_armed) { + && !current_status.flag_fmu_armed) { warnx("setting home position"); /* copy position data to uORB home message, store it locally as well */ @@ -1876,7 +1877,7 @@ int commander_thread_main(int argc, char *argv[]) if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL_STANDBY rejected 1"); + warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); } /* Try seatbelt or fallback to manual */ @@ -1886,7 +1887,7 @@ int commander_thread_main(int argc, char *argv[]) // fallback to MANUAL_STANDBY if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL_STANDBY rejected 2"); + warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); } } @@ -1899,7 +1900,7 @@ int commander_thread_main(int argc, char *argv[]) // or fallback to MANUAL_STANDBY if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL_STANDBY rejected 3"); + warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); } } } @@ -2047,7 +2048,6 @@ int commander_thread_main(int argc, char *argv[]) // ) && if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - printf("Try disarm\n"); arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); stick_off_counter = 0; @@ -2060,7 +2060,6 @@ int commander_thread_main(int argc, char *argv[]) /* check if left stick is in lower right position --> arm */ if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.2f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - printf("Try arm\n"); arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_ARMED, mavlink_fd); stick_on_counter = 0; @@ -2156,13 +2155,13 @@ int commander_thread_main(int argc, char *argv[]) current_status.offboard_control_signal_lost_interval = 0; /* arm / disarm on request */ - if (sp_offboard.armed && !current_status.flag_system_armed) { + if (sp_offboard.armed && !current_status.flag_fmu_armed) { #warning fix this // update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); /* switch to stabilized mode = takeoff */ // update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); - } else if (!sp_offboard.armed && current_status.flag_system_armed) { + } else if (!sp_offboard.armed && current_status.flag_fmu_armed) { // update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); } diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index ae7f2a1c17..61ebe8c165 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -278,7 +278,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta || current_state->arming_state == ARMING_STATE_ARMED) { /* sensors need to be initialized for STANDBY state */ - if (current_state->flag_system_sensors_initialized) { + if (current_state->condition_system_sensors_initialized) { ret = OK; } else { mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized"); @@ -392,7 +392,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to be disarmed and have a position estimate */ if (current_state->arming_state != ARMING_STATE_STANDBY) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: not disarmed"); - } else if (!current_state->flag_local_position_valid) { + } else if (!current_state->condition_local_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: no position estimate"); } else { ret = OK; @@ -416,7 +416,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to be armed and have a position estimate */ if (current_state->arming_state != ARMING_STATE_ARMED) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: not armed"); - } else if (!current_state->flag_local_position_valid) { + } else if (!current_state->condition_local_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate"); } else { ret = OK; @@ -439,7 +439,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to be armed and have a position estimate */ if (current_state->arming_state != ARMING_STATE_ARMED) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: not armed"); - } else if (!current_state->flag_local_position_valid) { + } else if (!current_state->condition_local_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no pos estimate"); } else { ret = OK; @@ -458,9 +458,9 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to be disarmed and have a position and home lock */ if (current_state->arming_state != ARMING_STATE_STANDBY) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: not disarmed"); - } else if (!current_state->flag_global_position_valid) { + } else if (!current_state->condition_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no pos lock"); - } else if (!current_state->flag_valid_home_position) { + } else if (!current_state->condition_home_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no home pos"); } else { ret = OK; @@ -504,9 +504,9 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { /* need to have a position and home lock */ - if (!current_state->flag_global_position_valid) { + if (!current_state->condition_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no pos lock"); - } else if (!current_state->flag_valid_home_position) { + } else if (!current_state->condition_home_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no home pos"); } else { ret = OK; @@ -524,7 +524,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { /* need to have a mission ready */ - if (!current_state->flag_auto_mission_available) { + if (!current_state-> condition_auto_mission_available) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_MISSION: no mission available"); } else { ret = OK; @@ -542,9 +542,9 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { /* need to have a position and home lock */ - if (!current_state->flag_global_position_valid) { + if (!current_state->condition_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no pos lock"); - } else if (!current_state->flag_valid_home_position) { + } else if (!current_state->condition_home_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no home pos"); } else { ret = OK; @@ -559,9 +559,9 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER) { /* need to have a position and home lock */ - if (!current_state->flag_global_position_valid) { + if (!current_state->condition_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no pos lock"); - } else if (!current_state->flag_valid_home_position) { + } else if (!current_state->condition_home_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no home pos"); } else { ret = OK; diff --git a/apps/drivers/blinkm/blinkm.cpp b/apps/drivers/blinkm/blinkm.cpp index 6aff27e4ca..ceb2d987d4 100644 --- a/apps/drivers/blinkm/blinkm.cpp +++ b/apps/drivers/blinkm/blinkm.cpp @@ -529,7 +529,7 @@ BlinkM::led() } else { /* no battery warnings here */ - if(vehicle_status_raw.flag_system_armed == false) { + if(vehicle_status_raw.flag_fmu_armed == false) { /* system not armed */ led_color_1 = LED_RED; led_color_2 = LED_RED; diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index adb894371f..99f0f35b2d 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -446,7 +446,7 @@ PX4IO::init() } /* keep waiting for state change for 10 s */ - } while (!status.flag_system_armed); + } while (!status.flag_fmu_armed); /* regular boot, no in-air restart, init IO */ } else { @@ -658,11 +658,12 @@ PX4IO::io_set_arming_state() } else { clear |= PX4IO_P_SETUP_ARMING_ARM_OK; } - if (vstatus.flag_vector_flight_mode_ok) { - set |= PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK; - } else { - clear |= PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK; - } + // TODO fix this + // if (vstatus.flag_vector_flight_mode_ok) { + // set |= PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK; + // } else { + // clear |= PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK; + // } if (vstatus.flag_external_manual_override_ok) { set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; } else { diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 34b267d56b..3423287283 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -246,9 +246,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) **/ /* set calibration state */ - if (v_status.flag_preflight_gyro_calibration || - v_status.flag_preflight_mag_calibration || - v_status.flag_preflight_accel_calibration) { + if (v_status.flag_preflight_calibration) { *mavlink_state = MAV_STATE_CALIBRATING; diff --git a/apps/mavlink_onboard/mavlink.c b/apps/mavlink_onboard/mavlink.c index c5a1e82a88..fbfce7dc95 100644 --- a/apps/mavlink_onboard/mavlink.c +++ b/apps/mavlink_onboard/mavlink.c @@ -290,7 +290,7 @@ get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct } /* set arming state */ - if (v_status->flag_system_armed) { + if (v_status->flag_fmu_armed) { *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } else { *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index da7550f797..79ca9fe2d2 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -247,7 +247,7 @@ mc_thread_main(int argc, char *argv[]) /* initialize to current yaw if switching to manual or att control */ if (state.flag_control_attitude_enabled != flag_control_attitude_enabled || state.flag_control_manual_enabled != flag_control_manual_enabled || - state.flag_system_armed != flag_system_armed) { + state.flag_fmu_armed != flag_system_armed) { att_sp.yaw_body = att.yaw; } @@ -291,7 +291,7 @@ mc_thread_main(int argc, char *argv[]) att_sp.pitch_body = manual.pitch; /* set attitude if arming */ - if (!flag_control_attitude_enabled && state.flag_system_armed) { + if (!flag_control_attitude_enabled && state.flag_fmu_armed) { att_sp.yaw_body = att.yaw; } @@ -395,7 +395,8 @@ mc_thread_main(int argc, char *argv[]) /* update state */ flag_control_attitude_enabled = state.flag_control_attitude_enabled; flag_control_manual_enabled = state.flag_control_manual_enabled; - flag_system_armed = state.flag_system_armed; + flag_system_armed = state.flag_fmu_armed; + // XXX add some logic to this perf_end(mc_loop_perf); } /* end of poll call for attitude updates */ diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index 4955422441..eba5a5047e 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -183,31 +183,35 @@ struct vehicle_status_s return_switch_pos_t return_switch; mission_switch_pos_t mission_switch; + + bool condition_system_emergency; + bool condition_system_in_air_restore; /**< true if we can restore in mid air */ + bool condition_system_sensors_initialized; + bool condition_system_returned_to_home; + bool condition_auto_mission_available; + bool condition_global_position_valid; /**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */ + bool condition_launch_position_valid; /**< indicates a valid launch position */ + bool condition_home_position_valid; /**< indicates a valid home position (a valid home position is not always a valid launch) */ + bool condition_local_position_valid; - bool flag_system_armed; /**< true is motors / actuators are armed */ + bool flag_hil_enabled; /**< true if hardware in the loop simulation is enabled */ + bool flag_fmu_armed; /**< true is motors / actuators of FMU are armed */ + bool flag_io_armed; /**< true is motors / actuators of IO are armed */ bool flag_system_emergency; - bool flag_system_in_air_restore; /**< true if we can restore in mid air */ - bool flag_system_sensors_initialized; - bool flag_system_arming_requested; - bool flag_system_disarming_requested; - bool flag_system_reboot_requested; - bool flag_system_returned_to_home; - - bool flag_auto_mission_available; - bool flag_auto_enabled; + bool flag_preflight_calibration; bool flag_control_manual_enabled; /**< true if manual input is mixed in */ bool flag_control_offboard_enabled; /**< true if offboard control input is on */ - bool flag_hil_enabled; /**< true if hardware in the loop simulation is enabled */ - + bool flag_auto_enabled; bool flag_control_rates_enabled; /**< true if rates are stabilized */ bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ bool flag_control_position_enabled; /**< true if position is controlled */ - bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */ - bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */ - bool flag_preflight_accel_calibration; + + // bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */ + // bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */ + // bool flag_preflight_accel_calibration; bool rc_signal_found_once; bool rc_signal_lost; /**< true if RC reception is terminally lost */ @@ -238,13 +242,10 @@ struct vehicle_status_s uint16_t errors_count3; uint16_t errors_count4; - bool flag_global_position_valid; /**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */ - bool flag_local_position_valid; - bool flag_vector_flight_mode_ok; /**< position estimation, battery voltage and other critical subsystems are good for autonomous flight */ - bool flag_auto_flight_mode_ok; /**< conditions of vector flight mode apply plus a valid takeoff position lock has been aquired */ + // bool flag_vector_flight_mode_ok; /**< position estimation, battery voltage and other critical subsystems are good for autonomous flight */ + // bool flag_auto_flight_mode_ok; /**< conditions of vector flight mode apply plus a valid takeoff position lock has been aquired */ bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ - bool flag_valid_launch_position; /**< indicates a valid launch position */ - bool flag_valid_home_position; /**< indicates a valid home position (a valid home position is not always a valid launch) */ + }; /** From 34c84752d1ff7494529dfea8e72f3c090b451b3c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 25 Feb 2013 09:24:30 -0800 Subject: [PATCH 015/486] Checkpoint: Added control flags --- apps/commander/state_machine_helper.c | 576 +++----------------------- 1 file changed, 59 insertions(+), 517 deletions(-) diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 61ebe8c165..742b7fe072 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -53,207 +53,6 @@ #include "state_machine_helper.h" -/** - * Transition from one sytem state to another - */ -//void navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -//{ -// int ret = ERROR; -// navigation_state_t new_navigation_state; -// -// /* do the navigation state update depending on the arming state */ -// switch (current_status->arming_state) { -// -// /* evaluate the navigation state when disarmed */ -// case ARMING_STATE_STANDBY: -// -// /* Always accept manual mode */ -// if (current_status->mode_switch == MODE_SWITCH_MANUAL) { -// new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; -// -// /* Accept SEATBELT if there is a local position estimate */ -// } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT) { -// -// if (current_status->flag_local_position_valid) { -// new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; -// } else { -// /* or just fall back to manual */ -// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); -// new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; -// } -// -// /* Accept AUTO if there is a global position estimate */ -// } else if (current_status->mode_switch == MODE_SWITCH_AUTO) { -// if (current_status->flag_local_position_valid) { -// new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; -// } else { -// mavlink_log_critical(mavlink_fd, "Rej. AUTO: no global position"); -// -// /* otherwise fallback to seatbelt or even manual */ -// if (current_status->flag_local_position_valid) { -// new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; -// } else { -// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); -// new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; -// } -// } -// } -// -// break; -// -// /* evaluate the navigation state when armed */ -// case ARMING_STATE_ARMED: -// -// /* Always accept manual mode */ -// if (current_status->mode_switch == MODE_SWITCH_MANUAL) { -// new_navigation_state = NAVIGATION_STATE_MANUAL; -// -// /* Accept SEATBELT if there is a local position estimate */ -// } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT -// && current_status->return_switch == RETURN_SWITCH_NONE) { -// -// if (current_status->flag_local_position_valid) { -// new_navigation_state = NAVIGATION_STATE_SEATBELT; -// } else { -// /* or just fall back to manual */ -// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); -// new_navigation_state = NAVIGATION_STATE_MANUAL; -// } -// -// /* Accept SEATBELT_DESCENT if there is a local position estimate and the return switch is on */ -// } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT -// && current_status->return_switch == RETURN_SWITCH_RETURN) { -// -// if (current_status->flag_local_position_valid) { -// new_navigation_state = NAVIGATION_STATE_SEATBELT_DESCENT; -// } else { -// /* descent not possible without baro information, fall back to manual */ -// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no local position"); -// new_navigation_state = NAVIGATION_STATE_MANUAL; -// } -// -// /* Accept LOITER if there is a global position estimate */ -// } else if (current_status->mode_switch == MODE_SWITCH_AUTO -// && current_status->return_switch == RETURN_SWITCH_NONE -// && current_status->mission_switch == MISSION_SWITCH_NONE) { -// -// if (current_status->flag_global_position_valid) { -// new_navigation_state = NAVIGATION_STATE_AUTO_LOITER; -// } else { -// mavlink_log_critical(mavlink_fd, "Rej. LOITER: no global position"); -// -// /* otherwise fallback to SEATBELT or even manual */ -// if (current_status->flag_local_position_valid) { -// new_navigation_state = NAVIGATION_STATE_SEATBELT; -// } else { -// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); -// new_navigation_state = NAVIGATION_STATE_MANUAL; -// } -// } -// -// /* Accept MISSION if there is a global position estimate and home position */ -// } else if (current_status->mode_switch == MODE_SWITCH_AUTO -// && current_status->return_switch == RETURN_SWITCH_NONE -// && current_status->mission_switch == MISSION_SWITCH_MISSION) { -// -// if (current_status->flag_global_position_valid && current_status->flag_valid_home_position) { -// new_navigation_state = NAVIGATION_STATE_AUTO_MISSION; -// } else { -// -// /* spit out what exactly is unavailable */ -// if (current_status->flag_global_position_valid) { -// mavlink_log_critical(mavlink_fd, "Rej. MISSION: no home position"); -// } else if (current_status->flag_valid_home_position) { -// mavlink_log_critical(mavlink_fd, "Rej. MISSION: no global position"); -// } else { -// mavlink_log_critical(mavlink_fd, "Rej. MISSION: no global position and no home position"); -// } -// -// /* otherwise fallback to SEATBELT or even manual */ -// if (current_status->flag_local_position_valid) { -// new_navigation_state = NAVIGATION_STATE_SEATBELT; -// } else { -// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); -// new_navigation_state = NAVIGATION_STATE_MANUAL; -// } -// } -// -// /* Go to RTL or land if global position estimate and home position is available */ -// } else if (current_status->mode_switch == MODE_SWITCH_AUTO -// && current_status->return_switch == RETURN_SWITCH_RETURN -// && (current_status->mission_switch == MISSION_SWITCH_NONE || current_status->mission_switch == MISSION_SWITCH_MISSION)) { -// -// if (current_status->flag_global_position_valid && current_status->flag_valid_home_position) { -// -// /* after RTL go to LAND */ -// if (current_status->flag_system_returned_to_home) { -// new_navigation_state = NAVIGATION_STATE_AUTO_LAND; -// } else { -// new_navigation_state = NAVIGATION_STATE_AUTO_RTL; -// } -// -// } else { -// -// /* spit out what exactly is unavailable */ -// if (current_status->flag_global_position_valid) { -// mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no home position"); -// } else if (current_status->flag_valid_home_position) { -// mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no global position"); -// } else { -// mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no global position and no home position"); -// } -// -// /* otherwise fallback to SEATBELT_DESCENT or even MANUAL */ -// if (current_status->flag_local_position_valid) { -// new_navigation_state = NAVIGATION_STATE_SEATBELT_DESCENT; -// } else { -// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); -// new_navigation_state = NAVIGATION_STATE_MANUAL; -// } -// } -// } -// break; -// -// case ARMING_STATE_ARMED_ERROR: -// -// // TODO work out fail-safe scenarios -// break; -// -// case ARMING_STATE_STANDBY_ERROR: -// -// // TODO work out fail-safe scenarios -// break; -// -// case ARMING_STATE_REBOOT: -// -// // XXX I don't think we should end up here -// break; -// -// case ARMING_STATE_IN_AIR_RESTORE: -// -// // XXX not sure what to do here -// break; -// default: -// break; -// } -// -// -// -// /* Update the system state in case it has changed */ -// if (current_status->navigation_state != new_navigation_state) { -// -// /* Check if the transition is valid */ -// if (check_navigation_state_transition(current_status->navigation_state, new_navigation_state) == OK) { -// current_status->navigation_state = new_navigation_state; -// state_machine_publish(status_pub, current_status, mavlink_fd); -// } else { -// mavlink_log_critical(mavlink_fd, "System state transition not valid"); -// } -// } -// -// return; -//} - int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, const int mavlink_fd) { int ret = ERROR; @@ -269,6 +68,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* allow going back from INIT for calibration */ if (current_state->arming_state == ARMING_STATE_STANDBY) { ret = OK; + current_state->flag_system_armed = false; } break; case ARMING_STATE_STANDBY: @@ -280,10 +80,10 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* sensors need to be initialized for STANDBY state */ if (current_state->condition_system_sensors_initialized) { ret = OK; + current_state->flag_system_armed = false; } else { mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized"); } - } break; case ARMING_STATE_ARMED: @@ -294,15 +94,17 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* XXX conditions for arming? */ ret = OK; + current_state->flag_system_armed = true; } break; case ARMING_STATE_ARMED_ERROR: /* an armed error happens when ARMED obviously */ if (current_state->arming_state == ARMING_STATE_ARMED) { - ret = OK; - + /* XXX conditions for an error state? */ + ret = OK; + current_state->flag_system_armed = true; } break; case ARMING_STATE_STANDBY_ERROR: @@ -311,6 +113,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta || current_state->arming_state == ARMING_STATE_INIT || current_state->arming_state == ARMING_STATE_ARMED_ERROR) { ret = OK; + current_state->flag_system_armed = false; } break; default: @@ -350,6 +153,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY) { ret = OK; + current_state->flag_control_rates_enabled = false; + current_state->flag_control_attitude_enabled = false; + current_state->flag_control_velocity_enabled = false; + current_state->flag_control_position_enabled = false; } break; @@ -366,6 +173,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current mavlink_log_critical(mavlink_fd, "Rej. MANUAL_STANDBY: not disarmed"); } else { ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = false; + current_state->flag_control_position_enabled = false; } } break; @@ -377,6 +188,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current mavlink_log_critical(mavlink_fd, "Rej. MANUAL: not armed"); } else { ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = false; + current_state->flag_control_position_enabled = false; } break; @@ -396,6 +211,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: no position estimate"); } else { ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = false; } } break; @@ -420,6 +239,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate"); } else { ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = false; } } break; @@ -443,6 +266,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no pos estimate"); } else { ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = false; } } break; @@ -464,6 +291,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no home pos"); } else { ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = true; } } break; @@ -481,6 +312,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current mavlink_log_critical(mavlink_fd, "Rej. AUTO_READY: not armed"); } else { ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = true; } } break; @@ -491,6 +326,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current if (current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) { ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = true; } break; @@ -510,6 +349,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no home pos"); } else { ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = true; } } break; @@ -528,6 +371,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current mavlink_log_critical(mavlink_fd, "Rej. AUTO_MISSION: no mission available"); } else { ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = true; } } break; @@ -548,6 +395,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no home pos"); } else { ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = true; } } break; @@ -565,6 +416,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no home pos"); } else { ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = true; } } break; @@ -683,38 +538,6 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat //} -/* - * Private functions, update the state machine - */ -//void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -//{ -// warnx("EMERGENCY HANDLER\n"); -// /* Depending on the current state go to one of the error states */ -// -// if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY) { -// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_ERROR); -// -// } else if (current_status->state_machine == SYSTEM_STATE_AUTO || current_status->state_machine == SYSTEM_STATE_MANUAL) { -// -// // DO NOT abort mission -// //do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT); -// -// } else { -// warnx("Unknown system state: #%d\n", current_status->state_machine); -// } -//} - -//void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) //do not call state_machine_emergency_always_critical if we are in manual mode for these errors -//{ -// if (current_status->state_machine != SYSTEM_STATE_MANUAL) { //if we are in manual: user can react to errors themself -// state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd); -// -// } else { -// //global_data_send_mavlink_statustext_message_out("[cmd] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL); -// } -// -//} - // /* @@ -827,91 +650,6 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat // } -// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_health |= 1 << *subsystem_type; -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - -// switch (*subsystem_type) { -// case SUBSYSTEM_TYPE_GYRO: -// //TODO state machine change (recovering) -// break; - -// case SUBSYSTEM_TYPE_ACC: -// //TODO state machine change -// break; - -// case SUBSYSTEM_TYPE_MAG: -// //TODO state machine change -// break; - -// case SUBSYSTEM_TYPE_GPS: -// //TODO state machine change -// break; - -// default: -// break; -// } - - -// } - - -// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// bool previosly_healthy = (bool)(current_status->onboard_control_sensors_health & 1 << *subsystem_type); -// current_status->onboard_control_sensors_health &= ~(1 << *subsystem_type); -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - -// /* if we received unhealthy message more than *_HEALTH_COUNTER_LIMIT, switch to error state */ - -// switch (*subsystem_type) { -// case SUBSYSTEM_TYPE_GYRO: -// //global_data_send_mavlink_statustext_message_out("Commander: gyro unhealthy", MAV_SEVERITY_CRITICAL); - -// if (previosly_healthy) //only throw emergency if previously healthy -// state_machine_emergency_always_critical(status_pub, current_status); - -// break; - -// case SUBSYSTEM_TYPE_ACC: -// //global_data_send_mavlink_statustext_message_out("Commander: accelerometer unhealthy", MAV_SEVERITY_CRITICAL); - -// if (previosly_healthy) //only throw emergency if previously healthy -// state_machine_emergency_always_critical(status_pub, current_status); - -// break; - -// case SUBSYSTEM_TYPE_MAG: -// //global_data_send_mavlink_statustext_message_out("Commander: magnetometer unhealthy", MAV_SEVERITY_CRITICAL); - -// if (previosly_healthy) //only throw emergency if previously healthy -// state_machine_emergency_always_critical(status_pub, current_status); - -// break; - -// case SUBSYSTEM_TYPE_GPS: -// // //TODO: remove this block -// // break; -// // /////////////////// -// //global_data_send_mavlink_statustext_message_out("Commander: GPS unhealthy", MAV_SEVERITY_CRITICAL); - -// // printf("previosly_healthy = %u\n", previosly_healthy); -// if (previosly_healthy) //only throw emergency if previously healthy -// state_machine_emergency(status_pub, current_status); - -// break; - -// default: -// break; -// } - -// } - ///* END SUBSYSTEM/EMERGENCY FUNCTIONS*/ // @@ -985,199 +723,3 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat // // return ret; //} - -#if 0 - -void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - /* Depending on the current state switch state */ - if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); - } -} - -void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - /* Depending on the current state switch state */ - if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_AUTO) { - state_machine_emergency(status_pub, current_status, mavlink_fd); - } -} - -void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (current_status->state_machine == SYSTEM_STATE_STANDBY) { - printf("[cmd] arming\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); - } -} - -void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - printf("[cmd] going standby\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); - - } else if (current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) { - printf("[cmd] MISSION ABORT!\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); - } -} - -void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - int old_mode = current_status->flight_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL; - - current_status->flag_control_manual_enabled = true; - - /* set behaviour based on airframe */ - if ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status->system_type == VEHICLE_TYPE_OCTOROTOR)) { - - /* assuming a rotary wing, set to SAS */ - current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - - } else { - - /* assuming a fixed wing, set to direct pass-through */ - current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; - current_status->flag_control_attitude_enabled = false; - current_status->flag_control_rates_enabled = false; - } - - if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); - - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) { - printf("[cmd] manual mode\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); - } -} - -void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) { - int old_mode = current_status->flight_mode; - int old_manual_control_mode = current_status->manual_control_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL; - current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - current_status->flag_control_manual_enabled = true; - - if (old_mode != current_status->flight_mode || - old_manual_control_mode != current_status->manual_control_mode) { - printf("[cmd] att stabilized mode\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); - state_machine_publish(status_pub, current_status, mavlink_fd); - } - - } -} - -void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (!current_status->flag_vector_flight_mode_ok) { - mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. GUIDED MODE"); - tune_error(); - return; - } - - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) { - printf("[cmd] position guided mode\n"); - int old_mode = current_status->flight_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_STAB; - current_status->flag_control_manual_enabled = false; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STABILIZED); - - if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); - - } -} - -void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (!current_status->flag_vector_flight_mode_ok) { - mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. AUTO MODE"); - return; - } - - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) { - printf("[cmd] auto mode\n"); - int old_mode = current_status->flight_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO; - current_status->flag_control_manual_enabled = false; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO); - - if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); - } -} - - - - -uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode) //TODO: add more checks to avoid state switching in critical situations -{ - commander_state_machine_t current_system_state = current_status->state_machine; - - uint8_t ret = 1; - - switch (custom_mode) { - case SYSTEM_STATE_GROUND_READY: - break; - - case SYSTEM_STATE_STANDBY: - break; - - case SYSTEM_STATE_REBOOT: - printf("try to reboot\n"); - - if (current_system_state == SYSTEM_STATE_STANDBY - || current_system_state == SYSTEM_STATE_PREFLIGHT - || current_status->flag_hil_enabled) { - printf("system will reboot\n"); - mavlink_log_critical(mavlink_fd, "Rebooting.."); - usleep(200000); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_REBOOT); - ret = 0; - } - - break; - - case SYSTEM_STATE_AUTO: - printf("try to switch to auto/takeoff\n"); - - if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_MANUAL) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO); - printf("state: auto\n"); - ret = 0; - } - - break; - - case SYSTEM_STATE_MANUAL: - printf("try to switch to manual\n"); - - if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_AUTO) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); - printf("state: manual\n"); - ret = 0; - } - - break; - - default: - break; - } - - return ret; -} - -#endif From 0fe5aeb02c6ab0ac9c50f54d028cd5dde908e051 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 11 Mar 2013 10:39:57 -0700 Subject: [PATCH 016/486] Checkpoint: More state machine fixes, commited to wrong branch and now copied over --- apps/ardrone_interface/ardrone_interface.c | 4 +- apps/commander/commander.c | 32 +++++++++++---- apps/commander/state_machine_helper.c | 34 ++++++++++----- apps/mavlink/mavlink.c | 10 +++-- apps/mavlink_onboard/mavlink.c | 6 +++ apps/mavlink_onboard/mavlink_receiver.c | 2 +- .../multirotor_att_control_main.c | 41 ++++++++++--------- 7 files changed, 86 insertions(+), 43 deletions(-) diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c index aeaf830def..264041e65a 100644 --- a/apps/ardrone_interface/ardrone_interface.c +++ b/apps/ardrone_interface/ardrone_interface.c @@ -331,7 +331,9 @@ int ardrone_interface_thread_main(int argc, char *argv[]) /* for now only spin if armed and immediately shut down * if in failsafe */ - if (armed.armed && !armed.lockdown) { + //XXX fix this + //if (armed.armed && !armed.lockdown) { + if (state.flag_fmu_armed) { ardrone_mixing_and_output(ardrone_write, &actuator_controls); } else { diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 2fdcf4ce3f..a3e8fb7450 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -60,13 +60,9 @@ #include #include #include -#include -#include -#include -#include "state_machine_helper.h" -#include "systemlib/systemlib.h" #include #include + #include #include #include @@ -80,11 +76,19 @@ #include #include #include -#include +#include +#include +#include + +#include #include #include #include +#include + +#include "state_machine_helper.h" + /* XXX MOVE CALIBRATION TO SENSORS APP THREAD */ #include @@ -101,7 +105,7 @@ PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); -#include + extern struct system_load_s system_load; /* Decouple update interval and hysteris counters, all depends on intervals */ @@ -120,6 +124,8 @@ extern struct system_load_s system_load; #define GPS_QUALITY_GOOD_HYSTERIS_TIME_MS 5000 #define GPS_QUALITY_GOOD_COUNTER_LIMIT (GPS_QUALITY_GOOD_HYSTERIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) +#define LOCAL_POSITION_TIMEOUT 1000000 /**< consider the local position estimate invalid after 1s */ + /* File descriptors */ static int leds; static int buzzer; @@ -1324,9 +1330,12 @@ int commander_thread_main(int argc, char *argv[]) /* make sure we are in preflight state */ memset(¤t_status, 0, sizeof(current_status)); + + current_status.navigation_state = NAVIGATION_STATE_INIT; current_status.arming_state = ARMING_STATE_INIT; current_status.hil_state = HIL_STATE_OFF; + current_status.flag_hil_enabled = false; current_status.flag_fmu_armed = false; current_status.flag_io_armed = false; // XXX read this from somewhere @@ -1542,6 +1551,13 @@ int commander_thread_main(int argc, char *argv[]) last_local_position_time = local_position.timestamp; } + /* set the condition to valid if there has recently been a local position estimate */ + if (hrt_absolute_time() - last_local_position_time < LOCAL_POSITION_TIMEOUT) { + current_status.condition_local_position_valid = true; + } else { + current_status.condition_local_position_valid = false; + } + /* update battery status */ orb_check(battery_sub, &new_data); if (new_data) { @@ -1565,7 +1581,7 @@ int commander_thread_main(int argc, char *argv[]) // current_status.state_machine == SYSTEM_STATE_AUTO || // current_status.state_machine == SYSTEM_STATE_MANUAL)) { // /* armed */ -// led_toggle(LED_BLUE); + led_toggle(LED_BLUE); } else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { /* not armed */ diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 742b7fe072..79394e2bae 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -40,19 +40,20 @@ #include #include +#include #include #include #include #include #include +#include #include #include #include #include "state_machine_helper.h" - int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, const int mavlink_fd) { int ret = ERROR; @@ -68,7 +69,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* allow going back from INIT for calibration */ if (current_state->arming_state == ARMING_STATE_STANDBY) { ret = OK; - current_state->flag_system_armed = false; + current_state->flag_fmu_armed = false; } break; case ARMING_STATE_STANDBY: @@ -80,7 +81,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* sensors need to be initialized for STANDBY state */ if (current_state->condition_system_sensors_initialized) { ret = OK; - current_state->flag_system_armed = false; + current_state->flag_fmu_armed = false; } else { mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized"); } @@ -94,7 +95,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* XXX conditions for arming? */ ret = OK; - current_state->flag_system_armed = true; + current_state->flag_fmu_armed = true; } break; case ARMING_STATE_ARMED_ERROR: @@ -104,7 +105,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* XXX conditions for an error state? */ ret = OK; - current_state->flag_system_armed = true; + current_state->flag_fmu_armed = true; } break; case ARMING_STATE_STANDBY_ERROR: @@ -113,7 +114,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta || current_state->arming_state == ARMING_STATE_INIT || current_state->arming_state == ARMING_STATE_ARMED_ERROR) { ret = OK; - current_state->flag_system_armed = false; + current_state->flag_fmu_armed = false; } break; default: @@ -157,6 +158,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = false; current_state->flag_control_velocity_enabled = false; current_state->flag_control_position_enabled = false; + current_state->flag_control_manual_enabled = false; } break; @@ -177,6 +179,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = false; current_state->flag_control_position_enabled = false; + current_state->flag_control_manual_enabled = true; } } break; @@ -192,6 +195,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = false; current_state->flag_control_position_enabled = false; + current_state->flag_control_manual_enabled = true; } break; @@ -215,6 +219,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = true; current_state->flag_control_position_enabled = false; + current_state->flag_control_manual_enabled = false; } } break; @@ -243,6 +248,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = true; current_state->flag_control_position_enabled = false; + current_state->flag_control_manual_enabled = false; } } break; @@ -270,6 +276,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = true; current_state->flag_control_position_enabled = false; + current_state->flag_control_manual_enabled = false; } } break; @@ -295,6 +302,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = true; current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; } } break; @@ -316,6 +324,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = true; current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; } } break; @@ -330,6 +339,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = true; current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; } break; @@ -353,6 +363,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = true; current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; } } break; @@ -375,6 +386,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = true; current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; } } break; @@ -399,6 +411,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = true; current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; } } break; @@ -420,6 +433,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = true; current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; } } break; @@ -530,7 +544,7 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat //void publish_armed_status(const struct vehicle_status_s *current_status) //{ // struct actuator_armed_s armed; -// armed.armed = current_status->flag_system_armed; +// armed.armed = current_status->flag_fmu_armed; // /* lock down actuators if required, only in HIL */ // armed.lockdown = (current_status->flag_hil_enabled) ? true : false; // orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); @@ -669,7 +683,7 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat //// printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); //// //// } else if (current_status->state_machine != SYSTEM_STATE_STANDBY && -//// current_status->flag_system_armed) { +//// current_status->flag_fmu_armed) { //// //// mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!") //// @@ -694,7 +708,7 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat // } // // /* vehicle is disarmed, mode requests arming */ -// if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { +// if (!(current_status->flag_fmu_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { // /* only arm in standby state */ // // XXX REMOVE // if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { @@ -705,7 +719,7 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat // } // // /* vehicle is armed, mode requests disarming */ -// if (current_status->flag_system_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { +// if (current_status->flag_fmu_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { // /* only disarm in ground ready */ // if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { // do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 3423287283..c443d5a4a3 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -230,10 +230,12 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; } -// if (v_status.system_state == NAVIGATION_STATE_SEATBELT) { -// -// *mavlink_mode |= MAV_MODE_FLAG_DECODE_POSITION_GUIDED; -// } + if (v_status.navigation_state == NAVIGATION_STATE_SEATBELT + || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT + || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY) { + + *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; + } // // if (v_status.system_state == NAVIGATION_STATE_SEATBELT) { // diff --git a/apps/mavlink_onboard/mavlink.c b/apps/mavlink_onboard/mavlink.c index fbfce7dc95..057a573b13 100644 --- a/apps/mavlink_onboard/mavlink.c +++ b/apps/mavlink_onboard/mavlink.c @@ -296,6 +296,12 @@ get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; } + if (v_status->flag_control_velocity_enabled) { + *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; + } else { + *mavlink_mode &= ~MAV_MODE_FLAG_GUIDED_ENABLED; + } + // switch (v_status->state_machine) { // case SYSTEM_STATE_PREFLIGHT: // if (v_status->flag_preflight_gyro_calibration || diff --git a/apps/mavlink_onboard/mavlink_receiver.c b/apps/mavlink_onboard/mavlink_receiver.c index 0acbea6757..2d406fb9f5 100644 --- a/apps/mavlink_onboard/mavlink_receiver.c +++ b/apps/mavlink_onboard/mavlink_receiver.c @@ -328,4 +328,4 @@ receive_start(int uart) pthread_t thread; pthread_create(&thread, &receiveloop_attr, receive_thread, &uart); return thread; -} \ No newline at end of file +} diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 79ca9fe2d2..3329c5c48e 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -150,7 +150,9 @@ mc_thread_main(int argc, char *argv[]) /* store last control mode to detect mode switches */ bool flag_control_manual_enabled = false; bool flag_control_attitude_enabled = false; - bool flag_system_armed = false; + bool flag_fmu_armed = false; + bool flag_control_position_enabled = false; + bool flag_control_velocity_enabled = false; /* store if yaw position or yaw speed has been changed */ bool control_yaw_position = true; @@ -162,7 +164,6 @@ mc_thread_main(int argc, char *argv[]) param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR"); float failsafe_throttle = 0.0f; - while (!thread_should_exit) { /* wait for a sensor update, check for exit condition every 500 ms */ @@ -247,7 +248,7 @@ mc_thread_main(int argc, char *argv[]) /* initialize to current yaw if switching to manual or att control */ if (state.flag_control_attitude_enabled != flag_control_attitude_enabled || state.flag_control_manual_enabled != flag_control_manual_enabled || - state.flag_fmu_armed != flag_system_armed) { + state.flag_fmu_armed != flag_fmu_armed) { att_sp.yaw_body = att.yaw; } @@ -313,20 +314,20 @@ mc_thread_main(int argc, char *argv[]) // * settings as well. // */ // -// /* only move setpoint if manual input is != 0 */ -// if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { -// rates_sp.yaw = manual.yaw; -// control_yaw_position = false; -// first_time_after_yaw_speed_control = true; -// -// } else { -// if (first_time_after_yaw_speed_control) { -// att_sp.yaw_body = att.yaw; -// first_time_after_yaw_speed_control = false; -// } -// -// control_yaw_position = true; -// } + /* only move setpoint if manual input is != 0 */ + if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { + rates_sp.yaw = manual.yaw; + control_yaw_position = false; + first_time_after_yaw_speed_control = true; + + } else { + if (first_time_after_yaw_speed_control) { + att_sp.yaw_body = att.yaw; + first_time_after_yaw_speed_control = false; + } + + control_yaw_position = true; + } // } // } @@ -337,6 +338,7 @@ mc_thread_main(int argc, char *argv[]) /* STEP 2: publish the controller output */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + if (motor_test_mode) { printf("testmode"); att_sp.roll_body = 0.0f; @@ -395,8 +397,9 @@ mc_thread_main(int argc, char *argv[]) /* update state */ flag_control_attitude_enabled = state.flag_control_attitude_enabled; flag_control_manual_enabled = state.flag_control_manual_enabled; - flag_system_armed = state.flag_fmu_armed; - // XXX add some logic to this + flag_control_position_enabled = state.flag_control_position_enabled; + flag_control_velocity_enabled = state.flag_control_velocity_enabled; + flag_fmu_armed = state.flag_fmu_armed; perf_end(mc_loop_perf); } /* end of poll call for attitude updates */ From 8e837dd1641941d670a30a6f4706a663efa03ca1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 11 Mar 2013 11:09:49 -0700 Subject: [PATCH 017/486] Updated the state machine documentation --- Documentation/flight_mode_state_machine.odg | Bin 23463 -> 22619 bytes Documentation/flight_mode_state_machine.pdf | Bin 23031 -> 110086 bytes 2 files changed, 0 insertions(+), 0 deletions(-) diff --git a/Documentation/flight_mode_state_machine.odg b/Documentation/flight_mode_state_machine.odg index cbdad91c9c0e539eb5b02b700e4b9d8eab5efd50..2d119bd8c68ce1753bbf4777cdc5d263220d83ad 100644 GIT binary patch delta 21163 zcmZ^JQ*`G+x8*;!ZFX$i>ezPD>DbmUPRF*ZW# zS)cThay4qklm~3IRfSGGg5?tPPyZ%3Je?w1bd)WhNx?(^t(Wbk(Y+V~`i7Na9&xKa ztaO-t zTJs1h6-$7RSGK_4Zbs|S8+@B5c3Ineg^;wm0)FTS@!MVWDGUWgq{_$_6nN|%#C|*i z!VPOWP44GAJ2^+h)C7aF$|uCAFnwS$3AExnvd!Kz-<>zjs*W`^jZ@?GV! z?Y{(9K^>!LlYszyha?uQCZrZ;M^Gi+7xn^B83@;bmC8~f@^_evqw1DCPxn~>5|xEz zmmTYieyY*<+biue|Hzl5kCMby=%N#nH(Hhg6n!V9#S$+}pZ+}4^4dSj2R7XRrZvxn z|1UtRT_b}5)V^_)m(8b5K3DJ>7aWhcN1fA6E8bfN_3=JVDp{FGZEse7d%d^w2C-UO zz5tgw;L?dg=br_=|HeU=+*cBYa~rUD5paN_EC&O7+QB*W2Mh!fg#>~A8+2G$*#Ceo z2KwI`;y>Hvo2zS*A2=+quDk5K!-MIUS0kfx%c7fve!U1GE-#I#%PTaRLN-@*S?`M*M1-WBM0RPi z)zPt1`PASh@v(0#gLZBCetY~6j&QLNrZ*{M>W*6fx8xf)4Jmo>6vY}*y_}5- zoAhKW&FMz_F73}tsc?t|j3~q@T3Mm`kB;*RgOI6x_}3D~6YrS=ypKt(0MbYO^xDhb zUyGW4w(T$WuNHNHyBK8}o&Bx}0rcQ-jLU+96=gob(8xpyZ^Je17g~fGSm?{YNqk%v zudLz&;xLwo;KAPI=i+rYd*Tuj73rz0i9UY_!QU}G5he#!Ts8bg45Nj_nzRXnrz#)w zh?&A_#P=9tGxNqPPu^RftZ+Q7By3wk9)xfZV{0@yq@W1_y&N-EMEr-`41E?n_;OQR zTvj3jvl08N7~zMl@5!dHG8H^9>HTt7Zc?LNaW#P(TM*y^!C?%gTTr*m5|`jGO<*pl z+oK5~fZmUuBup*G+1oSvhLA~fvmFKlI&KxVq+`>(MX);7J$&RIy51`!ajg$ElODX0 zsxS0&R03zLzbmC0UXRpOaR&(J$ zJlEI#{cW@u*Zvjy%2dsYfBe`3w27^zk$dAH4tMQ=5gN>uImI32a zmBzoL48Cv*63T|4ogN_rBP{eEqhL~8dj=yf94#m0A@*=u#-!$|V79`ph3woGTeb!K z>V(CyHd|p<@h{T*r~6~CpWaqa{M&Nkf8@T>n2uwzAS>zSY3x{h0KH19{&;OngQwDA zMqzJ2!TfcB_#i%>;KfM0FhtOANM5@5X&Jv9<5uwDJ>m3W$_y_K&tgyUOgcEI)lvHO zFJC1JsY!p8Cbv!P{Se?8KA?zc{~4$j$2}jmOT>eDh?Ba2BoxK=IHjJ>G1X6>B(4bt zu5YB193PLt*{99&T7@wfxA}SX6AZA2W%Eq}1{`>E2F+5$!obQOv@_aF?%98><0aly zM6x9=)_u1aq^JUQ{g|*C-V1TrG9$Bs*=flgIk+&(lkJgmP8-Ng!iZ}|4#C>f?#t%K4G4-K^AFu^Yde*P|4LMin^+fOj){Dvn(w*PZ=_s)Q;(v`^+ zPh|TtD*RHeSyB0uKIwdV^hs`-tv)IBuVGWnO#MStm-n*ClyA~bQ%TJ)=O%VKdI!5A zeB4RV#e|Z^Mu*PFDI|rw%$xUbRIAX{kuRJE8Hf$vGLkioU2Eirk>>v}Vq^ae0a#rn zUaQ({wQeVt*Kr)^@|1@B98$+CEffrv@0k%;E;y-qIYC9+dW&UT-zoe!Crkj{+3w}t zST+JAzxG1Ns>q?$eNvkCoW%@6{wgGh_JgwXn$8l9%d2D0$nU@N8g{>(Jk*`<)ToK; zT-;3ss=#<4Rau?OM(0~$$zNg*VV`HF%$?LWKHpZaYd2? z3<;39+mg7^nHQvD!|%Q*ihZrrF!d*d=oHE5&r|kNMhwrxM2PO-$xN&)om|BSFY~rs zcV6B=2B{<&Z9t*yFYBgELDnzvAW6Ce`Mt|<;rF{|rwm^P?$tr>`+lVHo2+8}iA9g7 z`EU~ZkNajoF)Az1|IGu?-P~k8*l%yYe*>QD^v`FWy&Hq4Agc}SHZ?kh*906iFBj_1 zO<(OfeHfZA-ETckOPw3LPjZA`a@w`rEZSdxpZT>CoEde}3tzq%Mw^W`)<3>qo|_7&vIEzJyViHXOEH}oFUyp{znrAB*GFS0zkT*_iYA&ypH7B{dX*cHW{r zRu?2M_?|O&P6+xi4vWCcagGP4K|WVp-m4!`m&$4T>jQUF@wjh0R{Qkc`vY_HQrgZ7 zBE7O7wG#6-x9~+uyzG&!Ll#E;;W|FAM{b*j`Ri_8#r(j#LIs!59SXOH(pia+lW=hJ z0YY}g!v3ArLqSJtjM0mUi|cXt#M!Y4>t>#RrkygUGmMoRS{E?uQ9rRh@!@kSH$$DT z(z?tzW(Q7>yk=kjQ6w+Sc%eLVsKay^$L&Am+o|90{o>L3>XEdi*56k9T(s7MWKzYe zKh9}*iV~2MRuk#S)7bLjJgI2-rLul~-YQ|HYtrapHvH424=_w%7+ zo$;c(0mgq|lN%65^wIB$(pxBU9GgMOx-Y#e(D(HO<+kK8Hc$I>L-TDEykuM~C_VMa z)A^d#?7n+9i*FO3=%qgLKe8)SnyIaqTm(jR-CMNTtUS9ZzFK$Eb-%V^9ZsFUymzPE zu9ps8n7nt0t9IEYPUVdLJB8!WGTz6C!&LUZY7{lMjX;#_s?XzPJ3ujiD&I~a&e>kO zH`Ro~{ZWoN_2zgx6{YdaKLvbJ^Uz(^-t#m2fAXxpO`czJ8|Aol#SZxE|H`cQ5*+um z|IDt6Ab}$|zJ`6s-^-QoH=g<9L3LA=Gew=yP1{v()!n_Q`{UAo<=lhoTPfNzgltA;ih)JX-G4THE7wqg zRkrvK9NbrCY`9K4RX7hnrQ|@rM7oJsy3d^exs@1{Gty0$828!KlSH4vyi%M;3=$o|t z!?t!|w7As8Q!?^Pa?{#d5ea|nf8&EwGSdyo0pr8KAJ4oJZ|5@6TT|V57>pZdRd2w# zOhZg8w!?$-yE|oplyp?@CR&<9o4&0LW6<>OclTWs?NpNSZ&kJ(fFaYkfvVe%*V9MV zN$rU&YwuNf*5@PesU$9ZDJMYJX2W7wpWPXrE0s5$a=#IG*B#`4eVR7>c)Fhpn$mNv z|8*hq?m=PmZqe)|L|&o(+57kBX&R6#3gZt()TC6;>ucrJy^-ugu1^v=V2o*s*DVV_ z3P!80|0Q(J7;`71Ll*wHmxi)BQOJVlMZY|o3A!l0LzUMKrg++I%qqGBNrL>|4$iu! zeWB5z=y=O*`1O80?I3^h_2XJ;GCXW~GljcqX5u=Xlh@P;|1(dHv?j+@j|I3kSn*tc zRN8+(^NN6uAz%SE`do@v!{aZaL;XNgg+8svbGDYSl1)KD0kh+N^xdsIp+|Z7BOV(-|kK6@wS|s^Z)Ic*)kz z>5T$s@O*N=bnXk?cW(8xCaxRw|7`0X{%hnZ|89Xe4BMZRN<)pOsKGa6n2(9$vTEA6 zr0~lf>Q@PDW%;;H6;PJkxUD@QIkgF7w47HuuX;Dq9k$<74Oo+pS)={k7rcM-x~=<_ z5zgyU$Aa|9VxBLry>Nw7`a8NEOcKLzu&mO@*2sQmv_G16mxLKQ48U~cCjGL}E0eNE zYZmdxt`8GqsfG7|@+dI^a(;bw*q%0Ks9RaY6b zLYCdKmo<{7*(5B9$T);iGxzm*E5Lr<&fy2{O{(>Nn>QvOnYCBUcaR{oW1sv&RzDH4 zAXKhU5<}HsAC@g`=Mo#=3}db5DN(C`ol7GRsg#iz3M|6U=zh(@w1zbGSF)BJwzk?6 zQ>}jZ9{egUR&1f7%8p&w7&)y=WNEjB7g%>)(vl7z7Q%pBg{&*L8oWTAY>4-z`6^}O zonAJMPk@W;=OX&gv}ajnx1|n|^L=jNkNv6cFpUklF4xxFPu2!%Zj?#-A(C~3%CkOR7;sll7@ap@IX$Bz63UwSB`8i>2QqS> z@m2bJ@(9B&Bb^6blw|UldHLuy(_iml<|Xnvl16;d1w(+P;-q&R~?e-H}Nib%jA=IWMILUW7Cij&CY z*PxUe*+QZjxPGq86d|~(EFjyk@dgM*!hu21!%m}AUaw=SK1rqM_YEc1)^%jZU?CY_ z%emTs10{BH@mAES8h122F63vTAG4^6w6SRbs$cPk>zT%G8ZnZ?{~v~461 zk@qB2Et@L(Bn?@G)R$z4Jt>D4Vzr(y^dwG2@l92$YcBmhz93kqV+;<&sPrqcumTAL z6htfzL{VDBhA7|ilO&ibu^g8(d>~0>gk6DxD~6<~fNMU(#z;DP;Xr^FouV-f6o@|H z#kW<*f&*^eIHG2OF^=gAIT9U)cu64)EgM=inr0htCPx*%aG6>^zh1m%5XjoAn~?@l zY`DTeEmcR7ToFj=vN$1hEVP6V>j5+U9m)ZT9mtTuVvveU-vf1kl5ZHqhNy z44m1hEZN1MCw_|Q_c$3-d^`$&-fTiYJoh%+kN2uAl$LD;oUl!?|BOuCwkzA7;MG3m z82sw4OU=vsX^PT~BfHHj@FjhvWQ>{}Iy7)0;`eWB$AT#1#A2?cMe2#;b3f7w($T&- zSj5&c7PCZ)%FOC3lQmhyLm;z`n7v>d?@@@3HbVk|h={b!O6tt{tRZ5{S3Zo_pv9n% z1c~e*V>=Q(Tk_dZ;+W_Uvb|;y6Z4NS7OPW)znLP}ii#xP{5&X@HlMGzOH(vREO`sG<&n@>jl?o;j+CfEtcsvqAfYA0VfnCjJuC=-XT7FX1K19Zi ztP*-E7#!!t9Ndq>`FZxNNW?R=z6pD{&}qE|8HYh2a>&h36#-!Woyax{HJw$dj<3U+ zj%4!(dQp%+OlF}jTBTw7?|8)baJHee(X@g&OQR0QG<`bO^mOZShSX)eZ>llJCJZ<% z2up}7jNm1`P|HRtgQUEB(bzxgXR&su1*HhtmCDC|DDAT)jURm}8Bn2{Ak*#ywh@@w z(?}xi#T|SEhIxUOra?pZOI%zz?kt)DZgRuaoW}El49efE&gE1JU zw$WtJF?8)-IV6<>WTdo%#rGx!zg-h`{gHE((3Ld4kWB>@g#a~X$Z7pOYlORlhzZf{ zS$SiMVk14*Sypzcg|rM?4bkzY9vGtR`|lquVK`3m!|9t^*Tr&gi7C_JSRhKIbk?yT z(ct^nYAnFiJ>eGe1^S9~FbX6i;&|zkFP)YQo48UX`@VouPC?5omkEO>=l9rOHwEkkeui%d`e%i;~w$z5VH* zdk%<%J^ga&{w#Pro!Gr~4%@2cZZ^QFeSrEDQu=Ue*M5IjdYpcGEgfW}zX6`YFPOK_ znFC7|KG6IQnxzod{pu`@qJ79dO7kwfX8dw&jhgj@?_1;>nO*o9N=ljM&Jil#34Qdf zg1S***Zp+RUIM9WekTkIc#-k%UO9Ft5E)FyM&&71T*dLA^ZW! z9zJ^}-3}0!ni>9zBsi6WyX^MSc*l8u)i0IYtqJAQYi%(KXx#!CjjQd zHaOZRUG3ODxVl+SF>0Mjivvz=doe^}h{etXX=U*7czF#5<6`4H_1_wJD6PEe%POCS zh~2Z5nMsavT8lb_#=$z()@#PLp$y*pGphGLtR38>>iBiesRbqMbUhPAzGB3!4vb^; z_ugpx6&`+_SIjb(=hZeomWg@x)CI6KuBsuj{7x8 z^0ymYSqjob?JkB;e5*r5BgsnbRRgczN%C7=YDnj3>^Lk^xD zGLJZ=cZY!FQnJebT+TWqo4koZdETC6zeqh6Mh2zk? zW|Ii$8Lc^&F?m?Y8#fvm{UxFL8$YXiba0iZ-B6%xXdNKX4_ECj@%!Q&^DBMXLF|*h zwAEm0#RZF$RLmFRsaAM1v3r0{FKAevWpMaUij!-@b2F4)YF4zY*A&v&l^CPqf@WqhdS{P4 zw4sS2mIv&({ol{>6P#>~y)YP|e>qoGel?k`Qk-)O z)!EG8VTJf~Np?q5Uw<*M zDR3jlGp3MUmgVgpnl1yk#M?Mj4RRuZoE1U8ha0R8ekE7SA|Do{7TqXkrOL)Pi%^dQ z=#u@IZRX2y;u-|n9;tYBH_!2U#7rNJW(9FpnPae z?_$-(=yJyu1J4^jSYd6Fab-&I;Ns@?+UEhrM{yC!IT&QnDh}yA<3h_C2x>kZxza z`u(nW1pSlJ#+u3>F8C3=)r&e-_)|xN<(9t0sNi&3ZdYR3g(xOq2_SD`59`C0vfM9q zO+6omIt@;V!9xLjZ0zWn3+f6_1e!lHRN%OuVM+N2`a>yM0&~|kXjPJAzc1Ix{mD4? z5=hNG6c@w(@r?pOeky4QJEGE^NmZjv-MT=HevoTFY*Zb4h!fpjk_PjZpBmTdyVd5ZFd$+=ngYbWh``3hx z2#1JB9f{rbm+9j7BmLmB=|7E{fX1A3CaoyO^)o8Lt8QYJ zS;_l2b(KjzNT_sm@+EqxGx>H4fjDj>ygsp5YS|L}h+w%F#2gc_TUQyE{erkq zA*mzf(#gVlbW95GX33 z4@E-*ilhQS@(y7;vyc?J;c1qT9C;IP&L+%v<4Thum+FjLt6vJ}-9@ue*lo&0&WL$Xk{k!mtjXIXy z+m|i?s_92G>e!=2-%-5z_hXR1h&SIY77Cpbd0fZN;9vS!-I-xavfYsi+)*1m<(t;< zJR=jLOrC}949n*=oH*Z_J>dWj#H84m@l8m0a|Ft2CrN3Y8P~E~Cm8D%Le{ZFAvU{M zR3~tfal3Sg8V+eA4D8T0>yz;@=QiJ3T-8>dAfcu{zk|Yy zLEoOhCF^Cs$(Vg~V8`WWha=X$X1)4WtYqyPPBm7cMklHp9wh3&wqw)uat(}3YZ-6;j25s2 z{)C3qW?y!*ockS>kwkgyN9MFTxNxX!PGQk+p|Sp*v;#TRdS>Y5d&QDNj8oQP?U|X? zI!J`>_C#hLW4AJ5%|ZA|}mBlqR-~ z(BzA_ct%8>#n-TyI2$Ge+ot!yiWaf#G#F?6b4YDxKol|Y&g}o^x6?-7w};u~b3ew) zD_Kd|5HmW;?gmX&`lD1CO7J5lbuJK%OkecKBY4z`%^#Vnl!x0MYh3nxU=P4-8rpM> zk)c75bCkxOz$Z?s58e=svC zpy8KVtF$4Is=D*YzB{|7@ORyAaY`c9jNTvzC~$EFOsq{U$AD~aG_f6U-X#bp<`&~_ z3IqR`_ho2@VPRR6{4leKEMQ^(mueDA!bZSU#c>s+GFjOFeV*g}ShsYubnyghe^zXV zW4Yxw=3GshFY96mUjo8J6Y>Wf<1wu2#Y07;s)q!W+)Px>2%{66k<#;x4Vb{abhrwCuPJgUifuCGv*R7!k_HyxC3Imq zO#k>SspWx_BNlgRQkj&886JYemuF3RwuyrSD8T8r$N?WZk z+724#jBc7xqN|s$oAT0%x%pfbQl-lkJ69TJ#F1-{m*{(J?8lcc{93bRTZgk?*mON7 zVm$+9ft`?`5=^@rF{O&g}B{EXOe zwvqfQ6PV`a@|>Vh?xS#3$S7x@=HyrH?K>W0l`d-@<5%uG1z-HbK8`Hl$9LciuYEgH zD;ItI9t10V(TAs7+ETAc=M>{QA#q@`3eShc@>zCs7q>&7R^a45l5sn8_l_uoV{-kM zhj+Ee$r?*~H>9hsz$x)8YC6>=h8LsdPKa(2H&-B_e-rrb0MRn`EsoQ%O+Ph`!tECQ8& zQ_4q(21J`FP9eN0)@M>yjX~lPQyrqp(_kTX34_h}I}2>w^q$G`g@swlJaKb!;6jLJ z)~VX`*uHp-B$r9$*T$S$mOAW3^fIQ0+IN#+Y$j)^^Q^&MtSVEr1S`^mU)IdL{@95a zLjJhfkIEt?xKG?z5F|hDEhY6_JmF5Qen*U={^Is6`I*kfXhi*PIz2ylNa?OElsPE* ztuqY=D{c&uy-NMhw-aV&{Q8p69$?9o$3PhcRfjOS@Z&dXVc8>H9!;4g>>RfnV-sBS zWia+{sv`k6!id)q>JHQk_Ol!@^ANSJa?vkTjDOVq;MtXAUC0cGfCsQx{)B zGH`(WomhPNcFyD}c(g{N&qb$LzweJRJuWn;9roqy$I%33Osgk+uh8v!6B~CVvQ4)c z#Y%1CCUpp-3O22F&jC4iN<9!`quu05?sAF`W-6W*Sbf1aVMvxHPI{~%#w3`)jv3QZ z2Bv(53D=^Zd(Uf!#|hniE^;P?0XYYP15aKTxzb1Ge&kD!n2i-#>>tkbhIS*F+8XsM z^Yx@>2f_Xc$@CONEVbNsbMgFp5bJClmUT5(9lA=xru+}A5v=W|aiFmePU)OaTVYlt z`~u^MXV98=z3IWZ^c3p#m2indbthT7#2c zvcp1p(8fLJ{_mKFsC3hOPxiss>w#*jYh;DPS!z_~2=BtC@e|bNQXAG7X}=$Lde>TDO&CR^m1y}B*sS-g zYGyz4?iMj!Za)c?TO6i;unZlO{L3{bLSXowVa~OCMn>^Bi;(W;4JpJ~j*CJR>Ce0B>CrB)U;T4zm8Q&TbAc`ao`^@rke)dCo0I>2$tz%<9GT3ud4$xImp zZOSW2PdNwjF=G>#zY`TWUjIRq#O=3p5b0;-7S3m|?!?5cw8gH~=j8Cy@ps{BP~m$i z$E0VIqV{g|KJyc=*WgCbi#8@&3a);nh&F9%&#*2QP zh|=hI4h*PkzyG1hVq=O@$7-{7M3eUp5ke^WXh|f->jjG769~c^;$P1Zeh>}8%zA~a z5StU3A?FNJa4(kELHX;Sc@W9O<@bIU6uD~sDUBGPv{WD=Z}Xl~b+7L;&%?RK5Gq0e zq1Z8hJ$G%q(-q68AcXuA8`)-o6nX#WP>-HUBTpRxFYDHGM9!v$@9iJQFweQZn?7pE zdaP#%E(kz^@eeA(NBpl$HJ18a!HqAB6Q?C7tr`z}n%{P`mdykzs45Y)4o5}DZo}pt zB-2b+4(4#^UeTn^!Sn;rN_9C?Obm>W3s1Yri<+!f4rUeYgf9#tYOUl;i0iB*XBYQo zJU7M$(Nnzii2bU!63TB{zO%>9%^+yENr7Nv0Uv39R_0T;Iep-YX9Rr~R zFaVr;*v7o1OSx}}2J(o6o(JqDWA+4KRl%?BdPYPP*}6~P%DZjo`4(4-FxPf1XwqaM zrafYMcG`zV#Q2#NHH|RbbFiMi<_1X}Bft?$utd?l5O+cekRK%Wnz331j`TFAw z%rRO78{c(IZnxsb_$yx$I~_pX{zPqUPXSo&(SLHLv;6c4{KPRFQYVuiLyK?kIg_=$ zf`#>tC6M1QlC3H({sqo7mt?&{`^yn7>ah02CF#QyeQ@05+ZdSB;XJW0xYGfBQ2TE` zOBoDmo;w5iD>5XuY3U9?>}$1(8H3`xAQlsYY{r4?PLIJ{LfT z`&<0VQYj14gKG5W;+Mxdj=#eM^h?OlakFm%fnqGQgnx4+zBc};3NFCxIZ(y6Dlp&H zC7Vj`dy4K55Ml>SRH_hD(v0J-?-b@RD*bM09fiyZKbU}pe_-cs`N2mqB4At%m@=L*jKox%5#mMiD)M;fj;I2R7I}@Ziym&%9EZt=Ts8Od2%;R4@7-mD(;b)O z!VY0Ud^J~r{mNKjVNPC)5)#kDiYDYda{lt+&&s^HT|Riyzdi7;&;X~#M-cF5&}zr* zvVh4G{s8_{`6gD98XiLh0#PM_{ZEnf|5V0{EjiE?>|%pJyVyy&9AZGlR|!#d&+PLY zA6rc6++iQXhpk75+uDg)Drk3z!+Tq2kz`zIN0sRawIvApZFdTy@s-lqkHCxKBxfK` zag8KAkplb3Rkx~Vg*q~nt0T6y)u0A~M-KcUgu&1jqqY|!7;n`qZkXNWTbjYeo&2M# zhlZAx$D*{UrPibTdj;_FSXEJdQJu$~A`3?tt9G4f2MRZ%(gzVr_e1qKas2-bV^XHH zC`G9^`x5&W5H{IG&D$#9!nA$mJpPaQQnM(azRpP#M< zs7aVL+StcLTO4#eZ`T?LG~s22^xlk`s({wktK^2*2XEjwjX(^G$lC)-PEB#<6jFx(ur-?eib_L>f$?H(?$h`fs_-nz8AaW>{I-odAAA=@Mj7!=gPzs+8q6jj7)lv^I#IU(BzTY1#r}ryl z+t2tX~1mA6K;6~L96tjx0V{JL!+j*?W%w>|O{Lb11WV2cON$hb z2BJ&M21N9}dZ7UCdRB9Wc4UeRYT_j3!LnaQa_q^8L%)E`mm@CD(1#ao7S14NsI}F= zGQ`ZcO)^|>8MQR~On0`SubT&8D3Fe%N^UI&A5W+Gh2GokX&@X)tRc*3m56cYi8N%_23@XQPOv1(>8;-Me= zeL>L421YL_t^NAc!u@#qmKdOsubs@~@o7i(i59UXnDL}^okq?A2&o81ZJIfc%>D>t zA`WPO076|bzl$UJ0|Ak&M~WUCo#_DNOX&dS6Z#-%Fyl6gC#HSfh0LOT_>o@eD$o)4 zP_j#ds{;@``z9`b3F(*#{OD2)I3`04H~X)kE0P5{_XjiA7)AQNCo`n&E*hn&u9HzB6i4v_Ux}#&b%cZ)$Q#XTFwz)ZVw;hSv*4CyPX6KW+oQjIK zH{Z*Nta=!b38CN%@6!Bq?VRoEA=rihUk+{@wII{Bh{E!2c?P|>nSw@W=k}qHe*i~t?;qldfD zuX0*&rj7agqpe<~;kZX+B*|^CEqX^Bgdy>;!s~?ztjqi8vtJBtSh;tt2K)X>hyB+_ z^616QP~tr;20_y}5m-+GjyM*v+Td6FFCL%k!a1L#-NK90uxdMMNdV7l(mX-8TBk|W zTJv762|9J*?AG;*Mb+It2#8bqnP|-4-D2A!%!3Bkk7RN&60~!35hcvbV1ON`0B`l3 z)hrgTK8f^WdF=~SaPI|{&4~y~{l~K%@KLB{0|uA)_v!cVHRCH<+!+b-YrECdJbsUw z29Cu;7aYu|fOaLt$!wu@n2%qT^kFa{yB>qUo}KB~plf(Z5HX673$Vs?C2V=tPOOl8 z;4Mln5A*qE_*l+wGJWD9h#%g*1D=|r4G!8Rxq5!#{|svC)s~sSVb`=D8c6#%Kz;J{ zGD3LLuXCqR$G;NJhhaeuBp`)ClvEw1T$386|7^K|3wxIJ=gN@(wM;1UXCRP4t9rG# zF#vM(>D5>rVr>P<7a-4tjP0^g`^uK#Jk_d5o&atb`ym0-V~`x9HL^cFw0u$a?e-Tb zcj^-XJwg&~Z|5{$uWK^-#!mtb@PI1t502+JaBF7eWETnI2rkN1T^IEc#kYFFDhyH#1r&#Q&x!ViWsI zwAZqIz%j1(|3OncAS>E#ME-1T@Wdnmat)GRpEDF)fWuk|3MM7*+-a`|`f#o@gdLCEFGzpuhJEotC-c(G(r z$5IYB7Q^GC(K9w23jNp6Q*2tPH)H^lhB0pc(WpSU%(frD6^ed7sT>niB1(qJ~q za1otN3KPZ_afrE{7x9;2`H8 zDk3;uRBZ24Jn;6k7-#@fZS;osM48xV{^oKZ|C!}bt@dnydxdF#I}Rep<%Gm1@yj++ z{WxUiD=jT#u{9ImdR|$TW@;u;d%|vnDK!}xZs%^#PxxE+SFigMm>>W?Yq~$Lx=m0= z$|G2DNq!ev3-UO#coO+0Hg0=#`|t=5cKZ1=wDFM)F?M=52>ipZ*=Hqo{&3t?Z5Lv0odjqC$ z&8u$bDov5Eq3IxaZ~*OXPzwuna(na}Sf%7HM1jN}Vd z)%yYh4Q#RV(pTTHvA6ScKQl8Y%w`V4TzKfbu>QK89l~0`W-#&#O>Q|0y)l_!1uYDQ zK3MVGpjNV+sIs<+z9}8zbGkh`JUH!lMq0H^uS4FiYpS(iyV)OGFt_ak*YbM0TnHJ3 zna++|v-u5QBg9&ZhZ!Yl69Ywq7c zYPewMJmzG;vuv?YjVWah_BR&J9%hh?i8G3EQ#X{(J>traGDdCS*4B)!X(4maqXMZ5;4PT*d!p zdtZ5z>qJoUovWpK3>_8Oun&2_7NGKXW3eXD?avSeo}BGnjSWC;+cniLb0W+lVaOiE zZ+lcY(GUgruw*VdYY_wzZkaDZI;5%#~04vV$J^!x?s4i04P zcE9|kW%(#C(-p=&W1?XS|F?uaLg(@jLll$61h&}XIxjk{yQb3*l2Idwi>=x|NVyBq zm{JvESmIABw2aT2+&L1%-oB-{y<5^BBcOT3jH?5K1)rTch+XL8+dzyqF>w?H3`V`k zv46P+v~!f=_&eflTleMYAnn=HMdH1@H_V4$&@a@9cE`7kj_&YE) ze~6Di!If%;hl?fh!r$IA*eiJzPSSy(*4EhC3kLQcy=L~WE1PAIzVKiDTJ*YKBZbr)?i~y_BdR(izz^Z`uY8zE&2`fNz zrgM`R2XyR+gc@Q6cGYORzm*0F6dUV;+DuHg8H={lASM2Rda~#c%+q^6_|J>#7lO#h zU3J?Ux_wZRex5ia0xXF9!FF75)`F%3qoBTTmuT^0ClBS4NWkY;w0y;rvU zfLsyVjsPU_kS-ue+ujA~iEDwTjT4YR`1-}jX$z}B_A-rdN?o%2&VTf}Evlyh^h9ud zl5q>N+lT`hnTtZyb6MW`Lp~LDk^f^|dCzE;f02$Aka=LlE0gP@Z2uwt|G00Y^@vdC zw?ALv;;y$hNldT8)zSyJA_7Q>-yxXvTSy}0YjicHXE8SF%t9AW+nm3Zm6x9kL>Rlc z*cln^&6V?OXgohZZ)9p=ba=V)u@WnJ>-Pm_{vK9i*9sfT ztbq6FPfGACWMihzkUg(pYI7C--Nb@(0k1HzwP*PS3>SxTidbHW+7zf8SgsGgzu#Y9 zFIZVAym1D;ANFTvl5g!87{nnHuQ|SXUQR1x5`((HJu>W`$i@G9o*qO#dP})$Vz0w1 zy{uI`cbDF7d%9A=&8!BIrIDVYhjhvO>NyN-XK^uC2jLA1^9AkkZnJ@6fUz;>;NDVW z8H|X5W;;Fqi?z;~89+a8%0PJw;zEd**X6~P88kZQ<0OkP&Uf(Z+AuVe81|dM~ zd(dLA1xzn_2xihu+y_adBY!-1UZ{EW<A`Z-uZH7=A1J#cg}}<=G?iz z=;|Nj&uwA~L{tD7fsJ&+o)?+&Hyi{m@u#6FVugus%M~5sv8iq#AvdSgF0s z^f{#vR_&$U5TVDeyMJ)T+=!?`KzvALAZ4>%zm^=E_fp|#2Ir+@f|V6;klI9NXI(O+ zZ{1~kcLo2`_i8dFVO*<}r#>1}FZt<<0blp4BAoRQy}*Zqn_>#tv`3#`upj?4cXKj~ z>ElWu=-H?W&D(DpP{LEQg9(fcQd8J3p_tl~T|4T$*jGqS5x%pGwTV>kk z655li$`t}~9u6%_3>411QWLD8D39HV(zX{NHNrdsfN3qpdB!;K9pvF+Z%VIDQeCImZXcfyW7KczA{d045>4brXa+Sq0tSCMIr*G3*`5 zxX5k~-N~Q8G}@e=>{zqPY8k$T?YrNAB=N0#*W5;m;KS!{b^V;!ZCE#up4zL@+6#dn z=Ie#5?LA#KGs~0G-IM|Fttn@w?eyJ0%cK_vQm(E!P%CU<(rsrt`UC1S=dyy7;4Unv z8;hj2b|V7an#_GUO_5!3mzGCLS=jGR?-PTt&-Tp;u2Ge(hNJkkW8xlCY;Oj=lgY>p$ngHcxooKcm=dx~8fF4O1e^#aY{xDuvX+;(I((+<7rjQ_Zq@fyn{x?b z>*Ttc_!xpohIEiLr4b(Ow34{V7qq);!_rQ!v>$2QKY?y3_HQB}oBn=1jRAas>Plfl zuZ_XLo<>=w4S$IHqs9O(KuDO7w&Y1EkEA3pCxrC&!omfEEq}HYK-3=j;e|B+yfB1_ z){g5YWa*Fu8DbLybny%9s4)_dKhKpI7aHk`Fy%-XO)r85rX_@656Z#GKCe+VW|(`u ze@!fg0bU}|nf#4BIO5i#AqS@eBUV|Tc|d}Mb&x)V^XytGik8--aZKI@Z(|B+-&;cilhp65S97Ku1^drR=4rmW_Ixk?66MJ%C3Od!(Nv9mCrhP%-QUljtlC& z3+agud0m;R(J(hK;3wLER|gJwb zr4TIVaJ}&K2F)PZt%273k25i2twjM&LFEur3R!SxjN*Y#k5QBmF=K$6>!$A&`5?f0 z&icWA7>oV<$bL1I7v){SL9I(}-}S@AfM?0E&{FUXn_V%&BRv}l-W`BNoCW4{J=NE~ zAknJ22L`6|k;jR4L$Nzzd`HdEWvr!W{M~cC{QWO2t zLved*biZf{_1`ij^_#!=0CnL%Jrp;rPV+Yn*7!|xYM_3}yfq*g6SMm>Rn(ZZoNMk? z8GC7n&D53$HziE&yHjV~VJ7uQ8&HkQd+}!#u){Asr;KQmLqs@74o45ae>Yps+L`G|oxqDWv!Eac@!yzA7Cz|vFR5&391ZkOd&G^?GbDC=nJ*-@}J zzaL1sBH(50hrH%%b;d_AjZ&QPK;8OB>R}%KyfjkT9*6;m>jRF~+wvtv!=FR9eCNc+ zR(36Q`!H|3b-VG}2N#Njbo8@<)baWxjb(3_`{*TgR6-m3 zD=SJQjTq+1KpDBnfznzmcThRe1Una;nfYSqW%b=z2P zO;5oYl@i3DmdyjN<*Z2TduptagB@hk-4K#XlXbf?@5j~(KHIyDQvD>j{VaXwozQcY z=z5rx*IOCi!USsV(+MK?V0Ek0M4CP6S)3EBq~%eQmSxo zq!Oz0*f`aGz>`+OD|!ZxttE!G$a)vPVAySIuOzR9I#s?UN; zsJZpAw}LHEm-Qm9Bb}~;QpZe<9YQz3q=kuZWK0n$37yq_moi}OTzP=+@(0?ri(@A~%LGfqp=iKvOlz4I!qk_e_KcL=a zaDU^-x9I8;n9ULqIaqec=dcsHXoNRUY4d2h_P87}3j=8>Qp2fqh^4ZqrDe^oS-j=s_%jsmJ23*VGG#I#ZNYP zn|_ux;6}=KlL{VQ<2YNeMvMj7`+5liL;sU<2Lwqn$^4Py#2UMC5?RD*m-PpaWWfLLvDY^hPspme3%*knxs;ZN@*EPwh-hK&3-#hq*vKY_K2;*&{mvQkBZEhLUMKK2h-)cQ2}xTFk5S)3T!7{c z**4P{+;x^K!`sq&rnCeZ)_B7zJxB}qc1c-3jnK3MXj>hL?>)aOi&Vf14PQObjd6^@ zn;D-TAIn9)x1+>6YVNA2Si^V$7**lnxU_%)MkC6ZdBN_<6(O*#MMAJCJ^6R?>UhZ< z5z!{|!wZnv2wP$%59TVOsTHV*6s!EJlC7Iz{Xd_Q2!7+1$Cg4x>f;+#f`$4*`=SF{~w?LW?MJJo42Xxw%Df>|jD`C!k8 zbb{u!`4Hos^=e~1j7#Bf67MZq2I*n?9yZ1m(nxS(xgyFL2Y^*}Y<3;30X?!BTq^MW zQ2c}Xcln0~N~KYU?OMe{n!U!P8!z>E6v~G(OHmh5GsycLv8coa9zZJ!{6=lnDa84> zu5F0TG`xS7 z;a+QIYoTzQg%8Sc;aGLI7$Mwb*8uN)z~LrzEBdg0{TTS@TnIGajfdoJy$CHX}wJ zrs50B!)_Yzq%q0Yw{Kk){*dpt*e{Wo*b?v~qaC`uDaql8HDB9hDu^CW4bXzf`D5e- z;9(|v7RfGs1QlPs8!bL#>ZhNHtb4Rdl+Fz6S+6uG?S{XG8@BV0%{IH4a1J=wg^FtM z^gjAYc$^Qx7!dnbaN2lAenM2(teH8U2b5E^e(^OKpbt^|1OB+rqjN`$M1r9Zs@CMc`AKa=I7W9X!V(01+Yr0&x9))J z7QVthd3F}>b*5uz$xBTTs?vFS!RdzRch*eNtT6RL`E$Yk7KahjSB7Ua(f0IMm+E*H z?sg1e8$@RoxgxJ%ndwzv#XfXHopx4r9_iF9BB!a~>b~teQ09n7$oT9`Q>w`LWSi5Z zdvegTHRjzD+H9{5<+Y5DkbXd&DJAoO0)3)=Ojkrrb(N5GB~hEas?FoEN7<|(_;dqb zF6pHSDcv;*7q`{7Q9^%Dh#5{+ElgtN(Z!gV8W$!lZ9%qu#TM;Lq#arZa)9WL(W{z|9|CEaW08IaxBz+ZUZE*RQchLr{AU&nC zf6};ceFkK|L|@ND&y{d^Lt0#u8aep4q6qarVMqWd#AmZ$bv7iI^V_2jYF!YI)19*4zavZ_sJWkaG`sw9AmFX!F#ob(JfBp)2rp*7<|f8S zWxMTKkieM*UGNO-FC#pc!S+@Y!H0r5sMOME{R7Jx( zMbod}+^H!Q@Nlr1U2nIx6H{}XY&fiTqbr#y{qEo>TptJKh`4MwiTHbnt8Yg_$Pn7|iz)KDd}X2R~e z*$aiHvy6Htqf6>0k?Oa~a_7i5BUF72zHH43a!J_Gsn}h?Rd`fLWYKDbX3Y|GGU)y7 z!E(-;+w$6;yrx{I2?fuGzw&S2?BaU!lX`>o*-ngBUBJKGZy^E1sbi@0^}nf(#6yO+ zqssJCNxQ(M$k=uJYX_Z&1aVDpr%2USj&26FP5h-`T%j4Opv{}oQ8day=i@gs8r`qo zT`?#0h-%Cbs{Qa-%t(+7KL>F}H4u~@1pJ@n>f5HF z*>+quQ2UsCOg|fQx;u!`dl4LP7_sGG>a?oLK5jnm?Z2vMrc6%NK;sST9RjoY=<{mVf2wPN<=l` zv5HJem|RA5Q@!5V=`EM9oJQpwuxIe36fxcRVl9#PViT!Lwex3Xv2pJ-?7$vPQ*@ zCdU};!gY+IOP%f6Zi%&;KWDtI`emY2d)@#eBxx?(zLbYXU4Z;iBi!S#C-&=cKOxyG zG@J_6FshjHB~X4RFOu$HjwtH+TK`BB%5bp=Akhka=p_a$(Od`s+G}$g_<{B{L;@5B$ zE8*}xI_zO09F=M5!Z%#42JGZVr{yw25m8`2*GYp%w_yezC9bz3eEQ<%5RA`Fp-AeZ zkv%0CN&e+w!0v9un8iB<-&^HIwy7N;hY9kf9a+N5M{vNS0A5^InHNFw20O%JaHYrN zcJj@MoiVWAD7@6T$)=|VEUZ<7FJrFg1a@v5clu}|V!}z`Nb{WYx)IVAez8Wc1QpE5 zghe_m$(Yra>{S<#C|?xEiEU?bM9Nq`+v=cwkT{~GMYLSi>%)cGx(Z=hQ)0P+Z+-FtO3b@!w>d zaXV|r_KIcKf9@8Ik&>RuI-gN^-)~}Genn1w4ax2f>Kd)`5PN_|sPwY4;Z2C-_yH7; z&PSQGdZm%pMv+H+W2!Bm`xzJWO+e`0?Kg?gM~XE>0APszVZBO#PdfnR+EMxrLbk|kT>Blak6W;YygQ7V#oa{@;{V^vP(5#J1(58xMNZVU_W3vMy-8`qZ^E5bG9o(|3Th+^0Ij~%a^k}M~;vm z7&^8x5TyAiY)-zN`LCH!(;Iw~l)|Tq5i7D@7%6=i+t-N7GHV0SOA8QCtL)Ram*(B+ ziQ;LjS$+efNt=k=xyx8@`k&@3(?oB~^sQ#&0W%E08%5IaIOS9%}HdE))@BVg_;3%&?e)AtT~p^2kS zmbcucm4HoL%2fdBe;~LjDBLmq9{`$?{r`X<07Ci?5zU+&-7Os5nY|qBFZ?%L_r{Vo zyYnFnT8KK&Kw&R+w)vR|{EGxV8P~iG>;x|7*$7+`Kine7Es{?2Z!voAUY})hbU^*G z^@z|C(kKnAG;#Rm5R^|Eoy|qzerG$FBz?@&B#s9<|Ql+)#d2o!%t(3jcdGFr2+@q&J za=7N%xzc&R=hxDEYT)(2|7mc*zqYYtaWwfpzO&HzoNllbeXZ|$ap(W&^gt7#wdGKn z-)R_g1X;dw@OV~>3#>5TzHsOXRl3i^FclRGc_7X~*)pD&Yt zdS?V@NJ=A;am$ouaH}pHfAayPP5#=9{C^;Gve8ygZ2#(DU9A^MS%nNFi0f}S9>$+d z*XI3ty^IF=A=Rg3o#w$e_n=HD_#Vj;0K>* zyjRG*$?WLP`am!^UgzlM29^`!)J{kEICL8c)m%<^(?a9`-Z@Suqz;^f3@_=>o4wc(8{=C#@ zF91<#TFa|SKmB&}Bm#Jzc6gaX%`TD?vM-G%!EJh9iwC1*Nq$He8ixM4+i-XDZ2DY3 zO?pFT$#U|De&!0nhU;-``Y_7_4A%=vzfp(fd|wZxXu5YrIXl_H{^-p{{)fL9HcfJi z;zw4^8DZBkVmFAny%YUq>s#6trNh$Xu)&q$yMbHmXO*P0={E4FaQojT?>gD#-2463 z_X`|ObG4Y+NHD~MJ+aHeDAcben1$g;82Ntjcc&5&6RqJfwtutTG zvPSo^%dw^_SYA>K$|#HxanYj9-IbNK*i}Z&bGVZ+?+RjB%Q^r-H)ICk4nWp znKq9sDNRUnAMUT$)P4-3o&oL8N5i%s{NDmTX}mal2aC2k-rvFQxBQ;HTYheTpSkc~ zO&Thc8GM<7W6N%$Y<83ch4y353{jBJ>I)D{>^=E?DT!q*7?nP0*NAP7& zlMSvZ7r2Oj`sY9U&A-(oNB)C(=z{!{l>BQx7=O6)W}Y|xM3Q`e_4MT8kq~c(3t46_ zWXtvMb8-yKg&IN{77i~cnt|A*DuUcv=i-Np)Vi}%s!eq={39^9<%hrm2Q_XKmu4+& zd2uRF_+5P*|_d6+$1F0k&Dk;uU;L}&;Gr+2f06eK}Lat4=#KmsJUBp+_&H1 zmm(65QOLMsL(SWz9o?Uw|S0hbg6m<{Z)7L(+|=c zyLc@)`X6|tssYH_Qp!&|o_)f!RMKmY_j`4D;Su^E%~v#x_$M86{p~gJ;En-DrC;c* z%%CG)M**%1GWcmxFk_0^@e`+=;_?ddFeLTH2w`#0;YalCf>bqc2UNPvJIj^>(DQQJ z;I-RZyTksodW;7wao_g43U#u~`=i~k+C%RSlsf{}Hi>>eSwPY=((YCv;_m=xpg~X> zSKdiTpDvPkFEyQY@u2B<*Y}ll%>(U26_kbE-M>pR?@1aYH*WW_`+KMP>bZBljh{PV z7uFMeGvrD$raJ34jxLXb)4PrUfbXB0XB?2xGx&CD955{Y6?>}wzUUR9HqVJ(eSuWq zzH0A!%lHjq{b1NfhnQ~B23)x{)0XAfLJye2(#Z0PNr~yS5nZ70@G*D`AS$`YmR<8{$@=4QrXW8r zt~!rV$zYP@Bz*J}x8nzGS&ZHuYFmsQoKoPvp&ydT?14{_O2ntn}LAZzVfbznbaY(@TSa2Xs{Z+LDL*3T4t^m8j}Li{R+&L*UYc2V~C zR3$|D^Y?kIQCFLNQPqksRv{5(L%%F#%D+aN)L>{=K`p{wqC*q(V&SUwqe=Vn*!JuG z{-T}F;$wl~h>qX=DwWb>8?`(XV<;74h=bqFVSJ0+l)m)_Jf#bL;&Xd{d!9Y;d63!) zcb~^A<+5~EVe=|xY25#0nSKP+u8W@SgZkstk!?P^g_37A&+{JY49C|n#IJ^A`uR^B zTy~d`fR`|r;*(RG--O*YZ6|A(oJWz(9g_=$+A@?Z4t}JsY^fX@0(W&si$!U^X=%Qk zdpXOjwyw^A{NEUewQdvEfv-~~Q+T-?W`#IKybi-GQ@5~9L*=HW$YO7Va7Q%k_|@(> ztR^v*ncIV`I1mkmJDjciu-i+#DHQTDR4+U;qcNBQ+MQo8reln5g^{L08M>sSgO3ms z)Q?>UM4)ZLh|5%=TzSNd9~IFO?U<=mIa8!CY&`Q$rH32rLCs9DNX}PQcrp-ZRPbJx z8O*5dQRFCQj3Euo%0g=h$1B2tF50pGMwr65ZTB%) z)5K$dg9Fp*V_2Nt`H`^ejl1~gmydvyHoMIM`agX3qx-I=f32tcIO=gt zm@#>x3H3AGNu(~`Y?#><19#squSq5jc=3)H|1A~974bBBEmS-le zRw9_KI!AAsj%2jDbK%ZKG+TJ5tcnD(Hdjsu4y=M|i(ZqCKhW27IGOl})#p7%`5XnB zuvXcc^LB7{5I_VzBQTL{7=$ttlNp_!ckz77eoeU)ojMN^apgg(C2_v znp3>hlpC_R0&!GSLTq>Be%hJlt!LGPUV&5W)8M_?gd9=9zwV=@ac1+@YS|q>e-zhq z9i1d%&5J-n+oF-21g;I}!_O`hj%7{~s$D5TTe_Qj;lBS>qbF48G5>CE#J1Ar-Q7A&7rwp(sR}^-7&=urprY2NnN%nJ|2ZWZ?=5mp8LxjkGjjM<2!@DXH9lx z&{`10wH3s1A8prwv1nFKbFuc{t)!kpYOoCZ41%j1Cj8jUl)qZ%($P#pF*gIT)H1dq zW(IRPhC;A~j50W-ipGrMVPBIu^cI>ONJ_hhylTImv0&m~x{wqlCCS{)vVscF21AYF zLYOggP{&!zklK`&dG*!K>@7V$WV@kj) zZ32(_ndHCOZ%=O!O+N`K%*ESZ1x zP{zT;k@NnSNp`$rU52WnNXS4|*kpc61A>N%=qN{RS7Yld*xi%vYL4ye>5(tI?5Pq4 z$BnjqSd2%Kk@|W%#8&`$rL&2=B@5TD1fglcpZ0ami_N>*u14GrgAz#bNw2@j=E;|J z#xwUJuMX+cCDY)hhM6ow&hiKb6c-!fRGMd5$D*tv*hY>{(tDJc zsNz&@zGN@F!fnE4cDyOO6Zo!zd1wB%Fx}TjI1=1OacC=DE^!s~WwPkLQZZ?*cg*jW z&Q=hN?0R4^?_xS)Yjj}F5cDINF8p=B!=xU`glQwf;Nf{IV;g+0r13U!Zh#U~{QXWw zyFWhwCz(4x7w`&rm;bd?fcxz_C(01Qjm!H$NWtf5?M@4QV670?nxGNIo`I)%ly<$& zb8;rq#@jCF42|@w@5L2Ifjf{&$%4fjiba4AiQi4(PN7f5rJ$Jx8IgLw!YRX~d5VW@V@Rn~u3s9-O8ShROx-oH4)IAbms6P(%Q%cV+UQ zsN_me89>(f&Soi45wK^E?<(@k68-FWXJo-C%#fr6!LQ3u{y$`*NGmqzr=9#wMh4p{ zo^mrirC{Nnyf~z|3*pq+Ww=#$7AFJ}7onL;;Z=S5h)y6hN>6O3z!s2YYH7T`KN5y7 zzNoAcwfX})3|qp)lQ_&%gPUt6Y(Dz~>PL!R)C)L=p=!|8YLXIU$1Lob)87$W) zft?#uo=x!qhd19Apt1#a7`~JF83Mm?SCk$xKI*nVv*7P7-=WDHgT7|h3eKaH5HEU0 zzYhB~N!Z1P@Y;*-^vxtrl@RaeU$Tna)?Iz(Uors#Gf#DsA7w!ATaeHo>)PjPujc2j zjL_2PqEX+&U8XPm=Ofr1j*k*6_w^mM+sKJHFHMcov%ABgGm!J!)1$;g{Mzv8Kj3#n zNH3tj&ZhS^DAWwVI|ab6J3PeWvG;Q79zR4&+?t+;J>>`dKGJ#p(dfP5_`bZuw$L@@ zsSQji8I@<_ro9vL>)RCeq6IqkPK^X?k5}pMaw!>(#hI6hQ+J^izdGvJq}<@CAA1Dl zA5g(vUWLav3YozrJa6QA^AN7}HF5sw-A9dH*xf-OFrywRGn%-*k*WQ0n)s!I^y`_$ zc+gG^DRz;(N!t2d39eN5_L2S2oT=_ZwJo5lSgMo6CeGR+5btIVPdxC&UI|i)u6o(# zOI0#tnV8T^6?Zvjul}iI>9!p@uPn`w;w?fh7B4sJH!@cWvn#aX%T)4U4DoRDN>d05 zs@LMInkg^Zse*~Pkw?|HT#<#G(7L-HxevkwQ-~VZuzM;~MOuPP8mQ$Dno6{uG*!TE zafCrOY(6Ea&D<;3$<5G{rFW$uYhW~)Hbj5tNbR0i@0Y`w4M zKMly=np~oqCBKneNpc-5+mQ0A;Vf)EvYr%m=*1}aOD&w!NvplAoU*fVZY4F686&aD zq1P2TYJSxI;lcdJ!|NcM4$Ug6$s!K~G(NSoBaQ1zDCv<<(W!}KB1`umq*Uuj8(P#y zV2b{nE^;o*+`#ZEoBH_eOF;vH+W)rDJZWYSs0m&rf>QA(B7Zn z?FOUA{=G$@Ff90f5-G-i1~sv!E`V1(J%U5Ban;G`_%Ydf%!bMR{U)i~TC4)-Qb+X; zr!cq(bl*7pnxv>aN%Y(iH}{i>b7O}v(>=|L?}&Z|VWGkv!I6Csi-cWmWbgQAn}l9f zwgEquf<07rSkomArBHGXt&K2-2l+*(CvE;}noJPycU3Fh27R2yv0XxImLX$nY2({^ zjG-cfQ3=~FF>Kv|jU=fIkN90+T~8o&5(klUiFO{=NnX3CyY58%vV9y2jE`!1(j=cf zbq!Hc%ewRnelgY>2~RqlTO(3#RL_gggsN7Q4~vH7$UB%5Rv)kQ)1(%0@diILDp(G{ z3eqD@W{>H%KE!WPCNSX5mq37(|7UmO%WcmL#y9C;2KugHI6AGnI=6Nm$n25eYV`&%3()-G1TW-qPRPA4( z+wzvfx7hrr7UCY9qx==xrdn^H#HG6PN|98L5s{uv2`}1u&xny!Vta3)Z~~tYA7jq6 za6J);=0}xrTd+TKnG=9$N4}INCcW+7A?n+YlgUguQ9BaSu1$dFBQY3Vns-I4s)Qe| zMRGAKB|GEPV|b4hRSGlqro&YEtP(4me=tfxadakmE_iKDj_m zo>2+hR>F(BOD9jX`uP0R#nSVnBVEuZX(>5Szp` zkex4{(`j{&;${CHUp99uDk~kW&oyW2lgBY**|WNGI8mu@Ti{0XU)m8quCKd?%62Jk zPb2FkD}6-`3kCR77}nwMyoB4;>T*dwWe$;#Pf5$dN7??Y*foob5k;5Zap-gB2~@SY zuM9nmXh0~c9F%w0{n7jjANN>&*pYTN40fX+b8EFp-wLBSO!I@Jv`}~O5m0%P&>^uY zg;p3k^5=N?LCHND^FAq6RZxcFr?@r~9y}6~a(fJww+VE|s}&X7Lt*@3^!(RBA7ztJ zj<+7Uj(K8xM5kJ~Znv6TfE+E&4Goq$CGbyyvRz7+Q}_8NsHj8uAn;>M!=gdYwKky? zk00J8)ht7B^r}JVC({co0z2FSNi=RUbsMV1jz_GeIp0QfH4OOHUT+{ZYWGa0y#XgU zdL5@nuMzmftq!7PrCWutRb~O}M^vH`mI(-&(|b82NE8UByYQzTV}|bWa*x$dU$rV1 zCdOiUv(=yw5V)m#t1xCw+-0S2` zoY^rA*?6!2nLI-p_nBzDX_Pmb;#Up!8%W|eYuXKxZ- zEP&_d(uf`}@P$XiMr`pvC`H1*-<}iLZC}BdPeCUqQ&Ix90#W4Gk!wX#gf+!Vl5lgX}9e-sQy4n`58wt1MrL2sFD8kjG$U^of2d{w%g(Jpx0_ zdjY(rs+3}utB`cN(3@#a8Bp;YquQ7_VJ{TtWQ20<2gc?D869Ys#QC3+Pcu@=Eo5|D zrQ>5uy2_hL0yy8=6v7k+UG=Uq%@417Tpj%-I>3y$%MI{pzGv~KUV7(vobV)-0@3HQ zc>d0+lU;1%2uxh6ix@&_lTig~CJ)3bUx8rx336mVnwRf5zFc;BU(uA@vzvt=DVB$! zzm71*8PJb)Uv?xgF_OHtm99j@jWl@a)h=iHXD3I!nAB#3=?#hVZ-F+Auh~2Uy@TkX zCMmCI=|@Mlmje+;MX)%*DD6ZV;GfK{SeK65=mF*kTkyyIZ7el0M{Dzl&MHr}cL3Zq zifFjmMdNBz@~v{GOW@^&vny=)MvJTK??MpWewat&RTjf#a(qq>1iS3{M+>ihwC5l@pLn_;&96v;hTxIiT*6-NJ=6wMo; z)0DvvR%y+Q)8SJUC@3lls#rM#u0P!oH8^&S^kkR_(Fbrt1GsZ?EC>GV>|-5W>XkIA z>hbc1Rq^n?i(3dG zIa^AMcjNFj3RudRoA^l#SX;oDn16ldsgu|?O!Uvg0dYL>3|5D#`Q@+~K%j9Wm`Bv$ zYPRq4J@Hgj_zsO!;7hlmi%+Bp+i1ttB7XCP=q@ck=G6HsT0b5xS+5hy+ldgYusOEt zFsM|#A}{t@Ue9Skjk!6@HkFo>Nj{xP_p&>6{vQz_3!mIq8pVVI_-eC1)%BCJgvO<4 zq2RxWJ;a2(8XKCq6;$34S15U5u_8qPE5SweQZzN?`1<(P8XYlu;D>cU7VRpl{uVbO5ra; zb}b9Oi04d6D6p{yHU~{XhIG{+Z{R_th=selg(JO?l@JOO zV=h~6JuVenQblNY!n`tBVqzWZH&+J*$Cxr!)ejIgt!{d>xM3VhIb&ODZ1VP+q&qd( zl^BL66aA~tHEiw^_wcZc_)W&org2wGT}mz#YmQ-coHD9KfM;SLN+l_VK#jQgmAYIQ=Bw$GspGm|@D|*{iakqhc4r?G2*Yw*`wp4G5JBycfNvPA>E4-A#Gh zyqLfO#aku_D?GewO)j>nmV2QM1r07q=h51eyyTyL`Q9GP^Vcw}srIs5P0j_3dNZty z^!+u%^f%hX1=tvRg0Z69g=!h4-?)QeznG)eG#jca2drq4moc`u7XYn zcTqar&2Z{$?o!s<68XUU3j4`rmDFN-+SmTTa<)ms*Ab-cj*;&oK%axa7Y3J-CK}=^ z#q+L+;~R(*;uUZHQWx7S&T8K={KaCNgBy;S#9n9k$|4drUWsQab<{VEu~MvB&C-Yk zTgq>Qzq?xIPIAyHlKE*mD9J5Gp`+~9I6El0k4#VIQu6jq!#L-XyXbaKtuyIlV~P|3 z%)J~#dL(-9IhuWxhGqX^SAQrw=OtNI`mshh5iaw5@B`_HS;y-=d`_WY&1rub%CN#~ zVgs^A3MUq`F05W}S2>h;yYww_u>)f$QG0!hCaU4NQ#omJdvO|ZGN_j!e$Qe!B}QeVm^KmtKo0~NYlpnUH*8q^Y-2gD#6b7BrW~qd$z7Llpqw@zxz%z!njm%Mu*9Fwhu|L6a>3Tfsrv)-RH_6; zs4@X`m}V(^{gW6c}OZQYWm7T1d?<&obVtCr)Ajc$tXG43@b zGWV_sh#n046PmU1Pp0^P+#9U~Ol$@)Jy^G7@`cwV9h!4FI`xxUc}puzenRFt?qSr8 zohg%qw;*TU`&sf1#B{M~7ij^(xv)AYh~_KX|5Q?XrMP(ZCiG)yNA5NKq{0yv3RvXH zz2h4>tq5VkiEJ})3wziLUEBlhwGRX(85(76PrsL)=(QisQO`EzR-B?WxC~;h??nil zbNy{6UTa>q-#sXMHX|r|bCkYM+ zWTJF^ch;5=z9k52sey@?ZQGXVv*jD4UndAo756o(A0Ws{3Sd!2v{{VDF)y0Qk>zzO zMycHMHMi<8*C*wK1PcIal7|yyA`&T~kl|R!1`{HG*|)0|4jSvrHW7eYiGvZMqZA>j zWk;eab!K4$>9cXilH2Z?8@6nk&3pOrfZuudXG=1>OC7M^7_zSdUOE?s}C%6Eo_P@)I>D$u;L)98g z%N-^K?KHV_lw`w*M|sLH)qP5gj>;^5S_LW9D`1&lpXp^WLu)Dsg6$vwiNo*`A-Jm8 zt7TEx=QD0|v4n?60Zcl<9My>viQ5Y40jvP@rBb5Kptm7T?Lq!-ds*=*8UZ9@}A^^{uPIh8*1gaCVkiYO?Mp10ykJ z?{yotSHlV*+y*s~aXjX<3n==b6o| zOP+USQgyo8u*70HXw~DVQm8BUL)FSlc@KBXOPdi3GMb#wHCCG@m3yz@=egu475E88 zo1vE2w8mzwnz`SQGgitm+IPLNrD7i|z9LU45KRL#)Y@O@`vpdfPxWv0{V(@%no=^@LZ<4{~G%rU3?PAB)p@dy+n zY-=-2$!TBPOGO6WLnZPfaO_xR1#8v5A_TRRJJ(Kp|zp4Ld ze$=ow*vSVFA5n)PfnmV_ELILoM6%B>Dd(!I*q`Q~I@VJDv*xD?cxJ55@xJZ6+ULCB zz($isfo(2C?igM|0-SY}Mi&Cxdp3*sL=8Z*6r7{0I$r3|0*F*`Xsu zeEUOgq>f)=FNLu98KInT79;P)qKXiURfaZ_aT&prEVukrS#ndF9-pevC`y@#Di|nL zJq1^!~vcNBsbh&Hmv3i@{wE;AXdYfV~{$%bmCmWMywAL_HJ^ z9Dn#}nluEQt^kijmW>BiY+!bH_lMwbz1lqcNtnG()i(-A& z;_w;skL${g{ZRk*?i+-&Z_gzHTohq*1VzfH^ghjZTAp<8->H1EQsVB&eBP6R!Kg&S z%jd#J+bTI)ne%!-=!WadNjaZR8e$F^hr+PQXiaNQr1RQ}BD>a0v!xy-@j(l+aN2y1 zLS-8;ZLS73TnY^crAaZg8kBorNbVLfXA}0xy_r7_F;w=O{l1dbCb8pE)jUAd%vI!- z=Du(7g&g0lXM>5jwDY-~Gol*c?8sxWO+Fjp>L^7}Dx{h=DD!mR)AWv5C#c=|V4Kgi zPIbFF3z5&Ov!kkuCU?hRA=Alo~+9y3ve4)TTz z>vyaN`ose%j z%mN$?nc_~>CZb9Zvg&Cx(mRP3(JAx6)aDvm7<9d&36QI=6kdTN*!8LiKuf=gj3 zTY(5f$K(K0mn?=rb!2)e`Q%2T#d5~y0fL+9M?q6Uuv^C$ReJJ1b+>Ahs!E@jLtb1- z%U0t95m^OzbH3wJfY!%!w0XX~{yQGFhTUw4XF!Jvt4BV5FvM)~*jHULmEJD#ePjnA zPJ78k)~o=9Ir%Dnc~tQg6q_Ca_*v$Oq9K(2pwrZx@{N$fy)wob_GesqdVp+dyvY~! zG`R&K8WYwq{$g_$S^!K1@sDGz`^;AcpPxMLv;gj2S$RSiSosClkP;)>nxK;54(p!M zuvm=YfneqkbSZdK9x#RZLr(r75xVAeQ2p;EuY$B#{4rLsRTAioa>o@!$3?J0k@ZXF zBFwW}3Rh^r$~yZF7KQqZ4<2p!ero-uxcoO%HB~{u{S@33Z8|%%Lv8hcpVX1bGR~CA zXoIpo=43Dq0G6YGgi3p|I|h^SsEnhE{es?9 zt+rc~TTuAsDCPk0vBBR(>yj^jJ*Ux>e6)*W31o|%fac%`bmf|_@UM1EmHl7Tu`~+* zyM{iFsogRfnV}jF?Iarg^m_~5jp;XCh9`$>$l%gqLGZGY@M=&y$TNo?tUlu4zY<;Z z863Pi)J#`alTlj4fFQ5NcP}k3CvT)G3-)HfwNqcT*Hhz&`STv|18d$5p+%vRWr>TE z{_^`D08dI4v;ONuTE>Bht=Q^AGuZ~ka%>u>2}k|<3(d@Fz>_?`&}Fs9f4or?I~LI= zR$P}5(B>s|4^8z;v+6I+?+V6T-zizx2dqH4PoknwTSrc;uupLI3y*jXyC^;fvH?ls z{59dKeBLyU^AQ&Lf|#Qz`aJk4g&g35`E{eZn#L(~~vcsC>o8iHg{^nTKpJQT#Ly6mx4BInX z6D7;RB6hD+ab-d}vY~~E>`38iLmcnfLpf*k0CongjkWTym4h1*%;3i3%qNo>9UZBI zXT;vkhYkyvZ$>ROh%Snqkzr4On>RB@p$O^Irtw={@5ATIAE@r4iEKD`v$`5(OS)IrC*7w)>bfC32no_)DT^e6&)Gv7pJtvcomzxj*HvsY+F4@yyj<)v&k{%48Z9nCs> z-1J8qnGrg$X=tk6uZ^Y*p?)U)--?M$3!cDqPaEW~BnXlNc2>yW&Dd1KbN|J(yCCMjOJEq321|?77l6{= zai4+SKBQL1W>5wpRsvg40>KB_r^D8Snxd(@IQXH0-=w5huJ*WC4+oU>mtssga{sRo z2Bf$GcXTS3qhI%<2&&nsz6jYhj5+LIcIa!qs~r_1T+ddWkQ~6W7-*-D2pYC!K|=nr zgp^V|4$+r!zD5qK7uBiW81pwB zS#_xNo*?ds>;M#RccJrnqazLL*tGq5HjC5FK5E@)3^7wp0;RMs0>%74>7EV)2T95y z<;A?G>Va>z%|=T)S_@j%8i_GOcDEB#eW&nqf|jlXLOTJ;z%%9q;cd?Uojfx+kmW{5 zXOY#B6`MoV;7 zByw(R6QzqDXJ=1r$mMIF2mu0sxAzq4?<~3$Y#Pmq|yoK0h~-64`;@V z@(!DX0PjB=e@V6_#TsFVAH3|%Pp@+y+?5;;5s>W_fBbm7q>S^H?%^4Tn_Nn^uQ^IzQy1q!Ks9lYLJ zRX+ZP6P2VjAZ+Vi`-Pb)Sv!pAPBx1|yeMd8dcpVWDpW0@-i^OHae2`eWur#x-zS{CVr0g17qtjX%P4=|#VnACRwS_}`$ZGyZU2vx6F>5sVtfyn2$`B3eM|yHDov zTk$f(u%+*=eLTk{>4UX;d$0~*20y-#-5{$LYpX^}TE{`(C0pC6g$GPL zxYPWKLNyxezV&!5%U6g~N|}YY!fK9=0EQ?=so*x=R8lCG2xgIJrdDLMSI$wc#f+R=slu zEC#DHBj#jEpR=M^r>yx2B?jpH6X@#&ZWo1i9F}n#-AtzkYQDA~X#E@L)${xp0jz1& zNXhE8m=MfEok#pa!BaiSDW_i@8Pzjde}OIXLP-yM&%b*yY|#&HJlF2-w}GWoy?v?I z0=F^IO{!KDi--Y^V!9ffPnegg8lW1&Pkp{$5ErI7UeVeQb>+o}!deI40C^&NICYdU zxfV>sQL-0<+6F3+Izxg|^5>aiCluBhUW3Fg%3v!PkEzg?8nL1=7QZBG`46c?+Gb2$ zOh}?X!-VQ9-CnF!z%JAkv-0?cNpBvMb{DjY!!A#;PrCb4m)93kG#+7iVKkd_#6Zn& zV}dbe=J_H&k9|chuNpo>0Ov>ga$?+flI?OBQV#Toce659SmqE&*Y-G3$bGsYOi*nR|S~ z|3UxJZwfMZ3x(|MG8gwU7L!wTDZga02EPF|5&z4AfGJ_ ziglt$|5B9Zx^$+17iScJWuOE*duJeS)pvl`cLlv>OgqL1ioIFrT|c7b8^9&Vv`8T5 zoUF9fNg0&eaaX+@=3@vS;AW0)G=6M0rNoEU!fhkVVSXs0aDvVm86!Yey}V}-vq#LI zVMI%-j;~8(iDZq;2i|-VT}PrTymX)Rl|qv0Ihdq-;|VODbfhA2^pQv-IctsPnHX*c zkPj}lbwQ!C4$#)I3~!>^fC3LTXtYE9nZw|k zs(Z`*M3-Fw)cbztfHD4*6D7`J_$^l>MF(-X<($aRHB2;Lf`r&<6kZC-36Q%?ca!kU z@Agg9wF7hi^0fbrWqS7BB`p8?0@ET=e>N;fex^6GK~F8m_Up~U%zWq4f~Ss5-#U_{ z<0c^-O;`l9Z>PoDL?4j0LaskP>sc08K<3H>LPgUBdIQP-7kb3#1G91wL!u?Vb4S4D z`+6(=;0ZBI5UjXuQu9fV$3e(zzXet*t`8qL-Z}$btL&5Ljl*e}`rh;{U#@dswF2~; zI)rYwsa@A-8LrRohUSYJvLKD8l(zit{606u&Ddor<*$Y|7@|tk-qx{PA$1t{6M(TZ zu@K?Pef=W3%%ki5Pf=Nqup(*xq1%{{x!w`J*Z$M8 z#Z$T?{s&(QK|bv!K0~#BrdWU+i6G;Q#U$YL=6^aT9dPc^QzhqpRS!Qn>WJH@!?{tC z!^b|Ess%>-FKHs)_&qzDl4OQ>Q%(gzmnd)WzGJ7|q?>baBBkf^gDgFsClO^lEiG;# zepIp^bByM(mh3a3q)Q`Mrrn+Qb+h1ZG+*A&*A?fP$-yBtdiW zJUxCvW;C35UAS|$dbmuA8dG{fYitHY&Yp4K28EK=uXw9d^ybvJw zBTgaT#p~xzf0a!J8YG%HJ|blOG0aM6W(4+4F`}sCzAi`}PAt3|)4V!>HV;r~=}odHcP+d9DnA_$`N7LeYNUWHIYZ=rWl zAQY7zdO(^A(gMc04g%6qq=|G8rFRenZt$FU?m73~KX3l5HEYkFnLV@1w`SJ< z0K7$gaNor{)g?4QopVIU5BM%66#bfOFUEv|ns{7`qR*V++;ut7clCwTTZ7{xJjb3~ zocWKGfVsCYZs8SQ2SSO1FXj2L=-ObbXoCkwA9T;nCBLBPlrRkU@T|&0Xl@c1_rh6!Dhg$R<}Ef-X-p@FAqWi z>y-Q&c+0Ly;!*Jq#O|i1%H6#ck{`jmMs8~~^F9y>0(mUd66l$qFXb3uVa)MCi!172 zW2WCOV|%X9p3|h`;Yt{VN;Zk^a|U%VY!Wh`W6;-y^Bp|8g!;I~N}&Z+R7&WtzmjUSBcVyN@v8^NrN1C} z@5>dKOkAj;BhC4jAB)u-MYeV%%b&JkKRX4jQ=tY#cB*`$LoB& zL{RUnj3Lg8riCBqhAAD^yErf|%_pZ_PYw!2pIl7OEnXj)dXBeVbKbZ8(C68Sb~nKZ zS>RM3OW0WIE(qyhqqY8Gj2rQtAX`#_A^{=cUD1 zV1mh6fimIlXmAzeEdYe8z%-v?MF9Ki0ld?Woxp(K0tS0V9LVqq0t^B16M4e5!GTcl zGt9?1d>HQvi-SRm(aNP0=gN&J+#Phe0Cy4T5L#-ffrx<|0m-D)~t2UF> zrYBAKzL|D}z3=!Izu0S4a_s&5ru06bJfjKrV01|ehpyXH);P5V>+jsh9s@XApNAnB z#TcvX&%>7LIoVUkpNP$RkYjJh6Grr`Gy(biz$yQZ0#ry_C0KsFr;TLBCdd&;%;*<# zeXXMuu%UN3uDO>T>~WaU8!stg9n5BT)b#38cxOyvjcBKNV6ak~$DxcE&$c}xXj&=I z5~#P7k7*)yJ{|aKR@3C8zxd_l&&0D7?9RlNpXbETtKmzb`t89ZNmK^(%mK}r79}dI zVpzJe08GyR6i$CHtg+@mnVVs#9Wz(m?kE;Th74Id$>xwhsc3^AGm}<&*S>F5gC0)TZRp0&) zN~@igId^jP%MKw5%*!+nU_<*Q@7@-h%)hwLSW~IkD8k2a8;PjO66Yw@?}%3Ki(?Hk z*c?J|aPVz0M#dt5pEoY+oZ~1$?*q0j-~RuQ@e5qP4e-K6bcNNhOJ=WbP6hptO1=?V z5f|SmX?zsMh#O^NoqU^e$R25^l4yl>&NRW=Bovx^jNpy2gGXw86?&ZZBO@8pF$VgC z7a@&$WH7ECnM5KatnuXjiwe*hwBa$PGFIGyDWRs7?b*IGa&zEp?2oMtuni_f zFJ1uWlLBKc)GRG^{PJX2%gh!pRAA(7-qb8f@Msy`-GT5urCeqY_S{@o>xNIrtb8q3 zDN;bM8nxH`14$;dcgnDIgSY)si73g-K_s;h79m{uh`p*(lK1!zk=(N*Xi#`p?~!|b z(nMn)L16^oo|@&g*63VWKC{=CgDo}RY^er=ELvm3Z zn{mDSQbN8dr}^|vs46*826}Z-9MEN0^x0L1hiuyL=x}}LmKI3R@ej&hmToj2Y_66u6q>Inf`kYg9zUYh@W~b*@sqtLHsZbf{b5#S3yS@G zmyJYKU2Z>XN80~vW@G5Www0tsStt0Cd9rd_1!ZJN}%d{v5Gc|=wPGhmxIH{Q}Y8fzypZ((s>p!w4$Ix4c z5X*C=~}r|*Uyb%YCLYIqDP+t$XDlsDWl0ZPzy}l_)(0G zPicT>c~t1^J%CE)qtH55*n%$R&vYM#H0cdW2~WJ%ll2SAGSd_~c#O6@Cm z2zVB~m}ao1_mzt3*K)FW@6wKEMUS?Det$OYIDI!;H#g-gMdLDG7zaV*EI7Nuv%JMl zU(hV@x~y?|?^8iw3?+ZpVdFJ-iu2I$DTu3-Ir`_!7!%nLOi93PwI_6Aa16#mOVVX_+w(NBQ>{|PFu7;PdzK;z& z{6`O0Hiced15dy+wP=5YQWiKVIh{)Cr@2U~2Fjy7!;o?(l*x+!{-}xZ8;Ggmi zW)gn;FP%vF51mO_@3+lInHFto8a-WeJ7Bx?uHM`4-t%kLPbxf@czk3(UK47U6Jq4e%n?leI0eex|kY3xd-!R&=sM)?%oEtZv^ zUv~MV?3DIm;6hsCL#;Bu!b)D#m$rKMFC~RNB-<_PQ|8s-Kt;w25)bvG6Yi9xsV;Lx zshO0xzK(Te=_KP*o`n=UZ6(b16@5?CAe$JdsbW5MEht;u>|Go@NE5E)p}XFc##!VA zM3_o_LxgUtg;__*rIQ(by2rCcH7HY@rq^e^(lEVwWG&e`&**Z#;`+nEq0o79k1W9| zg$v)jY($ZpG3mY0nR{3BSX|XZ6nA=8eW-E!K77zOmey#H+ zZ0yhL4)tK&L9GUj6v+)1rPm@ZII{R`gJ*f`|uS*K{x^X;qR_ z?n2Fq_-b*1Y8$B0Ky;@m)G?iBYg6Z_I5Q3saZjobYE54AY~KkYgr46wpNB1L*84f4 zl%^gQ2BF@oIBT-WmwMea(`Vm-^EA1GIqG6~79fhF>xQ$%I1xSD+poIe53JC(hI%5?0_e}41Kl00CJ9w_Rfw0>Occ{?Z~K1{RI zROXFb+fM?O1=Vaoeld~wl6GiI^WNtSpXnrlaovujrssTV#sh|?USx}*`J&?RwR%KB z8_xQmkEwl}!~=8)S8qQKmm^Z4?tWL1ayg=sOdr~(Dwz^+{Ds5bfIVEq{oN?&5vX0e zUlv`kBSe}2T38DdfVQtE!(2yWtcg(kDR2eFriR2Hwd{%q=RDm-%sfWKG5}6K)v`W1 z(Lx(TPdX#h1W>yo>!Rsf(bCxXY({FO=1JNKRNef_O*B12Qs0zsMcR3<;Oo?`bZ{6w z3v>8~U{XF(%jh?m0-$4H&l^?(A=88d!NA+x`1uo*Ll}IvGE%Bs;to^%k(I=`@S2eK z&lj3x=1FU$vJss|TdFD*@?+FKd6`{V?oO1Exreb6eIixE_VW4Fk0Gs1^tu`e3`$#h z$hnHBHT0Zor0Gb-jV~j*h=wPMYNxBddHc;4uSGTO@S0VmI8_vk6o!7}xAg_?K!=da zlRxgOF(t$`-F1TwP_yy{mpGR?g86V*e3WnAxjl9`5n>q)>^OKWyO3MSpV+|#irypZt8a|e$U0$q~jxQ7?8cFBb2&6EDiNfrMPlYMxqjJX2o4l{F zc~zqOMP7^Ny*0-WP$V`aHS}=b1BTBNIFFFsdIWK&dJ9}wxg#V)BiFLKPZ2({7zr_N zRu17E4%2on_lkuyOVh~hQ#>C|-Cx+3+dmkaA~tCnk!qG=lH(K9I-8whX!zyJ;t*km za@sOl@^DAu_{!H}*n>4FbZUtYtw&`K3^mY+pDZ0|;(d&xrFE^B{fB*X4250wZo+S@ zv*(gHSAx7*u%IZzHnxgUfUHSXS8uq-HT~uQJm9H8Y8vYHqU9KdxN>}hSD;!a7(Nn9w1T=0v-mIs$0_1(RO+#~eM zvJJ_OG8u3F$|9M97$q;y;QhV!h4ExKKND-sfeRDdsEQ^A`(W&{O)FN_F+|3BwV_UC zV%Bf)f94}LO&5vN;{Mou!7?f`O6q*B z4f&34m7a>TSzKCA;G?hvA|;g+#feT~Ws+ij?a^2NhGmTRoc%%OWv8q1vP065Sgy=6 zjcr*TlTF1M=d1L#ZH<$F*jF^r&2$8IwIi>=q9qLEQ=#t=M~x7Wh!2oh@7^-{W|)8R z_4uLSVDpc4;k}TEhnLXFuO)09vN`%8t-5dvZcW#RHgT7o3qa3X+$$jISHlY4P9-3 z5LL+Dj@qq32rt(z>8{VDdpbI&n)Y^p z3G*eH#_|>^Y~iVAhJ>JVg24OG8AoH=NZ-}QcVZvn4h>h8n=`>(7EJV{y9L6d`Am|DKS>xBO{L@A#|*lH_%laK7>*yeF0-ht*Ehg9w+Uyrf8 zvu^v;ttg4>?*16SN^|4!C;m<2s5hO6rT9zsC*?-E!o zOb8oK!1}w;-|h5&&j9q)FfK|=gnu;rXIKIzMxO>JRt;041jY30jdtQ-8%;*}|19Ybcw z`nv;H**SI0FE}mdKS$}Ra_m6O`J8sLboiB9#{wrDdRFh9>PV=d%H|B=|*OYhVG*%}pg?_?}&Y-np_%*zYy=;UCmZw>91F{wEoWjGACc}{(`#{azV4#W)VcF_cM zO=d$7PC!lSg4lrUR(Eq9d>r#!_R!YG6!NFX4uP_^Or2(FH~%2(^Wpufc~nO3ibI%c{95^^{eq7`2!DdW55-PE2us#z4Zu}&}_OpG?&Sfj}HGsCY>R${*bj7~4IZ|7C~JZzYg z4koL|HJK7x5^bR|wQ!8(z38~!qkM|IpSH)#ec(=ZXbX#)S%m0>Z%6xd{;2rwsl3v3 zkuTBeFZsh2sThoy{Qg4bdAnlld))PD{dVL3;+nbT{W>0g@A*{P~Fql*#c+(G6zRgO`*zmaZH+&ofo^H*wQet<4 z>YRy!=#i>M728Rzg&91HGla=vY$-DfXVe5P@iL>a+iPH;zdEwXCSM|k+EOGICtbKX zrdPr}5ehN9JZn`zst{(HgmG!+l)flj;hvUv)|-_w>2W@xKS+KB-kwA>Oa9r+DunDr znu3Zs(^=03tHp>6s7Y~SUX#xleO0?HzFUx=Fq9@OC)^i>zX*+7A>{oOgMMDub^f{y&qT5l(GDa7*<}qlbaElyIhkP5ha`FZ-(BM-eu= z9mBP~wEWbf?o>WD3a^qma}ZaXTqp1XEddxTAg$6@B&{Ni_3H%@$-R5uU?!LBRO~5M zTZVx!gMve+Q%IBt6SwdRittS`VP$=A4(SV2>NNI>ad_zEB)_?*VJ0b@a%0r9pOsA~2S;JHSED zfrU=MfA)*sMxyP;|8ooY72gvAdTj7Q9aP*I@dbo5(e7+V3k${!gK~x$3MLWrF_SCj$NCQC+6C-&PJSFEY4zV`w6ufcKqjr)y-KS^LeeHc)A-$%5D z$DD8V?DN+RaRoHt|> z^fCoheEGzmw5o{%CY<~3e}+MaLgOjO*0jcI1T>*VON~*!iR+*wgraFx1=hl@Ese#U zNar$P$Q16z=%d82C^j<{11hF=)>|smfCL1f4uDbMXJM9gFR$s*IQ3yRT2Y6wgyNSJ zsiyf91{LlYtyT$Z#RC;$YADHz5CMW7{8U^Dz6lvw+0t>rTzC)q$r?IG zGAMQZJCgV4yIw-N!7ncr5{Zn7>yHH&g$Ph%`}Lp{DtIDHy1Y_2uGNDrHFn6;Lj`8o zHuPqanw4B8b`h9{UuATcahWw~8=X7Mc^%4Mqt2!X0D0GmYa>aq)o2;`3?e*5uEi6R>>))C9SmBebvDe0>Ojn&`#}2F6$7f z!FaJ-zyt{L%u0ed&~7Q1X=y}9tpmZr-^*4^GE{#~&U^g`X<>4Z(rKX@C^OZYfUX7p z7B~$on_2F&Ng9VI}Jb0)k*kkcxvd?6;9Tn~t+xE+;@<-;<@}Kq^xvmY^tM z1=YshxF7@{GMNI0MysbFrVLw=B=Q-9C3s9?#JnU2HZ|!~rY7bB7sCdYEo9XYlae1N zAwtnSG;kJG%Uw+o|7`Q)&7}#df0&9D%V$_4zm(57VAV2f^0)=(p51Hg^PdIB8A4LD z3Q%`zY7mCvGu^MnSgn)9vUdDgTwmT%)n-NzQoUzl3 zm?ai5YgKG-&b0=-*2g4#`qmR?oD(i%DtEXn$s|kSe?9|Fc@b51kJMido**%;vogRKsZ( zxA2feuc8p71j4PRP%dTU^1gSS{TQvMX;A-mo8&}P^kEOMFER!Fz(4SG8_qXA5V|{BBz9F><%q-IUg|1_M7y?AWO^JjzEo+oEdrf} z_`HDnUgT-6@J?j}=c7GA3JHVG1PR)mwt>^cMvhgvHVg~}&|8Wk;o2mR7sxrJV|8LZ z950kupv2oV?fU$dDGMc^uY##ohM^C_gJ4epTAQ9TH@F^yi%SPyv;#WKfl;J9G;jo< z-EwOh8LP+2r(qTV2z;GkSjEGs3S-)J4y?(qhvp-ismRR#AmG?Jwq}EG15)ZUH*P9mrK~<2Zhro(_Rm4VRG)5iFFAX3bY=QByf04(jPP=@l z@aCnGOP4k$UVG>|HW38+7zJZLNZ5o+4w@DfM6s*I9o1^yFezQR0e;pW^-t6$Eh7=` z&z)#X;vb|v9;R!j0NDl1vL@SWi-@h9`RiJqRPCF7irK&oT0v-JGl*WX#u<~vboFAu-L6b|u6>r&McP~jP0C+i`1WEX;_XSi5}P-n(q zCHGK;bYoHgj-UgUq?q+S5{jpvJ7b7cD0>|dPX%n^HHDsWhDVOc9oTm`Fch(EI7Q{A z;NvcPtGUJmFVIj;ZyYKrkRT+hJ`x0AC*|eCFEBfo&eSNU+`0Evs|l8(tk|)fizlzK zWz>R?&*@Nb?70ZA$EjhYzUkGpfTSu0*7I2dpv`3GvC=!d`w zR8d_M5K9}x3WPVkl+8xJkynwB19ngn#c%Hx%Dbj_R$7^y(7%3^pPyF_4bK*KZ)A;% zm1U4%{}FH~e$%N;T?GpRfRDhPixlZ|Sb7rbmT{v?gdnBwEG<79S zP*QOe0J(u0fyc+7Ci;=_(_CQzBtGE{FDPj1rbWQa$Uwl(@Grr_ zz)Z(LpaV@W=3wh=_t*JXqE~j%w{f)7cQCdwbf*_mq8B!HF*h_;6cePEB5-nW{>T6S z&k%tf`oD<}p#T2?0?3cQ2he}h3zmNyEF?hii;e}FfPjGBQ2$@Wzb*V{$JqW|??2)a z_P?6{S6s5FBWG*fYQ6ogwBhV~94r0_h6ihuCgFQggqahbL*1O3SEFUrHnLp(LOjjU zu{<3Y$I2fLq${kaxDYEIMnp-PT-7W`Y8l>POfgTa)8i*{!Mdv3I!jDxP!)(oh4 zuOMD&Z5O2bC=ZHjj^hJ;IQTvY{a5`lQ^hnmzJqqjh4}FeMg}_Ud3`R}%Aj1Dy6elp zMdNSvY&c#958i(4jTt}r4nR;tK3G4MjwXMn?M<#OEBw#CZ5QZ*Pg|+D$j1^H{`v z_&#q!mY4GSMK>XB9z-&a>ugk#YipsuQ2FRMm||O)GSgulyudW}muB5~ST$|u)HJ*y z?8YT3(UTH^U$UO4)`u_$1meYV1%4{jo)yoiIH*Ae_s|NvzAzNd+=f&!>{fxIG#HQA z48{ktJz43Cr+S8&aG!W3k$#dyHx!eoLtesXikUsM{wxvEJ!a~RQTrEyU1R+_*9-llX1iFM}0)p5l^8nL+sdZ=og|}g~Nu-S-j!`;g>V^@2-R!v^6Es z?%t`-PpfnFej@OhUr{;R$@z~gixCqqcm8DA02AiLdrEy7g%L+eD9468O7&C!qE#zt zbBlopY#EhT^;x6o;w4;O+6+f7Gpx&--qF`Q%43`edv+`{0PqU;VxisZhrA%r6iu7eabeDqq*yH)TDD;N2LllMqQkiI7n71IaiX6c@nnAu?r&~0 zd;2>p+cowZ`i)LmS}fzr~*#gqd6 z5F2&xgxRVjYSOghc+%xGM2*uE7{%d{H`W(4X*1^mGm}}2$$Ml=ZbgF7Eh8g8QqBTQ zgVWjxv4vf+ec@a95}>mYAxlu|e4aN!Zto=wb7kq_fDG1L8-X>j%b=#AWfKLI$tbA` z`1-BYJhwC<@_{$z;3>1|JdU8me6M^#?nqsHbu10O9L`8xY}HxFFQCc%F9_8;z!-PC z-S5Znydhk=L=_>pG*e0V3KxY2ciy9(Ih-k=LR>l-M-d3-X?jd_qq~CBd+wz_g{{>f z7T5rG(0J3pBaK!NY7X9gt2%+%OsBC?BY&v_C9a4TbNT#sCeMx&*xZR*J^hm*!}@Y? z^qSzV(*H+9mse_Q)?cv`FQKcySaiDBW<--VOUjzdW5#dUpS9YOqo(d3naxr2cQGD2 zbN=nx-RkipKZL5IM@(N|I1`Nv-~do}M65!#`p-hTO=IE6t~g{G+&fS@GhgzEp9THTaDr0b}QO*O?aS!j4J)6`W5IBv%x552o z#p^EkAR?MFlm21s&1j+ex^@XGSQ1M6(^zEFN;rKc%UfssXZ#YIrHlnU5L4k|2blHi z`5yDyfmVTJ%Zj@UbvUuAtftUb;$<<#=nir8Rp|G^<+Z+W@PwhGNqZ#fUsh(4MtCcg^lB`B<__ zwT5_=I-jll6_iRqa*K%G?yXj+vX8btv7XIJX@#d;41Is_#>8!_-e>&< z8})~wE?73(8v?68mTfsTK$sE!!-$EcpYIr%g&yX@ZNmuh!|GqxjNs|T<`oM^a(U&( zo1WsVK3Kgv5?IgPPt%=rq^X{~@1`rqXx{9ksajfNSj`TSjLnB;n%tz3mU_>+=ai$0 zyKQmIqr}>u!G$D_=EVi}l;>YC*>Mvg%5*pP&i+A;I+BdX0E2nOIF4YYIF81`3VTjc z2&4)$mky8PuB*&*QuB=5#pV@#8<6-$5L^zD)|}t ze9beoF=Gz>+=2P=PS@a6N+bO6uhj6QSM*ww`)e8U{bA>Tc2XVsUnx9bPya!9s9*7N zdF$iz`YKU%UxOCW&39NOa*Aa&)nb5?4lY;VMyVT<@7aECe0+Di9ejR_ZN~HN)9r7f z%t>|l;SDiaVz9{X7vx4^wgU`C+)_}yi)TqL0jY^Sco2kdQTqa{*#yeouQLgW~l0c)dgX)Yh?(Z`CvacTryBUWc`_sM?~9NyhZqNzI&tVy%P53 zX+Hbwg=HZ5g$1_$>5kN^(TB1inSQ_e^)TD#?QV$lni;^m*Z zdAK=$g+*A=6-4x;?TQgZ;#G{Oa}{|>XV7)MoYr^ONPq)delQdr(QHa1tQ0^Q#&bN^ z4+&xnTeC8wkOD#tPSoc{B1qp88!8ghc2zMJs)XG=8qE z1m=oSszhF3G3*5l|HuM|UzV$Cd6!5K!B%`P-YJlk>%9vr0?#ka1Rkvi1tTE*R>FdB zfV|`u)<(BqVowdXAg3&CcL@uLI^NDq3k3KrCU^aPcIll^ruoMS9@S3+c>H7;zTS~` zSTS_!(++V64W%ZGEI5k~r+#8mRRa8anbAh3=>6KyPwyn2L6C=r+urZ-kE5pN8$532 z#$Sf8!4o={au+aF7ASOIxKDu#U4U=Ki7~#Ok&mJEc(FUe?_H2v1i=hl@Jn00u7qla z30g$~BRy&tNih~@B=dmyz%~-g_$@GI3h@Ae+ehBmr~J$1V!t!fcM;oJ%D~N%`7T&s zj6%e%oEAP3F~q^hX!eowP4vVMZS%g$^$H#}1+ycci`=Zp|QXionvk zyh8nO#dEDyFtXXtmy$*mJjY>P@{HHK&JF|lzIR^bUxr|f%)hEkuWqn@;u$KwSM-|P zP7m@jLE9V72^PTET1q+FWWbhba0Tku+IgwXcOmU=2NQ0N+xA8Bbq2nlVQNA##E4y`%xe$|T&+26+yFH$gDe^V51r3iF*-uKvCq5SR! z3?EBy&fxF6&TQ=LQZ=Auxu2{TNy!$hvKB)SBREzib;5EzgkDOz71A_Cw-3~K_O-6$Qg`#~)@@kA*h7y=8;v zI8Gr*9G%52G7$ZeyGq!?X{_J(WF0Bzl<$Mn{5kz>wWpPTJwrs3JQWTW>)9H=zi-VgdA8Iq6+N6EMcrwkMIreC+8#{@OTy&Z)KKz?9?mDyL)H|PcTS`KgPE5qrni^)M#BJ`?F*+|!8vYr`Uz^4^M z)ex(H)&%o%5bh3QwI(m~h1CzWNywh-w{-5Y%=F2XV9tKKCP0mVHCY9tF?8L9AILM4 z6$bW01X@lSmMxR zCSO5!BWB937rUzbww_zD10CieOk*o-`lXtVRBExyCgim2w#;WGHV1<4*^yw0FwI#cjd=qOk0_>|4K+&e*|+CejVH%6kB; zphRIdlkw6}3s>XCv7Ee(4GDWYbI6EY z5OEMo_sn%9FL6ahRx!$jY(hfJx&pLwS$;O2mm?sFbLCPH5$@$oi(1@X=dWZ+<~6#* zweg2=;fsRqm55>qbD8~3`IJ6u0W1V4j>QQ!;5>dmKz$sk+c!~Ox6p(QS!T`aSOr}h;Sj9NK!5tW4? zbY8lhD4{4wMH%XZv@G^8QEq5jyvfyELRk|N9rPCH>ZfRk%&LHc4?>vomOjrI<8Xnf zyrqZ84U*%qR;{APbYxVn1lOS^u)_+*WCeg?(Zx5Dy z&a(s0mKwXDF%huA{daF%$jy<&AS(u6Y_}yTJ0R~2p^4YYHQIRaRcN!qdsBX0pER@? z{evervUlXGoZz?=ED+PR2w_m_<&^Ga;yUuDld2q|$DleQ)r5V5cJibp(u8L@00liK zEM4X<>n4Teu=jL-1_12o9$%`e|x;{%J*@FVA%fl+HJG_ z`5@K(dBkVS$NR}vM=pT<-(bZ5Aie*f+bk?BZ2t!k=J+=t{J#U~X884Qi~p}sH^YC; z!D0BHB;hRTNJSg7*=#LWv!4UC(S&*cM}iO8d-|@*6#b#`tDwS!5ZDCWr#ZS7`xtQ? z&{9#0!vN>}C68YI!ih|(KvL*3{q-_Dzm4DuaG2gLndMt1Zd_RJ{u-X|{@8uFCil7V z>HfMqctIGx42{KFs}61uc=ur34f=TG%l>)_mL2;3yw7e!{k|A&cKOKv-kZ)m_2F5( z!HGV=t$$p*(B5B%zK~S?*uZol_#rMPG11PpudwGRz%=m^ zAe*3lS(1tHRsfP29gN(Ff&r1_+()`>dPY%bxSi#&YwJ+)o6hWj*p%R)l#Tlb zsn#+9D$>d5LErWb6C;=&I`if!p+HMuxd)e6vv=KfU>Ja$e`8m;aci(&C0nZWOvYwl zyTAc)#yK#AYj5cL!$}6%JG-SG#5?_M8VVnxG@w=%KJHcN?Hl;V$*|#bir4{CSffC) zdW!xb{OP2uw&gLd`-_L;5b1MObmC)3g?B}TyDek%2sg6Z0A}qltxJTufOqNZ+ok zYU>$e;{Ms7%;&_1IzbC-T7OMU7g=lb#sv&c;@-;33=3QrS!Z(vW%Ct|M=*04g|yh2 z<=nzdrR~`D97rz4jT*->XKVk2_}D)(P7=GpXo?otjB`K3QI}c8Db=BDtdCKL)qm$} z?9JCWgn9P^m%>q#k6BWahYf*KUX0icjR~#BAjQQZne|V-3o=#ARFl3~U2F`)LGk{5 zxPMBAfpz7xwK4PA*kq;k-Vz&j^SR9^;7XPDlDmQ3D4;9HYQzr1o7R#VmIaOrA#uqC z@66agcL5HH6oCtY)JFVdd?F@xeGOdbd3!uZ+>#3+H7GNP&0%4KuH?By;_K@FyV-Nw@W zE|u!&ws-Wn;2%~@FobxEZ|O(&huf=G-&`z;E$!9gaXWyBotiY@__62ZH|1DTg3F^-F`>`n zPC(EJ7Jt#G8p4!W8G{!7W)w~U2>9e$j2Dfo;#$z{`$44jTZ6u7ov<#dvzVldsw6zN zQqQrGHB)jXM@)JqNJtQkxnzTb?jMLS1et_R?tAdJ33;_B^Rfox_eZop~HKDkOO|WnL=Sg}LpD zDdSS==MW9#nMEeExtJuK3p#4EVJ4|M2kF&>U)&`Pf1k00QfDSuqRT}+8FjHugo)!4 z=XzB8AcPhZOpLAiyn1%H+{U>^Pw<14nac*9!_xKMN$(mj$IkDA7u@b}00SzQE(V0` z5KRt0_LD^K$zM2&4ju^Z{Genu2sO+E9r5x-2WN`WVJ~W1BsRiXapSg~LbE&E(B1OA z!03}sk_+`vcI+=UuPt=q$4|MUhAnh+XzE}lRNL&X`IY-TX-{E5hLMJp^FlkX$}Lg^@b!y}Hu=g>v+gp%h5 z7$C;b5wXP56IA)(Ne@XFMlK%W>JEi@YfGX(uv=8Bir0zA!hEr-BqAtFML3r@YXj*! zDM~-)j?v3_L_*l=U`I`KM|SDwyP-`EzXMt!!N!lHC=5Djh%Y;55dtH7+0&mFS7bS` zFLUA;Ju6;uxk`8cGe6iTky*n@;P&UG(ORm~1n{8%bCCN~yzlgto3P z3=M9O=(fhBM}At@kdx=lZ1W#v;lLy?_M6XaBW#)96Bc2Adz8a0=hf)Ae9MvO?IPxCWktQ zd#t}=g_Ws*u2CCk`A^C~mx_84>cWIf4f(XjskRYlD%FPioFm-xip4Q=Z&P&Ux~Eli zcjbRtaKbl{K+^f8n5ShFfJVJ7p`k_o4r7RZlhxfMyHy^Zt8p+5M$K-R$v{*05Qa8M z6LN#Q%se?p*v$Y#q-f%AqOhE!UX;C`=O9??mx#PSM6(lDX<~-XLg1I!kqX8z8{|Bc zP?4d`W+E^#H8vVvhB+9v#CtD_V04}>QM!H3K)`ruwlzul_J?sLYWBbSffs6w|D%UO6JNe0XW*#bK zn4guzEolw1R9%49!o(YqkB6`%67;g+lJq5-P-(|=6A_N8a_+iIl8ShljA9~q5vg`| zaI~t+(N9J`i0;rP6Eb?6%H~}8LPFCxcVF)+jM;dRv`{d^x!WOA@;1fJAXWR=0WT!` zuKh|o8wjoo#-%G%ZcdrC5I1cAqd`4|L|m#l>@7YZIJUwO7aOCLd3zhC*CQr*$|1U& z19tt~ab9d3+BsleV5CRHu2}#5GuyO3)Cc`(I#15|X4-ucpP>?EyZUuMq4b5KQe(Fm zCSA^x+A6^Y(!0X)ia~Fs;FG!k$)AhmvRLDz-t2o5kx%EY_y%z5uRaBgzepB_9~adR zo816m8^TQg`d-B2CPd>IhzsFb(MAnmqnV_AA1c%zR@YN~`P!R0-b}=m3fdcpmio$# zEN6I?Rk&;y@mUKbK=WQEUEaUyI38w5^Vr1LmfR0=0e7$++)F z$u1L%co^TBOCJgf4tv3HUsrWTRVEr5bqj+TFH+VA7FoZKn~t)AuZf)bs6Xs9u$Iv) zX;RA*WTlSY3Q5120T`t*nmfb0?Jq4)IxT7E1Mrqw;r*_$l67FbI!Qv7+k^4=N$7u} zd9@{-q==4XXuh9cmfIp{AZXjwUKmyU9W<-&jg_st?QYFD8W_Q(?Ty!cjcLWC&27vL ztbpVu7oD{3ii#@SWX_KytZV5O`uQq)&EfU$@3--IVR4~^4+%dXl9}^ZyW2@()|W6# z6mR~ZtH-GEDeL`hu`@%28to5T*O>^(hC3|G#uqyp0C?qpD(+|RXQA9pO<=kT;GRE9~W)Jf4=82 zrujeaN&zmXo1-XHY=xJ&Q=h;p5_wFV55Muu zI(jN8fM5DP5wy*-q)~xAdxPawjD~-8NfqM16Xpc8k^mdWv`pQ!_m83 zp8w`?g8<~(R%ERFc|5?=n>ftpHD?cpdklM=GL#wCuWx(H;c`TXnDKxg&ja=*hV&ne@S* zoKi)qrTDBKm6B9@Fs|EK{Ojm&mt5-EW7yc^0Ezpids9G(YPNse5*YohrC8CfjXb%z zzT{aVn|pV>c&zj3H+)f#m=nCnK=T7P(B+0_$8YU$&4DO#v=o4Kx-H$FqSMX(*RA?! zd<7n+WrB7peB0{S5Ms=Y8xPIVl4!+xaK~Amyl`2)c=|JMpYu=KF+KXl0zq9?%u@N| z_Ar_Z{DGUdxyhb2#WD7LfOCgM`h73nqj|=q%`e(-)m+c@Lx?ZpE9_nxTd_@1OM-yE1jHn2%FHwzi#q5gNi)qv-d_lX&mOIlvLG z6)=btL1AzAK}oinT;rESkF62ZAT|)+V5tj$FRd$j4cW}8?dqbbZhW=Pf@l?0*GC7tK{@N@b5ex^TNq?l)yWx@Fl z(^|9(tXVjhs%_Ep^1Iv8uvF&;)6p6WjyZN0EMgJcmLCqvr^p&mFhlR4XXzN&C5ABV z-77|`4;kv5sw}jCpCW(Nq8YW>$EHv3ej0dddT|=IS(ZNMV@K+fe~PK!;$F+k5Ux1R z&I2Z2I7`%z{olpeYW>Ix4rLgJG0qX1S-yfBREMx;`?HgO2MJQ0vS9?HW5(NkFYmPc z17o~fQC!QN5|mPMwhSF1+vD=*=y&q9TMG%~XKDAJqGID$VESbN+K1X!VeO{azB|`& zC~lDD>G4)i_&wSXx!3sbw-MaPJG-Ba&r`uBHHg(WeC^c}&5oI3>)5t#8RPBdKnlWn zJC8BPcrWzT!C#~!F6?GkYfRD7HDRbHT4q@rf4~K*Ks8Q!7arUOVHJ+*EixgUrKyhk;5jd(!QM4WI?`ovoX*+J+h+`6|J~=u>{hoNw04Je8AE91%NXN`= zIN5-(FMNk%+AlOKjxD&T@d~>)V`w%YMIx^Z5e!GJWg(i_J!r0m@D9J9?CG?ezPt7V z%WCE_W2Gm=E-tTsBv1^&SS(C<-&f~>P{jv7M=2YEBx8kIz9(u9!Q2`a_v`WwQ4j36 zR)XGx%DXJv`=XLAynyPj_B8UuN*mSy0a^WK8Dw zbHa_w=SI0pHwFmhjt>lMypU-Ml&mog6Yt^%qzq~OAl5Z=d6UJlBZQLkCFhPKP^;9v z=Jvx)BniN{p{i*(yhwlP#inc==$5y`qrFO;8|~FYS-=iN9{I>L-C+p4ns>478+23A z)x~$?xwyT5jOiXt*R%B-mg{Co_;~;O5g8r&*GI&2q9RzlYf}KB=_n%f57%aWCC7w| zNe2w#h8FyC*MxcS;E)xHS~JRBNxc{gfOO#GmhP~flDyYdMs_?inJC%>M*a}KQh*7( zaH~7yKuYRstQU%;m9=Y4$doII8iHJn7ZqOs{g4C*W3u;DPdrZ~Cx(^X92*%GKdw^t zU1Q+-9s4MCK*|-QvOd(o`OpP|XN=4P2YK^7kBKo>0E*0L8TG5pv@2J9L^3z2D!iRo z$N;5NoL{0+$Q4I^+e-i=E3FJSR+ef+CQDyU6odI!^U|#)D#!6Y(mKuc!_ej+3gvH~ zqi7*637g+LR+nb7Uxx^{u1rU5{2Cj~2WyPF1cV&S-V2GzgW^O5EDGTu+A=Pl;myZR zL<{H#mj-k)&~7F~xmd)+JjAVnl#m|RPJ3Sx+s#JVkZ1@Up3Va(2Hg}_zub=f0<1qP zL^+K~ZuRqmsUzkyd)KLB-*z_qlXL4Q6tyquWleNGawMUz!Q^Av)Sz`;i>X2fBb0v! zd#vw$1}VwSRK#+Z!U@Sj!rg00K90OuXn9?WuV@C@fVrVch3KZj%1Me8;6rRB8ITp( z^TLoK>M2@l+EIHAE;DZ1O1j=^2|^(oyPNyTKMx`TVWnKN75SyF0@)_rDow6b11%Ep z0zGFo>6_A%2c!?GP0AZy4qisVaF(2e^toGVNKPNM$&!;dB=iCmCEXE?OsQ+ ze~G*Ir!2V&DIMf$b4RQe*Kx;`e)InsR3`o$e+etUeclhQ_IWFp8>avE{J6PP!;jAX zI(`|S{|5S&X$}5wXoi1*?Em5!IGFyOQP1$dE~TE~f0|Rz@L!Ya8U9CE^=eYK*ldVh z_i8sd{EVZ!DC#uw-M|M&n*0bNa+2|RH%&F3Biq*ip{k_zB)=kY_@LkJqFi!QZ2cPb z&2QlzOQP2kTc;P7yyx=3^Yq>2dRwR635zcs099~=hXMlS0Pnd(^v&W`$dI=OgvZlN z#`h99Ei`P@kE{l{KaFG}t5-_<9ro`zUXVBn`8o!T`xBV;&utd+R8|dZWe*CmT%R_1 zD~PX>Y*LXZ_%YR4?|C#9J_r18u{?*QZVn6_G-yCrr6j%zfmstd>%Mk*&`f>hP-t?Q zLg%x+7q#e?q-^#17W454*A=&{<^u9`!`{?BiTsZusRQ<9b_wRIK17iGLcyVEEGiW z7B6JmCnlx#i@|2h+d9(QVVTV_)0j9RM>x*YnZq*StH8(eVO-JaMiQJv4jJjM4%pTm z@tFMslYVh!Q^!&qFh}AB_Zn@Y>_ovz_9JXboJxeA3(;o>JgE^$@ety)%pwI#?8h39 z63=n>D(n!mY+PEqMVad?9yll>CqokwqH$zbf+ytzW2=&n@zRENA2g^?rm^ebHK`Jb z;{i}ev{X93$!{EIph`}KAOj3xhWJ0KCR`ltlb z0SRfcUVFr{#}f3M)S8tjo=X?$R+OL4&pMqd?79y`NyTW*zAufckvSfjsvGVnhHyJX zm>Es{*>Vez57wCTzioyxaWCZZu`eF|zA7Ulq+4r0hk|x%$J0JCAyAgt8c?U0rslNW zgtU)12D?@sM>B@<>hr#ZcDt&Z!%0gN|3W^`jeU!dM}3bQfqiIg^6ZY2i)DdK%@~%d z;+hZAlUF%cVtgs@_G11l;gMfIyaLWWe(6xt)BZpjIB}0K=IF#?unBc8ub^IT#we00 zbO#?*53!bg�+?sA3y;WRBC1(~uNMGMY*b&?0HBCn4*ocf)i6xTg%?vzPh zVv;$;EmDUk>Ry{08>jEI-hKb9PVP}XEc-Mc^pYtbB+M#CbH&Sxh48Z4|MKByb~vrr zATsAlGi$JJx>3&UWlyOmyA`43tLYg%F*z*FfCy0Bc^t;h^{dFUKHQ}qm$-K?GX8lp znQCH4Ikd0pn zA|$;z+UdrR!!HlFXl(G4gGQ&0Ax0lHsC}VoHSPZJezCS6kN1TC>vOfzwwn5Nf1Ten zUnlwCfAoEKeF5w~1|2wmDPX*`{77mX?hOC}ypQh|U&CLu(nPgDNAKJ@>ejgqkOvI) zLBf|JDxCdMrPju}{rssu_5k&UO6BE;uoLZX+ku=yf68$R9hjJGKmi0dRQAhS z$a)R)FSEVy{A;wRL@NfXOD_^jeG&SDt>2_OH4Oy6Kt|A1MZ-cNY2;@`OW!jOd)e&Q z4TJ8`-S-`-GlJ4~Y~52(wNtupE2=H0iO$9IhbSA<;{Mbpl! zlMR<%g1%Ho_gB!oD(-xBgp~dt;&YO@Ep5d%g~GU=uVh-OKf4B$%f50w=5lY zzsl%&N3-YAzHg@1R))7@C@KoW2A@82 zP#xks|NeVBh1fM>+PMeDu+W$gc3?k4KUY+LrMjyR{<|i6rhGMEy2g|_2wnzNc=rFJ z?wx~ddAdFCwsG2apYGGPZQHhO+qP}nwr!iIZR73V6VIJ{=e>6#=HJ;76+5#MmAm$; zh^&?0{45C9#DrdaBd+nXIj*+-xyPP#8pEpOZ3<5Y9bSh==!naAA;%+i$oJw$bK4;L zVFl0IJ>w%xb{jKfuU=WWp!HWI+heQ9e44@1-#Rz(qJ3jTarwc(!RC>FQRKP9!$68c z3=0-MgU$`E_nf!b9a~mg&Xt!fQLB5cm8$bzcr@GQmrMI&tg!t3T~l6CNs5dml#v2u zc=cx>rWg6%hSgTgil+>xX+o69zobzdVo9iASXL)7HKVX;V}eodQQs zDRcq%4KqwKOp5vdLS{*i~hF^^J-7EK^9B>!xTicLtH@J&d`C`ly+<+~m%=@r|J z5rE2P>?S}Wo(0DA^yLFOC0ILkltF&;SsiF{%|8#h0_#8tQuLU+a16c_$s_K@B9hT& zmMay54WRWme(@NRJxIookK8yM)zf?NnXUw=)x2kymQ@9Z4IhI5q9{B7r^r$@yap_u zZU(9A9~Y*BxbY5Ug4#M@fba3E5Q$kTZymB`gQ>_KX&u6UzltZjIr_AdSk!hk&f0I^ zw^>$mX4_8h$*sCOi>ux+bl!6-y|RUXIWQH5kDH=Zq${K2iz$TQwn*6*k%Efw*Va2K zL^pI*EL6yN&$^;>>bbO;b?e`GYLb)ExJYifR9;LnRm1(N&+NWHCD zH=m$Hetk|>5huk|9L}{C5bUX*Hwe?|%5D!_@Bak&)SWi?E6Awk#V#ePlcxa zLP(0!auPkH9PeeLDOIhg>SchALD8)7H^As_B5zhAwSD8V+KGMhurSoz(p@nrU47!hTfACp6vg>eN38+I_MjL>@xWM> zFh@cLa$D=@bdxA}k1S&nM^jp7FwQ1J?!FLJ4+0N}y5WO^_&!pARV786;W2q>(=h0Y zp@j5oWV`{9o17Gm#eq13H}+zXh`0L4_khfbE5PW8Kv#?do-=XUAoGnxwS`2o{uHc{ z4?*XZJEKF3D8AsNy0Q2af@kFupLOR5Og%WwiU%lP&ZC%PEBY<|%TX((-=(Hz%4rHPU~GSEtt3C!%$m9T#xcKT)8DifHi1>A9VJTXWE5 zT-P=>-czt3-8#j<3CO{rCzzk;pL$@!^BqCT)ZQ;zXn&?s$`7B`Bl#&LEE<(H>pSOU z4EA*mRL+c5p-qD0Dmt2BfKqCe^Wq~LgvpNn28P1&$YCy4wPr z15Hz$!b*o-UwbKbgpb1T0e&gYOzR_l3z{}4i9S-}R7goAO4hMy`R!9K{k=IZN_8_S z)kC#iQ;o6+fK%5~7ISIcx~0b>a<F=_~W2nTzJuFT@Mwk&7nK=hD;|McfRw85+TpBHBs80N$bK~#ahB&H)wBp zK>9*v;d197R6tdV<)~#s+@?j!R+k=1WmI3)ODbcBSsWdZfL7n9IX2834%2NX*&bJH zJ04eAQu#GJV8W{alpyLI`CS{;JMDk~-oU(;RV>7VB3n+J^FI7*0u&x9PM6+q>pj4< zEkLF@>Pv0TMz=cKoItrWptLoXF+v;NUNecFzLDT^KGGae($Sj)_}}bz3mYAXJ6M9Ch~hbqFp?K;)cHzS(51|bBbJO^ShcPmBE_=D z6pG#VzCVXJdSQ3wvHLxGj$@A(i>fqXS$C~{=k60bTvf0iqPs58u5dni*BIYajt~+H zDR{nTI)BB-Sk)qY0X+9**@9pAQ`N?$>H*-(dPzADZ#Fj8>4|;r+h}!_TI*R!IsH*p zQnORa_3Zm_Q<Yv*Rz7w#OP|TOs#TvfBMHzZHFbj>p$D-SN^1l$ zVpeDj&X<=BUp_nv?|XPE@7&BqHjf)B{c{j75pQm;7>ArRgeo`PFXHImDg>8%JU+n) zEH>5KN>UekuZxSrk9ee@6z`hKNCQda<&lJpa0IXTW}b9huQ(hSITOOASX0`fFAKLd z%l*re4{xlNPlrrt_`$7>wvq+kj8{CH;1MnX13FZkmcid41ml;aqiUuZ?K;T{4J3F4 zC$|nUO^dR0Lir*|siM@lXHlh+R5;F}(<%wv2{%XWXDz65<01MF=j60WFD*)c2auVS zwpOkAcvMAH=?>+5TWln4fcq5jd@{%! zvsdgtWDH64W)SbEN054z;9&gm=$NJ$p}4qPwfu-w^S!lmtg~2ywgO)0QrNg$?NdY{ z%_4>p5*%q*Vp3d-vZNa+nn=lMld-F~LXclc65aAKGwjt0-!jrQRi-$|q8W--3Vm*&z= zJpbx>@SLo? z$x9^3Vy40Ckm7yGi#xEFe@%G@c~=gJ{c0I0!OLSI1Qjr#;bzEtapzDaRYj80CEIw< zl5pgQT%F4_`l5$x*MBcbIV+CO&ZnRKMgv)L`V9uIULH>^t1%ycgK!h`W#-y^>NXGG zKp-!aq?Hs}U%AWMYE^V$b=$btu0a;e#m)As4@RWWmXcpD1fhLj1{Vbz0fjM^tLc=ZDVEU!EG`qLx9nyW$V1QiR-! zaz99W+ndVVPM|{oW}rmIHLjztD6prf#$X&(Lb+a6dHX`xeVCLoq1%v(iS7ZxSjuD| zrCMCe$Lp!QTuO*J;RxkUM>$j&x=20l_ePtA1sj8Tq)5A%T4LYyfN0HMODnlMa7-5r zrT};3N&==*t^8`&d|ZFLUT18_8+(QcUTQ~X^lSEB9pg_RaFKG1I`pZ3a_v(%s^?{jzpAblNGSMfopbo)T3dS2@cK#m{Xy(nUFSha&x#|n57y$HogXC#()TA zYnzQ4Cim#C;`$;&dQO~;ei6YHw#3_*Q@StljBnpn; zm>g%yDFmk(G08s0ur_S@3+)v4s1q2=(s6_CNj-&arpsYZ#6tL5+j;c)l9 z!{cgr=>7JwW?5P23YBg5`z3}Ro(w@4l^|T!T-sPTxB~oBZ1$9e z(gxL5*+Koe3_cM(`RvGc}9``u6y=IM=Y$b*s;{4() zUzZ0?2VNu~v`IKuO1O~e+=e>Mb`6`B)e0grzz8+yyijJQN3oLl8|)bgHtC82s)p8Nv?=eXSct1(ZN8*i;XW4EXGGk?rH zg28R%;7HxA}BqVhOtt%Gc_I(Ohm9h1v=E5)~j9b?~v8SZ?*=&v`Zmgif^n% zB^vIO=S&>Z5$*;@&m2!{ru)}v{NRR~c)B`0IX%W$#ZPGsnpS@;d|g;3N|e_$D7+ZE zobO+MT@}R4baxFG9)Yn?z{6awGc+2^TzPm))`w#7w82w#r(?hG+*?F9M$3Xwey>j166U;)o#T1gKeg|<37bvj;uCu_G&(%bR%dav86Q25m%O-H*$rHF z3bb7wuYOt0WO2CcUox2JVPnVGSV*Vh*8ZLBZQsm<~s_XooS|2SCxM>z4WX$KAdjYx%KJWd`NON zjB!f1U3(|HlioMC7&(h>LaXButC_no>kZh-J3$qGaM!MAqKY(C+Xg!_kez5V976yP zVUn$X6O31XLR_iNl-u^O4BZ@+#G|`-%hAE(aEoO4x-E!1NEaZ=#(BT$4v6mbe&{?# z?=Hi<$wHxsf{jcKjC9OuV9q~Kb+8I-@-Zm74bxS+4%{bBgAjI*T&v)$;VifyqACp6 z6jSy@!FxSypy_#eXh1lIALlO0uq3)97H%f*8V?x1PE;riog`i>lx0h*oWw40gH*w* z{x&pfwiu>xuo@hyYnQ-o@1%B!cder~F)}go>Y{asyi18mg5JpR+2DRy{oruG)G;F6StwH!wq>hA~Dz zNC#c*3x|eQqQ|=x7ymA2&ybJx>+DxM^O}{ah*m6$ZK~RGnZd#q=T%cD=r38?yy%Xl z?Q}E7T2+GCOUg%Bj}K7kZidy*>U?u47^JWJ~1T{Kzc5J&2s z4NxZ*ci4@==|i)OK#G(J-Z*U~k68ZRQ3q$$(_Ge+RS zTZ+#=c%(XAfmGzytXjT4m`HAx_B@g)Ih=#plEz^&qUxG^Cw&JB7ydp*E*aBi@Lq#V z4TbAdP~?Oj(RoH*Do=i`w_TO5Wn^P)nxh@_3~O0zT6ufzj4EhONv!;qUs?_>Sk*(+ z#JDLw-vUc+I$ImVZ6QFm>+n0lF?U~}7Q0?ecZ$hU8%DHpVE3vvjCXU!ta;QsYJX)o z!k+7Vyc;1RnTi7*hzWV#_W4<1HU4r2PvdJ86cJC7`C7I67z%neeqD9J>#(_w`TJ3_ z`?26kBiv0%JEX_Bjv+i@u391yqmhgoDjXqKYcWL`)$QKe(OPM?ZlyXI3V-0cVNy55 zf{I=eM6h#l^1~374p4KP|cMr_2QBqA6%_tZGMe zOJL8EeHC^G*7jL==WGA=SpvuTHi?&-90$%6qRn0PVRs}7zfgZL&2hc$8&{WSGIg4k ztn3H_qNm1JV#LUl zA?!w_2UxWWVVZR=eH5w#4I z>Ol}~S1v%znGxGJVYqY^g&XsTEfZs4!8E;UBEo~Fjc)p_4 z>J*-;c%KiCC4z*OUHS;~Pl)&>pm*i;i6no7ql}(yXnwSdZ{`FcHF_5^xa?0eGNkyU z01m*~+BKxu5P=NouA6JF*|d?x%>A{3I#>kFHkCN#@Gt2nl3r^xG?{wP!q$e#F`R;Z zPrygvSws(6HGm>#{p^(inZ6qMKkrdT5G5?8t!JUbl7VLsTTLX^++qtuf%-WwtJTog z^b^9;3@5^H^xsZg9hL{f&?0lyz$tGTXBnlRW?-za6Z1-$g4{r`DrEP^qZ%QyH)8YXsmAx_MR)0kYYTl zA!|?{PWdK|0}b%F?l8bvI)Zv-pRw>f@Amw%Ca>~1o-Q3ItxeU6Cr?bSEvr?g$#dq2 z@ZV1gXpFKsZntY>x*&7EcmfPBsF(VycIal_u^q>D=4DXPtUF%b+xK^M_W$}YII^}J z27Lx@h`>p{AY^Mk6?Bef`5&yG^BKFkhH?Q{w5{Mzcg1{E>>lmcW^#mlu*I?gqF=#e z9@YdtYGPFid!=zjS$SJ};5{`cy=cPQ9siQt;|@yDL|bjO^sM%^oMK}wqY_|;BV99? z?VZ*1f`Kzf^SbHBd_nN!6Bol{8!e=hJN|W#kTLO8M^kHKXI&k((btt*XNJkhv&dQU zYSW#AcepcWY#Q*QR_#U4M1ep~`~0A9D?;UKA(siJlONeEO$a@FqRos8?q5ePF$bf( z7mhsDuUINIhkaHlK8Lv};I6<3544$>-xPYLq0s2-%AS!AON;Tv{cOx-0gp>aLnq zb5cp8S;A00ak8xR_bSy6@{Q`cS)I|K@1d0llC&1DLnGwq=FqGnoP^%1kvx>6qp&Zn zkvj4%hEVA0XqwB3vO^#9CUCa&Eh=dchEC4Dx}#k+L?|AqvU#p1L1H$L!nWH$K`oXm z$6|`pHR_mpno25mcba}LKmDjCZ=;pAPTjpd@+%zVz{>~iKD?bG1wAA?%URaLV-zK- zS(&HuKh%({D%4-OyegpB6Vh+}g_@H3Y?#3AWs%Brf#+5MAwQ}E&>IP^Ht@qek;{}% z`8=zGxRok!iU>LVcL-BKgI=a2R}8(A;K zqya)CwP;!85mPIS?KPO9R^p@rAw!V9X-s2)*{uZee(|nMM$)LJdg6|{{NCf7KW)2m zExB0o`=J=-y;Nl)%H7R*v7>hRx@w5IV`n)qOvivZD9gVD%@mYTw{d0w%h#yAu`2X^m%m2gj{SOi^ z!++7M{6~loX1f0Y0Q_IQ=34Pn)&aCIzSo>W(OY@`vfj#&1Wk>i*iQvc9sud7aAtvj zW`S*bR*f?nflnh0NOkrDl^o0^c|vNe712_KRCpP72_YvI&Cb3$ zWvjn$!u^t8YM%3M&q=AGd6jV0p$ttOj#pYN)vK=pT1m$O5kgMNFFmIRDEOYbT#mOW z;np}&D3XcFzfg3KPJ15Z*!K*{wCxm@PMaCUI3XTo8~J@l#4z+cNq6UWZ}v%Khe@C^ z2wCN`&wA-`ybjf7JlqhiLL@6O3-hIVq9Zpp;Oc*N}3#Cq`x zac)!<+F{}7Yy|ortdN23Kd}@40%iR3-~KNgwBirQ`Cn)G^&E`;Rr24>&dlDyQNUEs z{vT_S)cbdufewmB#mvyr)Io!pmKLAw$13QV=`3&Lz>lu@WqEWO_vNroCRt^5YjP`#w z@T$R>pebHiEMa--Sm zWUzTke5i7@`7+$9AU@nmFwI?I6lwoU4+_zU2@JtO zL?O4ZAOH&(nuvjhcW!-qaZ71xkT07L#q=-S3$%T|+)r?sjy1qxllHC zw+>Ye^|o7}uXAMHs=RU`0TGe!9SCT6c=+DM_Z=8s>{D~AzW})(ctAdK@VQw930$*% zQSkUrXw3}`%)qMaZX9ZDK$4SFe#hUYrs=DIR)F!#&dbu$(YOHSbm`w^cP(O@%fV?IEvlElye{&?tJvacrh3Y?mGgwpK-`UyN0VyRf zH2uDoQG0v9S#?y9;PHE>hHy@=-@i$|PN1TLKy|IHj=^X_)zvwAKU>~(&`odgEI^T1 z&@5z>;#+t90;c}{>1FX8NOFZ=^n0o~%c;5~V2r5GLm9_xP8-Rh&{5hyRGdZ%%Je-YY~gJQ#fdDdBDXai+= zRI-MeNE~-tkm*N>1J)$wDuE>nv?C=@8F*F*TTUJ@_H!rw+A2c}5b;@r&I%M=y|6XB zBBYNmk1vgMR4JGY=+}u7$i|6wdU6%r`I;o-_GRva9_Y*lsH`JE0nQ^en%6d@EaMa+R6q74LgQnOqkWJ9NuTJtuV_K`Y{cZUOJ#}@dxD0&k zvu3nlbRGl|!cOx)Gb*pIaHgWHI8gLh!5L&N7q{$*ZwTY|U##{;p_h^NqK!uIiAo%c zGQ|=siocfCS1&nKlQRJn(U@?^7`}`1Ml?xcL@%6!LK{DW;f+4}`jF&GS?hgqp?+J> zj6xrXwAC7zC>0|O)sQh=8(iaVK8%h5_Ip$4q=mszl>8F&kij^M#cM5&ADAT1VQ<5o z>9G+%mU(-E&4B`U??yWaZGJzlZxuY_Xon@GiwSO|iKH?opF2BH?Gry2W|PW3(c_-8 zREVX+?%xcgJ~Iw@8&h9%938Kkgnalq^@6A#roKUt2sf!R(`Q6T!O=E0XET02Q&dYp zGl?YdbOz(N3cV(L6I6^}p0UB<+=Czi6sJ)v<{>c~V>%TiQ-Z2+lq!`T@CIV~&7Pu5dq7kZKr)L68_TKYH$mZkrmf<);@CwDxyDSe?McUzu%4 zcO%bEQNRFqv^J=HRiEws9rDf8JQFSXED|dPYqD+J=a&YW`g(K+I~t>ZPsa^rK&({?v;E6K<)1M_?X&#iazwrZdNkd!Kh& zVf)s4>p)X_djmukG=Pg{A`8db`}(h72`iVIN~yR`729l1iFuOZ4^RY-;;<)ucghoy z&sO@xDwARLNCUo%Op5*{Q=TXx3rVVl9dmaR8I#Aq+nEjJVc6Sgv*a#d^Vb$KAQDPU z$bf|!X||Y-D|G84SAH2a4(2xO&TApjaA8vh9=6mgRBKrdEFW6RScjzd25JykdQQ=; zC@G`AGIu4Cipnq3;$>#Ylfvt*b|6$@c)lh5H?1kA5ED!7Y5Lbrlb9P$Y`K`J{BcZL zgI2%f4sm~lS3z`{bjDctLxgu;_74{hMQb8zz1C@q)tC&?pgMT^`MY)yq(f@*?;Bb^46{S zmi2Cdi5ML?pwFqmk>deQ1NF=G4s}&u+KtB+kA5-%pgMKr8D)xj*O;)E$J*b=e4u{n z3Lm;&q!zdow)1ganQe+ONdc`r6jA5_!{3864T%IUQlVkfmLFYm7y3&%RZMUiRY-k!Ko-9zR|5WL{ug7 z-oOFlBNIyW7+AO1cCmdT&R3f6x-YzDnG3Qa?$Xc(Rv0g|E9eWccB+YG6N(4K(?v2e zlz*UGMCDOij+EZs$y8be(X>JbfiR1x!-$^Lj8H(wp)`AGFg-z7n7ahJN?BqR&7Z{o z-I+)K0#kO-mi5L+MuM~b zo3W@xPDSvXkp`(WNCR!llda@$5mh=`gi<_SKWRvKqBZ;-Pg3P`*yqPJE}%Zl4N>7>5^ zZD$tW9dOc2!_pHS9Jos&6~);of+FjMp2J)&${CEDc_i9;t?~n4_6=GfG8^8xv)K|m zz{yXTI5ERc@u3kKw=UFlzvbHiKL=VRYYP)Rh5_J;w9tA(&=Qf;S4}H0<&A>W-LXU1 zXWJ|hIVd$%1fCJZh7{;g9GfCLq%CU;jL4Y*#i&{z5=(Z92oR*HaiDp@QV+7uc}~$D zue$hCjw`;u&ieBs-*Ef9@4;xiQ_2)&zElCC_G&n_@2MA>Loc<68w5xl^WhuYpvZc} zYh%LIl%ae^QO)x`ZR_24!0S05gM=#RExc|O>P>O%H7&&<6#J|FyMnPa0af&#F@u5L zH_Ppc_{V~H&URESZ`i2$kI74c%aOdGW{!0(3qyrEEl|Omz@H4bMn!8A_er zX*?YF+mauG6d8#_fvgPV?zx0ysDdJFH^0avJ6-@Ql>p)DDR3ifpQ=Szj5A@fb7eG3 z@?%h5E`bAY*_U!5GlHC(X2`*#103d9*-D^ax9(&ZQ~iVw)}RJSD4A zGr2PbWHMdF3ZgwCQnkz6L1@mp<|`DVg3047!@6cFbMeC4tr$uwnus2k{k7<+TS(Li z`A5-=)OlW>e)(a=@Y1Jx-Tb6Y1U(c5I;08M_^z|D`7!vz^VNF;M1Q%7I6@7Fr|}81 zAdduJHG-UAX>EE)lB)Z;1QtYPj6Oa{Vdva{Dqyo@Bs0KqT0PK>#tl@()nlnVo=rtb zv>vMMLgfwCp2R`AyIR8yhPLtM9zeOV+tt_X^C#5A=HC0TmW|mEF$^(dbD75LQ zRF!cOPli{JJmy0&(bYQ-EqdX{xJ5`1vooJ_O2G2w=IdEULy^LkxyOiT=L8b!q<9)vpod|!qmaAjbh&{@!cqezuXgOr-v9RD@Gj6$jeqnuR^QnS5P#uxAGua zM3BGR6Y^N9kmi6wvEX8d4=4DgPAzoIKIU;`BpH(Z+_L6b6QHELEEVh6ZDGJI1QkoZ zb6HCPyc9a0OKK`EuNT?Grddr)>IQ>Nwr9ei(~_jcc+7`yGx`yd^a_^DiQ}bA2iWhX zvQ7hVT@^{mk4PyE5ovW$kk>n_NZte8=RggzoX!2-rkuvj zq7KCLyJAEreFo}E0pK4gvisw}saY-om$p~ZUt9tpD$b+#@4fLjH~7~4W;i*}fSNdy z=HZHO&*CL+Nkd}a;5qkM4fA1sSq}%`f0sS)f*A2F6;42Hm@l7!(< z=h6uA{0Lr5to7Fv`q!@Qt(?3f11M|7)4edmVX5~B#0$le7R~j6Ygr_SYd1-rFVn%B7-429t=E0mRaQB1>KkUv&+_`1(_NCIcp4rZK%HwhBbGVPhCd;f;UHzN9p33U z@XAz?yvry>HUwkSkXoafCXR(5v!)UhWI{19Dx6)(l48$7bIUELqB)(+;Zxyzc6Y(Rbft}&WIP#n^uYXEH|PAjKj~%_Q;@r znc!mz2`l7}_)R@#SsDTf%djpMeG%k^hC-YqKZW{+0fV*(kGNbOzz9e{H_6mmFn7A2 z&U4??i1QaqAB_KViQBmdaWacJLO>^GTqj5RebS)U0@3vMF({&+Jl$X;;l32PLl|7k zddTY?$F)gO1(%1F#hfC^9Lg6UL!C4fH-Cpflq~(3pNO+0;UuKPDtnpys&$f%5nAsa zGlu_OsAvfqWPw-)r3s__$^a%G&`Df7Pv8!itb0)~cKfBoeI)5OI(TC=5iGNI7RQs; zZpw($AhBkIBvB|Z?efYAjkhbHgw@NsM`4k4iVsun{v9HVM|_}VD(qB^2|_5$4w++n zG#m6sXTH`_9*w`~Dg5ogrh;v1J63;c$M@QDoNLsjP?j8pbPs=g-%&Z_mMTezlouc4 zUke?7Ae{S8QqG3~=4cJ7IsNrpS60>vP$CZ8w3EJe^7roEuLEjWRB8hD(aGowmCco; zGRBMRRtFW4Idu&QDB*pyb?8|);7&Zz$UL#SNv|9z5!fBHTB#lehClUfb<_B;Zr4$` z@u7EJaP`kFg6-V}BkevB!fy?R3!`spmZM$_c9RRUG;aX$(Jf?{CaP$QSfDdpE%QjH zd-+? zr5d3t7pz$*A6C!9x);9uZK@Dm@P9Gv*wE8|7t~)yDBcoX7nTN48gaOt4V|uQ1aWx? z`Ed*k$hLO>1+ZTYjABub`c6E)vsiO#rz{4jon;TEj4b3$IgmVopEpw?`0GsyGyx&q z&%W?fDc(2gn_b_ei?*B^mw7kYtM#7Xe`YyyKNyik9YKpJfLCP_NOicN zSJO%A0@S-YdxO=->nMwR15)75Iy(SR%#(zKY_|woLg=q+*+_9#Ajb6lLVj`&($%o zmRC>1wvQ6r!P+AyQlLZyTC=u(mV2K9k&L&D%;GK`o&WB_vt*by4w-bGnQ&}KxVVcv zMb<>^nGX5-ZB{ND%n9X7n_(zpkf&YFju?@^riHlI28l&{UJ7!gw9w0VL(9+ZtEXmT zj+IAu5Maun3oR96sa=T&eCpfJXZp>E!L<-Vf_n&i<(v*rE#^4ylJ*E#3(0|Jv^NRF zZ+PyjBpUar+LC=E%V6c(IQwI{Qk9-RTvlB$fVg2W7BflcRo${KfD{@;F4;PQ4 zb3GRVp)|4v!?FTbSDbk!CY;c*6}`syAZa*2z1ia4qY#stbuNTEldY}~=lT#}Qk!d9 znZvT9hf%<^g$vZ_A!Q_{EGS#>b^X9N89Sr{VzMU8yN>a`+|-_Q#R+lZi$fX z8|*SzFze7ok)UIXvM{lEFaL;niULnOoDwZlb_%e%;A_)lOD*~S+0hCC9DHrDUxWT7 z4d-0+E3G=+qn~wf`RFu~{%P%Bo|m(%sR%SCrA9{xwrcYPfPZ-WaMx3o;Dyl&1EmU& zMP(N3-X4SrpMBG+pVut);39+sh>i~%+W}+Q6P3?i1S}%q5kZM~Ey4j?Yn#C?V6?r*79332bR5(GrhSpN7sMBOkFmsNguU!Qxg zJp17~#26WIiZ|9HjjNHnGsxTqOP!Cg|7lg0b)BOuxvEQmyAv!sFRM z#8ZT$B?I^j|0|rO6R*RAsp}6Z{kJd>)hfH`V9cL_jT$Qdgi09k$}$zepcsX!=if}8dD%h!!Pe_K*|d8ayK*8$#wGCf(@SoUj9`f}?I36Pg-F`#_ zE2z}oj&+|}wFT9rQ-(5!o@i~W*v#uGbUwCp2G)tK?GG7o;ZYJtyucgctp)eCRlCL# zAm1Rxw0UqsMR%@M+`Ck-h#TJ9bOd1EWG8&i(6TV7W4C1#9Qv>aon3@}qyn}jrr6R% zp)JA{z^%uS09lgHZ-Ycroq!&vPqNYJ1TR(6FLb9haT1la6I?dGN&fC@U|WBMlB=DT zfg~@=a}|2_*-2Hyz__7B_GdaUuo?lU!4Yw~;?0NYvurASM?=AM{#4F`t?8MlT8fB+ z@$RGc7;hG43^FCG)<|p>|KklVt=vcW+lmf12zSbQbGcd#lMk;JTO)Y0#pR}F8D^TY z=a7K}lN;jW@I@#H@lgPL5a@yoKjoU4%nSzE#f3cwri*y=9qW@tEh_N7wt|Z!Hbui? zn9Ek=s{L-SjF%92EVyI?C0gZOlwYvI_>v#bgC2t{%(*-#_|locoSnJs?U7rWIn_ik zsr^g?+o%-BQwOLPjemj{DLg}HMwfhF(zRQ7b`t4jNM=1woUY3XUIEJ9k~#=&Qil#9}td;B>I1^OtDdMzOp7Y;V2oU~GX zvaUnrD>2+v8~G(7ePKKQW8qx9nSSQBtuPH|Fms||`_H)%pF*#m+>u;vNd~w-8ulKh zeMomVg_pCUR24+-!%zw_u=QEkG=(#*EC<-{6&R!X3`H`^Y{LlC#p@_D5O`6~%j@q` zbYG|7SKj%ePu&r^8#$Gyg-oMt&%~2BHBhrEqwi_XN-#1-d-oL?ws^&X2fn)plf~U^ zdQGp#RN^x#%|bAgL+LkmSD*a}3s9`<_R*VNUmcbJtcAP0~_rb?(GR6zN6S$)7)vP3`+I9 z4S;H}wS#iVA!i)V(ShU(E79k2_1wz~`Gol}UzUR0$Mx}24!4#jECYSy2**tOG_>LT z?M5#NIrQ#UGYRd<8qkD#Lk$`&$lbFjTWgkojF7f9_`iPr&vBof#XvuI7VN&e}ELF~XWxse_^mr1m5VBfO=7Tr$p6@scpf zTu{TzErG^$^;wHdoXFh!(X*pz0qM{yxbbZEZ1TCK$3Y^LiPg*cg8?3x+^$GU+j11x zaC)IJjQR5*R0#Q&HG6)+%1PaYni}-WHHDfpRcq@j4#P9TxE+6skV@y419qcNQ8kCS zjU1_5pwSZdkF53_n;1QIRJ{3~@A@JoE#nwNn&Hg%m{b}6R0Orb$~FirfEa<^tI)!C zQ6&3jy2Fuy)ulwFI!a|=QtBlpJn^CR8EtMvqX0{9Rpg5?z{d;^`!)C zfANhsQ(IzA*rqz6Y6Qn7XxvnE$cb+R;v2@6({qm;^hnK#sgG1-R?l z5~f;|(L%gmX`z#gy9!316IaMg9z1NPb@R5j%9T(M@&adZ)2T#!5>$t zlp35KRvdBRym2+J7I?ECMcHmRJR8##Ta(ibO_3mi0Zf`Nz-{ip??zz<$6ok`BZ9Q| zGG!xI8UD?ZQH(Ikh-@et15epCnCr0-Ws$q3)!PO?fr#^QGs zIJ61N{5|b)#yile7`o>$R~QidSerh#PfjDYjHBsnEtt#0bOqO-YahInc9xnhUk|X6 zzlDLwmrvwDGSzID?E2NSll*+JB(^&ipyesC5A^-6$CvbZl5mWaX z#7S(G_x{;GHgT!g%xYC(c&y2?FlW3NuF^n9Ux*Q|*?>96*^ZFIm|T>@e&p1V3e?MX zr@PQ`NENu$A$JorrzCmi3d5@(XTr!PY|?8ac|(n_YqU@jmo2nSKsJz_X*gNpLBh;g z1J(uoA^Olic$G0sX#uwy*Ci)sFp&yXG)INDL%|r!=Hs~n$2V%_kRxL;{yM<9YI(kh zK+eaVR3z=1U3*5Pj5ylF7qi^sMN8@&t9a_hAIKN{k_xdvyk-j?X|J;ZTM5B?NF))!$m?%#2d;~i_iH>=?F{|YeNrXE+gzH%&wD5&OcRyxzRbu0#8Te@>4V8 z>~S7zMp-3`<;Tqcht&yq^@PnvtgH?Ab=}9Ju4MnwN559)dL7=jBPx^#^zN}wj&i0( zp|07WcFH`CfdR35NPfjF$6VR8jl*fu)>hM_@lPH?N9SxCs%_UKc5x~*Nb4*Tw;|~+ zYeCXhc1F1Ar(!1P&qU4vg*9i4iu$mn0w`=7zTW*hWwwV20#nD6NU@x|=WLg21pHGz z(`p&>^Tms`rbz65a+r6Bj2gw@vYH-Gr{+b*BGSSi299@OPH<5vOq0Rjp@Jl_A!&pp zm4{yWXfrt(J4VTm6b0(jO(wq`ik{`EZR=oMeb;aid7XujxkBZ`xzkORacLH#MiYHqkA=ILk?%(^eVE7l5hXhN z5~ld1#<-c;a2^1FgcTM!TncrEw)pgiO$_dJhd8OC2#`}gl(sY6SA4|X=Qj+P5D%Tl z-6}euG1{rwrH*W`wh*D>N)vw8d|^GSIS#8ADBms6$LJT#+rM59SHlOoTnOZ&jJI(- z_j0ja8NBm5UQeF3SIW4RnMj6wT{;!`=|vN)2yidLgm4`IMX-s?t=RW= z>YV@m?qRmgr*o&F=KF8r z2&HVQACpN{iL*G775%YEIM!BzgP+J6qW*B81{DyTmWqkfqpay>C3o6oiVUj1{mx8@ z7I-xFU)R}!D24xcKZ~PW#|>)}23S}f=?q4X~~e|*ts0M zI^PvvPz$K~W+!t->_k_=?ywU2H0tA(^xr*+50aESHafc$HRKlZY3ByBG`gAG1tYLw zfUqW_+Ee$6KO(hu-gjprJ&EYeoMB9$!sguQ7&FqT0Os~EOz2EuXPb8ELf>P7ZWzz> z&0ZT)X!peDm1M&>O*5Rmp%Ba&R#68d!K&VkZa)~_D4v%#q7*NbMDrlOyXXWnzJrEy z%|jabVAm5mm^El}L#WY~sFYuAVjYZ_e{{(e z22h_{EQ0qVbj^hVh{~_l`hOs0CMxm_E|s3l!BNO3PvN^?pGUqnx>XlMR|b3L zzEPNEf7DbpTBPrkvH>+V@EYy5m>DD!Y#GMUX60q6R+)qjPEexEQQK1zevXpT_H5!d z%@*jzy91GsgWX<0;5L5Hn?E1W^DG64-Zi>j2D$q9V!$kI5Eu=?@zQ_U{thT?G;xz} z9bn|N)N7Q2@PmfC-G|PoQw&q~rQva(32}HQYI0`8)_J%sDfL3=V|eSwxU`vfV#5r( zHUUe9M+~_e%F7xWW}FetRzoihdGI(Ff$)e7}8Vno95Z3n^%bUd`# z$OyRs741a^s{jnSvBg$87SY^1wtr*#DhoF^>yMX;&54RpF3|HyP9r>5c#AeyFhu=3!J1{-b4{N$es!qh)dcwdt@nP9Wol72j_hpASz7vy) z)(x3EqPswXAhnmxu7_QCXFG&ph2pbPv{5#!?iTswFx~ZdvU;Y;%KFL zX+_xReRqo-iD#Y&f~k^M(`zdDZNFpRyY8E06k~__9V(+pvvOHJ><}d^lWp8 ziY*73Gm*=82kv1ijH zEKK6qVfl^nC}txV-UdXO#l;rT_{YaZX8uhjc=h#@rQUR~U14-8Cr|%=y*P^r3tmME z0s(8j@34ACvH3`}B)L-!P|5lW@96e%RW>}?g+mqJG zEYle<=Tv>90n0R}g7p3I_gk|y*Dn>0k(3!cdhkg(%Df05%9kTDG@EWUI|r zqx#wfs)zdmWvqU$QSZ>4<@Qxg2%wg%&iDbWhT9iI42tNE0e?}}AKM&ENog^_+1V2E z4F^E5tBzH9Pu=cKihKrchOLcEFDH+(1Rj7I7kp_@?Ky6om!;(tEB^X^t2$eon8*3c z8oDSV^}XD;-Lo5i-96+`gCP>`3TMTzjq3@n_TY|>C}lWnYk(d^f(pO+w|&& z(m~7X@Z>N!wu98yhRa{y+&^t(d(_8HhzuH;&BmJE=4U6^C4vNh)(V?~7t<$xDTV0V zi?W(`-5QPeU5Xtx(4JRE;@mQkM?QF3((x?JC1XXTYV^^Kg0*Bu+y~>pNxKfgraH|3 z32U#=$rpe-ERey6fowQ`;>i|xy9(hs#I1edZj$CkNAIpXhHe*5Zynvuo|7__KncLg z5xEw%bgL^kj#oVk@8CjuAT2Q~hGF5uOWC;mE(6tZ81{Dq?VzWn;Ak_vk6fH8sXd5 z;Eop=qR|;WGFS<3yvN5;C_1kY>Ng60(c8k%^Ol*NVwJBS1yZ4CwO5Kh7y+6)^0q$t zMyYF9&b;;Sk%DDEyW5zkg&?B;{V!ZtV5M73sc8JCP_VqVf`lii1&DjS=Mw1WQ9?Xh zN~9+^LsZ)_9cK=*f6u`qdpU2uX%2!jlYcXDtVj#$a*eej!I6ycw~wtL-AapzcK6iU z&y%|-#l?68LHzvzo$?yM#ZEh;@f63)tyR#{^U5XrxCK(6Fj2X55y#1r3Q7j>O_rq2 zS(?3^6TRR$B#h%aWvIPYkD1|T^YY9V2=TGfed!FJ#Uafvm)IY<^KZ|spzBjn1vA

7Xs@4*jUrrRi5FuM7D0gci~lY_rSQU3K!3S!EtUQjf-F^-k&52}K2 zb&+?yL+{n+nCbRC3Oh@R3suQ_>alBnhRtwNLmGQIQ?J31<@HY zeWX2E6#PHXcy~kdtHK(f3?7FTQ8wy6uPYIl;vsXPYcFc7Uh_h^wGS~n)$gG~Ea!U! zu*a{MNzg?|Ry-yz11GLwcCQWI(BCF&<`JNUMmJwzbrl;Sr+w^_w;R@J5bjT#t~UEB zpq79PA0Gfpvt+$-f->0&{pV{-7*v(gomE_C-Es8qBwq3(oe=DyO&&|be`>I>C?7$N zGB`Ru<2@cBK^e_DgWj-E+%t~gN&OFcV197EWr`X;#(Gco5X0pK-|6SfyM;I&eNG=^ zz0R`jSgT~@w#^uBr=^y>G>jo+&xF_HdL~!nK(AF$|I9)Zwv0xTIA?a-TbjEpY@F2D z+wE8N(~sDE8r)SaDIjvqzC_$sqmiWjQz7-}=*n`~0yk^}BN6oh6-8&TF*3dbJ_{y7 zA9|J-+TU&E`#)9pK+DV(KEJq8Mv~=3jtB{6bYUveTqa(MUsFRj@+RN8+ zme8{0l~*W`ml)?uv5Q3jN;8*YF`-bgCEo`XPWq!cWy;0?oWX8 z=ZYlkP?~HXd!}vwtt;U;jYTG}Ua!fuqH`0`Lu&{AC$n8seB{NPId6UK?^JnPyQiG! z4%sT)_s@kfkq0{BtzVDm?J4uPeCq=**N}~g4%!tHlSc68=>8$0)igx*xbSU(B=PNB z$1R+VdEHA)JA7O=VK17tKjfblWVG!|rMc2V;zloNtYdXvjVMMPAeDOslF`)fQK9^U zSEmFs<=GLefa>E!bd9cU+))Vl*Bx z+rL57$BUCxUQ&(rD|(h8RsCD$>&YqF?1xH0u>zofxGDf|zpkgm}|Nn33nX_YSW+c*A-y+3018?10-?z1&#*^W}r5<6H!rK;VHa zmYlKWkM0TKtBXlPnC?a8zBvt% zvxFST%`l6~&sxq)Hi62y!3*O`gvu$?vYDwFKGfmV=fOSm6f`yjVq3sd^0^rBou zVHF7<{6J18G0DiB1X9;$I9|&unaH`r@<`!e?X$S)rkqk~@n;lwxdrSraod#+UM7-K zs}cYc(UstS;b{m@F_6ZJABgQ1;Xm(FhjB#Xlx+m$`sL8jsJ2m0^($K}AOi;W@zUaB z$lg33s4+qcW9|8RTPu`Zvp;9b%MpHU`PJL7D1yf>uuOLl_`!jK+@?powGk~%ULsxy zL8RB_Z455I;B~u)?FkK0eq>7H*{g*jja6gqmhP5sqjg-%-NkZqQ+cedQfvPdqS!Cw zJb0C_U4@GwygR=lf?Ya|vM$+ZRmx8G!Z!{Hv31Av3(!`lUHy4m_PYYnB%!qf-o-hK zLq6zFW#34F(a@xKT+SM5+Oms($e{xNB2!zRmq0S^7|qO2v`iu zet(snn4b%3#m~;-9-Cyo-s+o=uJ^jj`nAG6t}}&3k*gD{7?fjQIhkjCUzuze0-eyT zj8h@K?D73uA-33_tbY3oK3;v<$$p_!iBjdSPP9y~tM-K*Nvax&ni}qBJD;he;<2e_ z@0-O)mTS^(=N_{18UMSj3`)ppmw0sn1N|K1WM>-bvsjouDghH64jW4v2`N={ij$Ab z@FS%pL?c1f?+y79qaE&i!}ua9ArHtZ;PA1@v5l}i*KfD=v^D&9Z5}J@osxd#dr2I( zp>_xvuoxyP&fyjzb2y?U1UDj-{JSufxfHT^whohBDh@@?H&CU-j@CR=Jjls?3L z!I#pUn9O|*Fo~KAYhd$X{ISZME=6KyV6&;0q^ddzd15b$&STj;~XuWIX zq7R}SOwQ)7tUhptT9VSQy*3QPmRLAS^dtFWIjC7+wFxLm;UPnldjcq@8fJw2ntIr= zyn3wco{GJR6JH19Wbh$Ge#-E$&Q)XVo+omQ)=+CiJc8->WV3iiAnnI)$Bfp@&!ZH8 zXPYG7-*QFqtEIS>H{QcdgYQAddwjustfibrG`w)bT=Eis!kO14xIKER(3qW)$F)z5 zM|<uGO)A*6{;e1#{IkbcG3D0Tj;yur+W(-)YNLH=$e$7}8 zT6I>TiGa74bmgpI*`8oyNW8_ppj++N2&pcix0KV9QFABk;SbfLaI9+&sw>B=;{;nd(WuSIK;$t=Wn%hru9yyfxN+iXeO=vWO6-Cf&hdb}ky97fQFLDd3^3C; zPPO=b)QDqjBDMAC*1k@hAONM#sj$=VmQ|=WC_~oQKXh#82}xeJsOG)nk4iYAq4^VF zMw*MCFgyi^k2E2>8(A~Ti+rodtXRZ1Ag-wY=m&dVVkscVlAvgOX|c8H&LDX>T#|gp z7sE&1pZJ#k^y9Sp7J;=J&=+|*)y`6k6c%&&r3FyQgtCpqY95PM@|ztTtKUM&@=*4E z7`k2u_#N416P`TW%t&#Dt$q8na{C;7E=dKE93L*9au`mXiaO`HB6;BQeAqDAvrbTt zS&OwMbU&Rt;+8|4npv0EnL<#D$&->gx@*Qx?%2kg=()pJNxC<%v1$DV0@!_QdBVeL=KG4!Gr6k)jd`>i>H86lSW?%yZ@=!o zvQI{C27C*dIX6ksDQO49&tAp0m^*IENG~{8bo>?QI7-g#GRB!*ABtySrLpQu z27^92npy6X$^c3L+?aka2$tS$P; zpIf~sjg;%84NkUBRvzm1VCZrt`W`N(v(#{cyU9K?q$+$JpuC^W#uHOZ&27;+pMYx- zGApjXy=Hh%AzlQ_#zT&9@R?Q)`9~|pMCriEO>CUPT!f$@JKw$T%uQ|2Pu`F$B&{Q>_-q%YjO=K_`fhR6_8e}d=kD@7hFvol~{Ye%O%1#qaPAjy)u@dgdZ-D-zJ81`UtT*pCmuf zv;+{W$qq3JXy0ao*KGZIaSio;ygk{Lj4g=sFrjVHLxvd0M@DiORC>g&eE2YD%~ZUL zEgaPDf=Y+l(p4ARpnqp7*xc?Vf`DhQpEQcs{l)!y9?(k|4}p*;f>=}0FK5_8QzWER zSDQiUP|qP_MK+o7VSM8ez%;U20M7J@<$j9kxR|4%*@iArt@56fq*;nzAX5u*MiB=j zPf`j9%Bn#2o`~7A-zg3uJ-uMgYCy(xY&b#Nus%aB7&a1II|%dXS`R zlF}dodZ_rd5IK6aVM+oe+LjsVoIS*~PGcs?ZpwD27}uxcHI_sjD>xl?B5Rb77G5h0 z;Ni-tsN)-@vgEChS*U^E5DEJ_a(sK*GA7@mj$!#CW=gXQJneCwu2J@}ueWiGSwYbj zlABw8;HWW0zui){6DAPzjzk?tPOZ^-MSBjMx^dSWN&0S5WMGvfz#nyUjh5Lh;1M(? zzXL-6&5aE^7#)%KhNzBJ2`s;?&|m$@c$uslP8nvOLV+L)OXGyUhO6pl$oFKa>xged z6KNTrhVt-OHM)-H-4+VmQb^&`@XbEvi!8R!Llw=cJY4d^tCZ9GR9JZ$eSC<7TCSZ! z`*B!KTXvxQ2WXi2JJnrjk?gzTZEqGv4XWL&;p(`CE8&)%J3crM8lmcpmri0OiyoU~ z9ylfrqkiO}Fu7Tgcl9{5u3nZcA)3?0$tO5##ilf07dg$N%IEmv0W?VZndR2@$cVao zr!9LaX-YH{;Sz}cO3Qd`HA$(}HQ-GD4*(}X*uMn*+2Jih%!N^s_i#wAa(OZ{j`953 z&)nR2RIABmf44oS%uF=xh99 z_h6{bQ~u(9^9-=0lH2wliML|euPgtOwM**Vk)SbGr|a9w4!e+gb@N4FupVVh)j5_9 zWFZ6DpPh1>%WJTv6Hm~52W`tSf9t}k ze}R{tLl_jE`!SQCTSU3fvo$9H-SIe+ue4PJ`5RQAZCA=r+&?JP+U41Ym2HTrplgAh zeU-@a^vpcZc^qZNS^T8Z;l}t(9wPy=+`%1(k0;#xyLXRbxoVyoN&(Hq{7Qzhh!1%S z(Sba7bfDuzhcNtcwWYkmT`F7OWj*!-)e7+1VuG6y`jRbqdm&PDCz!? z(DtgAIaUZ102DtnwW8UFt2f#Z9n-RJ=-j@~v>dwKA_GD%jO3o<5hz6I@GSifZtU59 z=VHa1(Au1kWCyD#nhW5YYkxj0UFf1t*~d_@3y3#XcqnyX0s`)xN-_1q0{d2_7z+uw$rBv6(VuY%jP;f^#^Q9tB3{z2?*;W=ACghktV zz$ILO^6E`U%f}aOW3wsz7AAtB(Q0XDX^kxX1JHK~`eSS(Q|hzFo2u{9Lxvlc{M)Qa z(&lWtOSpVdL}J5j%FqGosCFpbD3+d^zwLhgXrpbEgyI*@^MF{L1ZqMC%C(X`=JO~i z`sjVZ`2utN>3TZ#zA6Y~K%mV+NU>U%vrF)0cMn1GLf-Hh& zxE|YZ7LiZyKIY$PkE~Gi6Lnu}_w-`+0=x)I(++(pp_sa$IzwpSS0Ls$kKa?A3lY z6#*kcsR_j83v$IgALwZp=TP-gs>w8C%!{Ftz`C>Wml}S3nO$8CUw#jFR$UKXPZ)oK z+!e3`DWwVT(b%X2_AskIvzVi@(04dWh$~#O zo{^DN6yHq|#7Co@OzvPDgda6+^XRJjD1e7QY`h((nEH3|nv7_hCBwf}pOv;BA=8yT?*3v2hFId|6Qdcp>BXxR=px9 zX3++nn|Zh`Q5!C9MfP0#a^Vpwxutb>t`k5A=z8%*wZGH?#n&CJI4EZ!%H%&uH@27_ z!vWg~TA6dnK(UEw;lqaNqDjyjsg!6Sb&M88k_L7?njCs{kTWVR>da5wtS%PIJIfa= zTfyD^?)3a#fB~VU(LG?ujjWPk9Mq{v7A#eyJU@+kt)I>{902_dOEIDUaFY9j0XB&k z2)1|irE0pLZ*w*M{8u47kM*YiK#g6z5)M>-_9sg4ywCkuA~N64tc^x%K8V1~Z3;D0EEr3$bR2(+msZ|w*E_6Xv&m1eY zEc>n-B-pik5oQgCZ>zw`Xqvo^Jc3s%XdcXyfL#$vy;(`Fb2vu=0ql4#7AL&2NJ-Pv zbXtT|C%4{22Q)63qbMP}+IrnMb+4HFALjmmsNc#g7SCj*e|_i*qo-eeyjF)P;`0P~ zH6KuJbfb?p7|k;{o3P&9>VlsQzOLQ-nuV|U;mRrcQSjkeNrq1h4R|vWSU8Vvbzo}D z7jSGG4CbJWhjV0kL~`8@<>fv;%vc1LSBvWk{Qrb;5Esu~dsAvT853_=Xrlp&E{|%c zd$&~GfCyI3kF<%gKzIA2afug}YSJUswcR4*#elsO~2ma}ooNU*DVpBz(~`{ejZeT?E&lQYxU7bWDOyDGHQZ zNIr;$P<(clOI#vZA+zkTPPNWWq`6Mkk~_p3K~aMA4^lDR-Zn8W6zN-*RvhC@A?W5J zc0z-v=6mO4yO&WTJUQS7WI<2KWS0Dq3yz5}Q{i(t>+GqF_|WXeI z=PG35-VcC@pR3rZ70+n{hDQTyQJ@;HvCD4WJt3@a5xZ(&3p^K_m$b5}L=%iu z5|6%3ddnw0XXQ+pkc;}+;-iY=wx)f?7`>2yye-I|E?!Pin~#~!X=HLCa7aG%OCf96 zr8s9*U}Cez^1SQ8!IM2%|;o#nU}PdI&L1prxkNsV45;NsP6?1~!JvlT>< z5#Kb|A@5xq{alz&QJd`BjP(EZdYk6A#eg0Z^k1+Jc9SaCI_~8 zR$G64_fKo^&wD<>fo)G zBT(>p{xx~%rnDXREVEA*R{kXmA_}V4;wNKb036iBRcTpp>eiV;;Y$+Wnk*Fi6we8; zsY69X<)EW(@vx(c2^#y#?!fy$J95Oys{r=kcpt(umPBL>N43T-4I5#B)8CycKwn~` z^dW63sbKrxP@Ch1k_pQX@}kIby2)M=D7{2YmK-BTa4kxYBkwHU$hpH)7ZHRVOlAvN z^OW&30*|e3>MV{k9vENwlFqI%nAA4O==N>V^ILSrvE!bRFluZ^9b=5o`ym?3&^SKH zkAr9IqJ}FU3mhpbC65AKe*M9%hJUV=h-Ts}IWr!o5&58t=nOs7yB;#*FpLKg`ko(H z?2uCF)ST%3ATA#B@s1v%!dN*UFJ9Z&;e=*9biE>d2@*eRN2L-6*3 z8Wl@mzj#J^9M!&D(0a|x>1PHn88lm*D{zj`x(U@~e*o;LN7m7tfHVE4V+JQDwPk<| z#k5P^qZy#sGC?BEPHCwk0Qfkbiur0S(A>5)^Af?AlT&mY8I-e#(A@?4c>EXKfK?5+ zh)NF7-jSGEBX3WoY?b_;C2+6J^c&5Ce!0;Sh`*<}LivQIE2HIzbzoYVQM#2+DXx5pKiMJEIL6N7GUl7ORp|k= zbl-yGghkr>c=L;ZFFyQlDi=~F|AIMH!>+2Bx(5c(B*d_RMW}G#IvI38X&^K@f~>Lf z8Z8%PI^S&!3oS^-ty!7_DwU{tT6YF76yL0OkQgw#!r*-cz(T|Nk>7zC;yaMr-eWVN%p1bV1!$}p=t7|&cYbrM0FN^$mF2kO63E?Oi4{9DwY$*UMM$B8KFY

`N6-tUgqR zBBOaOMN!@OAt}wbz?PW9JLxmC_-Ib6Ko)oAdO?rmtWkGZtUqxSzOud2C2S)viF3xl z1YokbQ4ZY12sQ$&o$&~G^D6kpQi<21mHOC$Ii~2#!;CjvCq^2`h;`*1w5>s^w+e#uOfr*H zo)%I&GNG1FBv8?o)d6A(NPf%%8m$hcupJ^G$r~i@M5NK1ml3miB{1(_cqOb$oV+Zx zWet<>FrQYgxbHO+oW_M*0<})z!&d;{S~zVGXuqm8GfVV^)q;(Kpt~4PQJSktWUY0I z2Q)~pfRS3#%ID<&ys3y4^}uL`5$NwaybhC0E{D$y_OM}ihhKw@&y}21Hv8fIUR7CuBZM^17mwdn@Rb^oV@v#b=08uRDl&b zYG-J7Lp*aiV<5gU>6Zm9&|d&w82vg|ipN>@Ul|LUzQ*z7`G%b6O1_eO*tR6@FoWRj z^M$3(Ig(7o7~K+M@=~#FGYTUtqUyiBx7EU~DaW#YTDS%7-?no;&ViMdqk}py|RZT-_MfXyh zj-^@yiih!w4c0^L88Ju0tR*TK-l*m@de>17y5iT&&jpA)Tbzgsi-N56J&B}E)VQJW zgL#rFV|Sfa&ziKh3Q$!dL6W-i@!u}uDBEPGMSq+qIb~Ma_~#2}2mDKX)*lh_;wS?DM*$lu03lC`r(M%V+SdTNGRpD$GdlHC~(BJ9~ z9w~t=jGSVFwwrPnO^~wCV+`&yeM`M3jO7s0^uCaO! zaIk5C)ZN9|MVi5>z;d&T3*F}KU1%N96C6h2O~wUYnba35c2Be&-x0Sw(qgE6WC*PM zA-e*!&4nl*Pxr8h6?2y-)mbFnqNrjv*^bS?hN z;<{)QA@nN>tG3hfj@LT@3u1E->bl3b!>3g?Ek9?9LiXI~9IW06#R6~B+15$%)D)?1 zVx~w-x<`-Pi=*6r+=m%s8wk;2Jo!`m4gwxTGiq*pqFY5%S(iA5fD8Bg2OqY)2xg8s z8s2QLIkdf3o8RE%09~)}7(>|LZhWqBKhd2o)96R+Pk+?tD2}Qc1XNlk>BX|=p6^=F z^51K09(mm_Oyf>O<{euL55}HVJ6Mw|=Jzg4aeDrbL&_!Ih6kOW>rM(xnCBG>)`XNf ztdPC<`dU6ml=S`@bR(A%=1lg{_fW^gzKUj3hrr3)RQzASC~Hw(Y~L6%MuSCh19T)4LL$X^`fscq4CMnI|>9qr-%_npx0M)y?zkN(2Hz zykLM~ik8)~Kl{3nZ*i1j^Fys(&P)CvNejyF`jC_;x22EbW7WCkJ9HPg6$8Lm-il|1TRTxmvLhvxZ>9)>)Z!^p`mu7+PRd5cR=tp9$v$uqXri_ zBi<>Jb(|hlgT2w3%a;AOW!VTjwi(8v0q2KC~%zi4bIL z<`q-w+50LQ00GLG0r#)x*DaU3lXu0u;^ps8v#~b*Z5R-#-g0?s#m;#m1hD;l@@qUx z!O(n)Oz4_*emHvf7_Xpa{sAeb;;JhZgOCmFAzbj|jb5wCeZ}|A0&+X@7bGc>Ijy+d z*|yDdyE#vtI2D|Pck-_y6=-Q>p|kmk{^4S3dSKAw=PHnxF1sppOgplRT|=-&)R`;P2~%lGEKGGfuMx`H`e-}wk8?4_uoJO1~Z zwN7{&nC;|N*d{%$GOSTyUCMTQwC3k{Jk|?BShn!LBO_GSIKAk*?jq;#68td7gv@lB z@pgt(EW*|E=qhV`N#D1|>*yOWaSDQxeALZFT(SVfU=TUDW(Egx_~l2>?c8`xyReHJ z^Z@j=qv$56zb{hG#Y5YBu9FfUqI%U^&{W`9)9D>R?FrK|eNVAjCk;TtNLl4Se@y%M z=CHW((MYGq5L4ojEgGi)+zFp}15lIdW+eS)@BZOIL+*o=tlyZyVH^k79DofGEN_#5 z-2S-zKfY?f(RtVs51$gxP*w+|KB0h82I<%ZaFpy9!Ch?c5?@uf54#A8b8P&R2AVFhEH zQST~BOih)q`lagV^Tn_ux9Mp)2E6xU!w!s=`rvxCDnS}L3b&h_2Q z0&yF}+v<5nT#;!X@2Lts(Gc^?I7vHysFg7YK%;}v>P!}Hx-Wt#U_6-`gM3G%E9B5RZ^IAz9&}5-FtiKOk*`@2?()lh zELsuf2#bfWi%bJ+F6!ORbR*AuI01JL;&!aX<1+d032UC~rQu5R*-ijkUc5GM2wo`7 zR8BhXxORH@G9=pDgfu)mrY?bm38wdK)*9k`19^-`QE}`HD-othAI@{fd;KS0YIE(! znCLO<%e^)65EQ*&5N%GWno5VOynb^C^P{*ggKPzjf`xDDY@}+zNL^&h@h6i_&zPui z!F)20zp*1p%oG8Ex3hYuG7@_U!~@PyuTseqct*ehvaEPpA}(FwzO};=u2)m+jWhG? z{;!k5vsT@p$#Xp>2V1pJ$D9j{@CGMNr|nO6+WTwDhCedkw04{4^qbaFYnvS$Tchrd zx-C1?J*xsfKk0^fZA3A1%3!OO+7dYc@#@b2R^~BNjM@+KpnzQS%oqx-5k&O_Mlsaw ze%Aqizj#3*q8!^^JLRDoa1$|HIZ>m?gVRd zzEbu^k0nLA{euN=TVh4vUHicDs>-r86a`ZnMcDqRyp7nN-_{T#_(`e>+8+5GgX;*~ zGoZhwefa~nodHS<1&sw^K^(+rp$9G0;3*^`!GCOZ>LTL7qB*2L<#M1hJB$U6_J>#J z2-vjrzfoJa2n;&A=m zLKVrD5be#5PcUSwz<22q<0-&uQ>f;FokB{*spmdw)ouxO+=BEY;cuteuACfUbPoH> z+_iQ<xV%7L>oH<2M`+T-?ES7$msCB^h{SM24LC4Q=e}nVCjJMHQmw&)T1x%Q z0vF;?jgxG+i+4I7x=qo!SpPye%TMSeN*^%UgnTxC)dOW7-}go(-ud z+8wnGtHe6E4W+bo|DY+}ZsB~5ZykTC;69~1wbMy=Y-cUgEDu`^nU%kTR`lQs3K8Fk z%WLy!Q?aut_)wSySxb9?g)qC9=kKJutp1QsfA7?`RlX(LwETZN7r%C|(BVTwuso6%`miv5PamN( z55ej@MZnB*-!o!^%R}}{Ai@tFX3msnU#kb!=I)ikz(-2?--&jnM$#*>3j^K+ZI3Jw zV8&$;4X4~Cub@iQDYMRM0P=Kl@_Ul{flD$wrEodnXxZrF0qIcHW1Xi4At+=BIpWGS zUx(-7x_IN2$>NAuvYsnHyAo^yUMk7~mlAkJzx_} z`Z1Z*y1>OICZ%*19jp?$5ol)(fDr(a04S^+JZL88SBj_Y5TtpgL-XtHI=poMj+q@r z_hUi)iI3Z}hZXR8u+o)I10ceeVQnWGv@@c0#K58LSl1Tp-5#V;Xp z+CX^ACRh_+NSh}@=e3a9i^%+irQEu0xuSBDYJ>tBM4L46ZS4Y0Zw~ui}JPgK|>x6Foh8(l`sgK6;Z<fK2Nu_8bo+j9 z$qT+U%3;Jh+%ei}od@>wmm~>_%OdFWlD3VqCG3l&xKt!8uVJlTV#MuPe*t#SvyMT~ z1z+&Til=8-P|X-!r#lNL*uhzO`4GO^vz|CiIP*WHD|xdN;$$MK;%==Vt6u(}@sCxB z?-e^4DE;~A#KZI{>?gq9aw<{h57aoFIcq!fO*GGU6wJ4&tc>XTtXp2l^Zj>qfuk%l zj&g<4?KvBuQ{)5)`LV1^0nX{sLXBrkuRr-eAMf<>Z+j0O=}(C^0GrZ0t1i=_V7;K! zx6sc?K)gC$8M2lpG}VaB9*t?T*?KaRTEWQQEtNzruuHm6_kNYz-~xNnvO^fy`pB{? zNq<*k8nWqj_6gm)wEoSkcgOYFT2%tmy~9Fq!uk~y*QetA7G5?3Wha-x`xwk*fW9w7 zBk-QyA!7!;r!*I(Pn2goQzbV7IT9@f?l|^(&*6By$}iR0L19t2KkM=%vcHU}GBkH^Te;kXW*U}YuscEJ9Xdo1M zbd{}pBerESN0YIaW`K1sWn6q3qxyd9mp-Dr;&723B{u|_rkY9Fp5_EH%L%Mh|L+Tn zB|0-Rtma6CI;YHB4Aq|*%B~A7&7@=nW;XHC#F)!8`U9bV4IU^PV-d8j;d8LMFIUNEnU zmMnRS3_3kL`X@?@fmnbOoit4#O%_{aQ2?ybx=Hmi!2a?~WMPYKenD;7qGE~%SFOqI zQmp7Skt28%^$J!aN&xWeTox8^R@h13_S7*x7nI+>U^`!}2ZtWGF~ZI8SpH&8-L?TE6_RlNKk}7) zlxku=0M`!v64A$+q~2#zK&TLOg?+}|XJE=7rG*^}$n%cuolotsOjq5*22N-=<^Wo0Y9JO?CJ;IJKjAGw5*Z(LtBxPv&nbMIO@ zHFAE2+Dj-yq9*d$s|m~E%`j=`Apuhc&$bUy2w__rkC)%=PYwvlrZ_?kVSgm>3WU^_ zJ;T#QxCU%PWT?()foOmB?UHH9T1P0)mJ9Vu_(cVRP;cbc{m9D+tEL0^#D>8=zsHWZJ<+fN>hIUJC zUN~7jcNH14h{4u1K&sOpQrA*0=9E|$7?vK`0@4az+Y;yHZCz_0>Z`f~uA?=BM=J%1 z2WQPqQoK?9txL71H3F9Scygp|qY&|Y9&oM+Nsnx$f{>4a2QSHU{Po)jWB1-hHq(T! z%uGY0K4v!;hu5?l$%hk!@jTiKV_8NX1Xx%I9x~p!SPnxTkt2V_?3Z$quy}c_u{2Fc;SW1_c(=h}00*tT{$3)kxPOy@S{wA!D%Dh}LzQB~@97pZ^bJuBq z)LuUpi|-p?BoYk8Wpvr=HMLekW(PHBX$D@PB4_2*oGf*4 zGffJa+DG+e&CeJCVuYKXnZ0Jt+hjsgpBU(#YLu+`fZ46i0lfS5KPc0!3u0qR4Vqb$ zj@W%lyGReP`5CV&T3uL-T-O+WyzSCSnv!|j2mLlgXC=9&yj$?dLj~zf` z&EdL&v(?VuIDp=;I%rJBMgP@cZ7i551h<()srYn*`ivK|Oksv4S7_ zi4B~joWb0LeNHLQIpiqO{A2EM4HrJur~kTFvwt6P$BbJFNy-qwAoQg?E_ka=LTrFEY3v3BzG)%mjedahTL@me64|JQ= zs7@^YJ*HMi8udRkkfDkgkkpzy$x%}Vovvg}IX2BhkqMb;Ue<8b^wEAnj1~o_8J#e( zTUQ}S(K|0%%+B!Q+DwEh&wiZnNF2Ef@TA=_U2lERp~IA$`4ba7*Hua0sg8FDXfX%s zV8)_|RsM-rX15KjDqT`*RCNC9lvjwsvS@#XTPpaR2~siW1TdSaMHO4^VreX#*wVGS zj{e?!6+}LdyuKZ)!1#ng*WFF}=8W^VI2kq-ppN)3)ml*ZesNZX3viCD19kkiN2XaAq&pKjDddrw1r1Sy|RI}(X_ifZ% zABeudtorRyh<+t5(qM(z&QTg!M-vQFqYNt_{Jv#4i7n3SKlnlph7XW)<7B|>*ZzJ5 zBZn4}JT=X9-6~F5l5oZXA*`6<*TzCnsPfSLA-^=-Icn8z36Tq#dkW_lv9!pJ`c-A6 zI9r^xolLBBmS*9j8gRy8O{NU9g@YFfC+Nc|&5NlhL%l_~vw^NKZ}Wb7F0S+a%{qX< ztCehV|E`}@2p9xs6$egc=Sj3LWgYOPQP)3hn?gtpN5P>CbRQm{-n)Bz_9sJ^ ze*eQkbfajQ%x2x56K$-QjtE4)NDt#T2QZ!)^V{Bky{Nc%WT+bOXSTh&<}j?W>`K@$ zQ#kx~6|SgE>eCmG6CrY83UQp&=|Q4O`cw;^ zJP%F%Ou6JKP8W!hq|g!}p+PqcUYjFl{hABCPq?m&E*Ds(w#aTo)1iyH;hK=|Y?6Eu z$a6tn=TXxO`#1-vtIk1JEgXP=c9~3d^~0+cWC>S{3Dx?am7Lx^E~vZ){mR}r*n{e*fc2HfcxNr zv2&~B2w+1}tjBj|%#4+P!nSJ=eolTYTZhk}oZ!oQJHKsXYz<2LVTbVOjSF#>@(b-a zaU36 z!gE7MU1oUcSztFvqV4VSnuH7-U-MB_|IWNMnj0@EITBZ~F54g@0`KBNs@uq(#*6ph z$Ry1pb-~%;WeO;eV6nq8irY^A^r~AdVB*B=)1a->kD=&b#dv7VG?|Y&AO`$-pQQwR zG$zLBKd-5z299-&o!HFStHrLgK}@%OD8Alimbra?iMITEJ^-B`=8v!we~0hBT<$_@ zy_$Mn97v_3KzGx|yB5q~%YNf(s}58Ho!^OQt*-#PES0(djc%pZ%ss7WIXvu^K8+=1 z1gTHU?cfM8WC7T*f$lB}JfO>$u#_fu8YmW3v2zQ$o>hwR>}J?XaevSWj33OkjsMw< z-Lb-KbwiF691^ApQFwx-U&I1giR`~2(PFO6#oZ;mnJTQ-tAFA8!BJ?*??PU_89CVm z>7wzlHQvHeuIM=eHa&+993Z!<;fdqudME}kc2HJ;`d#qeBKOkBmOsjGDf`>`B^5wH z&t=Gg53k9w5bjTU8`(^yCOF=(+WUVPeD0+mv$42{T0v_74~`ChP|!-%#HEI<%<%h}cIP~5F)B!zC@N()C-9F9D)Obb2`-y2F=ZV%VP{Ux{4EO>q3X$ZE( zRnKI$KMoSYAF$3$15b!e1;#2R zPSZa1FJkKw*}Q0mZD^L=s?qK zo4|bf<$>saf+4fET}09(kY)Cn=TbIYCdM&2oCaDmP=Hj;7Z zNubQbjdsl&vE7Kz+W+#>LYx6lB-0sHw-AhiOIi!$6(;x2HG$kz> zv7NOa+VMMiM?+XUn@;Wp9~F58<8w8e?JFWwhx(RqXb-}a9zKh*JRmNesT|!Ef`bEX z2>rQT$^G}Jo5jx@NJ)hFQ7y3m*w!20bLtO4Gf4$PtN%-wl0X2=L4jM$WwiKY`QKh^ ztT6j@YDx8fr?^6V2b1jqDqx`kl-=+~IRH-rm}GiySp%lJ?8i@aEIR&?&mlj2U~=p4 zO2ItaL(EwS2T%4X;?hq`pMH$Sxtli?GDwP&Dx^_G{JJheWwC@aQPsN1z$dL~%(cP^ zmP#*tw9%%QrcDwCF|E7q_?fYPS4$y6piHm&c!u5_iN`9W`}OP z#!5ZJZ~S2sNC3li%3I6l=${A*<_X^JOeXjv@%~FS|=d7`)3mFB&Io7WkEg)j$p3^#<-)4nVMuR7hc2XWdK#rMqe=7&^^@B$(H zQf`X!vQ!99!D67)0*SImJHK@RE04OFMvb{>S(%vbcz_{vq%EY zlC{=Rnv?jW?rHUi*3Qk1t3e6PN4y4^Tn-KeDMPttxgplo_P5o;yMFqfBKk)IjPFAVw{&$e4s!aFtr8Kxxw@iub+S2dXKBFh zZ9;syPdV=6M7sQ94VutbC_A&<7v)dEEd8`GdCX$r1$weo?1(6&bL}UZ7CS)X2!4e+^$m5**kTu zYr#v2W`a51Kqddm?J>_IeX$#z8g;+{XXJ(?j`0M)AfOpSUz7>O8`&60+EwapYF2hI zXZ1(}gm%cb4GslgIi9kZ-$G}LAK<#W@5J)W-^DN+RZGVq_Jy^7PF!%zPId0ZHi2he z{_PFfCrikaGo{EaPmQ_fhx6rokp#+-0PRj?iQSZAirt69`G+nijBzbL`D{ z0);+hZt1hq(p1mDV(5yMjK3oqL1TXql|yr@_+=FSqFaqBS1^`{n;Tf5!p&LHyg28e zJ>9^-8W+rzLSt<*UU~71?&>gMB3~pNN7l?+_k4*w7CdUZ>I zntrB_I<;@8a?Hrt(!kNTECrn#JjET=z!pY&YV@R#rNnDug!WVTyXnv{Tfc{0aEHZZ z(QeWGtR{KCXRmn=H-Eq+Yf)11roVjrAh3hPG33RNW2;NY@1IQgfS3wsPZ{pGJRk?5{YJ6UI z`{S3){0Us>^C=T5fC)OeI_f1)cS5ja#^hIuC%-(c?uU_nyn;@Mb+)JFd?JPs5+VL& zJp&miweQP0s>wn-6i?ih3eiU7n)H;9U&L1C<%1jHMzm=*xJq^|EpAjG%D6p60$Mg0 zT3yFSQp6O7X^ug{Uu?1;V_MdVT?wCzzD~+VBi{C%8zL!YnXi7`+^mTgAhzS0PV8X_ z^WIp7>dXHvrLE!=I)k@bi1Q-?CrcFmkV+Nn7ytuO`{mz&1|lrJ^A+<5;JResBgm{B zD-NXfI4pI>^;CS6P3JByn^{VBQ`&@$?J#(YzY;ozk~VFob~pOH8D-+LZ+|WhYvq^) z8ksrG70C^?Y+7rPN)TIgdL4e^Y$Lz-WM5Y2wjB!uX|eD2!g+fr_Ys?T&!nN0k7{j? zA>ubVBVn&^*`zs(Gc=u2kaQE#ar}>QxeBmWdB3Ts5R;>eR_KTkZVOBk9PEZ3TWU!) zP!*coCeWal5co8PG`SjSgpK#yr-GA~+0$^J&V#w_zJ&Z%@Oj!;LWd3;Wlm`dTy#Q` z#wyI-pL$j8Sf8^dQI%iV_8+yIC+H+nr;K`gam9Q^=dpQXp4>mGro@&vJU`&{Ez>PP zaHs&`RfIX&B;@USjm!V<6fZi+wHFPsL9ibwbV@X{dK9>+B_Zg3FXTVC2ecFy$o^uH zrLZ375A|L#aiLQUm$}Gq-hF=o12`gzjwRC$?kN6&&Ft@lyBhO^{Tjco(AL~IY!#Gp z!&#kG(6(PCLwe?cuKt|LTe>!bkHaApXJLn+h;!4Ckc0D&w_7XAl`=9sO)x5zSm-qp zbq-VJdqIJgXpb)p&TX1b)ekR<4^3NwZ-1+3fB}1pSm(BHY9uA_Z)8H9Hev6gk_3fF zFV;xFyUG$i-R0lul3F@jVq*|Aiw$e_)$eycmmyfD9&+lWCN7C8uA_Dv38NU_!o1TewIw5hrQrn4ARY` zS@VrR<6>2RVfI`DMC&6>=(somhFMtZlEk;YT z^O8_jNB)Su;cGN9gqG<0R1Uz}UknlL`k=c`*2F!xn>#?eN$uD3wqa?$^UGO8g}#3; zLH2F(z=~hkFZ=;no59DZRw!U=qJH#)K$n#FnFsEq2@brdi9YNV`q~S)fQD09lOt;q z_gX4T8~)1pg6<&5hfzTE6-iiOs1^zPP)B#`R1tM)GdQykPS)FGTG+e|BJ_^4>-GD|D0yM}|F^}_H7JV$2z`@O`B?ZmR%q6WFpe91N5{%K7 zo&&p|(>#5grO!d@yLY+*?94=zy(9EKBo$}L@Pft4U*`X`9YSKq6ouN+mK!)@Df ziu*~1zcLglfF#~|Sw`ouf&)#q16BPor~Rc0x=;)#_`gdMP5}E7d?Q8Xgy(_?Cs0(M z-twR+r(y$Iam4{OGzlwV9yqc{O{yj|4Fq+J4&W;Nv?Kl^xzh&0P>#Y+hKZ-}hHh8v z@QjJjyz2`Z8UgEPk>g*ry=p*{ml;nT_Oi4stI2RHJr1yp50cXs=`wl;d6V5?9|8+j z*x^rCylZK<>Z7$VQ^s-SQrgf?#@m+WE;*GQX?A zL^}ese?k8ezZM(O{nXZpIdg4J0}$VJi^rDR6-EtV3YqYk%fCv;{)vq&^>*6zc&r4H$l)?3%`a6v!ys41dQJ)&Z5)ax1$%rRYq-_0ZDIaDKH&1Ib!_- zRDm5C*8F<9@DfsKoR|$ML{u{=TG(V!_m1a4$o1Z#`E9d*x|~E?$sip~<^v$ze)C?kx)veJdTikt@U% zmdf6b47E()v)z_laqKg+Bm{Ee3o8o9}!ynHNuea zZSsG>Z=liRp)pZ~*Vci&GaqE?;*H#nd&^or%;JN&CcewBm3YjiwzxOmBVJ?Vt5-!J z;&7y0l>>$0KTL4hByzQ3^uFccN!)IRYfI%h%K-q^zx8PqI-}Se08`D`BCd{F`+Diq zoqgfAE2CDEwvm!fS}q};>u1`Zf{TkS0R|{C615i^lfTw_*XB)$KjT%?h4Dsbjm5xe zht+0O)V%KiQBJ@jkLRr@v~2I|Bk%sO-|eIIrG*WHc;klw5gO=F7SYntIv#<{X)POS zGMmCfa0|cecL_f0d=T*w=M``^4<8GnL($BAq*=$>L7?~0Bw%>fm2UwX z*4@`e7V^Ei63@NIS^SE;Sk*yeS)O{C)`C^F9Pn)A-!p^pTb3`ikue!ZNe|qXIv7~Y zDn{1tP?iC5RYjR_-FzyPoK`d` zP*c|L92XPt>gU0TX&rvKLw5y0^m!%(ylD)tXFTI$W$r%^WEgN-5(~);e`nMVij^GX z&Pt+;7|@3gBK!5mM56ymH|#o~EA=E+@>eaMQjuaR%WhA+afv8P7|VxE2$O>#ypp0U zF)k9|D-=@H4smLa05_Klqd{yi?&PMB@S-a{yTUC;)2(=2+ZNR!hG_ciIZ9EBw&2HX}S%oocNZ=qk`H1#GtGKr_MGnb;ZrWhnaYG$!dpk0%#L5 zWWoX0+h*x_JmQmZ{)i3kh6@kSI^VfVsAP)Hwq)LI=}vpL0{rh~)(I3t(I7@n8Fc)_ z^~)|M-SBQ)ws7rWML-;#<#r|g!Jfz5=N5A}Avf603vkkKn+&%M0r-cZTY=&=@#9U#GoF^b#Q=w{CQ5PVW%wB33klf+}bCQ9@{?Tp5mU)OJk zf7(z*ld>|qEs|N=)q2%$w$rA2GAaT_!|Ja~1|C~;Ty98*`S+qe;gISwF*}m6pHBDu zuvsF-8T^;oeRDTWt7jm_7(|PoA zhH4UjldQuEa_oYu_*m^L7NW4d6B9F^rh&tH5>c_(Z^sJyZqAo8=bq4QA}>lvJw#}? zgm4aT^{KOdRHn~ug*AN4DKT+%t8vP4;u&~Wa`nxbDr-C5B0k2*z9Eyd1pp;O)@Tj( z02)0G)}dKue(<-eG=Lpg-KnHbJqHBbk6eZHR2Ry`jpg2f)~)9m=SN3^gGDOf>YE^j z9!%~MB_v(*-~rGi4I`ANvYqY)C*koqX{0l%TY6gRze)9?hXl(Ir-!B7FD`yN2bHE; z)AoxRBYIkuK!Bh=T*Ztpc6bznec;R34k z0IsC%=0xp$8lGr=z0OH^7LcPMI{~ry_DzC;ji8TFj~IB3VRkUN4WSn zWeHtzRn)CR2GL+clqSlsBLAVKo)C$?{4gGU>P(a4tjkU>fG;&5ramnndblHptt79f z2SGi|bwyt`PO2KJPQzC!iN%?3SQ(OQiI_%MYmsfwhcx(&L!dMlzLm+@D{~`}AZzS~Y{GwBz8adRGDbrSl%sjlM%R@nMcB%YelOSjkz_ci%KSv| z&hbdnJVt`?wBu!5Gq}H89Gsn*76%>#!S?Zw*^D3x9~+2&k_qk&BHmFV$72in7uczvlLW4YU)+%OH3SE1j!wdC6aVIOH^x-54#m~zvj4s=II>MC z%(Bv~>JAc*WlcO5RG1t)yo1@W_JFN555vUJxq)h2U6%#rVW!bv_)i1VgqE&U%lXEe zn&HK`{3KYf;Vlqd&d0?#7j$c25i9V->i7MOim?ZJtjVd>L)^%6qb<`3YP5rm{$^Kl z3TEKr?Fyf2(^Qe8_CRZ5n0c3Fb`XH)Ya&^v(I2*S00o)UklJTWD2ZDWhvGTO|)7MySONp(v^q*uB**$PYRog2f z3ZZ8Ve1%Wgv>LAbq&!7S5`A>qFHq^V`>+^kJb8U2gTsvpAq!g9`ykzc1>429@-QEH z7$U<35qGcBjcrF}7NxoJRXNUq?*g^5V@}PqTXBN%6pJ^=-u$m=FvP@NEhpWuSTTgt6CQ`b8k--tCAeNm`0b)lG| zCc@So5$W|twvDnUx{*CL?r^Cqoj6A3uTm1jR#7=Yu6k4G&2K(n^2O}y@;X|wnWNU?>!*bRdtZ0s@N zq^-ZI+c56ybGgov1im_a<;J0yv>8}t9gKq8WZHiV0L@$1W0=b{Ywfu__-M>Y6mC+z z{k{86DdkMCb`g~@24kj2segclSV1bdSbjJ4=MNc;)G4GW%!ZtrtvE2JRRR0P%-e4J zYWDY!`Opc)w#{Z&!m>H&mAV4fel5AP(0rSa8?l_8C~kHOvxgSq;fJJanNkj2T#@a| z(4KE;AINi*^%7HuMVx=CbIH+f9U-NuMOu6(ZC1(j$@QFLuAtTkw$iwS!?>4pfE$c5 zFL}l3ScrxVmlKW_zpa!JN)$M|0Z_I|dCe{bnv-*OLED-`aFRMYn>pXMorA-C7c@7^ z%_#!S!o0J(7KK=QXi7r5&X&Mij0|3?C=`&AIx>64nk`@U$B6tygs&2!D7&QP5LD6y&K ze8&sCfh&9k43hTR=PzJ~@%IK6Ez5VfG{7T_abVWeq_==U9eGIn8NUlAwb(e%1B7;? zNpTo*3=n)fghB>0_m$3?gZc*93uRq_n1+we3$-5vUw9lY;(ws2s+w3}_-E zt|&QQQRP-tl?1Z>ns)-xZhhQ-hb)VaR=Zg0v3BXb`Hq9dLNu61r|=+_B8b!lB*?hd zu)qCiMHaMakCQbq-Dw(q$e@Umv(FU{3;UPU5gQxHqq*t2uWQ-draJ}7K8pxg%thJO zy;CEdmu?UotFo{8Z}5Z#=F+SaQ(G+4jVayE#S%ARnE$7YB%^C79)V*AtCVAnJ9JG` zZR)Fuw*+deC7qiX2ZYW!p+=Xlcl-}sB4Mdd0hwnn)dNU<*BY+9IEzwEi(gjcnw=$tdU*N+vuS(;*>FNiVM=PIlYOBDnwGSiI%lY`dB&|`ytB=q*^)ZKPg5xG*)fZGY_$*)P6*R6{KGwGNQdJp>J$BM@!~7~-BG~VU}eR`+dOF+n(*t# z^^^3OfSSrdD3^3nN-9M#e#bh00G?v)@_ycLV;Ih!9`<%i2izQNoyTeZ=|eQvfu+)g z4dI*P?k12lN$u&p6!Ig-%+EY?Lf|RNBU%L&^>|EYt9;@%Hp|Gk-MmBFcAWC}YvZ9{ znz%hkOmXPphI+fefR}rG!}t%=z#hcZJ$U&OaqJNYom{CePB0FAU96%d_ux?H^=88G zcD6FKv=&~I-zXmI72IcKm&w1hoO6XhltpqYEk6=4`smRpIQ5qeJk;cByh47w$=hz% zZf9yx-V5pw5Wv2;?A3vcB4(?ot4mAL@mRgCSfDwZj%cawIY61~XIE~Db&GDfg_obC zaQo}aK$=VLDf17raG zNUhfj%dKnLo6vDZ=`qcL!#a1pgSNCeNcCu$7+pi>e)s6pQ$X>~{ohb~s_>pgKOd*l zfCWR9NiQ^z%2EFC?rEKmYOdQYcd5Cs2A+iOeiP0u6O`v64??)>eLe#vp^#%k*-aUV zDiHF^^*fv)~btN~|X^jqDj?c-vw8%zqi@2DwY%9$LmlUwVj<}bKO zvP0hg*php923`?Y9{}SDRAM1eKGk@Y8kgu4E-k+P7M}*iJqd3NU2}0l<16-gi*-gL z|I#w0LVhy_8~UxzT1_gre!DORmEP1pE_)Kq4&KXzS)K`TDRu@QHKv~l5}9B)}=V+w|`PP z(R7u-tiBB>tIu2Y$SN!G`;tQ60Fd!9m;y#7lf#e#M7NK_vLPAN>{GMm(|E@bubA)VW5{VM&J;d)= z6<&uxkd>TQyt(>wp!#aA8^s5CW4nHLRaj~gcB~zNyulITc+y7&PhOW6M-|GTVg6YZ zs@u`y3vI;q2OhFX2Y^;}`kPFkh4b`r{IB6->14x8oNi7l(cK%Y@Ej2%OmPA7F3JbN z^HNK?waJ&?l6#|~P%)DUOJ88aP#i>H`B~9WBC#6t_9P-CS4ahhKrrLM2)F3?dyIp< z0^()&g{A8|z8ev~#BzQWMx)TIO(5hJ;C@1-GFhvCCIaz7(NFJ4U`5U;>m01X444}D z@+K2>Kn_`92v!J=h{d?(O4f-+KK5AB@GB%$qQJSYijW+L20O^lNS;ngj3P)AU>9iY zNAcvMF->JtGcvhjgOF#L!MU`+A*RvU&C^%T{IrwikT%-gDpPitT_Yn@5H-}J6MDA$p(>wFQXQphtp-1obX%2V9H8L8BRFLU|pZBW-IEGGqj+CFgLa+Ef@Wz%9Sp z*Bfyx6O!q@o(lphH283T6Z%w)y%9^561EGDD;xG|a!r3`LY}y6!(;%`qGfqt9&XjP zH|GAfSF^Id2?)sHQ+Oo0C3 z=Bl{xK@q!8j_3s}7My>06QPZ;V>*E@4477q1U}J%;X8KaBiVV|2Z+0*nQ(N+8PqhW z$8CJuYQ^U|`EY)wM>eqs^WyS2T0X+lm5g~vqLaiO)I6;?E{e%;eUCnRq*qaUeg9}f zO#LGG6*OPN*NDd|aj%LZhlEgWgpW6I{dH94c~m*}90tuD0HeR%y7_C%JD!Z9d&E24rr*QZDKibx1*4MUPfZrP*Tq{CR9+JgmVQmRr; z)yDFPs0>3YeXO&UQS0&TU=e41x@r8Oa&vPjrY@q(N37alD@$2X0nh(}E4xd@|B1ua zW@V#NI@H#N52h1YubB6y1;&GoE8NcSg*@3uLR8134v}J5X}GS;{4zl*8GKJm6Wpgg zNy#PTCQ0g)e&UZnWHlqW1;dc$f0~>-N5H<%nrElFpH>5XJ|n{l8}J(JYl7#LPy`!e z_+k>r7LZNufPvZa0Eu`#*3f;?J+L9kKrMuD^hvm2XPDUf_oGq^v?MI}fuHsa008vX zS~#U@=cn^DNp-@Iyh;u^>+On80I;teEZ+GM$*AZA~kNkj&hlgPxfG0&`ablfEHz}_c^2B*{& z;Tn3X>#+Q9`|AMT99(x$n=qtc;7C#pk#6*$Sz>pj^buk{IvbEisz?3Sd_rWu{{!?4 zLNlb%vKr$zOGOrrE@E%c8zTHW&jvpj<9oVMRe~S}Y;Vj3+%*{4Jmef@ph1-yGx3h@ z7b}2?IXGv!izbq-3_i{U4_bdA!<|94)dN6Uurlj5Zw_CPzp3QOlCuDY;;GT;-qc zaZWOI>0g7%(-CP8>fY5>-ZYJ!n+0l`vounAV#rUAIbS`LUqw5l@#Z&N;4G@AlavpD zM@qUXQ_6P#QFe5^;QZ2QIh`?A0A`tLmA$5uusZpf=^kr>xoE8*y!t6^EF4}*OIc(|BeMhQnZ*(~9VTmA90+rssvMhGAj z$O(G)Sq?iI&@5VzK^yD{$+w4yUuQamsh+l6j{aS2f<>{mugRGyfaQvL^Tg#}OEHl0-W9mU|*~cL1FdSisL+>4HP~1@&}&(hgD> z-7^zf#{+TaSVvQca(c^_xjtP5-srW49dD}F8lPJWkQk4dN`zu&8fZ}{k#4sU7DIYo z180o|+hC@4B}VCsCVSSf-2tC^WEITq^ruf(1Oi|>_z=TZK}pIVPM9SBb=Nsach=3d zCSEn|=hgYgwbX)ohH|&-7@6&IxH8DPZ7XhuVEILQpYtmlhR{Bf+cH_D>451f^8PJr zj=XB;){|{XOVMk`-p$?kW%+RRU7ZrYub@%&W>wrqWDWw|H=@=Pm=}x4&5$if%RheM+Ic&D}c|4tRN-boz(;gDOT@+PsD0zVSF5vGG}uUf-|vmO1zn z6Eyw83}GnBg1fQ`2p;c3k$0{{1T^29n;>5kF%QG=9cf$svovHEo@U=@!`@krpm*k{ zroBk>+pH83f!>JEBIEv%0v&{ZAMOX;YD-jL{1c9CGpYP!$*L$O>1%I;rg|%p4UP>| z4IPVBv052AP%NuDPr-{Kb<~;EX>_mz8e-JBoWBw1osNDdLGocRHb|#{CWX+%f2dMX}R0 z*1haoPH)B*g+2;MFJ6SCO*_1aKPvu6>Fm#S>a0HH`hBja48y$bnvDKY)o|6eU&nc4 zR%0N@*ChZ#x7P0f{xbxvP(deV2ewHVlOAF`tPnPYKr!P%i1JmSg*DMhZL;sIzZHrKNQ)N5uR`m_c}Q@9 zQsuAnR-z18eIR)ep@2me8`|>uUO3U6((Nl)b&cu|2Y1dhQgO~NJB{8L6A}fMVHM3N z3jNkG-~9SEsThzk3q$A3@e@}DNJ@J}D#)qV6uLY|DfKKKrVWMLgb_70)>1R!tF%i% z2^MD_hC&?=ymZ%9qQssND{DSCVS@an+T=o%4Mv?V58DsAR=Q#e<}RvR?Ek@_I*jR^ zm199Xhr)@ti^YOtJDm`Pt**{3zV$c870DK^3oME?ii}}dhpV7q%|nG;g1P?q{$2ML zXuQATD@8hwx$2{=9UYN*@PIC>lb3yfQu5{QV`IwP8YV)n=-2Fol~6`EL}GS|^mJ^I zFpSPthpeN&d}}ip@~43y`(W42sz^UnWKaq~eZAq6?rnBk!bzI3RYzK|III?j0gQXz zc9L-fl`JG$xi=UPQdb2SKd3516@T4B)za1-9sPimrXh(B*9#b5#Yf#9D0~7Bs=z1Id1nR@eq}P;0#tq^DA<7#8OltMkZTD8~gR z8Rr_-{pE1@HGN>805`Vq1vX(s9rrDzR*fUnVVSgvh8$kJ!uJhFnOd8&q#sp_7M_?p zYd|WD6`lprLZ}Ev#*l%GM>Oncn#}F~VMq*Pjc8P2Masa6w7&o~TM>ZODjO{sDFq7> z>X$Tp5v8GURRmsjgUtv8oEVpgq}k-?{Rd$@Ih-NFZbBGgfesa*`P(x^wec1tHf9!qeaLxA>Z9=mt*;v-bQw|4$H-O@^Cnp(($HAT zbZYf<#eZF%wp=xbi)|1td$Mzy2T&BPb1lX-55Mf>Nw$05As}|OAruD)d1Me2dLH;F z9DJr0?|hx_IL$LY>G|IvC{9Xr;Z-pBAK$llLN-N9JIx)gU{nO}y?>{@cSlRAGA*ui z%Au97epxpt(F+_F;J0G$ylB%6#!3B_0xC4j^hT^{LXTON9}z(5A<|acN=a2t2mG-r zj||XUBW#rC;-Q_lV|DsftwzY=MTP2z872m!q*!(hq+dbxK z&a)`xGy@v+5(f-f?p^7E8b8Ve!djj%wwVO6X2CzmcTQ1y)(e2a7_hsdd~M6;A2m0B z&R~dy8~0I&&1Ba%C4by|9nF)ljX1*s8rowT32X`!E*i(LRwF!YYofoLDDSgGH7bZbJ}R-l%ro6JkIgDMt)DphCY#)`&Tv@{e(+ z_9a^9hBqDr&cUe-y{iY3&+xB}Q$;+uY(IIFn-7XWv_iWp4c$whFi&At%P-07y%@zK z#KhNsLY~8Y}G+ z`#!vbV!Y@0oo5v8`=Li?->Imof7Eu9Xr? z+Ib@=_(o{_c*G2kkDge+PuU@2WKAVD28 zV_~hKQ-ng>i4Ld#q?bbew@(HW@)-%U44q;9g*uVNn_eLi`4jzoCZ6xaW-y#hQyP5^ z6{?d#&nimFuGKAcHAMNw?}?`)(DEYD zzPlGWSFb#(skA}tn`Xe1l!qal3{cmUj1KsIQsd*X&3(6u^da1g3d7fq2G?OBl?$an z=WyCD0i1D{(T|RlCc1`T z9VpYj;q<8H7{(K;^tfCE{)2bgauvs`Xd|}Kh8=s(_MKxe^plm4tx7imJTW&zIi17RmYkW7(GDwF9GZP#G-iQsU3{Q)49?U9H z?DPY4_iYyKf!J_O>-&Aie`Ny3nr>qHqnw!-ECuU)Y4#TR7t2DLV|EB+;A+(s>m^YO+Dz-;QE zZ=hr>_(&z`SkM+f14t6staM=P7w0-to(U-l#WD8_=$(2f>ejlk=uA+`&oYa~NxVF* z$HAW*s+5%t(dUg%Hkg06Dq(*Dw7>j@!)X}8z=s6#PbC}0ohevI6Cqs3?;q#Zy_mUh z(s-I&+95uSNyA!?z^|-8&ZDY2km}C!S*s+Xxq${`zqejpQcEpCkWb)MKxlRf61iN_~vp%vR*HV!?m>96pFAcTI5+lvBIaMaCY= zcC4(ZWv8v*7uZ(RyD~6k8LZJ^ z;LiW`;B88xXz4-cE^l0)_s=bBPrPG1Vfq@aNy(|*w>TL!Tr6@W`1cK-9nb@jFFs&>$;VlT?OUNvH@baLZJj2 z8|m_~)u|l7k8=7U4(~;_?kN0kb=#8wkJ5%)z*=m;di7lNb)a$qCOSLxaFO^%q<|T{ z??=8jn@7#SOSo8Cr-w3yVnhZB&0{(o&-?mye;9Z_Qw-i4`uor%M^Xc&9^wArEJ&fa zd>O`n?K%F^XV<1&`tHq4(N`FD8*-X66YNX@31IvZcf$B{{geIu9gG{o8%FJc;+N;| z=NXk|?%R~alRC-1DQenwl5PQ^^jB+i4>yuA7-9Pb}0 zxc9L_!q<*T&ZC&rwQG@YU{1c!?*|6}ZU%4{O<>9lBu%BF3#^o+TMvR=d<|CY58|V z&n1!y&^$|4F1r{UKY|O${+h58<6>dSqcAfPp-;boB6M1CG!PVv7G4|~zV7)3$U8f> z((WbVuaW?ot5nVRp|rxN{HZ%P*WxI*z49|U%WEP=lB)kbNZa`xnHT3}l%o#LbN~iA z{*>3YGbOVB0U;x`BhXQ4LyZv3<&Yy=PH)F!Axa2%-SY*i2@kJ>j%N5Zb@LY`Z?(%( z^bt@^gl?zhq0_?yDq%7bFs^JTFk?OD*C1~$>!gooEpVx=m>EN=l*A=hQIAmHtR|Qk zjj!zz%`N0$)vE#3XjwH(08y*cgFM>&A?KZp&!5^RkP*{ccs-U5_Ztl~vB8VRJCllJ zy$^Z)=G9K)By8hf`zv`q(s)V&61HXOZbSf<3QwSAVrDilhn-)MgkuH#p~#yz1totY z$Gnt?WkA}M-r!6}yv%V`|{v`zR?9a|Y`f@lz{F6&UpoN@@&1K19+=THu z%mVz}zB3uoW575ZNI^E?&AHX`ay~O{t9#tNa!YYVD(bn+vPwRubTk7dpnPtaZl)Kn z)9FfNmo1bIMY9Mg-R|`gBtB8G4pqWQ;du{DycnMIKn_N*;W_(9wg!4E@OVo35S|10 zw2qdYy~9SDMwd98-=!0MZ^lkAt8Xg`P_n=i&FYZoEn$=K`X ztl8n;1cyb`Vr^+2o*{G566Q25`Qc6HdqDQVBias3iT4q`dM2%duz$o}uFXTYi zvsfRwJ!K5>xLj8V9(k`cb^UK;NL*79%Tmk5w&*=pPp4j3=m%IG8h(h=AceNgLeucSmm%5)`cHQp~Wz1-!yd%tLFXwM5vTp z6GL+Bk#1{MFu(O8O27c)4>V~aT)i3tRbpq(&wkVbX*w+1%W(u`DDemY%sE$;>x&89 zD5NmB9}`(eS4Ytzf@RGy#ulY62=F^ueVlu%3xx=leB23 z1L0>6Sl%R#`wv3{#~8hFPY$9aJ?R1l75g8p>=yT6Vwjr+VRrzAhzOL zQ~H;kGn1)AXmo(9d+KC`5xMdvB9hz3YfRS1aiR|WvsBW}6elRxZFA88uUH#MnB?n^ ze?^pLJ;%I+m-TY$2~RbSe)EpP@u=61=21B)3|eG{6?CB`pzBXHC8&0VQ@Oh(pwXhe zI7ZD3lVBF=|B?Rr>=f@X_;GpC3C3vAIyVC{=)JoG;Lo&{_*%i^yXT;2#3Ky+iKk-h z5OLFov|1^6VOpA_k=6?Lgs=y^cjfWKCieIDs|o0yJK^;4)DVx$Nc(+N)$5Ps=$r+l+s#MQZZiQU^?C zrhr9*wO8AhiC(MMPG}P@E{}65wB5vInxmef<7M4fXvAq0g}K9zWrxKywhotYoMdVe zfy3~i&B43Ks!7I|EbL?G=V}DLrr?vC1MvL9t z!u`FAX|?~X&)ahH1@9wcThzS$Db-K}vT?nPA8g>+Pr9*DT1?L8!Smn$@px9LHabAi z=JKmmLJ!VAl)_q>Ru6wdR1Sc=%g`kc>Agb-tzEZtAf4({)evMZdQX(0#Gc1=e?U>k zmsYxWQhM!}&vB`D>kb-*FM63X(7nINc1d~5AlQ$K{N$dUXT5Jz7-RgF85p?&pVcB? zm)w5u}ABUcM6~;?Bbd`91{Pe8FF->gs?yfs_@&Vn8w1`5)en~5c6pGyt7HV^04mO4a zZZLUj>HZ2_jB>$-AGB_psO+qop=ic!IT72|tixa8Svqmzfg^3N z(yHq}{n_W*OJ`vlJ?1aAWNhf*!aPbZ7V08ZyET7^YjXa9o-**N0XplcFI&^acb)^D z71V_S@4Wi39gsRHj?On{_xzt6HFE@@{~H3SA$VE!0|tjmtC(jDKYHh^`9qvB10$Up zo>pauY30jR_e*;@x+s5^yq!AH#w@uM(61iR>#gGVs}QKg0@K>0yXJhUq;lBQ|PPEmh8f| z?U_H^!F}c0KdmeO$edrZgC>OI()H+*10fGQzchHsVhKIyMmz85vO^xKqbJwd%R&2j zo*m=Fy~`|pOf5)jW?c=4(dSlrIQHnkPA$zqCZ97$=aKI~eZg#1JFBz_0_%AV$=K=Y zlGAmV{S>Ad-;QUMKE~R6)wnW`_LqB4RP=M$gNzZU^pbI;bqWX_9xJoP304d}3WZQfe5Pg!V5F#Qw>`pn@|P1SfESgXGo zM`6p!hS}EkF7edTns=v=K3ebs z8HtlylLOf`z%M&Hb9B9=A~C>H(ya=PD|{CG`%x_ziOCrIOhm+~?HoM-hQ|)qfMSAu z*H{CrCtvi#WejQ1wKLLi?Yu;z_dsm~zls5J7T@xVA8WjDl7o;qH%(i@G|`IE1=IPs zRZ@Dw(IvM3Z^?rAI}n^p>X;q)?8#6zGz%TwY*yn!(ZM4b)lt&BF*2Z8EBWa=U#{(x z4WycF4ZfN|DrBbzDJPO8Sg0k1Mki!GS02thwGR{D%{uhx^%tyGrZf0)vBBrgI_qe> zXOJ;%dLX~e*YM$wf)kY8Mcj}4pa*=le+U=Cug|khZY^j2Er@7O9Cl%o_mdbO9G&lO zaL))n;$8PwqSnWG*i*xF)>Fpc`Jk3Lw~sel!_M!gW#&HN3eAL00T~Hias9yIiT%(y zJTa}mm9fX`M!1&E=VQo}#M*NnR{s0{Ut+akvb`Kr)6^1a+Hc!6?k`h8TUb zG^t-wR*^F_-F=4&ZljW>)YLs1gTM+MTN3-X5AbAC`mUe@S3KIdNCfpe_XhCL3Tl8a z@3ze#d6nOdY8pxJ#O?y@>V*plyz? zrL(w;xYyrw3TF^|#wKe4%3b?Z8)ul;!_e`N*>+XDqaYr;xLd6Oqx@GOFOKFKb&m-J zc!qSRMr|6ip|ZB8ZXW8+&4uHh6)3cTHjPzp8td9)h&bN+TC-Rfu>;bS4ylg(vAreC znYqRp>@yvB&8B(Gp4I450V8=yv8!aUq7WKh4>v1g^1|h<9;xEl=}3j? ziPuHgwJow5+Y6UaV@76N30Uv0+%_Ib&Rw{`i~3Q_njh3{V#q;wFBJ;v0_$(lld30B?|0uu0vyfKpk@pEc8@&yavTU z{{N6|TXM zsHjnUNLJf;k(e1=ME(2J36FEeQ?fV-WsNuPq%d^WSMiv6qU6^8Qr7l??tJ=ZEt1=& z6qd{qe-tB)SxN%=Gq>yEWLbkllUjVN+)hovr)>$eH+xW!XVw>%!a06>al?MkUG|)3 zah(n+Ct9tX4hC^EAIYQr_~pSn^ZqUa|5(hq%5e5uXoJwlv}A(xVOg7BwJ_=#UoL8u z(o&_+ru`n&rreQf;%|HnbG`0X98~edN6MmVjX8(%fh$!X2~b7!aC_z06wxbZ$v8>6 z4hXZaKsVL*C<+mdtfjt-SsKLGw|Bst8uX4Z;vM_+C#bVuW+^qK+ca|^L@HHgCL(P_ zl?Y^MMBRQ2;Wh~|Tv`By3Ozut#zKOZ%KDklLXGzf9PZxpj}jV#qa6;+iPL|Q1e;Jm*A-Fch9~dD z9;y4^)TBscC!S9c44r3ZVDv92lI%^;sh~&@6_J%G55#d(ez6*API{Tmw98|0!;bQ7 zZDK{Soxy-yX=DugTFqK!4{LfooF2#IU;xK7Cdh-GT7Y(^%StIhsgZLM{9G%=Me^h` zI{yt=#|)lL722>&{AIE62b7G7uiT8$7?;@RsYz}#*=;2~X|7lOOQocp9|`Z_*Lb0j zj4!#@mTPNwMB}ECl>t^&Am-zAD-mB!tHbAU#{c+US^C`oV^>j!DR=Qzrdim+65exP zg%poMclqcLMW*O52-gPgd?A3`0~}6u8y~?N9??MnpxA83kyU_au?KqFvt9tv=n7uw z8R29g#tT#nbVYoy6+BD-O7>>jW<4z*k#MLuad+Px`qK5&z}S2yrr3y4NkJkF9~Ci4 zYSOb*a1vIDjq%{LgIvfG_ajSmL$)Qa^#>yUipqqIa!O(j2R^t2ih!Ejj-3F5*z3RI zaDy;AaFa{S|EmZjcUKXQquJ5m=M+Driu=QD=Lu|kHVh!W#YDxgo3wEHWBXO7IrW&n zy+gz%?TE0PRX(cA=*+nzw~CH8(EprrRqn{KMN?etGfzaf<6fIKMt%-VD|8}$pgl#z z==bFK`hp_!gxB~uBc*4mthsF^{=y)Ax6KE!`xVh>?L#gULlj6*6F7VIHeIg(!-uI0PY zYS%;T?7!De))O69PyGM2w#>?H{aagbE{E38l_R%FjoRFwk*3X+n9 zB3iDQC)|5y-49D^=&x8=;C>)gU3Vy_J5zsjk*>yWO3~)FHhd=HIiF?mZQJbW5Z|HH zafLG$0edtSui6K18BOzf$*PHKV_gHRq?KO|Di`WRHG{mM;9!9OmPkB0?)K)8^tkb1mnag z`oU=!AlpbGH<<`!|B&FZ-`&Bcgp*Zfg>3?YO^&o0aR{FK?8>j-;%W1QhWMtbBfB+) z8!W14{!A2ltF{&XW$1pcIxarH<7$vQA)Fk}t=sCYU%R>3dhnjJXGTxV6=Q=O_gf2p zrs4g+rvMB<^S?uekd*Q*9Umr8Y=LXWZI>Qz@xwhR7@;`}XS`4CJz3kVinX77@x0X& z;bG8q#EAfWZn1O|`?!UOCX0)yWUOAO)O(UIkc{Rd|5W5Og_C~g>Ay%SJ#&lU92`8P z-CMQ`k_mADvLsn=_taz^pZqSORSz)Yn4wB<-}D#ONn_?kH>fYD};(weqqg7ZYS{Gs=YJ23Vv;>ESZ4RoeWIV z-}r&nX&zNJedlhD0N?ZifD zK)22|bF$vZP8VY{$j2Zd8~|fSOq9Ip>iNhERUFW-9g>wD z{Ixz`d;s@1)`iS++*R2pTscTD>~w0jNaKDM@iV=S3&7E&{1wRVO&ve!{ z6K+5g6M!xzx)nrU)>M=CP)V|BL+a)RpDQ*7h7Rx)OM`#ujJ%p4Kgu&1*r6R1V7Tsn zN^K5&9^B-(auUaT3u)ZU{{fNAtqK*TcKb&I80*Z*PI>OJN+*yQAPRpa=>dsyQ*`=C z#)*C=lB@cBGS5p#2=&ODY2ka4Jha5GMo{|RZr<86@XQp&mMWLEH#oA5irtB5?;@=W z3nR1jCyftiG5e6LHgMUk@aAYe>FOZSyi0V-WuG--sKjZ@KwJ1*b9MZ~X(?Hc&Y176 zm?4(d6K()mB(KiHB57(zD0LOB<|6$ftD6#XZ#G|VPvGp$r z{Pr;^wIn7p#1^C*!#O|zf`G3Vq>uE0MD~^318kJDn#Gp?eEm!`9%~}^^ukng0{2M} zZA2t}8K4Y~%a&co`&NR8!XOd}GVhtCAjqv10pe_d_5(r%-6uJz$#XhS7@K(gK}HdQ zF!77I&`6}IB>VU`LUmy)F+;Sp1D??!cn+(~;St#Tvm*Pa;z3n{oW_!phcE zg2dsJ^niB)l2VnNS!YP~eJvfG_CqJ@EOn7BO)>mqAq@{CYJJB`lYN!|_SX7eyy&NH z&C$DPo6Gml=%~n)4vY#~2OCs4@7r@Z>8Rgk1W%a}X_Ii@cK656jYDwBpLq`?WZ42Q zdFikmX*fEXebhno;;F^z&urg{&=zQm$n{3TZd38L@1aHMgBz?+ud&0nqNU!f$1z-* z+ucevyx>6ig#eI%+!8%7n#M5!ns>H*{cd^5T2la`DFaK<@)7-|JL&U5ky@~}=gkOT z`9P$Up=cmR=HNlfz-?QPkx`4{>NdL}25@S+40L zkWdsjxRd@t(&Tgd!?99_!(ybc1Fa5sSu@6 zxfT`k8xD)}QJX*~W2F>|aDYB4W9n%WHoUAGBHX&55C-%Ey3+^8d|y1o+h97P9GPmM zpgcyg29dBj#=j^q$p{f310e`O?Pvi59$durbT@Yt24iMX8mq~c$cY(ugI-yyqq~>@ z%}`9&$76_{Mb4KtS6#))2NJa32=w6T=xS`Hz!oj6x9yqYlNiy$n1Z8sDUdp7%6jkD zj$8DORMRj?v@A8hw@joPetRfMoMs)-6PL4f7xuMRMMtE2DU?B)b$(f{9B25CGPNRO zc}e2U69L3_XpjVDZG(W0LjF;&qU^6Sw8481S_wl?2Z}jM{`PZGN~Be2e=B{LsgfQf zkY~y;(FYcfb{4SO1yA$!)8-3U2zfT1kh~o-HpfDz=+p#ycMSg|&ms6YXK}9N)EO6i zO>FAE$qMi&la|<)?TwpEpNdOWdr`6?zrCu4c`(^0%7+z?h%<)9K3yWZ-uvE!9_QdK zQ8{}=^`9M#n`gyl{~SPqIB>iKIT_we-^=DrMdh`7S(2R>pO9QX)Fjc__(Ri5x=Y_) z1J}BZGVc=|Z}OdQu^G{XUc4hU>wAT^{2ANono~3jrwS+?nL=xy3H7#BI$7JrgvR5; z8UuwGfvDQ*Y52sx6$j@qr)>pJp4@5t4}AyimZU-E1-Lw;B}$>yb_ofi6Nk2HD7K>} z)Z+|xO2<-Vj&s6Xi(bQPiVEV*h9##(6tAxmYEhBHmdZ;E^C(LIY}5|dUAE!i(}m0g z-Z-2k%E|0T3%1jPrWH_pR#dzsdv>>}8PwL| zGoIl)BaV1r`;zN!Y3u27HJ7kQow9uXlHK#2i*r3Z z(`a?-J)%_?9iGD9OW(^goi65HN7jL5eJj^gC?KQ{q~ENYp`_|#dd3F$Sy_g&#H0}R zv??cZ6Fpokp@-}g0ICd?i8x(TeiSq~&Lkpz0b=9!H|w<|PdR z>GI}Wd;Q8$bu4JqmB0x+=PlBW?tB~c3Vv;0Ad{=eszvtGS|cT<0VhoLAS7wv151Ke zXmr4TEoM)M#IC*dO>l_3gMYcqkR@ES+LaJN2zE-rh+qaS80;>WA8Igl5qIgqS?vCv zR3}r3@v#pKe>$0tpXm|_l%v8HV4`H?CYh0n{~WRc9qm?NvjZ!Z`mIx&39FIM_29X+ zz5bxf-Lk9;-j2xd>LVOI3bC8UH*~2qDyt@v7I8{%bR-fiqU<6z5@xAr0i{+I!G-+h4hTU`SJ|LJ?ues# z@h>I2nIN9KbE3gzmZx`ToqTBQ+GumWC?^a!%YmrtM4aM+$xrpjYOg+VZgJZQ9R8Aj zjPc-~C)tvE8Q7ZiG!u35>)4+F2(Mt&l;2ccSSVsmy~zWWBq02TW895>Vs6}-s7JnM z|L;@S*z#B4RVgVAoz)ErA)MC&|E|Ep1q#xnjM;rvFM83jU}D0R(m%^QMpvOg$Y;yDxniw%>n3{NHB@<_)(F)(l8&fWVBRFK_2kVLJVR5=-3928722hV0U6Hm`l9%)tEu zZ02^VIXTYE_!=nOlbC)W3clKLW74A_scU)ocztGSaWOdFT3S}DQ z=^T`gUy?EX>R9>$Ck+6vO6ZthCRvq?E)f2KTvu#y-u9m%KmXMoc#f!lgvSC>N43+S zwhu|Xvdt+(gtwsXV$tIcsa9T}>{^*s&W-@O+(0|$3Hws(LE*GfXV4R8$q($MOb99Q zQQVIfxGXr#L?y^j=7Zy(UU}>`5(DMFqSG%h9IfCP=uAs^PftxOKxUreJ?<+geoX(> z2#@Weam_bu%_x-s!JNaEC^oE7J^g0!jn7a^(mseXvSSh0%H12Xwp$SC8X6reW1v6kltubW zfNW9^nI(5+!ec3$zIt(88YX{|iF5m0RP%=E${VTA^CChc9>{lG8XXklAN@sZz5S-af+TOefdzWL>7SYKL=P1U%Ern zJQIiesREDVQMCZ+^U%r%sPyk9__v*QfS+(nbmB0RApfVU_HVgYcg_EI1%+C#VtJ_@ zZZ{;)d=8yoo|qjJ`6IOK(|E}n&#Q>EaTfJxB!}c9lI?<=#LKt6XQ+A__@K?k_<6dC zrkHK}e_xNJW!+V?3w#Aj@#clDvGAXDqI;HC(#TnG?(v2GagQwdoDmMVm>A2}>^*tl z2_9S^_7VefX~uTPBHC){lx!o2*pN(WrofrXBmREW+n_jWJ9-SpvDgyxzfVkLAd#Mf zZAeI7yk+yFv*imVk^2vH_(OjOt4LtS6~*1aOq!=+7{ZkDf;$Aaw}!st&zRX~ z=DIzK(G=OG8!cP|=&{?HqG_>^;h@!doB*WnP6&^MQQSz*>7Ah(nm$Lt3KvV4yDXW1 z*w%E5U$zik7+R$fc9tH(mW)7QH^=7riF%6Pi~hw^OXeJr+u461&20MV!Hto(X3P`n z5FTeUumdf=3N#2?5lHT%)=bq$kLsO$S^z=*U|#G_m+FN3r+p>Aa%F*TDY<{>%`g_kSVG?=e=$1L+reh{gdTG7l|Y@(8c<% zaoMK&_ii>y@{XxJHVp0AYx^0y*F&>gB&XXv9X#lh93!p+Fg-+33jPQZsHKxtVKl;> z3||JN2*KDWB0iwxUvXDrb5*5{M2i}~6sMfW^VlxAE`65dbZMqYda|sw#J&rH?2yK! zlOt5o%#M2&=3RS@#Jrvvu32Hi>X#X8+1B@-|L-_JVhue`OBG;4LnHD+Kz>o#0f+8n z9TL=fyGN|h*hOBm3TqV9&Gg^DL2(nnms%t)?8`8R6_>FFk}&Y==9CLHWIzO@IAy3n zLp8ftyflcpHK{z$v2aAh5wSEZqL%{{1V_Lv?!f-^KJ*#$P3u3RSLk&_*w?~yx*&Vg z{~L#87~vZjM=b;F+ztNI-8ksRX7IWnuG^Z%kRkpTYMD)P%Eg8ozu(%mi~Kerhr{AW zv${YVd=O{qM{en4NMIsWz&la|uf8qie%Z@IJHlq(Ib(#D9Fg@B1eH~NY>1YJThET= zo3wskS4}igAMU^;i*RBFb%^PI*(Q=jI-&8$Lg}viTQ8w|zq^Zgc{HH+332OJ=CD4b zR_@shY!B(-fyN$)Hc+4OdZY@}b{D=k zzpDjhGRk+n;Sq<9Ek+pRN7@Npu8kgk1|c*(KXV1@3PcrpCLp!Ll*3VPjw*wd1|-e8 zwyrp(;8qZ@u^0gLkC~-i(gEVe_2(W@J|V|$v}D5!x>fYDZ3KAiB4BprVlhgHePs$b z2%=A&`WAS@!uS;0yu_e5g`TqRU(YSAUPIv4y*V!2H@;hFY!ZTN$YI^N+OLt=5nl!D zGw*|ERL0L~r*H?n;p_Ua@9L&E<1=si8pumAyFb|&OiMfLA!_yizmDXOx`oP@0(9?F z8IVH2{@u{3tF(M6>rHa%mOoqyG|RkHuNM(+ZN#}*fJ)=Pm_k~1(!E<~IWfk-`TgdO z#7DVyZR-=VBFYB{cXF;Dil+Bd>-{N8m3%yCdl^f=FO#=V$G`q-GBHwx zV)}F9m_(T`0!2qq37X>1DJ?X(Pt*_Sa@diVq-Z36R4Bl^)u4;Jz;H)=T5aeJXfHLoL7pfEBkO|o#o49zDyP5t~)DSHzwQ$Rfy5LnQEss zptWKAp<(Zy==3e+Qf^ow>hOtEe_!@FL+^s3EPz63f;LHJ8knKxi&ubDv`MnW>e+~0 z`%Hn~ZB`~$P3LLKN_~fYm5%suAYQ|ToN6wk(NVn`q^zd~t>r7MqJfMsb!bF%ZU`47 zP6QWwi{%8(>G;L(BHo>+N~@CI!|e3AwQFf+G|Cv{Y;=0DNbaz3o76fMA!K)qgz%Q> ztNu_1;#*(s>yOJ@A#DglcG2W@8Gx}X1L}{L+p_{`<5L1T16s-TnvN+vY8UG?+6FsAQ|XhPe{rmDQig84ie^ zW5=V=`lpOhUu<%d7_#X6D~QV=((P%Rh?JCoz6Re%lJZ32jwg@@CyH%Rq%cz%BF3&+ zj#Rs8`@pmG{D4%gz?4sIMT5%ogN`4Yuhkl#6lSbx3pUe1pj= zLsFM5_`IGDmPVg&>#l%yT02`$Ve5)&Acrx{WGsvfNXWrF5G8kRYh6VAW2M$45zEP? zqqoIv<@Uu)+|Dyar|Tn|x}aIr! z6TEKS-5)bHzYiuB2p`sUYgomcW+88e5_8tx-@S5ufvaIx^Cz3#*#$LHUUrB--FbW+ z2!bEDmG|tW661B2rQQ(uiG6V&LuQ{bwtCxeULQ+yI?J z_dc`lRs)&}4g8AQ^i?5N`q8KBTqD*EUrfL#lcUWO*~G(9FH!nsryX&0*Q~1@;!2$bV-TEKl$Cn4 zH@tm8z8lj8oE-!p%)C~Z2qij5%?e96LRICpe>o(0!Gh4!g$ssU+iS!=pb2IO8#J%+ z)QCST(XAvIel!d1%$hdCmG6`2opairW&sCW%1YX}%Iu(6qTU&IO#F1J{ zS%#*yGr_l{NDu#sPJK^B{1!)+oAC&3*!r_Jf#%9XkxjU zAm5S{xo7A~LUK@A03?H|bgiZUFQPms)pm?ZK^UZ6N>v%rnPnP3so&2rYQ-s}1a#0o zhs`w5iN>iKIrGnc^Sc(yM0aTv@hKg(tcpwT(hCx*(`?EQ()<56HDo%m6)LKPPBl}) z1?QFely7?}&nLHHqQV4ym7a7Hh<@%VZTQ>3HVrCtq$UMmDtP~Q!eAZ&FzOXGkkDbm zT*?U1k2)yg6vH+rb7`9A95Upb_-9D3FSxVLQXKG{L1D*x@Ff7E18VaF&ooGY869jc z1Sn#09@zfTIG(LDYn=;1siTWZdl5lD`hWx>;jh_(5!tu&x62$>UT)bAi2TR2#m2%Q zeJI8OAoHQzhKOilH>QrT&U{e`TPl_b|i)PF?OwfJJh>1={O{m&xuVft#$M=8vo`-?$VVM5!2N;~jNRm$|6M?NjX* zMeAurFvubmC`|uImcwedKtQDx?|7|u?_lCB(K_-?hC{y{a(GE0d) z0Z}&mFo`J$d`-J`csv;wXY6vrz>YyAi6R4OGt^MJh@J+N@4~m0*9zY5~>kib2P|hdBHBo zG6eq&T8|gR>f9Q1NLVtpUq2Ci14EudKB+UN}7dF z-Oh|c3q&h<$(z*DO#f-hG&~{VL^^MIj@UWLm*p+I&4%Jl+ihzL**2zfJ~J~axfyU* zqI6I0ptXONwLi*OV?1YNulvmfsofVU&YBo*#+~cNv8%V}BomsCYYwIZ8L}F9;@nP& zepl(E#~n!#07nh~RrR6>mWPS&il}WRWNndwz7bM7*~QY_MqSAQa{{NuX)|@u+d)MU z=fl0_>c?pcUKdjvQfx7f4=w+?JS9|y&NM5A> zV?3_!^2w*10Lae7H4_ob@`%n6LF&u&EzW*d83+BtrZcEi2}ShLxGa==l&<-+hw7Mb z56cAe?Dj$BR5=qDq-g8UHOnZzGVcUt&MD!!6+~zgdn_aEX8XiK#qqzCwpZtMk|s1* zT(V?xyD)8YWNvUmI@P@*tExsBNbDOGhPx$q2pbrDZvhYJ)(qiUC?vbEQx_7Y8)zc& zGAp}SfaH9J)E_5k8Kh!K0a`+nTOG#q0fV)e%JMVf)sL}9S&-x}ta$M|xfkWN2gQz}+m;5MI^ROWYpcQBb5B%&T%1xf+8#NXtt zeC}VgpqYMoh_!85@&(D^v-bx<&WXTilpTVCKK|gHvUrITD~SiPJ|i?I?I{_Q4Y%T; zcOPg=mn3RY&r43pub3%ys^x8?o}8q;FS}lp@yQSqZSdu9^S{eT21fu~U^FV79n%1= zGH@cqC*0zW>|8l-y^7GiS*VF-g)QM>LZ?Uns!TGh$^aD`W(yx7Xqb>5>`OrANI&a; zpqfLKMDfMl)P0O&mVL)rWyUK3FsetH5p!Oo!i)bdKajPU1#AZu@=n1XzT&7Drw@p* zqj0AHy~0}UeaSkqQx#&6?6E_k!$?a&WR_k$F4=4YcOIcSZ_2!+NIgB}^0$P6m9rbp zbXkJ!d_IvBARcq&F(cEv@iuZTcEp-vjK)%vIPuY4YZ!eAP4mOR+AaVmaRM>9$U>8S zeL+y<>Hq!gBmIxTx@pxrY$=K1SsXL;J>RhLIFkl7?_Gx%UWyf#51NWc@>F1Yb?}^f221B#) zS(kKBk}H-(dG^u>&?f-W)I$K30hWopp|6X#+G!MoTgFSkgTrTZkapOagw5=Dof=Dm`Pe8E0y2i3@7|tc%@GuVv(8GjF$ZO6YCKMLkL_e_PS8^r!rTlSa%~jq>=NC3if2d^Mn&=`mGX zM;$2*u{S-VDCtn{9HN(tQf^sLDwc@?s!dM=El?phY&IRi`{tls&Vv*%EjOTTg?Y{N zh{)$IX_z(UT9d$(^>>PB;H05GAU>lZqC4>?7L-L%R+b3u=3pRIopzYs9>dwz` zeg;6=*UN@SRvK7sB*4y(8XHm59}ut(Zct${{3{e0n3)Y#kf~kDL5o|cBOIrR)$+fX~;BRvsbHKE}1QZ|)@Shl;LqL}F9bw11Fp`o&{Qy+7k zgij}q-@FG6_WX(8t3neh!EhD>^hO`fN~+##;f4+>DyaxdvEF70vy|?V0hO{9Q?u@V z&d50mRzH<;7wI%{em1OV5)A4OTUkDoH8+{qW%X7Xj@K5_fR9r`=*KHhICA@Kb<{M9I(=D`;H1*<`~MDpFnaD|1lvBE!q2Pa`lIG z=n*7gZQKkgj4Z;+F?lk8zP6f1-APf~<*{pnKF}!@5JU*$wWhKq#GjSjdf{D40~=m6 z@c+L(+#*d&GGgl(5#ej+w0V9;bQh|QokSGjDQBh6RcP=G_LNzlEsSmE)aN3Pn+CgZn)X_ zun}`)@*P1+4dzKopAILZH-eeU;NGb(*%;eObdNnCX`XKqC1Q2e5SnqL{bxHjSXa|$ zzk1v=`X317qOIwY_}~9K{C!T|c7%iS!&hc{yd72G(C$fr15uswmi z)l0{APM8JN|HzWs(}+rF<|TEIq6If7+T-%KCdgTz`ihh7C$n4C zH#2Djq12LZLeRa#+iK_1yQDK1fzbqq5PFT7!B$sb%}By^8RTyO%pbv68nRB<$Nmzq++b+FK8psl2N^* z+F_Ej=h{PrgVj0HPi!axKzU?>_b*zS9u{wdiT`bZKsglynZv|i(t?PzL&v;JFup_X zb9l~GDsHNNp-w5XnK?(psn9prdaK(XASQ($hl%*3UOL#(0YjLrES)0j?qDDoM~gjO zHIDjoaV_ZYL_f&A66>~$u7oYTH=#*qhzpU&pvSL5OCmLtK*hWIdL{z2BYV%+{&eY- zKYo*QNy9xh5sBzysOrMW8{HTU|9TsG?H`JB$4lSRv=iJ5aTVV4X?-2e=`=S2c!Z*Y z271rkwz&&@3%fQYixWMRnfdW#X&LP>tH)z8?R_va9qvRP1a346W*S^4A7=|cWwcn( zkV1Ls^@H>MVBnRR92_+QG)_;YN{b2$A8QelzM@GII7YLk(524EG_VLtZ<@@}HV2wd z1L`nj%Z2;3k&XJ#he;RkAqtMc>ew1}>Mc5-{0YX@d#{Nf-@=y&i4F0lt^e^-76ldY zxQ8L5429*M3MKWR0g9lYaP+wqD{7@Zq_0}5RGKb5aVA_(NcIUPs-YZ>xe=lkOP*u` zWcH`KRs*@X-Ug#6cB5PuA0C0YQtwQ%p4x!JeOsJyB@~N6uloQ?v@@JtvW91SJyWT7 z%J-av=nO4>erGd}7Ht{NfK3ittGVNZZ)xcA{eXiBx?DaJ2rr>TGsK83#3JJ+heTr6 zkT5~lY$h4Yw%}J4zUi5RxBe!WJZujIgZQRmJ0n_YZa&t!Gyw~$-i`TjjyLHDpC*-V z-6MFKEQeM5T^hB;GMr`(AQtN4#`{C!(3Xf`u!=9{)N0N^L?N5rxIBK;#@W|gC1E?6 z`xBa9z-8+5xoYWe@urO>tH#C<^RqVITaY$G%U9FjA*8S5=~zbf z$D=kRGDME_i*2GBcH-F$Q##LO9T0=U9h%Xv`y2c!FYd`=B+3`fwkFq9VNyMl zKlaJ496Z4239EGjr;L|6H%|d}!Se_*zD3;|2#?^a5tH)F399raP7sil}@W&2+_yloo?ogZNX zM9-AY&fb9D_uK`YPX!(u2J&$&^;ki5-Ve_I>e74%PFTG zq$qs4MvuA?2>0Y|nkizRg2s`(4`Tq#qPMjH%E--RfNDoz+C45D+9HHKI1zOhAP5Fg z(l-wkw7k)uSlwCrkzC=v1`fOl`cdS7f@pL)s=1;?AHyWckILa>=2JUd`uPl5Y@45p zM~Dg59deO%awc7fMTt75kS-wYQ(av-Awmg z9*TX-kdD++1ULBq6%`+Es@h!kXb9VM>R^6ftFrLgPq{-!@iX-3q+&zZt4kgWL~G&3 zZ4VrtvQVSGHa|0XJ9NX7ciXLWf(7BfGFTdN>Jhl>4oPX4STC$|@frW@yyT|AD6 z6arpH=yl>b#I!(VU zVH)l?^xU@;oOZtCZ9a?xRG{IOLI6W_5gYu9DNw=j&l9txMNBPuKF2iG?8X_cihr2n zQt9p|D2et)IN~8SL>pr`*0PKMr0L1l5HF4`o}S&l>7RALW-Daz zr81kOJEDz=w3_;_cAo}`YO7*w=mBv&gL?MZgozn5(qa9et+A|Q8OvK>n+YjtYIoOR zrZ+Kl!I{;P(565VzUD%4dQGzmcD-hqq_t%u9=(PvhxgfqTYyjietNW`#)~*>KWZC~ zB^DA6%g*3oQ@<$?d!H5R=?hMlc=tQWVrGC}!CGSttved2a6opb2Xqd9x~-WD&a%ry zj)C*nI>+~`UGGUQK*>tL#^5bM=1V}!J)z$YT!q$Mu7?RJLxR76+JZkw9uzle1eaGN?P$)%U^7k?`EdRL?C=+VqT zC>%4~C8wtq#zYKO_{WgVwd*3)LO8btE-m+%IER#4T4*soCjt6Z8F!X9ANKr}r6wd&3<5BbQ?TT2kXjCA2gAGuv$vtztoU6`;FTAaV zmqocJ7Gk!b!olDT9ErMTb}omK-JwLHaSB z!V#!C3+9)IH(;ZKlVDU(;T~bykU>s7SkFqB{FB%MN5hDiU-d&xY)o4{nIR2XV(S{; zbTryGhiS*#XNtL2_iNp=)8Yq*B0cpGFnVn{(@CJzS z$IBR84cHb;D#}^irvFB9{1N=WFIuI1f#WMDhyD)N;6HH9ohc7;YQ3+Am=MS9HNC&9 z)B1tBT`ct&s0AugNl$^Sf*f8MF$crv0^c$aY5o&2Ov_KDWtjeSJo17kh=xp#w7hLO ztcDGt?XA%ubnq?52q{&6mQF;C-D}yfvlD$?J7f=Wn|RS}-2q_9MW`HSptcz7r~o=y z|27>^Q4m9wsSP7<6J~Tt(c*n&cpjaY{OdBgVjdorKajLIiNn_JqxOF+gJE4(l36}GPG8kRxWln%$e4F5N%&w!q^;AT zhVENjJ551&&OS~r0Cm`B=)6$TEw1ZuQiU8~4+J^zo=BMWSl$KH9CW>kw2YbQ+aUYZ z_BVihck>a4f*0)X=3^bvaRld)tC$UTc#g%SUHa{C=FLmVsPX1~yVU2*nd zbKZuHzlYut;SklZd77ci4hmn!8~Fw@j1sFk<;%t)x&8V2Xe zD}qXJ0J!YNL;Wn60tJ>ywIpWlFSB&9+M=tupHL>oLsPZSY9NI%eoE*6DVWzz&$~`k zAF5a~K#CTN!btGgw?(G9YX`HAvH?e0AeE#&s1Sy+)2H$eiX$ z-@f5Z_W86?2NtUO9E+R9tiVgm=$=l8b`uBotW73l%47${>ZNbxI{U4MX#y&G1U+BDo3H&bF z+Z_m%ui4NlxYm=x&<^!eiN|GtGikW3QIGYFd^PgfuG@XC{~q3^)rL*+!;&M3))UB; zbzw0zf~VxgLsHpIf(i_l=Vr2Fi6A7dQ%687VCU59X7yGSNRoCyS0_+Ce{7Bq31n^- zlf#vHQ#gQLiEn|v3mn6M!E4@z1@gppQvIiIi?cRQk*SZ^kI>!hT=druGIv`=%3-xK z-eyuW)X*?p;H6sQ7QLZsDYyEr3JLcP<_cL*{NGBqj!K3d0#h)F4R$KqnB=q3P!n4~ zA3)Rcb@*&}P80>T#sJrI!CmzH87eW8*ZXi=TRTcDYge5%o`U-Ty}ylS+c{)vW}-L3 zAl#f^eK_?<(W^V2_#YPgoU;`9B*M`6)G0+z8Q!cz;RVRMc zfkpz)r4O=)-lQ>qw_kduR7hL;yH9sJ_EXXRGzN$^n$63Jk)pfJjHs2C^)z#qqJT#= zx#2&(&FrJ|i`(Xox-C0;$N-;F>1|}8(n){codVDqAw+_aU_6Y@!Ab6Rg(FR_QUIEx8qe;C$+TI#2Sn!aXnFhj#{;TOgOirlC zU1hnVYph2wvqP#=M&4zAff+4NRQLq~81k-^x2QkOJyNx52h~IqN58qVGuoh$58dFN*13JqDe29ACfLz`s!U3 zh&AG@X=9uEzrPxsxB7zAPzbQEs?e-H^g5gFjmqZn(Hv4N@$rrbx%#2~*4Ngm?LABE z$s>_Swpg<7uze#0-j<26DeB*WK zIyX~o;JCT@lsKY@n)sVwdW~mo3cY=42Y6B614ae1VU!Dk^x(BdRe}V2f~fOR=%U5Erv~4Ic6`Cm-l1Q0&VxxPL8gbzY`su7hWO!`#o-yBYn>gBCeM8F9|TeeC>_4Qb#M9vENJNqWy+4v+QOqRe8 z-2^*k+u;FoGUn(q#w?Zra2{o#>Ix9pU@W7cDV_hI8JcNCo7-sCh%TtVts+o)U_rE= zy`s!i!Ug80WT^g^|syEx`9N^OzncsDhpLO&r3y!`Q+*tXGeh*kn?r=-bXzqkedAiHrktqaYyx!dS`35B+>ZAV^EjqmeWvDu?3CY=R<{sh6-fMBpc-f& zQzwJR)|Vaf(y*?c^;)fphD^XrWV=mVqBz90h%9GuDPSn}MUH6o|8uwyH!v(`!&pwD zeh#1O7LLNyteMkW5U{nSqKH6LI5mvs~E`osVYTA*6p2JzD%ai zmg$0jjT*J`PgrHt;I5RYk?Af$-r9-{=&uvjcUCVGLXG*H_zFnYv75yF0ilt+N4m+< zw;*#6gs9LAhRy*V_NCom=qE~QQnNz4zLvlXHLS>9ZE$axp&9?w0%=^imjp##|8v;@ zjG11Re{Xq|H3}r;um^%IzLDQ(n#aDUm2Gy0X1zN={!-dv8P>(XxwS7%JfJU|_pW7P z$b9Uuh$%2sbIFSo>DkRQ_%~}6lW%4E1w*P7!ORH_ahn|CNV}T`z!f?K9#E{#OWWYN z^y+ovS0^j)Q#gl}D=!^@VmILP_P_gBU`kEvCtu9VbgKyOEJEs?=gd%9K}%+2WYVjz zDWg#`RATkOGR^lR4dI8npPbY39xy=wms>H7rCgIZaggX+E#`Hz+P+&P6G_ z09(m4AAUj#e|JV*BsE^9xm-h71lOPSu#HNw9CCIGZ7e_Uxn~g_ybG|({Q*a7#y~F> z5~-2=$3PPDjd9gCQcyO^DR$Xs~Wn zFPIN#VY=MMb6OeQ^{+qf71`gF+ZtAOYZ%jmeWUYwD)E~hAAZ#r-ABo;?7Tpl_KPl4 z(o5_@B}IhujkH9^3a>3A4_w`%!{FH&8O&}`eM|y7>g?xr=XRK9F2%`BA0^L7AEXLv z9KRMhgA^#I;)X4JRLXRxblobm4S-b%Ju;wUJX^)P_fS2mTDN0wC-4PTxd$1`xn6mt zgY~y;-OExGkB>1!p(P!^v*jNeJy0GjtS%TEjwT%na;in2nU>4}&})a5-YMpjGOF_z zvVaA1z#Lb=bqQP%(=j}|ss}Po3N=ywJC;3xZ9@JmuFE3u0K1blv5c0R_mdsM|6ekb zXnQXgb6ZUZlmJyjBsK+;VCce_SFn|IJ*HxU>C1On2#vB8o}f zkav~KKFNM5t~zY4J%4daeFQmN5;W&g7=gR=6kQ%rQsc6avdKpCcEM6i>nU_X7H6sd z+b*hqPG;#|*F9_1-|zy(9u75{j5YVi84^Gyy@fJ0Ot;rCTs0xBHsRe8YQq7=7cLSJ zJJ-%eGkJe!%jl&)Y$&fMkB=<`W&*e*Q2x;=W-a2xwNAO%UjP?!0EWi|+?EY}iXspW z_CB}MH3?M_bJ%c?RY8XE=5}fxy~TsCp<5r}ncwvoK6Ux!(8hRa-6&021t9suxJ_ls zDn<-Pw(NILGflV`6ZtAIJSA!|{WfAS@G9sS#!aC>%~6q(Iy}kz*S$|p0U$g~c<%hH zZ77mjwPsVvDgg0xyv=mz<65hRf?$0uklgs8*KREd_zi&XR^kd~wQUbu|5gV748epu zd3UUm$52JjZx?A-G5lN_O3S4{$7C5nECtr&_3a)ifG>AzW`c9r5HUUZeJAcQ0pVUa zxtgLql#p#95Q2+pof%}n85*y^CdHUr)66u^yK}9k;^8CQk9%o8y=C=)&tCcfWf z*DjcrtvJbA8GJRm&NlGKhvINYur8;fIC~JmG9o+mhs*X{aBajSwBf?p%=X4lV2v|) z?MAu%J_hwcdxwBi#D`*e_4C_prZ|=1pNct^ah{vSA`ep;+`smG3H(qQ>I0mla*yE# zzNxbp_Y;-k=%79bn>FXb_ogqe> zQbN{ZD{0gPJ*(h?6`?5R)s9wlAH_79BV9cK}Op}oe+su*s{3eJsOH1ncRh5>s zl@Vom3~Tvrg{$X62IiPF+Xb6>+cw%;?2Yj-&b~A?^hu|Y^OVlp0AypU zRJmcFem_#-GF2zePplEf@1%>Ux1aU^F zP)`3Si;f(R7_QRNO_2)Tx?xp<(16)xI@P&anGc;U|7PGup(wUKFAs7c6SKIWM&U~& zv{5JwitdM&5O7XN9}1wqQyKL$x&6PFNL;a zlbI`lO)m9I8*3_Vq7F2zoj4JCs>B2IcQ|a==slDCX{qaPby560U){kVkV|0MTk3r8Bq!F>?IT z>CqZ0jHz-h_z*1(;(hI0hkJRv1d9;e|- z7?7Ck-KQ;TJqnz`;n8fiOq;_2Yd4iWARq%#R;NT2N{GoTf8&=5>wwTAcL_gD3x)I_ zdfI5yYLQskaAT9&Eag4bwnLy2zp-uudI89xk`Yi7w%{;bGz24qaL!fV1_p59rN@qO z`BGnq+BPosmIXXH`7)`;12Hgbssnl*Qy;PdO_}D6 z%KjKo8>u3c{bsO%`qS)=Fo@0kzTOWxmCLVCf8EflT^}IzXfu^7-fcmYTc=z}2R*Ct z`q^m$J~-y4lmJL;o5)fKRq~;~RP5KMCQLGjNh{i8aT?tQdGOY~XDCMU*Ua5%7&v7m z{HRxhzC(Lfo^8ARMf3ITDT`gkO*;RrvYAO^3SThn;uqqpcU0NLiWWScqM=?deL&~sP8*?FP z4Hc;ElTpTt=!DAcfqCOHtbGKmbr7#ok|Dz!V*R)kuVzFl?d)r#BX{Ij6W9Fm1kt%z z^5}AwpM%f_RrYDnVX}|RIFnJLEjW}XJ|)MBqx5~dw!@-vppg>@)5ae1HtIsq#IA+~ zKmHPxHVLZa3MY(C%&fvQTX{S@_4nN=>#_l%#+qwvj@4wnc=DI9$e&7>z#?txcILW(pX={h1a*lX=|X?MZwGBwMV~L(;zKcf$O&yZ?N#keg?JS{QQl-0Bht=>hIhG znOuDyEK-k!)hP|o4D$69FB=6~JWhO?tR*AIz z4i34#4_8k}TbB2_o)XF;k-QIyhv*s;cH;W$G>x*nbEPXWC^e;K_+@iw#E1RN^Ss4^ zc_cg4qw8mWAJQ zIzz1m2oS$3fFrain?ZQyX^dw3sMXj6wD$>jXl6Ertm za6|1fnyCXM;aJM;@lHJe{0va`cAVZqq_x;UkFo6_dcETiW`(_^;a^fenIAd^QO-vw zyQW{&$)Ji>7{*jcY)TjA$vCF8t5JO_orlaUsb7)CW41y+0J{mi5ng%y*D7IwNR_LQSuY4r~iena(qX ztyF3YRbhyDz4wj#r z=T7t?vNxs;#9se?uzRE=QGf4^pG8{L1vptkXNL-vLWlKVOAtEWDWL84hUu`mhXuDS zapsOHb>iIctSDqo*+;#Q2H{3xG6#rmXgtR>-jNv0VNTAa9BJU0%qPNmeBOmG-M$_{ zluK{UZL%g$vm2_Im278h>KgGy@>N8~LLV|C+O7`C7`r?Y;mDI!I;&dFQ{bG7n#QJhIOx(lOV^c>V!gXfqBq=i+z1@aYqYfCHaF z^B&ls{~i^JN(Y;3gKl5L zaD`z%V|dI?%=r_PxI&qX@wp8~F4?%q9vzfS-c_iyq zAIZ3EE`FEwsQ7%!d8f9qXg_1R5zCkD76cWdC4VCCA5C4LB3d(4VZ3pL^4tl73LQX5Y_O4A|Wr^=C%7PYZqsa<-AuP^x#r-bKeNFKnI zxhS3`>SdW4y-hre3ywMP)xDXjkt5VHtD?>TRZGgliABy@majyIaJUTjQ#oct)dfKx z5(}khMV5q#13tx59ZTX>1?k|NY>HfNZ(oawp7t&0dUG&uKU$P9GZ^It+DR(Pjr<@W zUt?4i@=A(z4~dlM#?Pt1y`~v8n49{I>!H9>5s1rk9aS4%GP5;@59W>xCt}p*X0)2# z5$W20v_@ncdbfs{MfZ*Hxw0OlK7}{>!za_(@e3|c@-zn$vcIa%%mKALNTyCeZrQN& z9oK`wZQ)Q22FViyiM;YV5|ZjDk2f%Y5VHvz?;ZVT=rQ)mGX}a};h8D+g_yO!6hB z`}x!ZudtG^+>s3WupNI)Zn5{C*)H94`iDq) z{-HsEa%0e4jh5|^(9U?V-{{>RDQH;R0UM2pkfRpJeV5LvyL8&Qq1~~zsmsP9)4fN9 zbNguMbg7DhCt3ttZc?`w!k02ZRqp}cj4~fq_eMHl{M619(UqJxs{X(y{1Y%@y8F|# zWl*CjFE?>F0Pu?Hb4=>j)8D3!vE_{pe81Fs^LKBSobd^atusGo9zp%8$j9S`6iac7mlrihgQ15F z>b#&-Nm&(pURBn%=`oJRm6rWtV%TmL+#B??N~e& zi#FXV{VKAQE`>~Pi)zN^pdg@x14sLm@K|;&waO&uvvNUyMp!I7Y5E1H==ovnbmW>VOYxD7%?XR5~qmZxiW;bn~ zRHBazx?PzRk}1+~3EZ`H!*%l6`sKm#o3{uQMjf^@aW`qD1xV={>Ni4OFxv+q36Xpd zbnbHj$c`RS$K{U<&R=a~+r=l4!B4aI{~-Sa-9LNaP63&O^#YN;ai`RSf(&!eZhnOe$h_19 z`TIy7ITKhAt-uEgFJFS*f_Ic-fnYE}_`#L->YwC*IkYX~BqulGN{A)JP5KutZp9L_ zH|O@HjIyZORpP-R3t#AFhH%4c)`3akaO~~PYMl&`kXYlc-==1HE#2;J$rw`pEAalh z=sy@GIWDeyW>iLD9&zarNCV zjae|E&VZ0DG<3o)c>Q4rm&B#Tm27KODX8GGZj!=NPoP6?lA_oGDS&W zEL;IB*f2=@oaN;QS3b1&&AGaeP^QIPeXzv(tGB%2wuZHQ?wZvU?cWM)7f(dE@Ekt4 zHaL+psmP!!LjyW9CS`b6;WGiuT72anp|5KBsHt`I@w2hHhFs804bn=mhC&nmYUE+Y zFY3}I5YgbLKL-mV;)8#4)V=U~F{mGMKRV!s@0TJS!V+>9pJIftgeN)ss}-Y&`4H(7 zQP6SCc}`L>SOF`dWOK!9Yh!kHbdn+Yg?3aSnEq1r>q#(<{$^Fa{XKObA|9XDlNf}29iuwU#x zJEgRlt*|2d#ijOC>Jongx_aBRMzrA-PXNAmz1E3s9WA=ddlCp#8X}}wN&7IxFt$=r zom{QfyM`Tk}!Qw z?y$z!+`w@Cz85qJ#xbF!F9K{RWkQ2#M~+={v_&v$P221R|p+KUx@u9_f~E$lcHkEbMU$u#!68zudK{s@S$? ze_pHg5X#uHTJGJjrTQldzj-CHdG<0*+3w(E#U)=qudcpGGAGnuWIs15Ez;%|hl(c5 ztxLfVsEEnJ$Dh$(NhwNiL55rZV9i?Wkeq6Bt$fU0jXaHk_f##;;IrnBwZIedGpBjW z&4VnCba#Bi!Q^y`4(NL>no6&4^ckve1e_p)4 z)HQ`5Op(RGp*HV4zY2oLQx$-)yBe`;NTR?qd@B^oo6EyrVe@oI?V#B4_3A6pX7R@HeCe}u-tffpzf9aboFf7<#;Tz{_*YrH|kkz`7b=%5mT$Fj< zQ_?pW;^Uk2uM82V(zUml5T4S`AJ#Hz-1-Mrf29hTx?8{MjgCdm^G3Bd0_&>eT8@+a zj;ROtbez>ANz}YFlI@O5{SZzeWl#}I1HiJ-WOIN=#*O0GSQW6EcaBl z)LMIs(<#I@&tiE@0?3ON=qf#|97af&c#aE9@*+_9)Uz@sF zJ#aiePVPcmD?55V+E(d72|?XS8Iv4Y zym=I7_+DtvonqfRM2L9zq~~c;u6@DZ3Fd1c7uLjoU3!Z86Ix>xIRZI>=51c=&$5tZ z95;Pkfnd=*uYo;+voK`Dd2#ERL$6Mp z=3E6pG4cHcU{cJUBxi#sg+?P&G0aKTCj}_Y$gdl-!NNW3TU~pE6wo#1i)dWh?WFnUw1^&a#*pnh z%jjukZ7zixbZ*6Yv`3k*uZ&=jYDG`O^iA(p?4z47a9$QrLGh23u~&h%wu|lM6!6Lu zn9}?v2d}kai2$*1Kf@P@0fznsdk7}f%M#tUaa^JWJOCJJl3QVwFbJyBz(yWU>$}=t zk><5bFYDG5ispxSxzx7$8zG2Jg1k|O>_3iaa#Dm9j@eeh>$4rbeI}Z~65Lm_T6P0> zRvEy;oav~(mVOKMy~{&&71FK54(qa9H^_T2dfk+N#i1C6n`=BUZJR3s79!JZ^cp|g zH1pbAl^j}m$*)V-eJE?TQM5IciS@+tW}Sn#A5@T-(@5{Qy`XtDdE^n44(8)h)*%W# zN9%!))I}NlcR5w~(`}D9_I$19wf)W8ac;s&%=|!wZBYcJS#yqs2F9HHep+-&uIur@ zZdizRJn((zmk7MvYbtu5IwMs&uG}uDfJTqOb--{6lHSkgsHmkQpYAKH%fPM94<8SqN%W^`T8D&!ai6M_8%FzE< zTQn##%M+voE?Ioy3U+DjL9l*b;tU7pFnGk1psB0V8l&T#5;isx0EiZ6M9z!lkdQuur zFmzvC&h&@aI348;a2*YnBT!`_tdydH`eX!@k(r%`Z_K~@@9d2W7At*#^YlXa)A zgyfv=F!~ISN+xcISfEGfkK;87wn_&VrkG$hO?&P;yuLP)tO@PMOH0lxj-;uBk4-ha zpLWCX0bYdFy4lzjjaTd3o)f3wF^(x$nfY<$`X|6-ljIg`#wYPJy<&2APF__y>oipX z5J3i0ntb(otVc%ZzwobBb$YMeioOUcAm7yP5>flums0xsjAAZua|4n^A(%EwUF@yJ zwEtva9lerienNr8YlxqpGsUn*F;T#dc`Al%M)R|2LNPIPfviTm-F5cF!K=j0HldTF z@;Bkj2;t}5H-|ron6>&Qd8{h_e%4=iIRHobdDC&WUP5|M_fo0Os0T3PbL zhWa|#1DKYjfN+l5%Iuj1j`l$IOA=n1o{xT<6Zq0rGKfuGJvU)cG|8ir3RoZ45V8K9 zfM_)|LTbtuVDUnEiD*R__@vlNDMW^lhwefzN6`icsu{%s=!nN|kc_8;XY}!!#ZWG; zE}c7OK)(LHC6a)kW9gm)3*xlh6MqUc&AV&jN$M=Hlf=T5YiW57@=x|TV{SplNWh~a z+WMT`K3_5ee|s0k*!H#kIYMxpy<`q#&=4B6*Eq!dp1^Ae6qA|t$m*)b-R%D$sKCyy z+=ZVN=J5CLj?Bta(A;B$IYF$#!KPPb93VXVQwe*8+wbAcQ=Ju3-vBBMLQLF%hpYWO zP2}y0+T+cE;OQo5$@}rrE&S;*$^++oo4L5=WDnd%Ml{9 zl$D#)5$YeKI0@X}#V1*NqRQ~2zhM9~mw>r%gJK5eU6lZMdBtrZF+uTUS)~X$KblwG8>cr2e`l9*{E7Ym#)-81OByYwgWN$3ZzWQkhPd3d8Vlh?pL){N zih#0{F>`{$2*E`cw$E(eD=ycq6N7n0b1j2CpB%d+UtYA?Ih{*jTQjaAbNkiSoD zMB#K8C4E^wAm4egBk28|J(S6+%=_>$NY;qYqO(f=qw2DQI2uq_^>kX9X`{#6Fg3om zfL0$!u$QSA6YgUxIsW)ymupZV;3VxGcTWhi-Wk}9q?xqb)9M^QkA!Y1TeRY9A<6NN z3%9@@R;gV!rEngirvhuD4g-`S0Ao++f2eqbIB_V7ONOoUlI%aPMjutk6_oGB1(#^<8%U zmo*SXSZtT3t3-Hu;8S31@H{ls(yg}&-^c>>zm>NST^0lFcIN_<&qb34E1f5*G~Da&3S<;4 zm+{S)5T5NkQUX*Z!3@Th zK@&J)sQ>L#(=uFXRs%>W9C3c-?{}CQBAPIxR{fn&8ah+|e-b9}Hx$8;@k6=PPe>{~ zl7kksJrq!gp}sbAT_Y)n_5+6XL9nnqrnac#U1zG)O17thqRpebyy>RY(zc!)GotDo zmCHXXyyUp2&+a{(*-l~tOi;w$eQIzPAI4iH+>2K{9=Qr~eXVn(@kZ4d8z&*7VblW- z|8SXiTvQD}I$L>F6>lwZeW;uMz^>?myO%sMqDU_0T|U!%O=g+wKPFYI_A^SvUZbR9 zo$c{IxVT!RpJ|INVWj8tcUE~$)X|{87ia{80E(jC0@Rn`!Mzm`$HJF^lQa=#1U11^ zz-NLBG&>L7Hx)9!9Qxy=SH%Cri7+4 zJiXs#k(dCc;465c!JkQ**2oar42u9ZnaWJKu+#~Vz-T?hPrPYSK@((Yuf0e{;VW#s zkKfI23G|%2=Jb`L|LlWT`;I-FBJ?0j-j=&b@XwKc(8jpPtoNLY+^F^X**9AVn#7h& zi3&`Upu($h?&y$!=jq19J$XFTU)SwwEcqpr$YhBu&AuTLMoDPwL`{sH(ahL|5=9be zu}hMz$Py|`rKFG~TT~=_i#1#7z28BLp7;6vp7)RUnVCEH-gD2n=iGDdIrrS}$H$~p zt|?E0DK$z2XsWEf;7ilBC1A6+r{&6FYJ8k-XP-{ud4hSfF+&5LC30a&4M%4wq2rv% z#@s3R-PYC=-ylyQt-K$+K2zU-XUbZU-LCYHz~FI(;jsGdHh8_4^uz-`1ZJ9~3Z4_OiDhYLoRx|h zD{z&ISogX;Z9qL~Vdtt8X_-KhF6-nFzVRI zlVbCh9mSWV^)^I0U{Y_s(8|ai{wyg}-F3h3xaN~Vfs4cC2BG*gv)SknnEXd;qw@G$ z#Hq;7YnObgT-~{T$bHjon22eNk@6Zp#&Fa&9&t*uOg7IeGR>;`Bkq_3B`L|!M$Yoa zx{87%dac6tvQu9Ihi%^<<7enOk|zz!^w>87bKLM$QIQWw1SnU_zVYe zpHWeJ19Guwb+?;BVd99x&70r*#wM%G_^-RK->Q&{Nu}&-RC;;0*YhOm#;ldd=Idma z<%O4HKUCfB3Vx}-y6EXmoudX`&zlOxUx#N8AMf0rEnqXdH|e=x9~WubPPag=w^q0f z$G^8wUXySKsh>g}vm%g-dqPt@uR+^mi- zIB?N!JVkTZ$GNZLOn7)y(%#MtozMXdsc<2kqAnrBEUvy??WrW*4AU>mtWuZqw6dPO zL+EOG5_@>)v~6qh_9fGwEjhU>u4FXZAvU5;@G%V8D3bLkHg@cN7)QUqcG*C>fcqmj*~UorK$Se$aQ}@rye?(fNk{yJXy8hifnT7$cA3#gT-Om@ zjKC4&9jNCbdvX%_Bw?qAu14?%u4*3~UFQ*Gf4h+WO>EnvPi=67J-GpAleozCLA(!V>}i@atus{t};`IX0h`tT<{AGsNrrXqDdn z%LNT}jNX86w?bd9Y)-g)<4IdfXzf-r#NB%@Cq!&krbuhsiWOYb-qb0SK14qlZpoQ> zQ2ki-Hc>|(IC?xH$?AhYO?fYBi}s6yHGUq%hu~K&x3^A^@)9F>1{=pm`g18WF&n%p z_qo!OJ>$DrJD*g3R`>f2Ky3cZ^;+q_DzouvHT=7`9e8;G{@LNTi z6!)jH5Njg8YUGVW;Mp#kS4)U0Yc!A|f90E+vhu;eF!`CNB{xjZNqR7}(Msz#b-T@W z_}PSayk@MQE=0ZJk$yX5BsefGpwblq{+>f=?}O$$kLa_Qk*2#wC$p|LlzBLFck}l2 zo#Bmd`I+|A=l9kk4A;Cl zB-%gzc}y4~G8Fdl!KZ^6k6&Ywb{Kh?c@3uBfL-&?_&Czen0{|m9T1RUJt~UfuRT?n zSkjYvRp;sU1}Uo9o82Ma%KQD7>}H%5zwp&l0&RM*bfau?&oax5m;)9?Infdd!{U54 zTbs5O1gE|nt5E6?+s(2*f6phUOySne^Wg_3;Ri+(g%}}Wjz-xURzAX;1`GXrUw!+Q zX7SGV^e&CWb97p_rm%d0l4|(T533&^_91|O5iVhGoJgtYd{xw{W9^mVcL|dcG4fvE zc%`#!_h6XAC6l91^Z2wPMZEVmyp}2Z)NUf4L-NK}`I;OcU7?~CS|q2^s;t+jx!m)Q zQQhzmzGEbj=L$@2XLN_8(%HDJ`!p;TjYIhH$QrEd1HPw(#+wlL7n23?i;ukIr!PeP|8XbH43j; ziBy2{D_vN@RCn{b;gwrQs&i8m`>zcynMsPwvT5jg5jPVx`Z%es?help=23D%fU2W8 z{9cgc#IU5Lw*$VprD^JMUo6J1lR?^KaeQLSdyl}zHJDYW2XHz5|?`r0`d9SgUOMXw4 z(cxA(*5pa^u;+kY0i_ps+U5Q-}W8BW_f1~BSaeeoo*QT68GfNLGftOg?IWeBb zFin5NNKdM>O5Cj#T4^e4YNT#YPVharF_1h+xha_TUaY)MMc;Gm7=6lKf*Tj z_pJpcVafV#$n6WeQ}oKgAvhKM<9(7)h23j)tQy1qoSo9_#J_vKk#Q4(VJdx^UbZwe?~E^QAaM&QCKbI9)Cwyi0|6%t*y<^tv|+}<|sbinsA}r`$#72LY|?;Y@e%nTD4?I{hGR@$_Cx4 zH2(d9@CnT6tv69bu^|QeJLQ)(%~@aXfs(V`i^m7+ClL? zN1$AOS6En{-&5solfaJ28TA9NL#{vR8?DLTq09MXcZ?Cw_u#mjf2Dn;PGzPGmgRWY zDHzsm@h#t5Ayl-(@yjbSi!UO*2-9Pww+64}*DcXsyH?BCBBp!I<$w!AF#UAJ6{?hc zjfvfb(w%&f$8d)^wRMbhP=@#O1jb+=EQHsoBnfUQC9S`aa2ft}mH3qNr!q5Ran+z6 zKHuEe)Z*1?^~J|4My7)Zb%HtToU+ai*jN_4-X1hPIfEA3p)NvE+N1fYaJu>4W>5b% zMHcum2%VhSR8MKU$T4qxI{6yMMx_TQhwY=T*tupX3RXP+`flVR!betnPt8QBVbQ&* zq5=P71K5wui?{F{RaulxKFi)EzH1+dc)>5Mn>e6c+od`-Oek-%WJ%Z0;B^giany-JN1RI7JSvO4t6Ux zz?}{y2Yq*W;iNAI+nOC?a98HRYjLysrkfIBcwwXt1BD9W9FV|H!7$x zf$2Fi{6hKN)2L6RKFNLb967nFvEa&*I@GZnZ*Djhq28@&NxUUN>F3>`@B8v zX#6tAsN?(@_YB^{D4~NR-|o0c)qd0(-|w)cT0rOhbK7%Ce-#B^Nvnaym8Yj*uXun>oNjd^= zUlEiekk))jtMM4e6)tXc`X+Uqrp6Rbf)>d5KYKNnJ5&S(2#uV%MJtVHehHur& zQO4`y)O^1;Mk%%6s1HWRHP&AZedBk;SjB!aJ)k=b@qKn__JA|ltwA9r+hxhxQdOt> z%dS^iT$rt@3EKQ*xkbz$wRC-6&N#E6x2k93S58r_ zcRJp0b^K(}dHe4QFdg{Dz4V&UB)Reohx)&K8IVyq%q4} zQ6{s%*|e>@Jj2F&s*mg!WGB2en|J!`vcSm=vS06%e}5a7_M|NmhS+$<1V6(su)B|# zaDQdIr?l8QHIu`KV7Nlsy4erpo3%>n_-WoNK1V)hKI&RJr1&({?ZwbL#Mo#;yqkeM zOaGYgSu57@+4~}6Y0=s>+xrvAT!GPNLhsh2uM?N?>!zWK4&*LhfEvs(&I@%=j zIsPg&({W7naujc@dF2+q8{*5>`Le=v&9UdNRU0>cbsf-vFYw{CfTEb>5MCG3qbd#*8&q zDfLZyNS}TdF&V8nWz`k1RpIR+F(W#peOjmpSL-Pqy31Y252 zh0$EBnJfm4>I}8$haF%r3B;th9D@d;Qeeu;KZVEr6keO=V#~6F6A3s3z-G~a=uT7?O`T>%x26G_d3AWkTQt?W zv=P^@>?lpzRqejtWZh?7t_`bpn=Z?X?!Rm=dM`KTx#xz8oDPV=#33tO8ch9NAM!d@#v(wu1x!^hAB_;@TcFqsypwP)lDdu zl)o9No}Fa1x)={4MsGK_jaeQ&l~!%_t=P;zQO8T+Y+d6?`s)hcuS4#o->MK^C5Gb; zMOivi_SvJ&ozoMYk~!5DNuP8_`$s>{HuzS5<}52a$ZWjW6@0#{`~1P2lU9j5T}(^o zL2qN{lhmT8UHxa7`z%HiAE$LqR1~}_EH2EpDve|6k9% zYj1l~?W(Epj~-Q=mGLycU+Tfm|F|lOW%7@o@e6$02jbD6RS_lWPv9IT z>09`)$FzuF+S|^5UY%u;iQ)-=J=<-20`s7?klQKp+0=5`S9y#lJ>NM^PGzvFPmp~c z1LTw0T&%XvH`&^!+|M5AJO9zuz0$p8@>$H@8FXUmjy#bbm)r|qcC=AHdD}$aJvNT| zHq(8YLs!>!c&u7tXKk5M);7+=bsok_N4;G11upq{ZO;}pNz+V79!uR8cmL+g`{T<8 zxzaImZbP3|HMOq&o?zXwGq>XVtLUt*Hf(JiV``(GyVZw^Uex!Qy`A2--(`Xy=j6rlTG=T_*3I>OR<8XL(AreV&Je~kYgCK_i zag|*F3`fMy;R4t^9c+Q-@9G6U&>TMs4i7^iQ8+d;h#>)uW*4Z9N1@;ZAQ%LLh;c|H z8xLYc1Ol*e2}Bs1ZXWLk9iV~QBrNvF+|YarLO?YZ4;BOjKm*(#!UG-)Z8pz&g+MrL zJTMN`5dV3+9}0k!1j+b=4zwW|A=pB}V1NSgNTAR;B_RLSJb!0CXs#c!z)@h$3-T}2 zDAJq}q5gs~=j8$17qFljk{m!V1h5(A*7*Y;jG~ZWA?z<;2NVj>E{y(f^-qkSEr1J5 z4rcndZ!q~h5{B>-5t0$w5$quh9&!=(8(>2_cCI(Kh&k~gle4z~s26Z8<__GS82}vE z0{Ggmr!D~ipiBa!3}m1@h?s&Pqv06HyMI5RA&o7d0-uHh<*^;tME9@*&58vj9Sjr} z4nEKvpiP2<&s-Y~Hp2oO3twoF;OucA4?@F}rK!&Xhdh`@LsK6GX@yM-bwCor4ieA^ z8m#xn2pR_mABY7Q0Zt7yq1p5pbSr%t%N(JnPDU8eJXwf6&Qx2P+FZ3~uG(esYyJVP ze13K8Xr>475qeYxl+~cw%0N(POu8GxipGRvAYD)p`)JnoR47}51R`RA1CVeQ2u%jv z%@r&|ALxMU!gPh^u<}Bv>4PlE-QJ2u(cFdD17|VZKq`Y!1DXUZm~eJN^owN{N7w&v zdDzDNMIQFP`fn--wOB)_|F()yi**FfkpJ=~TC5`syXU-o%!Pg3tkO3CFV0e%WSh!2g9YdV%qrul^gxdJLNT!t&>YSgbYR z$&0l14_A*_5Q6P36vW?3F{hT`1t~PEabfl9t-9}YXHwh zg1OWnRzPNTa1cwYE1;1m3~*`iLINszBtZ&^l-l--jyxGO8<3J<2{7RM^Dj6ahXXmi z4V(=_AqhwlSPb-nyUf9$Hk27aub(g=6VUi#7^LP!Ff|vIKtw!0T>izf9H!QEXD&o@Px%WMd681*7iFs35)v|us>-@_{DfABoPO0 z1B>Uw{~;e5hlMh!-}nL;xHT+>;c<)C3+@(w*aMBn;1|h^LShNXMLZxFl5!JTowqYgKR zE2(41XgrVrgCuI;HHc~?3=T_FCtwL`Xe^qz3!Hm@j{=2Rb-IE;Yakar>R xsyGq`32HP3j|7%SVKm6h% zT=xmtfOu~B87B>pjwBgD>VB>%ZTPeH>f6hZo1_l0X^ikf;B6_*WVrT6_u(0L`?sR5 z=Av#>bZsvUH_9w-JSHaQHICcUyj{t+jIF1`F+qut--!*PZ#1NvwG~5 z?tOE&I)a)$;1&|?jbEX9#QQJQx&DFi*G0j#&zmgYiyZvjIDX^#w%j)V#^h4%rr1T; zc;VjEG8&r`F5YYj!ARKZxQmW__kk1ZL<@zscv~~eK;CG53g&$-D}2hKqYZ6;3h-H1 zX1Lqo3kP5ZAAH<|IRQN>nXAwYQTh7)}byX*) zO&>s4iTNn#7O7^j*s9_Mf$@rEp_BTouH?;*hH(c7d$)`<4zebM5vnQSxH4sccO1D4 zkr{Y>953t=s!ycx(E10qN7GipTmc32?$Gow1ddffRj@F6 zVZ_Zh1kY%9s?#$|$bpj*E!5Q%Vo~I&Yx#f@#M;6i|6 zhH7QBmsR^dZ93BCLe+s!HXv~;W?0n>mDDl)i8zB0m$VVY;iV(m}cipltEqoy(D z$4UH#a|gWEMRrhE9kij@F$in&u_Ca*N$GNUtOLF-N&ELB+U|CKdVibo@pQU;nN@oL zgqx>y&6Rd|>=tiYP=tG@MogeV;u7GOX5k5i(r~FmS{+E1-TxCmL(B3x!YO4@ zOik)QYu9`TNi{^$5rTkQ7{PMI6ya!{?8E+TjplrDhEW&2QU ztIATwE}ijg(vmHY`@n!`G`3E=?y%s%ePUV$QY08&WFc9ggwyYsqOjo+t+U54?I}T{ zklHZRJ%eAtFq?W3a{g+lRQJ_}xL4t><;(`IY6n$YX5fnN$kRp?QY7o1#?0Z9(SM`O z<-_tsP%#Thp&|w&3Ksh_!Jz!N4fUNPM6hBoZVO}JLXw8b z96NDEJIm$TJ!pndc(wCNI2k`_X{5_K*G1~Y#%w(6$NxA%iT1Xl0{5kV2(^C-3HfC%e<-?{%-pYmw+(iDZoNC)yHsz$eju3rj4^>$qe`S$Do}M5N;p{1wGE zc%!U@v<@Dq$)#^P!TmI;wfH`&W>|j@R}HfwWj_~l_+m2L+C6*T9Vil7Rd<(hKwOQv(0x^PXVv0u%nNXlzQCX2QUn7i4e8&apxGeXdV6Z_; zFNr63>k}L#)one`EI*zY<0=867OV+cnq2OeOfpl!p?tm%kb?wt4!ed{p-C$(?ftQp zvMf{LSnilWT%S}>auiZ&b4k$9j|SJkhCMNeiJFvwL|jux5`c!D4KF-POhY>({>c== z9~F`|DOV*k^e1K*M5fHODYq-H=>6}Ua-Bw2pXwhuP^BI_vlM$M*QY*CrOWg`K67=w zLeDbl4Rf})=v?^#Qf(Lez{y8ABLxQd*|SPrsAu2m3?q6+<06)oB?cLFbxz73P zqM5$8kJtAh)S+n#rzH*7!f~Ze9gP^_m`!RG=7iQAlM#5g1x{w*x)z(HJS|2hN0G8; zpbhCjE99xmv0yPV8g|PlpJ8ZLyp)~O2huvD*(iH_(R1;G`IhOOtn%*sla6L@QvSUm z`TFx;1`1h#pHED;=p}XwzoT_6F^o!kHFGc0dw=2bQy2VAHuxQ8(St$6t_o+C?-yJ} zV2xsd&l+y9Gj#I!pJCf<^q$&wVHbA=gIv$jO|xb zx;{;V!^m`2Lx{MY6DIvIn^y;_z&D*Ml`J+UeXW?5O z6uM+j)hhyR8O@M(z;hakaMZ}j+ta>zW<_0t-yu^I2M3^_YAqyzN+k)bvoyR`y#Ht&E;}D)`l=<#mMs&t|_n!OC9Vf)QgVm?=Pvh%|tPoOhh1 zC)8~E!0|vO4yJ^SJzpZ2P7no`GkVa;t4uLao!IpHaM4fV@SO#m_*C368=tGKUra|9 z6SSwBiUm?nUr*@Ybz>)9dzBE=(B!Diq_yh1(q6lfihmR~hPN`%?rdLq(3K9JNZpwL zNO&i%qL^g!twh~thy#a{q>*Q5Wz=_r7YQ8EUU%uu5EG@8w<)JXbuFR`bZvLa#h4S= zKR3@VOAfy>?NHG{)yr zM^K!Sz^ZyNrK;JC+8XK{!U~$)^9O5JWdK zpL&Ut`hE5t=JeBiOu6W?S^s8HoIB@g_z?`v>btaT+yODMF_eh_;-IQtCu&f_HBzqU8N}hq@@VG^6s9L2pB>#o-v|RxZ*JF%n zVt=-xIbA449*h>dm-^k1axK0}DFS&9dV9nOM-xzKYX~`f`QvsNz`sYRI&_ubIV=cE;@*pWi!NGLs7xZNguAtWNpJV<0@ISy z&%LP?aebNWg?GvS{l3osefE@NcT4vD`5J2{h_`WN*S|3P)oFq4!(W1EaWeny{ytCx z5MzJ6#{V{mLT1#G`Q+VngcVkkdE_nl^oT6`yHh}0LRMs3JPGN8*=s9dBzK7=-o~Y&V z_M4jm{fw-NLrZozrXIR%^ds9-TobrZU;Un2=$5b;bC+FK*EwV^-bhbvCvSZSNG!K` z_|~~yWD?_kMV{%}i`3IQ-LFdV9#Y$&@Q7r^=dNaWvK&pyNJ$=-(?WO7_GVfp)vxDe zlv3z98rRdyujmwy8VK+aW)$hiB}km3u1Ubmw!^k zithzB*b0RFbiN607gmwb+nV_az!nMKVd(9$IdGtliDVDkyY1q#sIo&G+8>XKp}#y} zb^JH9SoNH2b$@5MMG!&FE#eT(?NRj4Xx8AS92B9g5kyGaW9)Q#EN9~8bKFDx?M0#9+jV*PLA>^QcXZ(ykc4}{Pfp{2J!d4B10G|$=MXn_Ima%%n*Jov&gM(z?$ zacL49gt@oB+}g_-+nwd}o#o~zw*DVS@6Ns-s&?XKjc~=K?%Yx)X5wY7tW(_7QmfjE zWB*JVE(FE(1x3*jfI(EUwqz@)abLCzR_gF9R5Xpa$oWqJoYZG4gI0diPz{o#I=*$( z&?XPZER+xuPYKhS;rdVuk$20q&*5ZP!(%d1u%Ww+5qu_tV}MHC*YqdzFo7r>nndJ@ z@{P#LU`=9AXfPxNw`}>(U(}*p`jtPzf77;SoA373L1&orbL?y=`yTF)!;vXc}^@u2C7-DCv}Xt%qvjP=v+2+!MlY#BRBs6B&drvk0kI zFpKbQvXnJ4)v<J>)p;_j`yMavsw4m;iG%!CyM^D5or+p^}(Z4G-c5K zCVm@598io9_xfu>J;nvGyBqj>zYac-{tf~ZwISvAI)rkN(^8*!BQO-WJKEv_bEbzi z0#_WiYT)P2OvWBcA!QKwW3hx>e~mz@>1lM??at8|5Qc13C-&oT@9?l$5H)FsQNf(N z#=W{dX{oCb%Pp+@0!d*UUa`qTq^VX|fSA7K5I0}z5u?|`cXYWxTxI)0lwMEE4qL?G zmmF5;l&sE+DvFfo#yuN~ zl+FR=TeigWwGcESVpsd`L$M`ePo~xdNwLq8F zRMt6qUOJO|7i?)3zhgSAl6{4MtdVQVak_*hx(LO}T7ZIc)kI|du(4FpJzlAr*zJYs zX5uOrt28FHJ6nf&t$^Q9Zu(PFzKe?u;0O}r9o-1C24};|&!5xCQb1s4Td8Bl9c{Jc zZ*RKX30m;!J=Z|ax%QE?C+%#;H(NSact+Z8Bp)GGM&=T2I!Qh{PQ84x;you{jpa5+ zMSU0DE_QVUbaia9!m;O@b>A(Vlhc}i*)Obikn#e7#qs{cdd`$*;o~Z6jvpZeV3;Ph zi{IugC+R21v(MA3QsvtR{6t#W=a{%jvfEv1P84z)A8|qjKGQ+k469yEoQY_8gK}f8R-% zDe_4Js^H+oX7Y=J@SK)p^3}?Vmp3^*o^7>{PF4sd8hFrzP;DbBMf+4K=V;Yw^40MI zs|zo_Q$0L=qtkmyM+yn==*j7SQ7qj(t*Qt0wAzp0j(}$j4FBBK$pE3Hi8Y$huZ27k z9j1~OlHqti|20d}f=fizYE2w3r7m2wX`8^(be{aPsTg>>iv!E$sH8X8Ju_JgBvsHG zg2MD4*%IQqeNJ6M{T5VBmZiCFRZ0{$zFgiEV%lHG=)R~QIQRMWuRKuXY-y4KV6Wqg zUe;>ciYaOZhE$ln^!pxrilL6KJoOT~v(=`O5a38d3*Vf);*Vz{(RQvH z=jdx$jk_Z&XFo7js#F}$)p0KZ)B}89!i1L8ASW8q%j0m@cg$cmmO8r)qJIFJvu{gn zdfsIyaDMJU@+oARXT*zmx_Kw8!BfV)zH66x`gV(P9zg_0KlrlnoiNw>>o_Y9XjdEDl++lRmkOK}oCF zTa4H`Qd)5aBd9RsL%?Kvt3fzc{~MlygrrsMYhCZ&?FiNBo@lj3j% z?ts#|?@-F(>0OcaoipLRn8M(>+XKyP!YOi+!$L+{cO*)1^BbFm!4O>TXxxDE+#)xj z0Ck=cTq3=7!hGK=vZ_nOS&LAXr3I~Y`V*K!Q_{up7W{B2Bq z*4A@aJtTz1F-j2_D~q6zU>`+(_~yeWp^5$%0e8y|fC+fnq}}>@V8?pYh&i4mFp4=U zQ}1$ChI#bWv*K<_2V`!oQ&TgxN< zmR2(hVk6PSCdS|wdvbbnU+-yuY-#g4TMStJ2+XGBb$Oa2#cq9nUDpm$r}y>h_IYnv z-~Lo1bANvd1-v5fEx(Y$ZW4XhHqS5T4EMaA_xQeVuG4RKx4fa=yE_H&*=m5dTMPwm zSaPf}5)4I~jq?W#L#imL66G`f>|DzfiVAQ)9LZ&iPZ_V`W!!A|bSz`s z&3B27(V%Zkz7 zy?uisZb??)eg(J4dAm1O(1xf6eaVvwKITbUHJ%jD0OmYwvY+U!R!0Mn+Z^=vNdA^@ zwIPJivsHBmw`_Ky6-#07K$I9^PiKf4h`OsXcS-jszkqqXhZYU-rZ9U;3!`PYBZbY3t9Q3VGp#6ew*>bvnNhYmO^J_{`v4-Et@qbbBFfshJ$&g>ZFn2@Z< zrzjb>Byb6H&d0bLPlp9uzbNcz@#8qKeTW=|ilDAtpuSszoQ1Q)6;S(kXas4KOCq}& zZdmc;(;cHGaIiH{d#cLTm|&p_(&V0dmFZcB zQU?Z?R<}-|HckmtbmVyj%gKGexE==Vy6;kMo=a?7pYXJ?>3jh3-KSP>$iCdDGO+xm z{V+sP=|!}-i^`&Yzs#W9Oss%Q=GbTlBo!Z8CFPEdpSNlXNG9@)SXy2hCXHiE&;{lQ znD3dh)Hb$0E?WTc_eH$X3E<@Emq68X8A+9Xn7R*q?7_m(>}=1+PP=7M$0_46yiVbOcJw`hZDw&$w!v48wE!h+`z6L?3oR?e*EkU=Di znr+~@OhYz5?=zah^{w|j)`;&3MDe|JiutSn%T=Rh6%jz@7|3NzdmWulWk^fHMoqGy zaHR-*$aX5pK!LZNt)|0DvR6!kPA(x}gdKXrqX+g){#l&FB1Fy6c4jUxq$Hb+%JJ4# zKHZ>ClW1^-78c&63qA@WGW3?8cLy^UqHNP6f9b>=N+3|*qE%0(3{i*hUq zLsc=LhzD#{YpX2$b(v&A-{3CJeKj~GBYOBsJ@#(aYR5EgVXLBoN<0WXleJSZ=XRaU zzzD|h+w2-zdb9ZwVDmpl$(`~vUpJ;K`d3rj&FS-f$jaUCl;;%-^mG2>>(XT{*k_GO z$9`oiACi($!{lShKw-^_Ejr;o>qI3X2MrOX8-Sufp@A)N`S~}c3L%CKl_W+q1Yat*;jmo7tT-ApvWfhb`XM-BR zUczKyNK{QhDxYA>RIx^94Ha1C59{;fai6LE85(ORH9jc*Eu&>v8E%M59pbGze4N75 zF0O>H&=u^L0hWjD;%IvbB3C}Y5#3bBCTwQu%r1!6R{1(VHZgh=CMsDOm;_at-4Wub@M!n1{=O^NfDRw0f0_i{yX{V zS)3af)Yr=&;~dK=8vokod~Bwqy&b>iq)s(d6X)M$0%1zk9h>HcEiYE+TaI)8ayDS_ z(};q*%@=OO!b=k`DP@t-ub!BTqX*0Qs(-zvKri&s0K5`@TKPZc7+N}d415JEp;+Nh zUI^eOa7$n^Y<@dlHW>@>Z9M`%J9H;9hliy($JMnu(4;5MD|@urqz@u5-i$q%y*9*K z@wqpq1I(pU8Y%h0PCe5fPygtIvwQ#(ujhOS!Q}dcib>|Mtl29)g09FKtnvU4PLH$JO< z{c7~CCM-_aDk`)#)M!OIsxX7Rk7rJUEB5VXRt4!16o$00 zZ|4IMm?lS2_)0JM5a_XgM;t1dr^Sfy&z#2@MU)^%aV7B>kL38=RPt4fy??F2qx#w% z-|t2>ej;zWp23M&kgLFuh(JBG7&o@xV494$hX{S>IuXo(KGQzlv9Nhr%7 zuneYdHh^=^A{ zxf=H=E4B$pmpu&pcn5vtrMHXm2IR8o>GfMMx*ltPFCM9F_;51vMZPE+of~~VONy!h z^Uy0#hf>HWE62&J4W!5YExa4fOe(2Ib`| z0}K!z(V%}uH(AG8Kq8k~QuLPkZe?>0wD3);IAk@HO;NlNBLcZztd~DsqpHU?9+y{S zW(2b`wQ=38|1||-_^3yw<=hf(6C!Ta=%0mItP&n~)=hlX6LKl_h4T(HFSmnWpZY2p zPojYCH>kO$-pSJ!6=>g2*{fOrU$6DG1EGMyb0kzxWbcct+~_kr48~#8uidJkiG#J{ z{6DJ7P=t-vTc%vTQxo^A#s|-TP9(l#8EutDvUe#X0@EezqCL88EJdk7J+=%QxCBv6 zbBj^d3Lrj%(9`fd3YXW76{)*_+n#Nt(5V><0A8-|k2qVrDo1|S`>{|28%=puVM@w> zwozNEt1lf3kKnD_8J}uatk_v$wBsJ5Z{j|S%V^1R4EdF}o)h%wv8jn4I=0J=lz4{z zzzGavozP$kZ+x#~a*xCKo2d+=}L%J;fRH!ZLk83G|63Tzb@mMPcRNqFL^8M!@3IyP zJ~=``GbP`jcO~0V0L!~;{V$J3GC?)qTA7d%wu>RvT+a+$6N+yN z9w3*b>X)4ikMS>E2wA4I!0u)|1%B;Mzc&ke@#NcIyq^7V03C}3kJ4$huHUZ(e7_qd znGB~&i7SX$0;f{mqSR_3CR!GX7XK0%%mgO9m9>aY-cWroEh(nFo%kn;jRz)^Yl;cN z^+>-XJeK9dEvXhzJ?x|)6~-XJa=~l)y=bVLQZ(7xkjF0+s^*DjEg7Q@8ht|el*zv6 zu9Kcxjh9LX0;EbY`{L6IKFspP&uXj{M2%(t5g65DhFhOtADn2RRGB_yNIQ-JVPweweZH!1xEP^ftF#2lO1a;SwR|%MWB0PvL zq>%2(ok3`IelL(b$lC-bDx@f~7tj^h0w56Rr?BV9=iGRqY@s;fl;b3@RN&(pYTKC& ztefdg>x7F)Trijkycx$MVBeIq%|@-cIM z3Y*#kYCXJ8VQD_sITddDwt8Vyr%eJsWsWvvo14K}ce(ERq$QkejuE(Q0EX`E!4-qzP;Sd~$nxP=DL3xe;<*ARjNJiijcD6tBSp&r(u}KCV#ad* zhMQJm$Nx6(oaB@j&gRg1p~Z3kaBdz~br!rVBNtd<>wMl`#c5294PxBi^BIvwK^3AJ zVU$Z}^Z%F;I0yi;7jC-yJQaJWU!p4)3#sEj0z9I6UHEUt*lZPGj2k!Iu}<@3C!_WJ zM!9?_yc=Ms5puNCwT*Zk;!dv!dqly47J`A*B)=u(y!xS6=!lpX`-}dRBf&# zPv!T|I<^ZRjm802_2lzKXM||m4gf)mMjh3HzcesaW=Y~XF6Hg| zMpNb0VQ(ca(`V)#tEZ=P_vrS+LYKuQOd{y%*<*!vPMe0AvKP52 z9nb=v;?1Rl=mQYi_3F0r?3`M%rxV1i5~pJ53c8cV77Ms?N{?NVU3Df~G;6lDUPZe4 z8xrZV(_Tev4S4JcNzjp z8Qx+?Yo`$&bB<+}D!*2-AGzE*fbc?Q7L5a}A)7@vemvo@`(?0BT z0RhbYLQkvj6M7HD4Hg#N8lO@joap|+ z4){fJaF|q4H{h2gUv|ZR^3am+D(MFQGm{f>~BQ zrW}&cQm~9algnK^G5>Vs7j5}&D{pf?fP+&)bzSjmP-kNzTbNWC2UwwVbkHgg2RDCE z>R%Y5c{x2>Y>{x@lY=Cy13NxRJWvznh&`98;j3*svvAxjoX-<%wJbWdEfJ|88%Uez zOpqq5K~rj&48p!-;sy?kB46cmVJ56G*-PZQ!|EsaN}w1UeQl*dU)d!P(MuIjAz|Gt zy=@>FZW7{Rw@Jg?)Dd*W-t_n2y2-UEIbqeaw%XS6QF8mAzaZ|akxdqHRU~XecI7nq zDz$SIDgA|v#C0*aHrOUDzKE0AAab&Iy7;-)Ffj13-Lf0W6h1l9_@XdlZHH5b_YMm0 z6o*BQv?xsJCj+GG%fpxeRDWRz9Y%YhFZ7`)27TLNm^A)sC?AF-3o5DGl!-bkBc zm|P7c)ZwsSQ(tr^0s`@SE)_YhE?GJd%M&e6!O~8fM!+UECJ>`ol7DK@AkO?%jpvP z>BGWSvLr}zqjgKdd%HY^gH3q(?i2Pp=Y-)u@M(vd@Dj}L{KMmxQ-77<<<~9=V;wfj z+;?LEMaIwj1Od`A5(l4gb81F&7+(h(R)nLFsi`wE{^0Vwk8#Ghl?-_v(EpN#8gf3y z9SK#s%kiR)ci~|@8H9Ch;C+9ePSc-hrR*T?FZO6}{PbXA7`;wb@ zNdCF#`g~fx9l*D}eM$i+RE#+DG(%uEu5RCZ*Z@iK70nob5=3&b8*HjS`5C87w->GclYCW zRu}`eZI#`Rn2tUjleMapc+FYoyJ2VW9wHN&yr9`7aig@G;j-w)UNLLXQ? zL3NR%06?Yl+uP;~Kj1SK|NG|da)gj(DuW&MQ7!V~7LIsnqd!ciDNtW}+N2DNj_0U#O6v8zlaV3v(stKXQC+mn*X+Avg*gIukWF++ni zc5ybL8vA9o)a77xP)l~GPmq5T(2z>OR##n{rqMOnkgn0GM!Hb5J^MRrAKmli(l%L? zI_5rTYjO_Tq=5t!KU$i2bdEUVlvW+~N*E<5CqMaN_(NmD&@QA+Nvh^B;>WY*5e}f? zM25^b4XYyXV3YcwKY}#ZJOg~m4^J`0UJ1VUs~;MZpNP*yY4o+GHH!TzAomtL+%w8G zdL1&4-L4IWgHgKP-WAmKgucIYRWU$7oal|X+{l)wIUO`4B;X3%^>{e7%qD@6qOrj@ zdB9T{N;p+iC`N`YnnP&7(YKdjNGAnL(HRV0vP& z#AUcWeb=^Jca@>raK3XPCar;emq2x%aF^rPOthCfnfp#r&*WYz=CkD-F#)?S(n+DP zmf%)lvSV<3D2AT@H~W*TZhk&ho0WMP!EDqHn~yjvQhcrf@9{+pfA)C*B15^P`KA=7 zNC)41bc`-AuTllc6JYHbB!D- zHGEe;c;gQwy;MTUcQ(Z=&u&G-{jjFk11iK5ZeKX zI88$zAk~)JOc*{?r*%l#K{lG8JR$t(5P$Mq$U0{311PisyjkYbC+tG=E+P8z270GkFZ4$V5Lzj?Uig$4n)%AhBU4lBxE zT#qf*tnaXJkE@T+_vPA?FNTKQ^gg7&*rdaZwc$3NnKW~^^)Q})Ur456HhIh`tEF;g zy;TKrXuEyB!HW6P?yo?4u-wnTT}#R1!gGPVTPD(1&GzHmn80=qjHRR+g+HFh0@8jE z{ty9tfY+0o%6`tnz+(Cf(3;3DSxHbRU_mL^HGq9rE`~`^D*Vee{Iqd&z48>Te@ZDS zy%us~HC`>B<3Cbk0K0xEU^U&{F?auh|6sd7&EDO}4Sq}<{7@fnP7)^MubRttaJ}F1 zS*D?r19b{QjnC1Ch6MPv9nv|?iC1bWvIgLzxhCmz`SzXf$hbTBG?ll9eE4Lu;yClr z=r#7`csuPVp#_?@zt7PzD8zrS$5KLB#=Bk2^#m3@YNxGUx9Ii6NUQx4--N-uf5W}- zA8IJ?W=FmI+MA`1xw1}ryV2z1kuWsi4&;X9gbG)S1P?LS?rV40G zgn~f+GBx%Xw?Q6F>w3e%L1amy45*_-jjPm_)4+jgcP_-gA%ZORGJ8d0Mf}xT6ZkDxC!U)VtR{_$58dYfi`l>%o(-* zE5UH6;*t_16xc#^QghAz5pn-_)C(Zazdo7>!^D3jHLb_e7(G8z$i}S^|C#Qfd1?1t zDt-A+?H`(cUtqUw9N_+=NG)cuV*h~XJRS5W9#Vmp_&Mk7O#;YDLHy%+ zemI{0#i!GQ1wF!7scyeXYEpR7vvDmYX9G%dk{wbF-K|DB{X==A?Yz2u@7FesvQ}(W z8!K3UNPdmROzTbHREw|}6(3nM!m0__!O`racj#4t1p=L}d)Pz3F?ZCWGl#>YrnRp0 zR^R(w!_}6!=o5m4O9flfG#_C4NSfd5WGeYz0kDOCdlBs-?`&7uV!qjE@B&KeANz`8 zHTzT69FdM?j9M4gtA>W&A?4e^!xxM69s_-}%vaf70w?oEY20iQGJH3r_Vu*p+%fj+ zgJ;X|9GJ)^duNPdPGo^#p2MRTQ282a!WI%tShN`YT2DT2-i6mGynvA7+3tet>W6zI z{?k>=+xa=Vz2EvS)cRWMTdUea%X=;IKh)=o$n776nGo1!burX>L>8SRsYQLL)VN+? z3w>$DR86klEF@5z6Xm8ZlYJ`Ff-Icb&WdUTr}xK1T;cKHeq}8N1MaMoG+|u%VU>Y$ zFwHv|zo7IPsoGzkMFC%}$C|z4#3=HJB`OaZan`oqu<3+3(8JJ-IDAqtRCO>B! zuS4L!E7ZuiiYm%Yr`v~8Q|qeO!@XT)F89PRs$f9qJj!_wMFRR|_dUT|wpz>BSDn5l z(t>DI0uzK7_thGGPit1(x~J&LGSAN)!H3`Wm|X@vX=tz}NrjvDH;Ay+{ehAdt$yW2 zV}B#44OP{W#!uI9b08SOPM(o8ao1Eovn#g|ZZg+b9c-gtN#SWpF}BlxQ=8fiEE~{k zUG^nKN(+1Ho}l6e?8)-eKlmOSHeG$(J^}Qi(!T#gvSB&@56Na{W>2vz_=yJf-@l3( znf}3R0ssdaGZP;lw3D-=iGdBYd*-I5m%GYhlm2_>(FFB+C9gKR*v!!9>(`wfG5w=RhY>yvtgYx5&ySv}8usiT&Zrs9bK3fV%Ih+(Y&y|9hX=)2rU3mn zMnX!3Yy(ccnQ+g*S!1~d4I9_c`K;`C&QWHMF_jvy?`!@5l(}8*55Z1^8^~AS?-oaw zouH1NV$+IO{kkr5+)s{~J#G)eF7qe)i=P0BQ3b$ljt3zbzV;vDkDph89e=Pdf+lO@ zHU1zHD0rDzk!;jAwpmE-?_KNEl^PiM=-NqDWXtOm{G~)P) z$+@lCc_xo1?bj-kSsG?@f4hK8*TMRGOA^`$22Unf=au3l7Px@FDlrr^6cg}MgsuTl zDXA!5cIpq?xnxPi(aL0%>Ti*<2yB%>O55&`Ck2$48LA~K=X6q~L*qyfjiYIBiK?Xg zCAKw*#Yql@qq~I)+N`4)iTCdf5GJY5s!&ygmR;c?)@Q_-tXWjX7a4^G`Czf5B(UO@ zO#k`}7!P~rBTYo@WEwz-Fyo?|VMPI4DWawO`>d#Y*}6s)+#!NJ3CRj=G7jvrth*1KGr?o1d-b?cz0 z+?6hd6yG{v(ZNch@{LnT2kQTj6aTu|7)mggA~M$O`5{Fm(x`4fEn11#c>2zp*j zI~&`|p=l-=&WLt@bKQ8tM%W49uCQBw#(h8_<}IzVS_?AUlujnrOl1LhemM~AkS(W( zFmFfpRNbB>rSxWlfbx>P@N3P}g{Z;;K0P1o3nReC*)&L_F{S6Z1~}HzKDybML14u( z%(!?+641a=`(u%S%&H6r3@}I5iZ{Zc{F7Ell_?Y6QA>1KvvtqZYAXPol`fJl%9)GM zjZGzw{jXI^ z!E%XA!k)q;eT$j>QHFL^;u5X^+ZuXuHBP5NTjhUTQs}^7>SSJhB1deX5qMzL`}Wg>Al- znpmiZAxDCiI&zp`51o2Hp?3w^CJPop+H*}RyAse+8k=(=LqUJGr*a=F5Ig#eEy@S!$4Fq`*S{Jvv&tF`%}+%18-8U24hL%AS0gf4U-(J9QChn@(1bt z1CwG66=mT>fdZ_DOP6_v1%=b>?S&llx(!l(ewz*=>05GUm?bUMv`G-MQLZk7&Xw*f6V2V;vU(G59m?1A|dKe^$?#{ zk|IfRp1>spi((=T6^N>9v|Oi=n~y)e%pfpa_PmpIJOg!He)$PR{FHaUUn9-9yxH_I z^+s0S?&aM8Ov|wwQ}`dp{c=}6Q7r*xz?8NMfe-}&*Hvp*MTGh-x`z1%vJPE3FEX(#+~7=O5;s1E{*SiT3uev^vCxn!6+(XDD8&_P~OM zhDL*|03O;5n)Qz7vbzmTr=y9#w8v+EP0)%O#>dCUPSRW+PdM3P>QdnwK!GJ(H#g~h z+^v-W>v@%>%cmeB+lt;ZVWcIvB#$^nmGFH5(V>@*NHb(E5xKJUOZqfjE^6+QILad7 zlJiCZj?F9QmW=40_pyv~^l$4^T5T`4>kW<9quJfhl<$*fs~YN)m8S}BiLI%wK3!($ z3~dngsTy<5dHeNGmzHH3ojCrU&c-K5j_()138yXin|T0iDv~`{Zzbqvp-dn*Dx-$x zd8=EJ7UJ+nJ&0TuWm7;8SQzsm-rHc60{{C-wwoPEaqJd(^8NIM*Td}RJK*6&k4*YA z<86(=Ae(?Jp`<(C_w#JsuFLi9*(@P!*K!7w92JtO*8*9zOZnP0?Lm$DcZ5b21k4G5 zQick6i|MwHZ2vMc+soH@l{|u9EO3M0ezZ)_M8DyB9?z_Glp@mDh;kE?ZS-_YXg^Tt zH*)5D7VXJBZc>Piqb6=cLi?xbgaCE6sF?567&&|2W0WdQ_XrtGvwrnpuAZsqf7NlF zQB5st)F4__3`E}Nw{nhzq)>(7r%%_pL_IS6N#NPY;V;|3xSNcMe*uOBY z56yw{$Ww$oiag;pYR6`Vn{ZK55E9qzp;e=s#SiR1QV{ZzNOxy8e>NztB{=ZaU8wCc!PE618+-CkqBop z-eJgZ@t*tr2Adi}fkf&$7Ar$TT05r9)J-)qdRMy{PvWmSW|}c3ORa*OoJQ6)JEwcS z>&el&s8@aH&6sM_yUClF!o?Z6xt)y7KxPYkH0An5-lsI)@3~UYy1R*PuO6l5NSVyV zRt)BL?m>Y#y^ODf!ViMR%YEQsO9zVUK3;P1v==#AH{879nC2`h6;r=9NQqWe@m!d( zq?Y((%SZ*o^X*!{{2`+J#$;iM>9`yeQlAy!J&^|2wJ?qzrtZW3`^$PChAdxaM?5^eFbL zk(iB0uQ@UBKwG|Y5=K|*dVh4rzKnYEi~96p-6PM%X;gk|hW5$zwHFZ8)xaAop>HiF z#VvHCZ^tM%pIoq6@MY>6X?W=J%9@;!l7lm5AOp5l`_ z`hrXkpVK~zXdXBtoxGx@QeLo7n4lS5U+la}N<4R_e8p?5UbJ|Q!m&2LA(h-{IVBwFGv zD2X@8nc98byLoRrC@svv${!_R^{&(;hl7J#s(IEc6Rk?wAt1;3Hi1$QML+cEaNeB6 zf-`;7c2<*XBiBN&d3*H9K*m0@fBZ2X{ld_}Gy3ICDmbgf#v`;|9py_1QnOw!V({OmV(OB{b;H!HF= z_Fe0(DYM*(QR1-uakvX_O$T=oD9tPzEhO}~+X)t0!L=3^VVj*EA3g*NjG8q0oXEnC zA@fiwc}u*vFw^c+$D7ECpNXW>t=8EI3!_07k%^S z*cf=%X@ydvV2ZCxU21fKSx;B4|)g4muI&LWQSb|D$*>#m3sIO>|Y0sdQ>IG$!?$b8j@FZ35 z5P7(GlrE|1(B2&h1Y8#`VM}<1XU^~spZc51R=vX zq{(-k!0;|gS}?s#cj`pa$!CeIzUM*FD)T#2T?cJ8`sC7@RkMW096Db2dbf5TfdW3? zU-GLoHRwf$faT?w-tvsE+L(vOUQ90E%*u4JmQ6hVNI~;5jBJ`FFyAA#aThP-SQ{MX z<2SRT@dD!b=q?^7F6}Sd?nV%14Vb0%-nQZGH`3;PeQo$ZRCK2b&EDs6$l2!4F+UG{ z$}{M>8u%&{<0s|YVNWtiP0H}kO=&hd91%`y#qVjVvW#@v-(6H>;CieTNX|t8@0p4( zDQ{F{-%$i>qsa85Cn`+`*CS=#dQA<+VzoOl1$>B+$9OqMv&ArFlLj7XJ%h4PASG|K zKX-ZP!wN~&G``T^W*oBb#EY`bjUit#7o9;9&g%-M<+I}s>MgJB#>ejQ@>gq zc(NJPCzX{Yr&*Tjv*c-eRbRd5PBHJv)tSXRlT=|!iYM8^b129;k@aBSnf--mgQ;_6 zuQED#vIs)bt9i6!8%62cXV2UXiW%_htCQM4ARkeperb1h|ILT_CHlNP2yb)Q`S!IE z-_a5!L=~VJ1vG{i_xq$uKfv`LW+}_x=LEF6LMcQdp<=>oe<+1NN=abJzIgHSU}2i6 zrP8*TZgT$38vb;%qF6f$N?riij!9hpQzi$$dk9K)*Y1ia^(R) zqLk#(W*b_Kn&0z@A0?{=FlN*p)IK6SloCb@A{U8(BWGkaf5e5C{*j|09|ZM?LtzxM z{8}(MGwvJgi%S24P*GhcY{C`cXWHsbJ)KR}@6Ogg77J!Il!<)KN^&jsRTe}SC>W3I z&hyDfWjHq1k2Ty~pO2yC{UJ);Ores9$b;FRt=3rM8D);Kt6Zbh@>fq}6nZ@ReSKSG zOQOyTKywZBZmd@+YcHN!c2BgNvn!2O-p%kLw_dBuN;MT5i#PHyyHc@7bq<50PfDUwnfl{jq^;A=(j+$K?PGaY<17+E8UkyJZ6eG zJeKpWXqbk037^icdtB4CqkmQ(wnIIcbhN(IJKR_# zKz37J;vkKr&3#&7EE})gs>JtSo4Kv1U&vYh!n^)5d|sV@SXFSOzR%5q)3pCaQ=i=Z zhOfHz8_R2QHFoOIi~O>26?&Er=w^?&7i8l!6VA)VF@l~!a6I5jimdsq+A<_AGWAbE7Nfc4?)L5eb`p)q1RF$#&xgVJK-=-3>1 zrfl4;J%m#c0`ER{#nHWUZWW<%kjs^QC(ZS|D?zI?BO4qT^%X3;JA*e$;cwF9oTZ#}l!hmih8u#TZjY?l!Nap5^ke ztlVXfOR|zhUQIb|wir!+d=-POXV_5MK#^x)P|6y;AXjZ z;K7YM_JP6|yfRmRE2r5dMv4Ll8|J12aJzK3t!K<#9nDI6YXkr@wr z(EG^q+`M^Xeimbd`&37EU9`DA*{SDPa#srFYnHB4K6UMKym5JIXUNjx?3?O_X%c?8 z)6QW2zS@}ZOLN=>p0UFJbV8=O6PXt2hTZ8aPZst2ELZc*s`GWSHM;Gad6190g%WTb zF18&8_vXbOL8CdD4LS;H%7)GQ9)e3ryNVHyB{7u-IvdWLy@4Fc3uQ{r$+)A&7A4*; zk-o=Y8Dz$@2G1=<+;o|y?1Uu*s1Q-TIV5m)YXAi<) zY_btIdsq7JJ27C0JhXi#hy#YCMJ9kmsM`rt4iuXDh@f@|2LC~ZoFkogBifPd9l$We z4}`Xv3=&ku!@#hu8#R7ahh`&x-LZiuxp{$MTj#xvxFKgfNOr~!UKWtET6oAg2R|>G zJ{=^>2ZL^PLyR0eN!}iI4xV5bn)dk_$cWZX2kiqPXo=52NB$qlkn4we>eD>}wDf17 z{r{Jzw{^VS=;xq+I&ZE?@@BgUhv11Go@{rwHua4l1~%+x;P+%>k16U;6)vX40`==K`upR#eg92DI zdt`sbFbE90(0{=I7#zzUqF*p51_@=C?JpP%0pF^7D4C7^WEhKLm*&5T@ZXF;p%`{e z{K8{r;fF#t_&{;5L?ic!~42hb=i641b$ Date: Mon, 11 Mar 2013 11:42:23 -0700 Subject: [PATCH 018/486] Added missing prototype --- apps/commander/commander.c | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index aaabe7f4b2..28f0999910 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -172,6 +172,9 @@ static void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) static void do_mag_calibration(int status_pub, struct vehicle_status_s *status); static void do_rc_calibration(int status_pub, struct vehicle_status_s *status); static void do_accel_calibration(int status_pub, struct vehicle_status_s *status); +static void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status); + + static void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd); int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state); @@ -801,7 +804,7 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) close(sub_sensor_combined); } -void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status) +static void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status) { /* announce change */ From 6039582928e199976074a4033336316c65737b66 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 22 Mar 2013 09:04:08 -0700 Subject: [PATCH 019/486] Compile error after merge fixed --- apps/controllib/fixedwing.cpp | 26 ++++++++++++-------------- 1 file changed, 12 insertions(+), 14 deletions(-) diff --git a/apps/controllib/fixedwing.cpp b/apps/controllib/fixedwing.cpp index 9edb84723a..56b42253e6 100644 --- a/apps/controllib/fixedwing.cpp +++ b/apps/controllib/fixedwing.cpp @@ -345,23 +345,21 @@ void BlockMultiModeBacksideAutopilot::update() #warning should be base on flags } else if (_status.navigation_state == NAVIGATION_STATE_MANUAL) { -// if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { #warning fix the different manual modes - _actuators.control[CH_AIL] = _manual.roll; - _actuators.control[CH_ELV] = _manual.pitch; - _actuators.control[CH_RDR] = _manual.yaw; - _actuators.control[CH_THR] = _manual.throttle; -#warning please check whether this flag makes sense - } else if (_status.flag_control_attitude_enabled) { + _actuators.control[CH_AIL] = _manual.roll; + _actuators.control[CH_ELV] = _manual.pitch; + _actuators.control[CH_RDR] = _manual.yaw; + _actuators.control[CH_THR] = _manual.throttle; +#warning should be based on flags + } else if (_status.navigation_state == NAVIGATION_STATE_SEATBELT) { - _stabilization.update(_manual.roll, _manual.pitch, _manual.yaw, - _att.rollspeed, _att.pitchspeed, _att.yawspeed); + _stabilization.update(_manual.roll, _manual.pitch, _manual.yaw, + _att.rollspeed, _att.pitchspeed, _att.yawspeed); - _actuators.control[CH_AIL] = _stabilization.getAileron(); - _actuators.control[CH_ELV] = _stabilization.getElevator(); - _actuators.control[CH_RDR] = _stabilization.getRudder(); - _actuators.control[CH_THR] = _manual.throttle; - } + _actuators.control[CH_AIL] = _stabilization.getAileron(); + _actuators.control[CH_ELV] = _stabilization.getElevator(); + _actuators.control[CH_RDR] = _stabilization.getRudder(); + _actuators.control[CH_THR] = _manual.throttle; } // update all publications From b1e2011fcc068709574ddaab3d8bc831abcb7de8 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 22 Mar 2013 10:36:23 -0700 Subject: [PATCH 020/486] Added feedback before rebooting --- apps/systemcmds/reboot/reboot.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/apps/systemcmds/reboot/reboot.c b/apps/systemcmds/reboot/reboot.c index 47d3cd9486..cc380f94bd 100644 --- a/apps/systemcmds/reboot/reboot.c +++ b/apps/systemcmds/reboot/reboot.c @@ -47,6 +47,9 @@ __EXPORT int reboot_main(int argc, char *argv[]); int reboot_main(int argc, char *argv[]) { + printf("Rebooting now...\n"); + fflush(stdout); + usleep(5000); up_systemreset(); } From 3665d7b86fdbd8489099dafb436aaddcb816efde Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 25 Mar 2013 14:53:54 -0700 Subject: [PATCH 021/486] Improved command handling, added a low priority task and various fixes --- apps/commander/commander.c | 608 +++++++++++++------------- apps/commander/state_machine_helper.c | 16 + 2 files changed, 312 insertions(+), 312 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 388ba19642..27abb9d45c 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -143,16 +143,26 @@ static int daemon_task; /**< Handle of daemon task / thread */ static struct vehicle_status_s current_status; static orb_advert_t stat_pub; -/* XXX ? */ -// static uint16_t nofix_counter = 0; -// static uint16_t gotfix_counter = 0; - -/* XXX ? */ +/* timout until lowlevel failsafe */ static unsigned int failsafe_lowlevel_timeout_ms; +/* tasks waiting for low prio thread */ +enum { + LOW_PRIO_TASK_NONE = 0, + LOW_PRIO_TASK_PARAM_SAVE, + LOW_PRIO_TASK_PARAM_LOAD, + LOW_PRIO_TASK_GYRO_CALIBRATION, + LOW_PRIO_TASK_MAG_CALIBRATION, + LOW_PRIO_TASK_ALTITUDE_CALIBRATION, + LOW_PRIO_TASK_RC_CALIBRATION, + LOW_PRIO_TASK_ACCEL_CALIBRATION, + LOW_PRIO_TASK_AIRSPEED_CALIBRATION +} low_prio_task; + /* pthread loops */ static void *orb_receive_loop(void *arg); +static void *commander_low_prio_loop(void *arg); __EXPORT int commander_main(int argc, char *argv[]); @@ -161,18 +171,23 @@ __EXPORT int commander_main(int argc, char *argv[]); */ int commander_thread_main(int argc, char *argv[]); -static int buzzer_init(void); -static void buzzer_deinit(void); -static int led_init(void); -static void led_deinit(void); -static int led_toggle(int led); -static int led_on(int led); -static int led_off(int led); -static void do_gyro_calibration(int status_pub, struct vehicle_status_s *status); -static void do_mag_calibration(int status_pub, struct vehicle_status_s *status); -static void do_rc_calibration(int status_pub, struct vehicle_status_s *status); -static void do_accel_calibration(int status_pub, struct vehicle_status_s *status); -static void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status); +int buzzer_init(void); +void buzzer_deinit(void); +int led_init(void); +void led_deinit(void); +int led_toggle(int led); +int led_on(int led); +int led_off(int led); +void tune_error(void); +void tune_positive(void); +void tune_neutral(void); +void tune_negative(void); +void do_reboot(void); +void do_gyro_calibration(void); +void do_mag_calibration(void); +void do_rc_calibration(void); +void do_accel_calibration(void); +void do_airspeed_calibration(void); static void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd); @@ -184,7 +199,7 @@ int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, u /** * Print the correct usage. */ -static void usage(const char *reason); +void usage(const char *reason); /** * Sort calibration values. @@ -196,7 +211,7 @@ static void usage(const char *reason); */ // static void cal_bsort(float a[], int n); -static int buzzer_init() +int buzzer_init() { buzzer = open("/dev/tone_alarm", O_WRONLY); @@ -208,13 +223,13 @@ static int buzzer_init() return 0; } -static void buzzer_deinit() +void buzzer_deinit() { close(buzzer); } -static int led_init() +int led_init() { leds = open(LED_DEVICE_PATH, 0); @@ -231,12 +246,12 @@ static int led_init() return 0; } -static void led_deinit() +void led_deinit() { close(leds); } -static int led_toggle(int led) +int led_toggle(int led) { static int last_blue = LED_ON; static int last_amber = LED_ON; @@ -248,59 +263,50 @@ static int led_toggle(int led) return ioctl(leds, ((led == LED_BLUE) ? last_blue : last_amber), led); } -static int led_on(int led) +int led_on(int led) { return ioctl(leds, LED_ON, led); } -static int led_off(int led) +int led_off(int led) { return ioctl(leds, LED_OFF, led); } -enum AUDIO_PATTERN { - AUDIO_PATTERN_ERROR = 2, - AUDIO_PATTERN_NOTIFY_POSITIVE = 3, - AUDIO_PATTERN_NOTIFY_NEUTRAL = 4, - AUDIO_PATTERN_NOTIFY_NEGATIVE = 5, - AUDIO_PATTERN_NOTIFY_CHARGE = 6 -}; -int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state) +void tune_error() { - -#warning add alarms for state transitions - /* Trigger alarm if going into any error state */ -// if (((new_state == SYSTEM_STATE_GROUND_ERROR) && (old_state != SYSTEM_STATE_GROUND_ERROR)) || -// ((new_state == SYSTEM_STATE_MISSION_ABORT) && (old_state != SYSTEM_STATE_MISSION_ABORT))) { -// ioctl(buzzer, TONE_SET_ALARM, 0); -// ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_ERROR); -// } - - /* Trigger neutral on arming / disarming */ -// if (((new_state == SYSTEM_STATE_GROUND_READY) && (old_state != SYSTEM_STATE_GROUND_READY))) { -// ioctl(buzzer, TONE_SET_ALARM, 0); -// ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_NOTIFY_NEUTRAL); -// } - - /* Trigger Tetris on being bored */ - - return 0; + ioctl(buzzer, TONE_SET_ALARM, 2); } -void tune_confirm(void) +void tune_positive() { ioctl(buzzer, TONE_SET_ALARM, 3); } -void tune_error(void) +void tune_neutral() { - /* XXX change alarm to 2 tones*/ ioctl(buzzer, TONE_SET_ALARM, 4); } -void do_rc_calibration(int status_pub, struct vehicle_status_s *status) +void tune_negative() { + ioctl(buzzer, TONE_SET_ALARM, 5); +} + +void do_reboot() +{ + mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); + usleep(500000); + up_systemreset(); + /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ +} + + +void do_rc_calibration() +{ + mavlink_log_info(mavlink_fd, "trim calibration starting"); + if (current_status.offboard_control_signal_lost) { mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal."); return; @@ -311,7 +317,6 @@ void do_rc_calibration(int status_pub, struct vehicle_status_s *status) orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp); /* set parameters */ - float p = sp.roll; param_set(param_find("TRIM_ROLL"), &p); p = sp.pitch; @@ -320,22 +325,21 @@ void do_rc_calibration(int status_pub, struct vehicle_status_s *status) param_set(param_find("TRIM_YAW"), &p); /* store to permanent storage */ - /* auto-save to EEPROM */ + /* auto-save */ int save_ret = param_save_default(); if (save_ret != 0) { mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed"); } + tune_positive(); + mavlink_log_info(mavlink_fd, "trim calibration done"); } -void do_mag_calibration(int status_pub, struct vehicle_status_s *status) +void do_mag_calibration() { - - /* set to mag calibration mode */ - // status->flag_preflight_mag_calibration = true; - // state_machine_publish(status_pub, status, mavlink_fd); + mavlink_log_info(mavlink_fd, "mag calibration starting, hold still"); int sub_mag = orb_subscribe(ORB_ID(sensor_mag)); struct mag_report mag; @@ -350,15 +354,6 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) /* limit update rate to get equally spaced measurements over time (in ms) */ orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); - // XXX old cal - // * FLT_MIN is not the most negative float number, - // * but the smallest number by magnitude float can - // * represent. Use -FLT_MAX to initialize the most - // * negative number - - // float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX}; - // float mag_min[3] = {FLT_MAX, FLT_MAX, FLT_MAX}; - int fd = open(MAG_DEVICE_PATH, O_RDONLY); /* erase old calibration */ @@ -404,10 +399,6 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) return; } - tune_confirm(); - sleep(2); - tune_confirm(); - while (hrt_absolute_time() < calibration_deadline && calibration_counter < calibration_maxcount) { @@ -421,9 +412,9 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) axis_index++; char buf[50]; - sprintf(buf, "Please rotate around %c", axislabels[axis_index]); + sprintf(buf, "please rotate around %c", axislabels[axis_index]); mavlink_log_info(mavlink_fd, buf); - tune_confirm(); + tune_neutral(); axis_deadline += calibration_interval / 3; } @@ -559,28 +550,19 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) mavlink_log_info(mavlink_fd, "mag calibration done"); - tune_confirm(); - sleep(2); - tune_confirm(); - sleep(2); + tune_positive(); /* third beep by cal end routine */ } else { mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)"); } - /* disable calibration mode */ - // status->flag_preflight_mag_calibration = false; - // state_machine_publish(status_pub, status, mavlink_fd); - close(sub_mag); } -void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) +void do_gyro_calibration() { - /* set to gyro calibration mode */ - // status->flag_preflight_gyro_calibration = true; - // state_machine_publish(status_pub, status, mavlink_fd); + mavlink_log_info(mavlink_fd, "gyro calibration starting, hold still"); const int calibration_count = 5000; @@ -631,10 +613,6 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) gyro_offset[1] = gyro_offset[1] / calibration_count; gyro_offset[2] = gyro_offset[2] / calibration_count; - /* exit gyro calibration mode */ - // status->flag_preflight_gyro_calibration = false; - // state_machine_publish(status_pub, status, mavlink_fd); - if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) { if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0])) @@ -671,10 +649,7 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) // mavlink_log_info(mavlink_fd, buf); mavlink_log_info(mavlink_fd, "gyro calibration done"); - tune_confirm(); - sleep(2); - tune_confirm(); - sleep(2); + tune_positive(); /* third beep by cal end routine */ } else { @@ -684,14 +659,10 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) close(sub_sensor_combined); } -void do_accel_calibration(int status_pub, struct vehicle_status_s *status) +void do_accel_calibration() { - /* announce change */ - - mavlink_log_info(mavlink_fd, "keep it level and still"); - /* set to accel calibration mode */ - // status->flag_preflight_accel_calibration = true; - // state_machine_publish(status_pub, status, mavlink_fd); + /* give directions */ + mavlink_log_info(mavlink_fd, "accel calibration starting, keep it level"); const int calibration_count = 2500; @@ -787,28 +758,19 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) //mavlink_log_info(mavlink_fd, buf); mavlink_log_info(mavlink_fd, "accel calibration done"); - tune_confirm(); - sleep(2); - tune_confirm(); - sleep(2); - /* third beep by cal end routine */ + tune_positive(); } else { mavlink_log_info(mavlink_fd, "accel calibration FAILED (NaN)"); } - /* exit accel calibration mode */ - // status->flag_preflight_accel_calibration = false; - // state_machine_publish(status_pub, status, mavlink_fd); - close(sub_sensor_combined); } -static void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status) +void do_airspeed_calibration() { - /* announce change */ - - mavlink_log_info(mavlink_fd, "keep it still"); + /* give directions */ + mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still"); const int calibration_count = 2500; @@ -857,11 +819,7 @@ static void do_airspeed_calibration(int status_pub, struct vehicle_status_s *sta //mavlink_log_info(mavlink_fd, buf); mavlink_log_info(mavlink_fd, "airspeed calibration done"); - tune_confirm(); - sleep(2); - tune_confirm(); - sleep(2); - /* third beep by cal end routine */ + tune_positive(); } else { mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); @@ -870,16 +828,11 @@ static void do_airspeed_calibration(int status_pub, struct vehicle_status_s *sta close(sub_differential_pressure); } - - void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd) { /* result of the command */ uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; - /* announce command handling */ - tune_confirm(); - /* request to set different system mode */ switch (cmd->command) { case VEHICLE_CMD_DO_SET_MODE: @@ -935,12 +888,16 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request for an autopilot reboot */ if ((int)cmd->param1 == 1) { if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_REBOOT, mavlink_fd)) { - /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ + /* reboot is executed later, after positive tune is sent */ result = VEHICLE_CMD_RESULT_ACCEPTED; + tune_positive(); + /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ + do_reboot(); } else { /* system may return here */ result = VEHICLE_CMD_RESULT_DENIED; + tune_negative(); } } } @@ -971,234 +928,155 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta // /* preflight calibration */ case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { - bool handled = false; /* gyro calibration */ if ((int)(cmd->param1) == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { - - result = VEHICLE_CMD_RESULT_ACCEPTED; - - mavlink_log_info(mavlink_fd, "starting gyro cal"); - tune_confirm(); - do_gyro_calibration(status_pub, ¤t_status); - mavlink_log_info(mavlink_fd, "finished gyro cal"); - tune_confirm(); - // XXX if this fails, go to ERROR - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } } else { - result = VEHICLE_CMD_RESULT_DENIED; + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } - - handled = true; } /* magnetometer calibration */ if ((int)(cmd->param2) == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { - - result = VEHICLE_CMD_RESULT_ACCEPTED; - - mavlink_log_info(mavlink_fd, "starting mag cal"); - tune_confirm(); - do_mag_calibration(status_pub, ¤t_status); - mavlink_log_info(mavlink_fd, "finished mag cal"); - tune_confirm(); - // XXX if this fails, go to ERROR - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } } else { - result = VEHICLE_CMD_RESULT_DENIED; + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } - - handled = true; } +#if 0 /* zero-altitude pressure calibration */ if ((int)(cmd->param3) == 1) { - /* transition to calibration state */ - // XXX add this again - // if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) - // && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { - // - // // XXX implement this - // mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented"); - // tune_confirm(); - // - // /* back to standby state */ - // do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); - // do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); - // - // } else { - // mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration"); - // result = VEHICLE_CMD_RESULT_DENIED; - // } - handled = true; + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } } +#endif +#if 0 /* trim calibration */ if ((int)(cmd->param4) == 1) { - /* transition to calibration state */ - // XXX add this again - // if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) - // && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { - // mavlink_log_info(mavlink_fd, "starting trim cal"); - // tune_confirm(); - // do_rc_calibration(status_pub, ¤t_status); - // mavlink_log_info(mavlink_fd, "finished trim cal"); - // tune_confirm(); - // - // /* back to standby state */ - // do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); - // do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); - // - // result = VEHICLE_CMD_RESULT_ACCEPTED; - // - // } else { - // mavlink_log_critical(mavlink_fd, "REJECTING trim cal"); - // result = VEHICLE_CMD_RESULT_DENIED; - // } - handled = true; + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } } +#endif /* accel calibration */ if ((int)(cmd->param5) == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { - - result = VEHICLE_CMD_RESULT_ACCEPTED; - - mavlink_log_info(mavlink_fd, "starting acc cal"); - tune_confirm(); - do_accel_calibration(status_pub, ¤t_status); - mavlink_log_info(mavlink_fd, "finished acc cal"); - tune_confirm(); - // XXX what if this fails - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } } else { - result = VEHICLE_CMD_RESULT_DENIED; + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } - - - handled = true; } /* airspeed calibration */ if ((int)(cmd->param6) == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { - - result = VEHICLE_CMD_RESULT_ACCEPTED; - - mavlink_log_info(mavlink_fd, "starting airspeed cal"); - tune_confirm(); - do_airspeed_calibration(status_pub, ¤t_status); - mavlink_log_info(mavlink_fd, "finished airspeed cal"); - tune_confirm(); - // XXX if this fails, go to ERROR - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } } else { - result = VEHICLE_CMD_RESULT_DENIED; + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } - - handled = true; - } - - /* none found */ - if (!handled) { - //warnx("refusing unsupported calibration request\n"); - mavlink_log_critical(mavlink_fd, "CMD refusing unsup. calib. request"); - result = VEHICLE_CMD_RESULT_UNSUPPORTED; } } break; case VEHICLE_CMD_PREFLIGHT_STORAGE: { - if (current_status.flag_fmu_armed && - ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) { - /* do not perform expensive memory tasks on multirotors in flight */ - // XXX this is over-safe, as soon as cmd is in low prio thread this can be allowed - mavlink_log_info(mavlink_fd, "REJECTING save cmd while multicopter armed"); + if (((int)(cmd->param1)) == 0) { + + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } - // XXX move this to LOW PRIO THREAD of commander app - /* Read all parameters from EEPROM to RAM */ - if (((int)(cmd->param1)) == 0) { + } else if (((int)(cmd->param1)) == 1) { - /* read all parameters from EEPROM to RAM */ - int read_ret = param_load_default(); - - if (read_ret == OK) { - //warnx("[mavlink pm] Loaded EEPROM params in RAM\n"); - mavlink_log_info(mavlink_fd, "OK loading params from"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else if (read_ret == 1) { - mavlink_log_info(mavlink_fd, "OK no changes in"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - if (read_ret < -1) { - mavlink_log_info(mavlink_fd, "ERR loading params from"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - - } else { - mavlink_log_info(mavlink_fd, "ERR no param file named"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - } - - result = VEHICLE_CMD_RESULT_FAILED; - } - - } else if (((int)(cmd->param1)) == 1) { - - /* write all parameters from RAM to EEPROM */ - int write_ret = param_save_default(); - - if (write_ret == OK) { - mavlink_log_info(mavlink_fd, "OK saved param file"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - if (write_ret < -1) { - mavlink_log_info(mavlink_fd, "ERR params file does not exit:"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - - } else { - mavlink_log_info(mavlink_fd, "ERR writing params to"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - } - - result = VEHICLE_CMD_RESULT_FAILED; - } - - } else { - mavlink_log_info(mavlink_fd, "[pm] refusing unsupp. STOR request"); - result = VEHICLE_CMD_RESULT_UNSUPPORTED; - } + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } } - break; + } break; default: { mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command"); result = VEHICLE_CMD_RESULT_UNSUPPORTED; - /* announce command rejection */ - ioctl(buzzer, TONE_SET_ALARM, 4); } break; } @@ -1207,10 +1085,12 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if (result == VEHICLE_CMD_RESULT_FAILED || result == VEHICLE_CMD_RESULT_DENIED || result == VEHICLE_CMD_RESULT_UNSUPPORTED) { - ioctl(buzzer, TONE_SET_ALARM, 5); + + tune_negative(); } else if (result == VEHICLE_CMD_RESULT_ACCEPTED) { - tune_confirm(); + + tune_positive(); } /* send any requested ACKs */ @@ -1324,8 +1204,7 @@ float battery_remaining_estimate_voltage(float voltage) return ret; } -static void -usage(const char *reason) +void usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); @@ -1405,6 +1284,7 @@ int commander_thread_main(int argc, char *argv[]) /* pthreads for command and subsystem info handling */ // pthread_t command_handling_thread; pthread_t subsystem_info_thread; + pthread_t commander_low_prio_thread; /* initialize */ if (led_init() != 0) { @@ -1477,6 +1357,16 @@ int commander_thread_main(int argc, char *argv[]) pthread_attr_setstacksize(&subsystem_info_attr, 2048); pthread_create(&subsystem_info_thread, &subsystem_info_attr, orb_receive_loop, ¤t_status); + pthread_attr_t commander_low_prio_attr; + pthread_attr_init(&commander_low_prio_attr); + pthread_attr_setstacksize(&commander_low_prio_attr, 2048); + + struct sched_param param; + /* low priority */ + param.sched_priority = SCHED_PRIORITY_DEFAULT - 50; + (void)pthread_attr_setschedparam(&commander_low_prio_attr, ¶m); + pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL); + /* Start monitoring loop */ uint16_t counter = 0; @@ -1487,12 +1377,14 @@ int commander_thread_main(int argc, char *argv[]) bool critical_battery_voltage_actions_done = false; uint8_t low_voltage_counter = 0; uint16_t critical_voltage_counter = 0; - int16_t mode_switch_rc_value; float bat_remain = 1.0f; uint16_t stick_off_counter = 0; uint16_t stick_on_counter = 0; + /* XXX what exactly is this for? */ + uint64_t last_print_time = 0; + /* Subscribe to manual control data */ int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); struct manual_control_setpoint_s sp_man; @@ -1562,7 +1454,9 @@ int commander_thread_main(int argc, char *argv[]) thread_running = true; uint64_t start_time = hrt_absolute_time(); - uint64_t failsave_ll_start_time = 0; + + /* XXX use this! */ + //uint64_t failsave_ll_start_time = 0; bool state_changed = true; bool param_init_forced = true; @@ -1727,7 +1621,7 @@ int commander_thread_main(int argc, char *argv[]) } else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 0)) { /* For less than 20%, start be slightly annoying at 1 Hz */ ioctl(buzzer, TONE_SET_ALARM, 0); - tune_confirm(); + tune_positive(); } else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 2)) { ioctl(buzzer, TONE_SET_ALARM, 0); @@ -1916,7 +1810,7 @@ int commander_thread_main(int argc, char *argv[]) /* mark home position as set */ home_position_set = true; - tune_confirm(); + tune_positive(); } } @@ -2206,13 +2100,19 @@ int commander_thread_main(int argc, char *argv[]) current_status.rc_signal_lost_interval = 0; } else { - static uint64_t last_print_time = 0; /* print error message for first RC glitch and then every 5 s / 5000 ms) */ if (!current_status.rc_signal_cutting_off || ((hrt_absolute_time() - last_print_time) > 5000000)) { /* only complain if the offboard control is NOT active */ current_status.rc_signal_cutting_off = true; mavlink_log_critical(mavlink_fd, "CRITICAL - NO REMOTE SIGNAL!"); + + if (!current_status.rc_signal_cutting_off) { + printf("Reason: not rc_signal_cutting_off\n"); + } else { + printf("last print time: %llu\n", last_print_time); + } + last_print_time = hrt_absolute_time(); } @@ -2270,7 +2170,7 @@ int commander_thread_main(int argc, char *argv[]) current_status.flag_control_manual_enabled = false; current_status.flag_control_offboard_enabled = true; state_changed = true; - tune_confirm(); + tune_positive(); mavlink_log_critical(mavlink_fd, "DETECTED OFFBOARD SIGNAL FIRST"); @@ -2278,7 +2178,7 @@ int commander_thread_main(int argc, char *argv[]) if (current_status.offboard_control_signal_lost) { mavlink_log_critical(mavlink_fd, "RECOVERY OFFBOARD CONTROL"); state_changed = true; - tune_confirm(); + tune_positive(); } } @@ -2298,7 +2198,6 @@ int commander_thread_main(int argc, char *argv[]) } } else { - static uint64_t last_print_time = 0; /* print error message for first RC glitch and then every 5 s / 5000 ms) */ if (!current_status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) { @@ -2314,7 +2213,7 @@ int commander_thread_main(int argc, char *argv[]) if (current_status.offboard_control_signal_lost_interval > 100000) { current_status.offboard_control_signal_lost = true; current_status.failsave_lowlevel_start_time = hrt_absolute_time(); - tune_confirm(); + tune_positive(); /* kill motors after timeout */ if (hrt_absolute_time() - current_status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) { @@ -2352,6 +2251,7 @@ int commander_thread_main(int argc, char *argv[]) /* Store old modes to detect and act on state transitions */ voltage_previous = current_status.voltage_battery; + /* XXX use this voltage_previous */ fflush(stdout); counter++; usleep(COMMANDER_MONITORING_INTERVAL); @@ -2360,6 +2260,7 @@ int commander_thread_main(int argc, char *argv[]) /* wait for threads to complete */ // pthread_join(command_handling_thread, NULL); pthread_join(subsystem_info_thread, NULL); + pthread_join(commander_low_prio_thread, NULL); /* close fds */ led_deinit(); @@ -2377,3 +2278,86 @@ int commander_thread_main(int argc, char *argv[]) return 0; } + + +static void *commander_low_prio_loop(void *arg) +{ + /* Set thread name */ + prctl(PR_SET_NAME, "commander low prio", getpid()); + + while (!thread_should_exit) { + + switch (low_prio_task) { + + case LOW_PRIO_TASK_PARAM_LOAD: + + if (0 == param_load_default()) { + mavlink_log_info(mavlink_fd, "Param load success"); + } else { + mavlink_log_critical(mavlink_fd, "Param load ERROR"); + tune_error(); + } + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_PARAM_SAVE: + + if (0 == param_save_default()) { + mavlink_log_info(mavlink_fd, "Param save success"); + } else { + mavlink_log_critical(mavlink_fd, "Param save ERROR"); + tune_error(); + } + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_GYRO_CALIBRATION: + + do_gyro_calibration(); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_MAG_CALIBRATION: + + do_mag_calibration(); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_ALTITUDE_CALIBRATION: + +// do_baro_calibration(); + + case LOW_PRIO_TASK_RC_CALIBRATION: + +// do_rc_calibration(); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_ACCEL_CALIBRATION: + + do_accel_calibration(); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_AIRSPEED_CALIBRATION: + + do_airspeed_calibration(); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_NONE: + default: + /* slow down to 10Hz */ + usleep(100000); + break; + } + + } + + return 0; +} diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 79394e2bae..ba01f84105 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -117,6 +117,22 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta current_state->flag_fmu_armed = false; } break; + case ARMING_STATE_REBOOT: + + /* an armed error happens when ARMED obviously */ + if (current_state->arming_state == ARMING_STATE_INIT + || current_state->arming_state == ARMING_STATE_STANDBY + || current_state->arming_state == ARMING_STATE_STANDBY_ERROR) { + + ret = OK; + current_state->flag_fmu_armed = false; + + } + break; + case ARMING_STATE_IN_AIR_RESTORE: + + /* XXX implement */ + break; default: break; } From f8e5b0cb0e01f89e7cffcdb97f4d5daaae499f72 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 26 Mar 2013 10:44:49 -0700 Subject: [PATCH 022/486] Small fixes and comments --- apps/commander/commander.c | 48 ++++++++++++++++---------------------- 1 file changed, 20 insertions(+), 28 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 27abb9d45c..bde4f2856d 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -91,8 +91,6 @@ #include "state_machine_helper.h" - -/* XXX MOVE CALIBRATION TO SENSORS APP THREAD */ #include #include #include @@ -161,8 +159,8 @@ enum { /* pthread loops */ -static void *orb_receive_loop(void *arg); -static void *commander_low_prio_loop(void *arg); +void *orb_receive_loop(void *arg); +void *commander_low_prio_loop(void *arg); __EXPORT int commander_main(int argc, char *argv[]); @@ -190,7 +188,7 @@ void do_accel_calibration(void); void do_airspeed_calibration(void); -static void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd); +void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd); int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state); @@ -1101,7 +1099,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } -static void *orb_receive_loop(void *arg) //handles status information coming from subsystems (present, enabled, health), these values do not indicate the quality (variance) of the signal +void *orb_receive_loop(void *arg) //handles status information coming from subsystems (present, enabled, health), these values do not indicate the quality (variance) of the signal { /* Set thread name */ prctl(PR_SET_NAME, "commander orb rcv", getpid()); @@ -1669,8 +1667,7 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (current_status.arming_state == ARMING_STATE_INIT) { - // XXX fix for now - current_status.condition_system_sensors_initialized = true; + // XXX check for sensors arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); } else { // XXX: Add emergency stuff if sensors are lost @@ -1730,7 +1727,6 @@ int commander_thread_main(int argc, char *argv[]) // current_status.flag_vector_flight_mode_ok = false; // } - // XXX why is this needed? /* consolidate state change, flag as changed if required */ if (global_pos_valid != current_status.condition_global_position_valid || local_pos_valid != current_status.condition_local_position_valid || @@ -2030,12 +2026,12 @@ int commander_thread_main(int argc, char *argv[]) case ARMING_STATE_ARMED_ERROR: - // TODO work out fail-safe scenarios + // XXX work out fail-safe scenarios break; case ARMING_STATE_STANDBY_ERROR: - // TODO work out fail-safe scenarios + // XXX work out fail-safe scenarios break; case ARMING_STATE_REBOOT: @@ -2050,9 +2046,6 @@ int commander_thread_main(int argc, char *argv[]) default: break; } - - // navigation_state_update(stat_pub, ¤t_status, mavlink_fd); - /* handle the case where RC signal was regained */ if (!current_status.rc_signal_found_once) { current_status.rc_signal_found_once = true; @@ -2068,11 +2061,11 @@ int commander_thread_main(int argc, char *argv[]) * Check if left stick is in lower left position --> switch to standby state. * Do this only for multirotors, not for fixed wing aircraft. */ - // if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - // (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - // (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) - // ) && - if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { + if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) + ) && + (sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); stick_off_counter = 0; @@ -2186,15 +2179,15 @@ int commander_thread_main(int argc, char *argv[]) current_status.offboard_control_signal_lost = false; current_status.offboard_control_signal_lost_interval = 0; + // XXX check if this is correct /* arm / disarm on request */ if (sp_offboard.armed && !current_status.flag_fmu_armed) { -#warning fix this -// update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); - /* switch to stabilized mode = takeoff */ -// update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); + + arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_ARMED, mavlink_fd); } else if (!sp_offboard.armed && current_status.flag_fmu_armed) { -// update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); + + arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); } } else { @@ -2229,7 +2222,7 @@ int commander_thread_main(int argc, char *argv[]) current_status.timestamp = hrt_absolute_time(); - // XXX what is this? + // XXX this is missing /* If full run came back clean, transition to standby */ // if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT && // current_status.flag_preflight_gyro_calibration == false && @@ -2241,8 +2234,7 @@ int commander_thread_main(int argc, char *argv[]) /* publish at least with 1 Hz */ if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) { -#warning fix this -// publish_armed_status(¤t_status); + orb_publish(ORB_ID(vehicle_status), stat_pub, ¤t_status); state_changed = false; } @@ -2280,7 +2272,7 @@ int commander_thread_main(int argc, char *argv[]) } -static void *commander_low_prio_loop(void *arg) +void *commander_low_prio_loop(void *arg) { /* Set thread name */ prctl(PR_SET_NAME, "commander low prio", getpid()); From 3cb3fa224baffcbb5ea228287f3d5b4f226b813b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 26 Mar 2013 15:37:13 -0700 Subject: [PATCH 023/486] Small fixes again --- apps/commander/commander.c | 6 +++++- apps/mavlink/mavlink.c | 6 +----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index bde4f2856d..77853aa7ae 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1082,7 +1082,8 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* supported command handling stop */ if (result == VEHICLE_CMD_RESULT_FAILED || result == VEHICLE_CMD_RESULT_DENIED || - result == VEHICLE_CMD_RESULT_UNSUPPORTED) { + result == VEHICLE_CMD_RESULT_UNSUPPORTED || + result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) { tune_negative(); @@ -1828,16 +1829,19 @@ int commander_thread_main(int argc, char *argv[]) /* bottom stick position, go to manual mode */ current_status.mode_switch = MODE_SWITCH_MANUAL; + printf("mode switch: manual\n"); } else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) { /* top stick position, set auto/mission for all vehicle types */ current_status.mode_switch = MODE_SWITCH_AUTO; + printf("mode switch: auto\n"); } else { /* center stick position, set seatbelt/simple control */ current_status.mode_switch = MODE_SWITCH_SEATBELT; + printf("mode switch: seatbelt\n"); } // warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode); diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 084ab0d322..2643cbf7ba 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -236,11 +236,6 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; } -// -// if (v_status.system_state == NAVIGATION_STATE_SEATBELT) { -// -// *mavlink_mode |= MAV_MODE_FLAG_DECODE_POSITION_GUIDED; -// } /** @@ -577,6 +572,7 @@ int mavlink_thread_main(int argc, char *argv[]) default: usage(); + break; } } From 83e356fda41b0936d924e4e74673789bfdd0b29c Mon Sep 17 00:00:00 2001 From: Anton Date: Wed, 27 Mar 2013 16:12:29 +0400 Subject: [PATCH 024/486] Initial version of position_estimator_inav added --- apps/position_estimator_inav/Makefile | 47 +++ .../kalman_filter_inertial.c | 21 ++ .../kalman_filter_inertial.h | 5 + .../position_estimator_inav_main.c | 349 ++++++++++++++++++ .../position_estimator_inav_params.c | 59 +++ .../position_estimator_inav_params.h | 63 ++++ apps/position_estimator_inav/sounds.c | 40 ++ apps/position_estimator_inav/sounds.h | 16 + nuttx/configs/px4fmu/nsh/appconfig | 1 + 9 files changed, 601 insertions(+) create mode 100644 apps/position_estimator_inav/Makefile create mode 100644 apps/position_estimator_inav/kalman_filter_inertial.c create mode 100644 apps/position_estimator_inav/kalman_filter_inertial.h create mode 100644 apps/position_estimator_inav/position_estimator_inav_main.c create mode 100644 apps/position_estimator_inav/position_estimator_inav_params.c create mode 100644 apps/position_estimator_inav/position_estimator_inav_params.h create mode 100644 apps/position_estimator_inav/sounds.c create mode 100644 apps/position_estimator_inav/sounds.h diff --git a/apps/position_estimator_inav/Makefile b/apps/position_estimator_inav/Makefile new file mode 100644 index 0000000000..4906f2d972 --- /dev/null +++ b/apps/position_estimator_inav/Makefile @@ -0,0 +1,47 @@ +############################################################################ +# +# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Makefile to build the position estimator +# + +APPNAME = position_estimator_inav +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 4096 + +CSRCS = position_estimator_inav_main.c \ + position_estimator_inav_params.c \ + kalman_filter_inertial.c \ + sounds.c + +include $(APPDIR)/mk/app.mk diff --git a/apps/position_estimator_inav/kalman_filter_inertial.c b/apps/position_estimator_inav/kalman_filter_inertial.c new file mode 100644 index 0000000000..7de06cb44e --- /dev/null +++ b/apps/position_estimator_inav/kalman_filter_inertial.c @@ -0,0 +1,21 @@ +#include "kalman_filter_inertial.h" + +void kalman_filter_inertial_predict(float dt, float x[3]) { + x[0] += x[1] * dt + x[2] * dt * dt / 2.0f; + x[1] += x[2] * dt; +} + +void kalman_filter_inertial_update(float x[3], float z[2], float k[3][2], bool use[2]) { + float y[2]; + // y = z - x + for (int i = 0; i < 2; i++) { + y[i] = z[i] - x[i]; + } + // x = x + K * y + for (int i = 0; i < 3; i++) { // Row + for (int j = 0; j < 2; j++) { // Column + if (use[j]) + x[i] += k[i][j] * y[j]; + } + } +} diff --git a/apps/position_estimator_inav/kalman_filter_inertial.h b/apps/position_estimator_inav/kalman_filter_inertial.h new file mode 100644 index 0000000000..e6bbf13153 --- /dev/null +++ b/apps/position_estimator_inav/kalman_filter_inertial.h @@ -0,0 +1,5 @@ +#include + +void kalman_filter_inertial_predict(float dt, float x[3]); + +void kalman_filter_inertial_update(float x[3], float z[2], float k[3][2], bool use[2]); diff --git a/apps/position_estimator_inav/position_estimator_inav_main.c b/apps/position_estimator_inav/position_estimator_inav_main.c new file mode 100644 index 0000000000..e28be5b51d --- /dev/null +++ b/apps/position_estimator_inav/position_estimator_inav_main.c @@ -0,0 +1,349 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: Damian Aregger + * Tobias Naegeli + * Lorenz Meier + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file position_estimator_inav_main.c + * Model-identification based position estimator for multirotors + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "position_estimator_inav_params.h" +//#include +#include "sounds.h" +#include +#include "kalman_filter_inertial.h" + +static bool thread_should_exit = false; /**< Deamon exit flag */ +static bool thread_running = false; /**< Deamon status flag */ +static int position_estimator_inav_task; /**< Handle of deamon task / thread */ + +__EXPORT int position_estimator_inav_main(int argc, char *argv[]); + +int position_estimator_inav_thread_main(int argc, char *argv[]); +/** + * Print the correct usage. + */ +static void usage(const char *reason); + +static void usage(const char *reason) { + if (reason) + fprintf(stderr, "%s\n", reason); + fprintf(stderr,"usage: position_estimator_inav {start|stop|status} [-p ]\n\n"); + exit(1); +} + + /** + * The position_estimator_inav_thread only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ +int position_estimator_inav_main(int argc, char *argv[]) { + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + printf("position_estimator_inav already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + position_estimator_inav_task = task_spawn("position_estimator_inav", + SCHED_RR, SCHED_PRIORITY_MAX - 5, 4096, + position_estimator_inav_thread_main, + (argv) ? (const char **) &argv[2] : (const char **) NULL ); + exit(0); + } + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + printf("\tposition_estimator_inav is running\n"); + } else { + printf("\tposition_estimator_inav not started\n"); + } + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +/**************************************************************************** + * main + ****************************************************************************/ +int position_estimator_inav_thread_main(int argc, char *argv[]) { + /* welcome user */ + printf("[position_estimator_inav] started\n"); + static int mavlink_fd; + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + mavlink_log_info(mavlink_fd, "[position_estimator_inav] started"); + + /* initialize values */ + static float k[3][2] = { { 0.0f, 0.0f }, { 0.0f, 0.0f }, { 0.0f, 0.0f } }; + static float x_est[3] = { 0.0f, 0.0f, 0.0f }; + static float y_est[3] = { 0.0f, 0.0f, 0.0f }; + static float z_est[3] = { 0.0f, 0.0f, 0.0f }; + const static float dT_const_120 = 1.0f / 120.0f; + const static float dT2_const_120 = 1.0f / 120.0f / 120.0f / 2.0f; + + int baro_loop_cnt = 0; + int baro_loop_end = 70; /* measurement for 1 second */ + float baro_alt0 = 0.0f; /* to determin while start up */ + float rho0 = 1.293f; /* standard pressure */ + const static float const_earth_gravity = 9.81f; + + static double lat_current = 0.0d; //[°]] --> 47.0 + static double lon_current = 0.0d; //[°]] -->8.5 + static double alt_current = 0.0d; //[m] above MSL + + /* declare and safely initialize all structs */ + struct vehicle_status_s vehicle_status; + memset(&vehicle_status, 0, sizeof(vehicle_status)); /* make sure that baroINITdone = false */ + struct sensor_combined_s sensor; + memset(&sensor, 0, sizeof(sensor)); + struct vehicle_gps_position_s gps; + memset(&gps, 0, sizeof(gps)); + struct vehicle_attitude_s att; + memset(&att, 0, sizeof(att)); + struct vehicle_local_position_s local_pos_est; + memset(&local_pos_est, 0, sizeof(local_pos_est)); + + /* subscribe */ + int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); + int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); + int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + + /* advertise */ + orb_advert_t local_pos_est_pub = orb_advertise( + ORB_ID(vehicle_local_position), &local_pos_est); + + struct position_estimator_inav_params pos_inav_params; + struct position_estimator_inav_param_handles pos_inav_param_handles; + /* initialize parameter handles */ + parameters_init(&pos_inav_param_handles); + + bool local_flag_baroINITdone = false; /* in any case disable baroINITdone */ + /* FIRST PARAMETER READ at START UP*/ + struct parameter_update_s param_update; + orb_copy(ORB_ID(parameter_update), parameter_update_sub, ¶m_update); /* read from param to clear updated flag */ + /* FIRST PARAMETER UPDATE */ + parameters_update(&pos_inav_param_handles, &pos_inav_params); + /* END FIRST PARAMETER UPDATE */ + /* wait until gps signal turns valid, only then can we initialize the projection */ + struct pollfd fds_init[1] = { { .fd = vehicle_gps_position_sub, .events = + POLLIN } }; + + while (gps.fix_type < 3) { + + /* wait for GPS updates, BUT READ VEHICLE STATUS (!) + * this choice is critical, since the vehicle status might not + * actually change, if this app is started after GPS lock was + * aquired. + */ + if (poll(fds_init, 1, 5000)) { /* poll only two first subscriptions */ + if (fds_init[0].revents & POLLIN) { + /* Wait for the GPS update to propagate (we have some time) */ + usleep(5000); + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, + &gps); + } + } + static int printcounter = 0; + if (printcounter == 100) { + printcounter = 0; + printf("[pos_est1D] wait for GPS fix type 3\n"); + } + printcounter++; + } + + /* get gps value for first initialization */ + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); + lat_current = ((double) (gps.lat)) * 1e-7; + lon_current = ((double) (gps.lon)) * 1e-7; + alt_current = gps.alt * 1e-3; + /* initialize coordinates */ + map_projection_init(lat_current, lon_current); + /* publish global position messages only after first GPS message */ + printf("[pos_est1D] initialized projection with: lat: %.10f, lon:%.10f\n", + lat_current, lon_current); + uint64_t last_time = 0; + thread_running = true; + + /** main loop */ + struct pollfd fds[5] = { { .fd = parameter_update_sub, .events = POLLIN }, { + .fd = vehicle_status_sub, .events = POLLIN }, { .fd = + vehicle_attitude_sub, .events = POLLIN }, { .fd = + sensor_combined_sub, .events = POLLIN }, { .fd = + vehicle_gps_position_sub, .events = POLLIN }, }; + while (!thread_should_exit) { + int ret = poll(fds, 5, 20); //wait maximal this 20 ms = 50 Hz minimum rate + if (ret < 0) { + /* poll error */ + } else { + /* parameter update */ + if (fds[0].revents & POLLIN) { + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), parameter_update_sub, + &update); + /* update parameters */ + parameters_update(&pos_inav_param_handles, &pos_inav_params); + //r = pos_inav_params.r; + } + /* vehicle status */ + if (fds[1].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); + } + /* vehicle attitude */ + if (fds[2].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); + } + /* sensor combined */ + if (fds[3].revents & POLLIN) { + orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, + &sensor); + } + /* vehicle GPS position */ + if (fds[4].revents & POLLIN) { + /* new GPS value */ + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, + &gps); + static float local_pos_gps[3] = { 0.0f, 0.0f, 0.0f }; /* output variables from tangent plane mapping */ + /* Project gps lat lon (Geographic coordinate system) to plane */ + map_projection_project(((double) (gps.lat)) * 1e-7, + ((double) (gps.lon)) * 1e-7, &(local_pos_gps[0]), + &(local_pos_gps[1])); + local_pos_gps[2] = (float) (gps.alt * 1e-3); + } + // barometric pressure estimation at start up + if (!local_flag_baroINITdone) { + // mean calculation over several measurements + if (baro_loop_cnt < baro_loop_end) { + baro_alt0 += sensor.baro_alt_meter; + baro_loop_cnt++; + } else { + baro_alt0 /= (float) (baro_loop_cnt); + local_flag_baroINITdone = true; + char *baro_m_start = "barometer initialized with alt0 = "; + char p0_char[15]; + sprintf(p0_char, "%8.2f", baro_alt0 / 100); + char *baro_m_end = " m"; + char str[80]; + strcpy(str, baro_m_start); + strcat(str, p0_char); + strcat(str, baro_m_end); + mavlink_log_info(mavlink_fd, str); + } + } + /* TODO convert accelerations from UAV frame to NED frame */ + float accel_NED[3]; + accel_NED[2] = sensor.accelerometer_m_s2[2] + const_earth_gravity; + /* prediction */ + kalman_filter_inertial_predict(dT_const_120, z_est); + /* prepare vectors for kalman filter correction */ + float z_meas[2]; // pos, accel + bool use_z[2] = { false, true }; + if (local_flag_baroINITdone) { + z_meas[0] = sensor.baro_alt_meter - baro_alt0; + use_z[0] = true; + } + z_meas[1] = accel_NED[2]; + /* correction */ + kalman_filter_inertial_update(z_est, z_meas, k, use_z); + printf("[pos_est_inav] att = %.3f, %.3f, %.3f, %.3f\n", att.q[0], att.q[1], att.q[2], att.q[3]); + printf("[pos_est_inav] z = %.2f, vz = %.2f, az = %.2f\n", z_est[0], z_est[1], z_est[2]); + + local_pos_est.x = 0.0; + local_pos_est.vx = 0.0; + local_pos_est.y = 0.0; + local_pos_est.vy = 0.0; + local_pos_est.z = z_est[0]; + local_pos_est.vz = z_est[1]; + local_pos_est.timestamp = hrt_absolute_time(); + if ((isfinite(local_pos_est.x)) && (isfinite(local_pos_est.vx)) + && (isfinite(local_pos_est.y)) + && (isfinite(local_pos_est.vy)) + && (isfinite(local_pos_est.z)) + && (isfinite(local_pos_est.vz))) { + orb_publish(ORB_ID( + vehicle_local_position), local_pos_est_pub, + &local_pos_est); + } + } /* end of poll return value check */ + } + + printf("[pos_est_inav] exiting.\n"); + mavlink_log_info(mavlink_fd, "[pos_est_inav] exiting"); + thread_running = false; + return 0; +} diff --git a/apps/position_estimator_inav/position_estimator_inav_params.c b/apps/position_estimator_inav/position_estimator_inav_params.c new file mode 100644 index 0000000000..5567fd2cf2 --- /dev/null +++ b/apps/position_estimator_inav/position_estimator_inav_params.c @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Damian Aregger + * Tobias Naegeli +* Lorenz Meier + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file position_estimator_inav_params.c + * + * Parameters for position_estimator_inav + */ + +#include "position_estimator_inav_params.h" + +/* Kalman Filter covariances */ +/* gps measurement noise standard deviation */ +PARAM_DEFINE_FLOAT(POS_EST_R, 1.0f); + +int parameters_init(struct position_estimator_inav_param_handles *h) +{ + h->r = param_find("POS_EST_R"); + return OK; +} + +int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) +{ + param_get(h->r, &(p->r)); + return OK; +} diff --git a/apps/position_estimator_inav/position_estimator_inav_params.h b/apps/position_estimator_inav/position_estimator_inav_params.h new file mode 100644 index 0000000000..c45ca81359 --- /dev/null +++ b/apps/position_estimator_inav/position_estimator_inav_params.h @@ -0,0 +1,63 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Damian Aregger + * Tobias Naegeli + * Lorenz Meier + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file position_estimator_inav_params.h + * + * Parameters for Position Estimator + */ + +#include + +struct position_estimator_inav_params { + float r; +}; + +struct position_estimator_inav_param_handles { + param_t r; +}; + +/** + * Initialize all parameter handles and values + * + */ +int parameters_init(struct position_estimator_inav_param_handles *h); + +/** + * Update all parameters + * + */ +int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p); diff --git a/apps/position_estimator_inav/sounds.c b/apps/position_estimator_inav/sounds.c new file mode 100644 index 0000000000..4f7b05fea5 --- /dev/null +++ b/apps/position_estimator_inav/sounds.c @@ -0,0 +1,40 @@ +/* + * sounds.c + * + * Created on: Feb 26, 2013 + * Author: samuezih + */ + +#include +#include +#include + + +static int buzzer; + +int sounds_init(void) +{ + buzzer = open("/dev/tone_alarm", O_WRONLY); + + if (buzzer < 0) { + warnx("Buzzer: open fail\n"); + return ERROR; + } + + return 0; +} + +void sounds_deinit(void) +{ + close(buzzer); +} + +void tune_tetris(void) +{ + ioctl(buzzer, TONE_SET_ALARM, 6); +} + +void tune_sonar(void) +{ + ioctl(buzzer, TONE_SET_ALARM, 7); +} diff --git a/apps/position_estimator_inav/sounds.h b/apps/position_estimator_inav/sounds.h new file mode 100644 index 0000000000..356e42169c --- /dev/null +++ b/apps/position_estimator_inav/sounds.h @@ -0,0 +1,16 @@ +/* + * sounds.h + * + * Created on: Feb 26, 2013 + * Author: samuezih + */ + +#ifndef SOUNDS_H_ +#define SOUNDS_H_ + +int sounds_init(void); +void sounds_deinit(void); +void tune_tetris(void); +void tune_sonar(void); + +#endif /* SOUNDS_H_ */ diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index 80cf6f312c..c92e29930b 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -101,6 +101,7 @@ CONFIGURED_APPS += multirotor_pos_control CONFIGURED_APPS += fixedwing_att_control CONFIGURED_APPS += fixedwing_pos_control CONFIGURED_APPS += position_estimator +CONFIGURED_APPS += position_estimator_inav CONFIGURED_APPS += attitude_estimator_ekf CONFIGURED_APPS += hott_telemetry endif From b837692d48e6dcc568bdf7009f515504c0772cf8 Mon Sep 17 00:00:00 2001 From: Anton Date: Thu, 28 Mar 2013 21:35:38 +0400 Subject: [PATCH 025/486] Test rotation matrix --- .../position_estimator_inav_main.c | 110 ++++++++++-------- 1 file changed, 64 insertions(+), 46 deletions(-) diff --git a/apps/position_estimator_inav/position_estimator_inav_main.c b/apps/position_estimator_inav/position_estimator_inav_main.c index e28be5b51d..774125e837 100644 --- a/apps/position_estimator_inav/position_estimator_inav_main.c +++ b/apps/position_estimator_inav/position_estimator_inav_main.c @@ -154,6 +154,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { const static float dT_const_120 = 1.0f / 120.0f; const static float dT2_const_120 = 1.0f / 120.0f / 120.0f / 2.0f; + bool use_gps = false; int baro_loop_cnt = 0; int baro_loop_end = 70; /* measurement for 1 second */ float baro_alt0 = 0.0f; /* to determin while start up */ @@ -200,40 +201,42 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { parameters_update(&pos_inav_param_handles, &pos_inav_params); /* END FIRST PARAMETER UPDATE */ /* wait until gps signal turns valid, only then can we initialize the projection */ - struct pollfd fds_init[1] = { { .fd = vehicle_gps_position_sub, .events = - POLLIN } }; + if (use_gps) { + struct pollfd fds_init[1] = { { .fd = vehicle_gps_position_sub, + .events = POLLIN } }; - while (gps.fix_type < 3) { + while (gps.fix_type < 3) { - /* wait for GPS updates, BUT READ VEHICLE STATUS (!) - * this choice is critical, since the vehicle status might not - * actually change, if this app is started after GPS lock was - * aquired. - */ - if (poll(fds_init, 1, 5000)) { /* poll only two first subscriptions */ - if (fds_init[0].revents & POLLIN) { - /* Wait for the GPS update to propagate (we have some time) */ - usleep(5000); - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, - &gps); + /* wait for GPS updates, BUT READ VEHICLE STATUS (!) + * this choice is critical, since the vehicle status might not + * actually change, if this app is started after GPS lock was + * aquired. + */ + if (poll(fds_init, 1, 5000)) { /* poll only two first subscriptions */ + if (fds_init[0].revents & POLLIN) { + /* Wait for the GPS update to propagate (we have some time) */ + usleep(5000); + orb_copy(ORB_ID(vehicle_gps_position), + vehicle_gps_position_sub, &gps); + } } + static int printcounter = 0; + if (printcounter == 100) { + printcounter = 0; + printf("[pos_est1D] wait for GPS fix type 3\n"); + } + printcounter++; } - static int printcounter = 0; - if (printcounter == 100) { - printcounter = 0; - printf("[pos_est1D] wait for GPS fix type 3\n"); - } - printcounter++; - } - /* get gps value for first initialization */ - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - lat_current = ((double) (gps.lat)) * 1e-7; - lon_current = ((double) (gps.lon)) * 1e-7; - alt_current = gps.alt * 1e-3; - /* initialize coordinates */ - map_projection_init(lat_current, lon_current); - /* publish global position messages only after first GPS message */ + /* get gps value for first initialization */ + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); + lat_current = ((double) (gps.lat)) * 1e-7; + lon_current = ((double) (gps.lon)) * 1e-7; + alt_current = gps.alt * 1e-3; + /* initialize coordinates */ + map_projection_init(lat_current, lon_current); + /* publish global position messages only after first GPS message */ + } printf("[pos_est1D] initialized projection with: lat: %.10f, lon:%.10f\n", lat_current, lon_current); uint64_t last_time = 0; @@ -245,6 +248,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { vehicle_attitude_sub, .events = POLLIN }, { .fd = sensor_combined_sub, .events = POLLIN }, { .fd = vehicle_gps_position_sub, .events = POLLIN }, }; + printf("[position_estimator_inav] main loop started\n"); + static int printatt_counter = 0; while (!thread_should_exit) { int ret = poll(fds, 5, 20); //wait maximal this 20 ms = 50 Hz minimum rate if (ret < 0) { @@ -262,7 +267,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } /* vehicle status */ if (fds[1].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, + &vehicle_status); } /* vehicle attitude */ if (fds[2].revents & POLLIN) { @@ -270,20 +276,21 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } /* sensor combined */ if (fds[3].revents & POLLIN) { - orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, - &sensor); + orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); } - /* vehicle GPS position */ - if (fds[4].revents & POLLIN) { - /* new GPS value */ - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, - &gps); - static float local_pos_gps[3] = { 0.0f, 0.0f, 0.0f }; /* output variables from tangent plane mapping */ - /* Project gps lat lon (Geographic coordinate system) to plane */ - map_projection_project(((double) (gps.lat)) * 1e-7, - ((double) (gps.lon)) * 1e-7, &(local_pos_gps[0]), - &(local_pos_gps[1])); - local_pos_gps[2] = (float) (gps.alt * 1e-3); + if (use_gps) { + /* vehicle GPS position */ + if (fds[4].revents & POLLIN) { + /* new GPS value */ + orb_copy(ORB_ID(vehicle_gps_position), + vehicle_gps_position_sub, &gps); + static float local_pos_gps[3] = { 0.0f, 0.0f, 0.0f }; /* output variables from tangent plane mapping */ + /* Project gps lat lon (Geographic coordinate system) to plane */ + map_projection_project(((double) (gps.lat)) * 1e-7, + ((double) (gps.lon)) * 1e-7, &(local_pos_gps[0]), + &(local_pos_gps[1])); + local_pos_gps[2] = (float) (gps.alt * 1e-3); + } } // barometric pressure estimation at start up if (!local_flag_baroINITdone) { @@ -320,9 +327,20 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { z_meas[1] = accel_NED[2]; /* correction */ kalman_filter_inertial_update(z_est, z_meas, k, use_z); - printf("[pos_est_inav] att = %.3f, %.3f, %.3f, %.3f\n", att.q[0], att.q[1], att.q[2], att.q[3]); - printf("[pos_est_inav] z = %.2f, vz = %.2f, az = %.2f\n", z_est[0], z_est[1], z_est[2]); - + if (printatt_counter == 100) { + printatt_counter = 0; + printf("[pos_est_inav] pitch = %.3f, roll = %.3f, yaw = %.3f\n", + att.pitch, att.roll, att.yaw); + printf("[pos_est_inav] Rot_matrix: %.3f %.3f %.3f\n", + att.R[0][0], att.R[0][1], att.R[0][2]); + printf("[pos_est_inav] Rot_matrix: %.3f %.3f %.3f\n", + att.R[1][0], att.R[1][1], att.R[1][2]); + printf("[pos_est_inav] Rot_matrix: %.3f %.3f %.3f\n", + att.R[2][0], att.R[2][1], att.R[2][2]); + printf("[pos_est_inav] z = %.2f, vz = %.2f, az = %.2f\n", + z_est[0], z_est[1], z_est[2]); + } + printatt_counter++; local_pos_est.x = 0.0; local_pos_est.vx = 0.0; local_pos_est.y = 0.0; From fb663bbb0c02ab6d6b6e1b360f5833028cd25bdd Mon Sep 17 00:00:00 2001 From: Anton Date: Fri, 29 Mar 2013 13:08:56 +0400 Subject: [PATCH 026/486] Acceleration convertion from UAV frame to NED frame --- .../position_estimator_inav_main.c | 75 ++++++++++++------- 1 file changed, 46 insertions(+), 29 deletions(-) diff --git a/apps/position_estimator_inav/position_estimator_inav_main.c b/apps/position_estimator_inav/position_estimator_inav_main.c index 774125e837..4277fe19bf 100644 --- a/apps/position_estimator_inav/position_estimator_inav_main.c +++ b/apps/position_estimator_inav/position_estimator_inav_main.c @@ -87,18 +87,19 @@ static void usage(const char *reason); static void usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); - fprintf(stderr,"usage: position_estimator_inav {start|stop|status} [-p ]\n\n"); + fprintf(stderr, + "usage: position_estimator_inav {start|stop|status} [-p ]\n\n"); exit(1); } - /** - * The position_estimator_inav_thread only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ +/** + * The position_estimator_inav_thread only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ int position_estimator_inav_main(int argc, char *argv[]) { if (argc < 1) usage("missing command"); @@ -147,7 +148,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { mavlink_log_info(mavlink_fd, "[position_estimator_inav] started"); /* initialize values */ - static float k[3][2] = { { 0.0f, 0.0f }, { 0.0f, 0.0f }, { 0.0f, 0.0f } }; + static float k[3][2] = { { 0.002f, 0.0f }, { 0.001f, 0.00002f }, { 0.0f, + 0.99f } }; static float x_est[3] = { 0.0f, 0.0f, 0.0f }; static float y_est[3] = { 0.0f, 0.0f, 0.0f }; static float z_est[3] = { 0.0f, 0.0f, 0.0f }; @@ -167,7 +169,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { /* declare and safely initialize all structs */ struct vehicle_status_s vehicle_status; - memset(&vehicle_status, 0, sizeof(vehicle_status)); /* make sure that baroINITdone = false */ + memset(&vehicle_status, 0, sizeof(vehicle_status)); + /* make sure that baroINITdone = false */ struct sensor_combined_s sensor; memset(&sensor, 0, sizeof(sensor)); struct vehicle_gps_position_s gps; @@ -239,19 +242,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } printf("[pos_est1D] initialized projection with: lat: %.10f, lon:%.10f\n", lat_current, lon_current); - uint64_t last_time = 0; + hrt_abstime last_time = 0; thread_running = true; /** main loop */ - struct pollfd fds[5] = { { .fd = parameter_update_sub, .events = POLLIN }, { - .fd = vehicle_status_sub, .events = POLLIN }, { .fd = - vehicle_attitude_sub, .events = POLLIN }, { .fd = - sensor_combined_sub, .events = POLLIN }, { .fd = - vehicle_gps_position_sub, .events = POLLIN }, }; + struct pollfd fds[3] = { { .fd = parameter_update_sub, .events = POLLIN }, { .fd = vehicle_status_sub, .events = POLLIN }, { .fd = sensor_combined_sub, .events = POLLIN } }; printf("[position_estimator_inav] main loop started\n"); static int printatt_counter = 0; while (!thread_should_exit) { - int ret = poll(fds, 5, 20); //wait maximal this 20 ms = 50 Hz minimum rate + int ret = poll(fds, 3, 20); //wait maximal this 20 ms = 50 Hz minimum rate if (ret < 0) { /* poll error */ } else { @@ -271,16 +270,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { &vehicle_status); } /* vehicle attitude */ - if (fds[2].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); - } + //if (fds[2].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); + //} /* sensor combined */ - if (fds[3].revents & POLLIN) { + if (fds[2].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); } if (use_gps) { /* vehicle GPS position */ - if (fds[4].revents & POLLIN) { + //if (fds[4].revents & POLLIN) { /* new GPS value */ orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); @@ -290,7 +289,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { ((double) (gps.lon)) * 1e-7, &(local_pos_gps[0]), &(local_pos_gps[1])); local_pos_gps[2] = (float) (gps.alt * 1e-3); - } + //} } // barometric pressure estimation at start up if (!local_flag_baroINITdone) { @@ -312,16 +311,25 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { mavlink_log_info(mavlink_fd, str); } } - /* TODO convert accelerations from UAV frame to NED frame */ - float accel_NED[3]; - accel_NED[2] = sensor.accelerometer_m_s2[2] + const_earth_gravity; + + hrt_abstime t = hrt_absolute_time(); + float dt = (t - last_time) / 1000000.0; + last_time = t; + /* convert accelerations from UAV frame to NED frame */ + float accel_NED[3] = { 0.0f, 0.0f, 0.0f }; + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + accel_NED[i] += att.R[i][j] * sensor.accelerometer_m_s2[j]; + } + } + accel_NED[2] += const_earth_gravity; /* prediction */ - kalman_filter_inertial_predict(dT_const_120, z_est); + kalman_filter_inertial_predict(dt, z_est); /* prepare vectors for kalman filter correction */ float z_meas[2]; // pos, accel bool use_z[2] = { false, true }; if (local_flag_baroINITdone) { - z_meas[0] = sensor.baro_alt_meter - baro_alt0; + z_meas[0] = baro_alt0 - sensor.baro_alt_meter; // Z = -alt use_z[0] = true; } z_meas[1] = accel_NED[2]; @@ -329,14 +337,23 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { kalman_filter_inertial_update(z_est, z_meas, k, use_z); if (printatt_counter == 100) { printatt_counter = 0; + printf("[pos_est_inav] dt = %.6f\n", dt); printf("[pos_est_inav] pitch = %.3f, roll = %.3f, yaw = %.3f\n", att.pitch, att.roll, att.yaw); + printf("[pos_est_inav] accel_UAV = %.3f, %.3f, %.3f\n", + sensor.accelerometer_m_s2[0], + sensor.accelerometer_m_s2[1], + sensor.accelerometer_m_s2[2]); + printf("[pos_est_inav] accel_NED = %.3f, %.3f, %.3f\n", + accel_NED[0], accel_NED[1], accel_NED[2]); + /* printf("[pos_est_inav] Rot_matrix: %.3f %.3f %.3f\n", att.R[0][0], att.R[0][1], att.R[0][2]); printf("[pos_est_inav] Rot_matrix: %.3f %.3f %.3f\n", att.R[1][0], att.R[1][1], att.R[1][2]); printf("[pos_est_inav] Rot_matrix: %.3f %.3f %.3f\n", att.R[2][0], att.R[2][1], att.R[2][2]); + */ printf("[pos_est_inav] z = %.2f, vz = %.2f, az = %.2f\n", z_est[0], z_est[1], z_est[2]); } From 382af2b52d98de82c5cc0953e2c2ba7acf65501f Mon Sep 17 00:00:00 2001 From: Anton Date: Sat, 30 Mar 2013 01:54:04 +0400 Subject: [PATCH 027/486] Acceleration vector transform implemented. Accelerometer calibration procedure defined but not implemented yet. --- apps/position_estimator_inav/Makefile | 1 + .../acceleration_transform.c | 93 ++++++++ .../acceleration_transform.h | 15 ++ .../position_estimator_inav_main.c | 216 +++++++++++------- 4 files changed, 242 insertions(+), 83 deletions(-) create mode 100644 apps/position_estimator_inav/acceleration_transform.c create mode 100644 apps/position_estimator_inav/acceleration_transform.h diff --git a/apps/position_estimator_inav/Makefile b/apps/position_estimator_inav/Makefile index 4906f2d972..e5f3b4b42b 100644 --- a/apps/position_estimator_inav/Makefile +++ b/apps/position_estimator_inav/Makefile @@ -42,6 +42,7 @@ STACKSIZE = 4096 CSRCS = position_estimator_inav_main.c \ position_estimator_inav_params.c \ kalman_filter_inertial.c \ + acceleration_transform.c \ sounds.c include $(APPDIR)/mk/app.mk diff --git a/apps/position_estimator_inav/acceleration_transform.c b/apps/position_estimator_inav/acceleration_transform.c new file mode 100644 index 0000000000..9080c4e204 --- /dev/null +++ b/apps/position_estimator_inav/acceleration_transform.c @@ -0,0 +1,93 @@ +/* + * acceleration_transform.c + * + * Created on: 30.03.2013 + * Author: Anton Babushkin + * + * Transform acceleration vector to true orientation and scale + * + * * * * Model * * * + * accel_corr = accel_T * (accel_raw - accel_offs) + * + * accel_corr[3] - fully corrected acceleration vector in UAV frame + * accel_T[3][3] - accelerometers transform matrix, rotation and scaling transform + * accel_raw[3] - raw acceleration vector + * accel_offs[3] - true acceleration offset vector, don't use sensors offset because + * it's caused not only by real offset but also by sensor rotation + * + * * * * Calibration * * * + * + * Tests + * + * accel_corr_x_p = [ g 0 0 ]^T // positive x + * accel_corr_x_n = [ -g 0 0 ]^T // negative x + * accel_corr_y_p = [ 0 g 0 ]^T // positive y + * accel_corr_y_n = [ 0 -g 0 ]^T // negative y + * accel_corr_z_p = [ 0 0 g ]^T // positive z + * accel_corr_z_n = [ 0 0 -g ]^T // negative z + * + * 6 tests * 3 axes = 18 equations + * 9 (accel_T) + 3 (accel_offs) = 12 unknown constants + * + * Find accel_offs + * + * accel_offs = (accel_raw_x_p + accel_raw_x_n + + * accel_raw_y_p + accel_raw_y_n + + * accel_raw_z_p + accel_raw_z_n) / 6 + * + * + * Find accel_T + * + * 9 unknown constants + * need 9 equations -> use 3 of 6 measurements -> 3 * 3 = 9 equations + * + * accel_corr[i] = accel_T[i][0] * (accel_raw[0] - accel_offs[0]) + + * accel_T[i][1] * (accel_raw[1] - accel_offs[1]) + + * accel_T[i][2] * (accel_raw[2] - accel_offs[2]) + * A x = b + * + * x = [ accel_T[0][0] ] + * | accel_T[0][1] | + * | accel_T[0][2] | + * | accel_T[1][0] | + * | accel_T[1][1] | + * | accel_T[1][2] | + * | accel_T[2][0] | + * | accel_T[2][1] | + * [ accel_T[2][2] ] + * + * b = [ accel_corr_x_p[0] ] // One measurement per axis is enough + * | accel_corr_x_p[1] | + * | accel_corr_x_p[2] | + * | accel_corr_y_p[0] | + * | accel_corr_y_p[1] | + * | accel_corr_y_p[2] | + * | accel_corr_z_p[0] | + * | accel_corr_z_p[1] | + * [ accel_corr_z_p[2] ] + * + * a_x[i] = accel_raw_x_p[i] - accel_offs[i] // Measurement for X axis + * ... + * A = [ a_x[0] a_x[1] a_x[2] 0 0 0 0 0 0 ] + * | 0 0 0 a_x[0] a_x[1] a_x[2] 0 0 0 | + * | 0 0 0 0 0 0 a_x[0] a_x[1] a_x[2] | + * | a_y[0] a_y[1] a_y[2] 0 0 0 0 0 0 | + * | 0 0 0 a_y[0] a_y[1] a_y[2] 0 0 0 | + * | 0 0 0 0 0 0 a_y[0] a_y[1] a_y[2] | + * | a_z[0] a_z[1] a_z[2] 0 0 0 0 0 0 | + * | 0 0 0 a_z[0] a_z[1] a_z[2] 0 0 0 | + * [ 0 0 0 0 0 0 a_z[0] a_z[1] a_z[2] ] + * + * x = A^-1 b + */ + +#include "acceleration_transform.h" + +void acceleration_correct(float accel_corr[3], int16_t accel_raw[3], float accel_T[3][3], int16_t accel_offs[3]) { + for (int i = 0; i < 3; i++) { + accel_corr[i] = 0.0f; + for (int j = 0; j < 3; j++) { + accel_corr[i] += accel_T[i][j] * (accel_raw[j] - accel_offs[j]); + } + } +} diff --git a/apps/position_estimator_inav/acceleration_transform.h b/apps/position_estimator_inav/acceleration_transform.h new file mode 100644 index 0000000000..3692da77ec --- /dev/null +++ b/apps/position_estimator_inav/acceleration_transform.h @@ -0,0 +1,15 @@ +/* + * acceleration_transform.h + * + * Created on: 30.03.2013 + * Author: ton + */ + +#ifndef ACCELERATION_TRANSFORM_H_ +#define ACCELERATION_TRANSFORM_H_ + +#include + +void acceleration_correct(float accel_corr[3], int16_t accel_raw[3], float accel_T[3][3], int16_t accel_offs[3]); + +#endif /* ACCELERATION_TRANSFORM_H_ */ diff --git a/apps/position_estimator_inav/position_estimator_inav_main.c b/apps/position_estimator_inav/position_estimator_inav_main.c index 4277fe19bf..1c6d21ded7 100644 --- a/apps/position_estimator_inav/position_estimator_inav_main.c +++ b/apps/position_estimator_inav/position_estimator_inav_main.c @@ -71,6 +71,7 @@ #include "sounds.h" #include #include "kalman_filter_inertial.h" +#include "acceleration_transform.h" static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ @@ -147,9 +148,18 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); mavlink_log_info(mavlink_fd, "[position_estimator_inav] started"); + /* kalman filter K for simulation parameters: + * rate_accel = 200 Hz + * rate_baro = 100 Hz + * err_accel = 1.0 m/s^2 + * err_baro = 0.2 m + */ + static float k[3][2] = { + { 0.0262f, 0.00001f }, + { 0.0349f, 0.00191f }, + { 0.000259f, 0.618f } + }; /* initialize values */ - static float k[3][2] = { { 0.002f, 0.0f }, { 0.001f, 0.00002f }, { 0.0f, - 0.99f } }; static float x_est[3] = { 0.0f, 0.0f, 0.0f }; static float y_est[3] = { 0.0f, 0.0f, 0.0f }; static float z_est[3] = { 0.0f, 0.0f, 0.0f }; @@ -160,7 +170,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { int baro_loop_cnt = 0; int baro_loop_end = 70; /* measurement for 1 second */ float baro_alt0 = 0.0f; /* to determin while start up */ - float rho0 = 1.293f; /* standard pressure */ const static float const_earth_gravity = 9.81f; static double lat_current = 0.0d; //[°]] --> 47.0 @@ -203,6 +212,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { /* FIRST PARAMETER UPDATE */ parameters_update(&pos_inav_param_handles, &pos_inav_params); /* END FIRST PARAMETER UPDATE */ + + // TODO implement calibration procedure, now put dummy values + int16_t accel_offs[3] = { 0, 0, 0 }; + float accel_T[3][3] = { + { 1.0f, 0.0f, 0.0f }, + { 0.0f, 1.0f, 0.0f }, + { 0.0f, 0.0f, 1.0f } + }; + /* wait until gps signal turns valid, only then can we initialize the projection */ if (use_gps) { struct pollfd fds_init[1] = { { .fd = vehicle_gps_position_sub, @@ -226,12 +244,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { static int printcounter = 0; if (printcounter == 100) { printcounter = 0; - printf("[pos_est1D] wait for GPS fix type 3\n"); + printf("[position_estimator_inav] wait for GPS fix type 3\n"); } printcounter++; } - /* get gps value for first initialization */ + /* get GPS position for first initialization */ orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); lat_current = ((double) (gps.lat)) * 1e-7; lon_current = ((double) (gps.lon)) * 1e-7; @@ -240,20 +258,38 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { map_projection_init(lat_current, lon_current); /* publish global position messages only after first GPS message */ } - printf("[pos_est1D] initialized projection with: lat: %.10f, lon:%.10f\n", + printf("[position_estimator_inav] initialized projection with: lat: %.10f, lon:%.10f\n", lat_current, lon_current); + hrt_abstime last_time = 0; thread_running = true; - - /** main loop */ - struct pollfd fds[3] = { { .fd = parameter_update_sub, .events = POLLIN }, { .fd = vehicle_status_sub, .events = POLLIN }, { .fd = sensor_combined_sub, .events = POLLIN } }; - printf("[position_estimator_inav] main loop started\n"); static int printatt_counter = 0; + uint32_t accelerometer_counter = 0; + uint32_t baro_counter = 0; + uint16_t accelerometer_updates = 0; + uint16_t baro_updates = 0; + hrt_abstime updates_counter_start = hrt_absolute_time(); + uint32_t updates_counter_len = 1000000; + hrt_abstime pub_last = 0; + uint32_t pub_interval = 5000; // limit publish rate to 200 Hz + + /* main loop */ + struct pollfd fds[3] = { + { .fd = parameter_update_sub, .events = POLLIN }, + { .fd = vehicle_status_sub, .events = POLLIN }, + { .fd = vehicle_attitude_sub, .events = POLLIN }, + { .fd = sensor_combined_sub, .events = POLLIN }, + { .fd = vehicle_gps_position_sub, .events = POLLIN } + }; + printf("[position_estimator_inav] main loop started\n"); while (!thread_should_exit) { - int ret = poll(fds, 3, 20); //wait maximal this 20 ms = 50 Hz minimum rate + bool accelerometer_updated = false; + bool baro_updated = false; + int ret = poll(fds, 5, 10); // wait maximal this 10 ms = 100 Hz minimum rate if (ret < 0) { /* poll error */ - } else { + printf("[position_estimator_inav] subscriptions poll error\n", ret); + } else if (ret > 0) { /* parameter update */ if (fds[0].revents & POLLIN) { /* read from param to clear updated flag */ @@ -262,7 +298,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { &update); /* update parameters */ parameters_update(&pos_inav_param_handles, &pos_inav_params); - //r = pos_inav_params.r; } /* vehicle status */ if (fds[1].revents & POLLIN) { @@ -270,16 +305,41 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { &vehicle_status); } /* vehicle attitude */ - //if (fds[2].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); - //} - /* sensor combined */ if (fds[2].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); + } + /* sensor combined */ + if (fds[3].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); + if (sensor.accelerometer_counter > accelerometer_counter) { + accelerometer_updated = true; + accelerometer_counter = sensor.accelerometer_counter; + accelerometer_updates++; + } + if (sensor.baro_counter > baro_counter) { + baro_updated = true; + baro_counter = sensor.baro_counter; + baro_updates++; + } + // barometric pressure estimation at start up + if (!local_flag_baroINITdone && baro_updated) { + // mean calculation over several measurements + if (baro_loop_cnt < baro_loop_end) { + baro_alt0 += sensor.baro_alt_meter; + baro_loop_cnt++; + } else { + baro_alt0 /= (float) (baro_loop_cnt); + local_flag_baroINITdone = true; + char str[80]; + sprintf(str, "[position_estimator_inav] barometer initialized with alt0 = %.2f", baro_alt0); + printf("%s\n", str); + mavlink_log_info(mavlink_fd, str); + } + } } if (use_gps) { /* vehicle GPS position */ - //if (fds[4].revents & POLLIN) { + if (fds[4].revents & POLLIN) { /* new GPS value */ orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); @@ -289,75 +349,65 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { ((double) (gps.lon)) * 1e-7, &(local_pos_gps[0]), &(local_pos_gps[1])); local_pos_gps[2] = (float) (gps.alt * 1e-3); - //} - } - // barometric pressure estimation at start up - if (!local_flag_baroINITdone) { - // mean calculation over several measurements - if (baro_loop_cnt < baro_loop_end) { - baro_alt0 += sensor.baro_alt_meter; - baro_loop_cnt++; - } else { - baro_alt0 /= (float) (baro_loop_cnt); - local_flag_baroINITdone = true; - char *baro_m_start = "barometer initialized with alt0 = "; - char p0_char[15]; - sprintf(p0_char, "%8.2f", baro_alt0 / 100); - char *baro_m_end = " m"; - char str[80]; - strcpy(str, baro_m_start); - strcat(str, p0_char); - strcat(str, baro_m_end); - mavlink_log_info(mavlink_fd, str); } } - hrt_abstime t = hrt_absolute_time(); - float dt = (t - last_time) / 1000000.0; - last_time = t; - /* convert accelerations from UAV frame to NED frame */ - float accel_NED[3] = { 0.0f, 0.0f, 0.0f }; - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - accel_NED[i] += att.R[i][j] * sensor.accelerometer_m_s2[j]; - } - } - accel_NED[2] += const_earth_gravity; - /* prediction */ - kalman_filter_inertial_predict(dt, z_est); - /* prepare vectors for kalman filter correction */ - float z_meas[2]; // pos, accel - bool use_z[2] = { false, true }; - if (local_flag_baroINITdone) { - z_meas[0] = baro_alt0 - sensor.baro_alt_meter; // Z = -alt - use_z[0] = true; + } /* end of poll return value check */ + + hrt_abstime t = hrt_absolute_time(); + float dt = (t - last_time) / 1000000.0; + last_time = t; + /* calculate corrected acceleration vector in UAV frame */ + float accel_corr[3]; + acceleration_correct(accel_corr, sensor.accelerometer_raw, accel_T, accel_offs); + /* transform acceleration vector from UAV frame to NED frame */ + float accel_NED[3]; + for (int i = 0; i < 3; i++) { + accel_NED[i] = 0.0f; + for (int j = 0; j < 3; j++) { + accel_NED[i] += att.R[i][j] * accel_corr[j]; } + } + accel_NED[2] += const_earth_gravity; + /* kalman filter prediction */ + kalman_filter_inertial_predict(dt, z_est); + /* prepare vectors for kalman filter correction */ + float z_meas[2]; // position, acceleration + bool use_z[2] = { false, false }; + if (local_flag_baroINITdone && baro_updated) { + z_meas[0] = baro_alt0 - sensor.baro_alt_meter; // Z = -alt + use_z[0] = true; + } + if (accelerometer_updated) { z_meas[1] = accel_NED[2]; + use_z[1] = true; + } + if (use_z[0] || use_z[1]) { /* correction */ kalman_filter_inertial_update(z_est, z_meas, k, use_z); - if (printatt_counter == 100) { - printatt_counter = 0; - printf("[pos_est_inav] dt = %.6f\n", dt); - printf("[pos_est_inav] pitch = %.3f, roll = %.3f, yaw = %.3f\n", - att.pitch, att.roll, att.yaw); - printf("[pos_est_inav] accel_UAV = %.3f, %.3f, %.3f\n", - sensor.accelerometer_m_s2[0], - sensor.accelerometer_m_s2[1], - sensor.accelerometer_m_s2[2]); - printf("[pos_est_inav] accel_NED = %.3f, %.3f, %.3f\n", - accel_NED[0], accel_NED[1], accel_NED[2]); - /* - printf("[pos_est_inav] Rot_matrix: %.3f %.3f %.3f\n", - att.R[0][0], att.R[0][1], att.R[0][2]); - printf("[pos_est_inav] Rot_matrix: %.3f %.3f %.3f\n", - att.R[1][0], att.R[1][1], att.R[1][2]); - printf("[pos_est_inav] Rot_matrix: %.3f %.3f %.3f\n", - att.R[2][0], att.R[2][1], att.R[2][2]); - */ - printf("[pos_est_inav] z = %.2f, vz = %.2f, az = %.2f\n", - z_est[0], z_est[1], z_est[2]); - } - printatt_counter++; + } + if (t - updates_counter_start > updates_counter_len) { + float updates_dt = (t - updates_counter_start) * 0.000001f; + printf("[position_estimator_inav] accelerometer_updates_rate = %.1/s, baro_updates_rate = %.1f/s\n", accelerometer_updates / updates_dt, baro_updates / updates_dt); + updates_counter_start = t; + } + if (printatt_counter == 100) { + printatt_counter = 0; + printf("[position_estimator_inav] dt = %.6f\n", dt); + printf("[position_estimator_inav] pitch = %.3f, roll = %.3f, yaw = %.3f\n", + att.pitch, att.roll, att.yaw); + printf("[position_estimator_inav] accel_UAV = %.3f, %.3f, %.3f\n", + sensor.accelerometer_m_s2[0], + sensor.accelerometer_m_s2[1], + sensor.accelerometer_m_s2[2]); + printf("[position_estimator_inav] accel_NED = %.3f, %.3f, %.3f\n", + accel_NED[0], accel_NED[1], accel_NED[2]); + printf("[position_estimator_inav] z = %.2f, vz = %.2f, az = %.2f\n", + z_est[0], z_est[1], z_est[2]); + } + printatt_counter++; + if (t - pub_last > pub_interval) { + pub_last = t; local_pos_est.x = 0.0; local_pos_est.vx = 0.0; local_pos_est.y = 0.0; @@ -374,11 +424,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { vehicle_local_position), local_pos_est_pub, &local_pos_est); } - } /* end of poll return value check */ + } } - printf("[pos_est_inav] exiting.\n"); - mavlink_log_info(mavlink_fd, "[pos_est_inav] exiting"); + printf("[position_estimator_inav] exiting.\n"); + mavlink_log_info(mavlink_fd, "[position_estimator_inav] exiting"); thread_running = false; return 0; } From 114685a40bf38dc4281224ea18f898b6159df037 Mon Sep 17 00:00:00 2001 From: Anton Date: Sat, 30 Mar 2013 21:30:58 +0400 Subject: [PATCH 028/486] position_estimator_inav - first working version --- apps/position_estimator_inav/Makefile | 3 +- .../acceleration_transform.c | 4 +- .../acceleration_transform.h | 4 +- .../kalman_filter_inertial.c | 14 +++- .../kalman_filter_inertial.h | 7 ++ .../position_estimator_inav_main.c | 82 ++++++------------- .../position_estimator_inav_params.c | 69 ++++++++++++++-- .../position_estimator_inav_params.h | 31 +++++-- apps/position_estimator_inav/sounds.c | 40 --------- apps/position_estimator_inav/sounds.h | 16 ---- 10 files changed, 135 insertions(+), 135 deletions(-) delete mode 100644 apps/position_estimator_inav/sounds.c delete mode 100644 apps/position_estimator_inav/sounds.h diff --git a/apps/position_estimator_inav/Makefile b/apps/position_estimator_inav/Makefile index e5f3b4b42b..192ec18255 100644 --- a/apps/position_estimator_inav/Makefile +++ b/apps/position_estimator_inav/Makefile @@ -42,7 +42,6 @@ STACKSIZE = 4096 CSRCS = position_estimator_inav_main.c \ position_estimator_inav_params.c \ kalman_filter_inertial.c \ - acceleration_transform.c \ - sounds.c + acceleration_transform.c include $(APPDIR)/mk/app.mk diff --git a/apps/position_estimator_inav/acceleration_transform.c b/apps/position_estimator_inav/acceleration_transform.c index 9080c4e204..bd933cab4b 100644 --- a/apps/position_estimator_inav/acceleration_transform.c +++ b/apps/position_estimator_inav/acceleration_transform.c @@ -1,8 +1,8 @@ /* * acceleration_transform.c * - * Created on: 30.03.2013 - * Author: Anton Babushkin + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin * * Transform acceleration vector to true orientation and scale * diff --git a/apps/position_estimator_inav/acceleration_transform.h b/apps/position_estimator_inav/acceleration_transform.h index 3692da77ec..26bf0d68fc 100644 --- a/apps/position_estimator_inav/acceleration_transform.h +++ b/apps/position_estimator_inav/acceleration_transform.h @@ -1,8 +1,8 @@ /* * acceleration_transform.h * - * Created on: 30.03.2013 - * Author: ton + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin */ #ifndef ACCELERATION_TRANSFORM_H_ diff --git a/apps/position_estimator_inav/kalman_filter_inertial.c b/apps/position_estimator_inav/kalman_filter_inertial.c index 7de06cb44e..64031ee7bb 100644 --- a/apps/position_estimator_inav/kalman_filter_inertial.c +++ b/apps/position_estimator_inav/kalman_filter_inertial.c @@ -1,3 +1,10 @@ +/* + * kalman_filter_inertial.c + * + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin + */ + #include "kalman_filter_inertial.h" void kalman_filter_inertial_predict(float dt, float x[3]) { @@ -7,10 +14,9 @@ void kalman_filter_inertial_predict(float dt, float x[3]) { void kalman_filter_inertial_update(float x[3], float z[2], float k[3][2], bool use[2]) { float y[2]; - // y = z - x - for (int i = 0; i < 2; i++) { - y[i] = z[i] - x[i]; - } + // y = z - H x + y[0] = z[0] - x[0]; + y[1] = z[1] - x[2]; // x = x + K * y for (int i = 0; i < 3; i++) { // Row for (int j = 0; j < 2; j++) { // Column diff --git a/apps/position_estimator_inav/kalman_filter_inertial.h b/apps/position_estimator_inav/kalman_filter_inertial.h index e6bbf13153..3e5134c33e 100644 --- a/apps/position_estimator_inav/kalman_filter_inertial.h +++ b/apps/position_estimator_inav/kalman_filter_inertial.h @@ -1,3 +1,10 @@ +/* + * kalman_filter_inertial.h + * + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin + */ + #include void kalman_filter_inertial_predict(float dt, float x[3]); diff --git a/apps/position_estimator_inav/position_estimator_inav_main.c b/apps/position_estimator_inav/position_estimator_inav_main.c index 1c6d21ded7..b8ef1f76a4 100644 --- a/apps/position_estimator_inav/position_estimator_inav_main.c +++ b/apps/position_estimator_inav/position_estimator_inav_main.c @@ -1,9 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: Damian Aregger - * Tobias Naegeli - * Lorenz Meier + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -67,15 +65,13 @@ #include #include "position_estimator_inav_params.h" -//#include -#include "sounds.h" -#include #include "kalman_filter_inertial.h" #include "acceleration_transform.h" static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int position_estimator_inav_task; /**< Handle of deamon task / thread */ +static bool verbose_mode = false; __EXPORT int position_estimator_inav_main(int argc, char *argv[]); @@ -89,7 +85,7 @@ static void usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); fprintf(stderr, - "usage: position_estimator_inav {start|stop|status} [-p ]\n\n"); + "usage: position_estimator_inav {start|stop|status} [-v]\n\n"); exit(1); } @@ -106,12 +102,14 @@ int position_estimator_inav_main(int argc, char *argv[]) { usage("missing command"); if (!strcmp(argv[1], "start")) { - if (thread_running) { printf("position_estimator_inav already running\n"); /* this is not an error */ exit(0); } + if (argc > 1) + if (!strcmp(argv[2], "-v")) + verbose_mode = true; thread_should_exit = false; position_estimator_inav_task = task_spawn("position_estimator_inav", @@ -148,17 +146,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); mavlink_log_info(mavlink_fd, "[position_estimator_inav] started"); - /* kalman filter K for simulation parameters: - * rate_accel = 200 Hz - * rate_baro = 100 Hz - * err_accel = 1.0 m/s^2 - * err_baro = 0.2 m - */ - static float k[3][2] = { - { 0.0262f, 0.00001f }, - { 0.0349f, 0.00191f }, - { 0.000259f, 0.618f } - }; /* initialize values */ static float x_est[3] = { 0.0f, 0.0f, 0.0f }; static float y_est[3] = { 0.0f, 0.0f, 0.0f }; @@ -213,14 +200,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { parameters_update(&pos_inav_param_handles, &pos_inav_params); /* END FIRST PARAMETER UPDATE */ - // TODO implement calibration procedure, now put dummy values - int16_t accel_offs[3] = { 0, 0, 0 }; - float accel_T[3][3] = { - { 1.0f, 0.0f, 0.0f }, - { 0.0f, 1.0f, 0.0f }, - { 0.0f, 0.0f, 1.0f } - }; - /* wait until gps signal turns valid, only then can we initialize the projection */ if (use_gps) { struct pollfd fds_init[1] = { { .fd = vehicle_gps_position_sub, @@ -270,11 +249,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { uint16_t baro_updates = 0; hrt_abstime updates_counter_start = hrt_absolute_time(); uint32_t updates_counter_len = 1000000; - hrt_abstime pub_last = 0; + hrt_abstime pub_last = hrt_absolute_time(); uint32_t pub_interval = 5000; // limit publish rate to 200 Hz /* main loop */ - struct pollfd fds[3] = { + struct pollfd fds[5] = { { .fd = parameter_update_sub, .events = POLLIN }, { .fd = vehicle_status_sub, .events = POLLIN }, { .fd = vehicle_attitude_sub, .events = POLLIN }, @@ -288,7 +267,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { int ret = poll(fds, 5, 10); // wait maximal this 10 ms = 100 Hz minimum rate if (ret < 0) { /* poll error */ - printf("[position_estimator_inav] subscriptions poll error\n", ret); + if (verbose_mode) + printf("[position_estimator_inav] subscriptions poll error\n"); } else if (ret > 0) { /* parameter update */ if (fds[0].revents & POLLIN) { @@ -359,7 +339,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { last_time = t; /* calculate corrected acceleration vector in UAV frame */ float accel_corr[3]; - acceleration_correct(accel_corr, sensor.accelerometer_raw, accel_T, accel_offs); + acceleration_correct(accel_corr, sensor.accelerometer_raw, pos_inav_params.acc_T, pos_inav_params.acc_offs); /* transform acceleration vector from UAV frame to NED frame */ float accel_NED[3]; for (int i = 0; i < 3; i++) { @@ -384,34 +364,24 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } if (use_z[0] || use_z[1]) { /* correction */ - kalman_filter_inertial_update(z_est, z_meas, k, use_z); + kalman_filter_inertial_update(z_est, z_meas, pos_inav_params.k, use_z); } - if (t - updates_counter_start > updates_counter_len) { - float updates_dt = (t - updates_counter_start) * 0.000001f; - printf("[position_estimator_inav] accelerometer_updates_rate = %.1/s, baro_updates_rate = %.1f/s\n", accelerometer_updates / updates_dt, baro_updates / updates_dt); - updates_counter_start = t; + if (verbose_mode) { + /* print updates rate */ + if (t - updates_counter_start > updates_counter_len) { + float updates_dt = (t - updates_counter_start) * 0.000001f; + printf("[position_estimator_inav] accelerometer_updates_rate = %.1/s, baro_updates_rate = %.1f/s\n", accelerometer_updates / updates_dt, baro_updates / updates_dt); + updates_counter_start = t; + accelerometer_updates = 0; + baro_updates = 0; + } } - if (printatt_counter == 100) { - printatt_counter = 0; - printf("[position_estimator_inav] dt = %.6f\n", dt); - printf("[position_estimator_inav] pitch = %.3f, roll = %.3f, yaw = %.3f\n", - att.pitch, att.roll, att.yaw); - printf("[position_estimator_inav] accel_UAV = %.3f, %.3f, %.3f\n", - sensor.accelerometer_m_s2[0], - sensor.accelerometer_m_s2[1], - sensor.accelerometer_m_s2[2]); - printf("[position_estimator_inav] accel_NED = %.3f, %.3f, %.3f\n", - accel_NED[0], accel_NED[1], accel_NED[2]); - printf("[position_estimator_inav] z = %.2f, vz = %.2f, az = %.2f\n", - z_est[0], z_est[1], z_est[2]); - } - printatt_counter++; if (t - pub_last > pub_interval) { pub_last = t; - local_pos_est.x = 0.0; - local_pos_est.vx = 0.0; - local_pos_est.y = 0.0; - local_pos_est.vy = 0.0; + local_pos_est.x = 0.0f; + local_pos_est.vx = 0.0f; + local_pos_est.y = 0.0f; + local_pos_est.vy = 0.0f; local_pos_est.z = z_est[0]; local_pos_est.vz = z_est[1]; local_pos_est.timestamp = hrt_absolute_time(); diff --git a/apps/position_estimator_inav/position_estimator_inav_params.c b/apps/position_estimator_inav/position_estimator_inav_params.c index 5567fd2cf2..fb082fbcb2 100644 --- a/apps/position_estimator_inav/position_estimator_inav_params.c +++ b/apps/position_estimator_inav/position_estimator_inav_params.c @@ -1,9 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Damian Aregger - * Tobias Naegeli -* Lorenz Meier + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -44,16 +42,73 @@ /* Kalman Filter covariances */ /* gps measurement noise standard deviation */ -PARAM_DEFINE_FLOAT(POS_EST_R, 1.0f); +PARAM_DEFINE_FLOAT(INAV_K_ALT_00, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_ALT_01, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_ALT_10, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_ALT_11, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_ALT_20, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_ALT_21, 0.0f); + +PARAM_DEFINE_INT32(INAV_ACC_OFFS_0, 0); +PARAM_DEFINE_INT32(INAV_ACC_OFFS_1, 0); +PARAM_DEFINE_INT32(INAV_ACC_OFFS_2, 0); + +PARAM_DEFINE_FLOAT(INAV_ACC_T_00, 0.0021f); +PARAM_DEFINE_FLOAT(INAV_ACC_T_01, 0.0f); +PARAM_DEFINE_FLOAT(INAV_ACC_T_02, 0.0f); +PARAM_DEFINE_FLOAT(INAV_ACC_T_10, 0.0f); +PARAM_DEFINE_FLOAT(INAV_ACC_T_11, 0.0021f); +PARAM_DEFINE_FLOAT(INAV_ACC_T_12, 0.0f); +PARAM_DEFINE_FLOAT(INAV_ACC_T_20, 0.0f); +PARAM_DEFINE_FLOAT(INAV_ACC_T_21, 0.0f); +PARAM_DEFINE_FLOAT(INAV_ACC_T_22, 0.0021f); int parameters_init(struct position_estimator_inav_param_handles *h) { - h->r = param_find("POS_EST_R"); + h->k_alt_00 = param_find("INAV_K_ALT_00"); + h->k_alt_01 = param_find("INAV_K_ALT_01"); + h->k_alt_10 = param_find("INAV_K_ALT_10"); + h->k_alt_11 = param_find("INAV_K_ALT_11"); + h->k_alt_20 = param_find("INAV_K_ALT_20"); + h->k_alt_21 = param_find("INAV_K_ALT_21"); + + h->acc_offs_0 = param_find("INAV_ACC_OFFS_0"); + h->acc_offs_1 = param_find("INAV_ACC_OFFS_1"); + h->acc_offs_2 = param_find("INAV_ACC_OFFS_2"); + + h->acc_t_00 = param_find("INAV_ACC_T_00"); + h->acc_t_01 = param_find("INAV_ACC_T_01"); + h->acc_t_02 = param_find("INAV_ACC_T_02"); + h->acc_t_10 = param_find("INAV_ACC_T_10"); + h->acc_t_11 = param_find("INAV_ACC_T_11"); + h->acc_t_12 = param_find("INAV_ACC_T_12"); + h->acc_t_20 = param_find("INAV_ACC_T_20"); + h->acc_t_21 = param_find("INAV_ACC_T_21"); + h->acc_t_22 = param_find("INAV_ACC_T_22"); return OK; } int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) { - param_get(h->r, &(p->r)); + param_get(h->k_alt_00, &(p->k[0][0])); + param_get(h->k_alt_01, &(p->k[0][1])); + param_get(h->k_alt_10, &(p->k[1][0])); + param_get(h->k_alt_11, &(p->k[1][1])); + param_get(h->k_alt_20, &(p->k[2][0])); + param_get(h->k_alt_21, &(p->k[2][1])); + + param_get(h->acc_offs_0, &(p->acc_offs[0])); + param_get(h->acc_offs_1, &(p->acc_offs[1])); + param_get(h->acc_offs_2, &(p->acc_offs[2])); + + param_get(h->acc_t_00, &(p->acc_T[0][0])); + param_get(h->acc_t_01, &(p->acc_T[0][1])); + param_get(h->acc_t_02, &(p->acc_T[0][2])); + param_get(h->acc_t_10, &(p->acc_T[1][0])); + param_get(h->acc_t_11, &(p->acc_T[1][1])); + param_get(h->acc_t_12, &(p->acc_T[1][2])); + param_get(h->acc_t_20, &(p->acc_T[2][0])); + param_get(h->acc_t_21, &(p->acc_T[2][1])); + param_get(h->acc_t_22, &(p->acc_T[2][2])); return OK; } diff --git a/apps/position_estimator_inav/position_estimator_inav_params.h b/apps/position_estimator_inav/position_estimator_inav_params.h index c45ca81359..694bf015bd 100644 --- a/apps/position_estimator_inav/position_estimator_inav_params.h +++ b/apps/position_estimator_inav/position_estimator_inav_params.h @@ -1,9 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Damian Aregger - * Tobias Naegeli - * Lorenz Meier + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -43,11 +41,32 @@ #include struct position_estimator_inav_params { - float r; + float k[3][2]; + int16_t acc_offs[3]; + float acc_T[3][3]; }; struct position_estimator_inav_param_handles { - param_t r; + param_t k_alt_00; + param_t k_alt_01; + param_t k_alt_10; + param_t k_alt_11; + param_t k_alt_20; + param_t k_alt_21; + + param_t acc_offs_0; + param_t acc_offs_1; + param_t acc_offs_2; + + param_t acc_t_00; + param_t acc_t_01; + param_t acc_t_02; + param_t acc_t_10; + param_t acc_t_11; + param_t acc_t_12; + param_t acc_t_20; + param_t acc_t_21; + param_t acc_t_22; }; /** diff --git a/apps/position_estimator_inav/sounds.c b/apps/position_estimator_inav/sounds.c deleted file mode 100644 index 4f7b05fea5..0000000000 --- a/apps/position_estimator_inav/sounds.c +++ /dev/null @@ -1,40 +0,0 @@ -/* - * sounds.c - * - * Created on: Feb 26, 2013 - * Author: samuezih - */ - -#include -#include -#include - - -static int buzzer; - -int sounds_init(void) -{ - buzzer = open("/dev/tone_alarm", O_WRONLY); - - if (buzzer < 0) { - warnx("Buzzer: open fail\n"); - return ERROR; - } - - return 0; -} - -void sounds_deinit(void) -{ - close(buzzer); -} - -void tune_tetris(void) -{ - ioctl(buzzer, TONE_SET_ALARM, 6); -} - -void tune_sonar(void) -{ - ioctl(buzzer, TONE_SET_ALARM, 7); -} diff --git a/apps/position_estimator_inav/sounds.h b/apps/position_estimator_inav/sounds.h deleted file mode 100644 index 356e42169c..0000000000 --- a/apps/position_estimator_inav/sounds.h +++ /dev/null @@ -1,16 +0,0 @@ -/* - * sounds.h - * - * Created on: Feb 26, 2013 - * Author: samuezih - */ - -#ifndef SOUNDS_H_ -#define SOUNDS_H_ - -int sounds_init(void); -void sounds_deinit(void); -void tune_tetris(void); -void tune_sonar(void); - -#endif /* SOUNDS_H_ */ From 1be74a4716444e08c47ad3eda1f56db6f2893615 Mon Sep 17 00:00:00 2001 From: Anton Date: Sun, 31 Mar 2013 12:24:37 +0400 Subject: [PATCH 029/486] Accelerometers offsets calibration implemented --- .../acceleration_transform.c | 78 +++++---- .../acceleration_transform.h | 3 + .../position_estimator_inav_main.c | 158 +++++++++++++++--- 3 files changed, 179 insertions(+), 60 deletions(-) diff --git a/apps/position_estimator_inav/acceleration_transform.c b/apps/position_estimator_inav/acceleration_transform.c index bd933cab4b..812920878c 100644 --- a/apps/position_estimator_inav/acceleration_transform.c +++ b/apps/position_estimator_inav/acceleration_transform.c @@ -17,23 +17,23 @@ * * * * * Calibration * * * * - * Tests + * Reference vectors + * accel_corr_ref[6][3] = [ g 0 0 ] // positive x + * | -g 0 0 | // negative x + * | 0 g 0 | // positive y + * | 0 -g 0 | // negative y + * | 0 0 g | // positive z + * [ 0 0 -g ] // negative z + * accel_raw_ref[6][3] * - * accel_corr_x_p = [ g 0 0 ]^T // positive x - * accel_corr_x_n = [ -g 0 0 ]^T // negative x - * accel_corr_y_p = [ 0 g 0 ]^T // positive y - * accel_corr_y_n = [ 0 -g 0 ]^T // negative y - * accel_corr_z_p = [ 0 0 g ]^T // positive z - * accel_corr_z_n = [ 0 0 -g ]^T // negative z + * accel_corr_ref[i] = accel_T * (accel_raw_ref[i] - accel_offs), i = 0...5 * - * 6 tests * 3 axes = 18 equations + * 6 reference vectors * 3 axes = 18 equations * 9 (accel_T) + 3 (accel_offs) = 12 unknown constants * * Find accel_offs * - * accel_offs = (accel_raw_x_p + accel_raw_x_n + - * accel_raw_y_p + accel_raw_y_n + - * accel_raw_z_p + accel_raw_z_n) / 6 + * accel_offs[i] = (accel_raw_ref[i*2][i] + accel_raw_ref[i*2+1][i]) / 2 * * * Find accel_T @@ -41,9 +41,8 @@ * 9 unknown constants * need 9 equations -> use 3 of 6 measurements -> 3 * 3 = 9 equations * - * accel_corr[i] = accel_T[i][0] * (accel_raw[0] - accel_offs[0]) + - * accel_T[i][1] * (accel_raw[1] - accel_offs[1]) + - * accel_T[i][2] * (accel_raw[2] - accel_offs[2]) + * accel_corr_ref[i*2] = accel_T * (accel_raw_ref[i*2] - accel_offs), i = 0...2 + * * A x = b * * x = [ accel_T[0][0] ] @@ -56,34 +55,35 @@ * | accel_T[2][1] | * [ accel_T[2][2] ] * - * b = [ accel_corr_x_p[0] ] // One measurement per axis is enough - * | accel_corr_x_p[1] | - * | accel_corr_x_p[2] | - * | accel_corr_y_p[0] | - * | accel_corr_y_p[1] | - * | accel_corr_y_p[2] | - * | accel_corr_z_p[0] | - * | accel_corr_z_p[1] | - * [ accel_corr_z_p[2] ] + * b = [ accel_corr_ref[0][0] ] // One measurement per axis is enough + * | accel_corr_ref[0][1] | + * | accel_corr_ref[0][2] | + * | accel_corr_ref[2][0] | + * | accel_corr_ref[2][1] | + * | accel_corr_ref[2][2] | + * | accel_corr_ref[4][0] | + * | accel_corr_ref[4][1] | + * [ accel_corr_ref[4][2] ] * - * a_x[i] = accel_raw_x_p[i] - accel_offs[i] // Measurement for X axis - * ... - * A = [ a_x[0] a_x[1] a_x[2] 0 0 0 0 0 0 ] - * | 0 0 0 a_x[0] a_x[1] a_x[2] 0 0 0 | - * | 0 0 0 0 0 0 a_x[0] a_x[1] a_x[2] | - * | a_y[0] a_y[1] a_y[2] 0 0 0 0 0 0 | - * | 0 0 0 a_y[0] a_y[1] a_y[2] 0 0 0 | - * | 0 0 0 0 0 0 a_y[0] a_y[1] a_y[2] | - * | a_z[0] a_z[1] a_z[2] 0 0 0 0 0 0 | - * | 0 0 0 a_z[0] a_z[1] a_z[2] 0 0 0 | - * [ 0 0 0 0 0 0 a_z[0] a_z[1] a_z[2] ] + * a[i][j] = accel_raw_ref[i][j] - accel_offs[j], i = 0...5, j = 0...2 + * + * A = [ a[0][0] a[0][1] a[0][2] 0 0 0 0 0 0 ] + * | 0 0 0 a[0][0] a[0][1] a[0][2] 0 0 0 | + * | 0 0 0 0 0 0 a[0][0] a[0][1] a[0][2] | + * | a[1][0] a[1][1] a[1][2] 0 0 0 0 0 0 | + * | 0 0 0 a[1][0] a[1][1] a[1][2] 0 0 0 | + * | 0 0 0 0 0 0 a[1][0] a[1][1] a[1][2] | + * | a[2][0] a[2][1] a[2][2] 0 0 0 0 0 0 | + * | 0 0 0 a[2][0] a[2][1] a[2][2] 0 0 0 | + * [ 0 0 0 0 0 0 a[2][0] a[2][1] a[2][2] ] * * x = A^-1 b */ #include "acceleration_transform.h" -void acceleration_correct(float accel_corr[3], int16_t accel_raw[3], float accel_T[3][3], int16_t accel_offs[3]) { +void acceleration_correct(float accel_corr[3], int16_t accel_raw[3], + float accel_T[3][3], int16_t accel_offs[3]) { for (int i = 0; i < 3; i++) { accel_corr[i] = 0.0f; for (int j = 0; j < 3; j++) { @@ -91,3 +91,11 @@ void acceleration_correct(float accel_corr[3], int16_t accel_raw[3], float accel } } } + +void calculate_calibration_values(int16_t accel_raw_ref[6][3], + float accel_T[3][3], int16_t accel_offs[3], float g) { + for (int i = 0; i < 3; i++) { + accel_offs[i] = (int16_t) (((int32_t) (accel_raw_ref[i * 2][i]) + + (int32_t) (accel_raw_ref[i * 2 + 1][i])) / 2); + } +} diff --git a/apps/position_estimator_inav/acceleration_transform.h b/apps/position_estimator_inav/acceleration_transform.h index 26bf0d68fc..e21aebe73c 100644 --- a/apps/position_estimator_inav/acceleration_transform.h +++ b/apps/position_estimator_inav/acceleration_transform.h @@ -12,4 +12,7 @@ void acceleration_correct(float accel_corr[3], int16_t accel_raw[3], float accel_T[3][3], int16_t accel_offs[3]); +void calculate_calibration_values(int16_t accel_raw_ref[6][3], + float accel_T[3][3], int16_t accel_offs[3], float g); + #endif /* ACCELERATION_TRANSFORM_H_ */ diff --git a/apps/position_estimator_inav/position_estimator_inav_main.c b/apps/position_estimator_inav/position_estimator_inav_main.c index b8ef1f76a4..552046568b 100644 --- a/apps/position_estimator_inav/position_estimator_inav_main.c +++ b/apps/position_estimator_inav/position_estimator_inav_main.c @@ -72,6 +72,7 @@ static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int position_estimator_inav_task; /**< Handle of deamon task / thread */ static bool verbose_mode = false; +const static float const_earth_gravity = 9.81f; __EXPORT int position_estimator_inav_main(int argc, char *argv[]); @@ -84,19 +85,19 @@ static void usage(const char *reason); static void usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); - fprintf(stderr, - "usage: position_estimator_inav {start|stop|status} [-v]\n\n"); - exit(1); -} + fprintf(stderr, + "usage: position_estimator_inav {start|stop|status|calibrate} [-v]\n\n"); + exit(1); + } -/** - * The position_estimator_inav_thread only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ + /** + * The position_estimator_inav_thread only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ int position_estimator_inav_main(int argc, char *argv[]) { if (argc < 1) usage("missing command"); @@ -132,10 +133,112 @@ int position_estimator_inav_main(int argc, char *argv[]) { exit(0); } + if (!strcmp(argv[1], "calibrate")) { + do_accelerometer_calibration(); + exit(0); + } + usage("unrecognized command"); exit(1); } +void wait_for_input() { + printf( + "press any key to continue, 'Q' to abort\n"); + while (true ) { + int c = getchar(); + if (c == 'q' || c == 'Q') { + exit(0); + } else { + return; + } + } +} + +void read_accelerometer_raw_avg(int sensor_combined_sub, int16_t accel_avg[3], + int samples) { + printf("[position_estimator_inav] measuring...\n"); + struct pollfd fds[1] = { { .fd = sensor_combined_sub, .events = POLLIN } }; + int count = 0; + int32_t accel_sum[3] = { 0, 0, 0 }; + while (count < samples) { + int poll_ret = poll(fds, 1, 1000); + if (poll_ret == 1) { + struct sensor_combined_s sensor; + orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); + accel_sum[0] += sensor.accelerometer_raw[0]; + accel_sum[1] += sensor.accelerometer_raw[1]; + accel_sum[2] += sensor.accelerometer_raw[2]; + count++; + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + printf("[position_estimator_inav] no accelerometer data for 1s\n"); + exit(1); + } else { + printf("[position_estimator_inav] poll error\n"); + exit(1); + } + } + for (int i = 0; i < 3; i++) + accel_avg[i] = (accel_sum[i] + count / 2) / count; + printf("[position_estimator_inav] raw data: [ %i %i %i ]\n", accel_avg[0], accel_avg[1], accel_avg[2]); +} + +void do_accelerometer_calibration() { + printf("[position_estimator_inav] calibration started\n"); + const int calibration_samples = 1000; + int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); + int16_t accel_raw_ref[6][3]; // Reference measurements + printf("[position_estimator_inav] place vehicle level, "); + wait_for_input(); + read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[4][0]), + calibration_samples); + printf("[position_estimator_inav] place vehicle on it's back, "); + wait_for_input(); + read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[5][0]), + calibration_samples); + printf("[position_estimator_inav] place vehicle on right side, "); + wait_for_input(); + read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[2][0]), + calibration_samples); + printf("[position_estimator_inav] place vehicle on left side, "); + wait_for_input(); + read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[3][0]), + calibration_samples); + printf("[position_estimator_inav] place vehicle nose down, "); + wait_for_input(); + read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[0][0]), + calibration_samples); + printf("[position_estimator_inav] place vehicle nose up, "); + wait_for_input(); + read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[1][0]), + calibration_samples); + close(sensor_combined_sub); + printf("[position_estimator_inav] reference data collection done\n"); + + int16_t accel_offs[3]; + float accel_T[3][3]; + calculate_calibration_values(accel_raw_ref, accel_T, accel_offs, + const_earth_gravity); + printf("[position_estimator_inav] accelerometers raw offsets: [ %i %i %i ]\n", + accel_offs[0], accel_offs[1], accel_offs[2]); + int32_t accel_offs_int32[3] = { accel_offs[0], accel_offs[1], accel_offs[2] }; + + if (param_set(param_find("INAV_ACC_OFFS_0"), &(accel_offs_int32[0])) + || param_set(param_find("INAV_ACC_OFFS_1"), &(accel_offs_int32[1])) + || param_set(param_find("INAV_ACC_OFFS_2"), &(accel_offs_int32[2]))) { + printf("[position_estimator_inav] setting parameters failed\n"); + exit(1); + } + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + if (save_ret != 0) { + printf("[position_estimator_inav] auto-save of parameters to storage failed\n"); + exit(1); + } + printf("[position_estimator_inav] calibration done\n"); +} + /**************************************************************************** * main ****************************************************************************/ @@ -157,7 +260,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { int baro_loop_cnt = 0; int baro_loop_end = 70; /* measurement for 1 second */ float baro_alt0 = 0.0f; /* to determin while start up */ - const static float const_earth_gravity = 9.81f; static double lat_current = 0.0d; //[°]] --> 47.0 static double lon_current = 0.0d; //[°]] -->8.5 @@ -237,7 +339,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { map_projection_init(lat_current, lon_current); /* publish global position messages only after first GPS message */ } - printf("[position_estimator_inav] initialized projection with: lat: %.10f, lon:%.10f\n", + printf( + "[position_estimator_inav] initialized projection with: lat: %.10f, lon:%.10f\n", lat_current, lon_current); hrt_abstime last_time = 0; @@ -253,13 +356,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { uint32_t pub_interval = 5000; // limit publish rate to 200 Hz /* main loop */ - struct pollfd fds[5] = { - { .fd = parameter_update_sub, .events = POLLIN }, - { .fd = vehicle_status_sub, .events = POLLIN }, - { .fd = vehicle_attitude_sub, .events = POLLIN }, - { .fd = sensor_combined_sub, .events = POLLIN }, - { .fd = vehicle_gps_position_sub, .events = POLLIN } - }; + struct pollfd fds[5] = { { .fd = parameter_update_sub, .events = POLLIN }, { + .fd = vehicle_status_sub, .events = POLLIN }, { .fd = + vehicle_attitude_sub, .events = POLLIN }, { .fd = + sensor_combined_sub, .events = POLLIN }, { .fd = + vehicle_gps_position_sub, .events = POLLIN } }; printf("[position_estimator_inav] main loop started\n"); while (!thread_should_exit) { bool accelerometer_updated = false; @@ -311,7 +412,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { baro_alt0 /= (float) (baro_loop_cnt); local_flag_baroINITdone = true; char str[80]; - sprintf(str, "[position_estimator_inav] barometer initialized with alt0 = %.2f", baro_alt0); + sprintf(str, + "[position_estimator_inav] baro_alt0 = %.2f", + baro_alt0); printf("%s\n", str); mavlink_log_info(mavlink_fd, str); } @@ -339,7 +442,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { last_time = t; /* calculate corrected acceleration vector in UAV frame */ float accel_corr[3]; - acceleration_correct(accel_corr, sensor.accelerometer_raw, pos_inav_params.acc_T, pos_inav_params.acc_offs); + acceleration_correct(accel_corr, sensor.accelerometer_raw, + pos_inav_params.acc_T, pos_inav_params.acc_offs); /* transform acceleration vector from UAV frame to NED frame */ float accel_NED[3]; for (int i = 0; i < 3; i++) { @@ -364,13 +468,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } if (use_z[0] || use_z[1]) { /* correction */ - kalman_filter_inertial_update(z_est, z_meas, pos_inav_params.k, use_z); + kalman_filter_inertial_update(z_est, z_meas, pos_inav_params.k, + use_z); } if (verbose_mode) { /* print updates rate */ if (t - updates_counter_start > updates_counter_len) { float updates_dt = (t - updates_counter_start) * 0.000001f; - printf("[position_estimator_inav] accelerometer_updates_rate = %.1/s, baro_updates_rate = %.1f/s\n", accelerometer_updates / updates_dt, baro_updates / updates_dt); + printf( + "[position_estimator_inav] accelerometer_updates_rate = %.1/s, baro_updates_rate = %.1f/s\n", + accelerometer_updates / updates_dt, + baro_updates / updates_dt); updates_counter_start = t; accelerometer_updates = 0; baro_updates = 0; From d536e3d5f9b0fc3187cdcd8fc20bb8bb600b9e98 Mon Sep 17 00:00:00 2001 From: Anton Date: Sun, 31 Mar 2013 20:42:15 +0400 Subject: [PATCH 030/486] Complete calibration implemented --- .../acceleration_transform.c | 87 ++++++++++++------ .../acceleration_transform.h | 5 +- .../position_estimator_inav_main.c | 89 ++++++++++++------- 3 files changed, 119 insertions(+), 62 deletions(-) diff --git a/apps/position_estimator_inav/acceleration_transform.c b/apps/position_estimator_inav/acceleration_transform.c index 812920878c..3aba9b403d 100644 --- a/apps/position_estimator_inav/acceleration_transform.c +++ b/apps/position_estimator_inav/acceleration_transform.c @@ -43,41 +43,31 @@ * * accel_corr_ref[i*2] = accel_T * (accel_raw_ref[i*2] - accel_offs), i = 0...2 * + * Solve separate system for each row of accel_T: + * + * accel_corr_ref[j*2][i] = accel_T[i] * (accel_raw_ref[j*2] - accel_offs), j = 0...2 + * * A x = b * - * x = [ accel_T[0][0] ] - * | accel_T[0][1] | - * | accel_T[0][2] | - * | accel_T[1][0] | - * | accel_T[1][1] | - * | accel_T[1][2] | - * | accel_T[2][0] | - * | accel_T[2][1] | - * [ accel_T[2][2] ] + * x = [ accel_T[0][i] ] + * | accel_T[1][i] | + * [ accel_T[2][i] ] * - * b = [ accel_corr_ref[0][0] ] // One measurement per axis is enough - * | accel_corr_ref[0][1] | - * | accel_corr_ref[0][2] | - * | accel_corr_ref[2][0] | - * | accel_corr_ref[2][1] | - * | accel_corr_ref[2][2] | - * | accel_corr_ref[4][0] | - * | accel_corr_ref[4][1] | - * [ accel_corr_ref[4][2] ] + * b = [ accel_corr_ref[0][i] ] // One measurement per axis is enough + * | accel_corr_ref[2][i] | + * [ accel_corr_ref[4][i] ] * - * a[i][j] = accel_raw_ref[i][j] - accel_offs[j], i = 0...5, j = 0...2 + * a[i][j] = accel_raw_ref[i][j] - accel_offs[j], i = 0;2;4, j = 0...2 * - * A = [ a[0][0] a[0][1] a[0][2] 0 0 0 0 0 0 ] - * | 0 0 0 a[0][0] a[0][1] a[0][2] 0 0 0 | - * | 0 0 0 0 0 0 a[0][0] a[0][1] a[0][2] | - * | a[1][0] a[1][1] a[1][2] 0 0 0 0 0 0 | - * | 0 0 0 a[1][0] a[1][1] a[1][2] 0 0 0 | - * | 0 0 0 0 0 0 a[1][0] a[1][1] a[1][2] | - * | a[2][0] a[2][1] a[2][2] 0 0 0 0 0 0 | - * | 0 0 0 a[2][0] a[2][1] a[2][2] 0 0 0 | - * [ 0 0 0 0 0 0 a[2][0] a[2][1] a[2][2] ] + * Matrix A is common for all three systems: + * A = [ a[0][0] a[0][1] a[0][2] ] + * | a[2][0] a[2][1] a[2][2] | + * [ a[4][0] a[4][1] a[4][2] ] * * x = A^-1 b + * + * accel_T = A^-1 * g + * g = 9.81 */ #include "acceleration_transform.h" @@ -92,10 +82,49 @@ void acceleration_correct(float accel_corr[3], int16_t accel_raw[3], } } -void calculate_calibration_values(int16_t accel_raw_ref[6][3], +int mat_invert3(float src[3][3], float dst[3][3]) { + float det = src[0][0] * (src[1][1] * src[2][2] - src[1][2] * src[2][1]) - + src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) + + src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]); + if (det == 0.0) + return -1; // Singular matrix + dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det; + dst[1][0] = (src[1][2] * src[2][0] - src[1][0] * src[2][2]) / det; + dst[2][0] = (src[1][0] * src[2][1] - src[1][1] * src[2][0]) / det; + dst[0][1] = (src[0][2] * src[2][1] - src[0][1] * src[2][2]) / det; + dst[1][1] = (src[0][0] * src[2][2] - src[0][2] * src[2][0]) / det; + dst[2][1] = (src[0][1] * src[2][0] - src[0][0] * src[2][1]) / det; + dst[0][2] = (src[0][1] * src[1][2] - src[0][2] * src[1][1]) / det; + dst[1][2] = (src[0][2] * src[1][0] - src[0][0] * src[1][2]) / det; + dst[2][2] = (src[0][0] * src[1][1] - src[0][1] * src[1][0]) / det; + return 0; +} + +int calculate_calibration_values(int16_t accel_raw_ref[6][3], float accel_T[3][3], int16_t accel_offs[3], float g) { + /* calculate raw offsets */ for (int i = 0; i < 3; i++) { accel_offs[i] = (int16_t) (((int32_t) (accel_raw_ref[i * 2][i]) + (int32_t) (accel_raw_ref[i * 2 + 1][i])) / 2); } + /* fill matrix A for linear equations system*/ + float mat_A[3][3]; + memset(mat_A, 0, sizeof(mat_A)); + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + float a = (accel_raw_ref[i * 2][j] - accel_offs[j]); + mat_A[i][j] = a; + } + } + /* calculate inverse matrix for A */ + float mat_A_inv[3][3]; + mat_invert3(mat_A, mat_A_inv); + for (int i = 0; i < 3; i++) { + /* copy results to accel_T */ + for (int j = 0; j < 3; j++) { + /* simplify matrices mult because b has only one non-zero element == g at index i */ + accel_T[j][i] = mat_A_inv[j][i] * g; + } + } + return 0; } diff --git a/apps/position_estimator_inav/acceleration_transform.h b/apps/position_estimator_inav/acceleration_transform.h index e21aebe73c..4d1299db5d 100644 --- a/apps/position_estimator_inav/acceleration_transform.h +++ b/apps/position_estimator_inav/acceleration_transform.h @@ -10,9 +10,10 @@ #include -void acceleration_correct(float accel_corr[3], int16_t accel_raw[3], float accel_T[3][3], int16_t accel_offs[3]); +void acceleration_correct(float accel_corr[3], int16_t accel_raw[3], + float accel_T[3][3], int16_t accel_offs[3]); -void calculate_calibration_values(int16_t accel_raw_ref[6][3], +int calculate_calibration_values(int16_t accel_raw_ref[6][3], float accel_T[3][3], int16_t accel_offs[3], float g); #endif /* ACCELERATION_TRANSFORM_H_ */ diff --git a/apps/position_estimator_inav/position_estimator_inav_main.c b/apps/position_estimator_inav/position_estimator_inav_main.c index 552046568b..292cf7f215 100644 --- a/apps/position_estimator_inav/position_estimator_inav_main.c +++ b/apps/position_estimator_inav/position_estimator_inav_main.c @@ -76,12 +76,15 @@ const static float const_earth_gravity = 9.81f; __EXPORT int position_estimator_inav_main(int argc, char *argv[]); +void do_accelerometer_calibration(); + int position_estimator_inav_thread_main(int argc, char *argv[]); + +static void usage(const char *reason); + /** * Print the correct usage. */ -static void usage(const char *reason); - static void usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); @@ -143,8 +146,7 @@ int position_estimator_inav_main(int argc, char *argv[]) { } void wait_for_input() { - printf( - "press any key to continue, 'Q' to abort\n"); + printf("press any key to continue, 'Q' to abort\n"); while (true ) { int c = getchar(); if (c == 'q' || c == 'Q') { @@ -179,9 +181,11 @@ void read_accelerometer_raw_avg(int sensor_combined_sub, int16_t accel_avg[3], exit(1); } } - for (int i = 0; i < 3; i++) + for (int i = 0; i < 3; i++) { accel_avg[i] = (accel_sum[i] + count / 2) / count; - printf("[position_estimator_inav] raw data: [ %i %i %i ]\n", accel_avg[0], accel_avg[1], accel_avg[2]); + } + printf("[position_estimator_inav] raw data: [ %i %i %i ]\n", accel_avg[0], + accel_avg[1], accel_avg[2]); } void do_accelerometer_calibration() { @@ -191,49 +195,72 @@ void do_accelerometer_calibration() { int16_t accel_raw_ref[6][3]; // Reference measurements printf("[position_estimator_inav] place vehicle level, "); wait_for_input(); - read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[4][0]), + read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[5][0]), calibration_samples); printf("[position_estimator_inav] place vehicle on it's back, "); wait_for_input(); - read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[5][0]), + read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[4][0]), calibration_samples); printf("[position_estimator_inav] place vehicle on right side, "); wait_for_input(); - read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[2][0]), + read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[3][0]), calibration_samples); printf("[position_estimator_inav] place vehicle on left side, "); wait_for_input(); - read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[3][0]), + read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[2][0]), calibration_samples); printf("[position_estimator_inav] place vehicle nose down, "); wait_for_input(); - read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[0][0]), + read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[1][0]), calibration_samples); printf("[position_estimator_inav] place vehicle nose up, "); wait_for_input(); - read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[1][0]), + read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[0][0]), calibration_samples); close(sensor_combined_sub); printf("[position_estimator_inav] reference data collection done\n"); int16_t accel_offs[3]; float accel_T[3][3]; - calculate_calibration_values(accel_raw_ref, accel_T, accel_offs, + int res = calculate_calibration_values(accel_raw_ref, accel_T, accel_offs, const_earth_gravity); - printf("[position_estimator_inav] accelerometers raw offsets: [ %i %i %i ]\n", + if (res != 0) { + printf( + "[position_estimator_inav] calibration parameters calculation error\n"); + exit(1); + + } + printf( + "[position_estimator_inav] accelerometers raw offsets: [ %i %i %i ]\n", accel_offs[0], accel_offs[1], accel_offs[2]); - int32_t accel_offs_int32[3] = { accel_offs[0], accel_offs[1], accel_offs[2] }; + printf( + "[position_estimator_inav] accelerometers transform matrix:\n [ %0.6f %0.6f %0.6f ]\n | %0.6f %0.6f %0.6f |\n [ %0.6f %0.6f %0.6f ]\n", + accel_T[0][0], accel_T[0][1], accel_T[0][2], accel_T[1][0], + accel_T[1][1], accel_T[1][2], accel_T[2][0], accel_T[2][1], + accel_T[2][2]); + int32_t accel_offs_int32[3] = + { accel_offs[0], accel_offs[1], accel_offs[2] }; if (param_set(param_find("INAV_ACC_OFFS_0"), &(accel_offs_int32[0])) || param_set(param_find("INAV_ACC_OFFS_1"), &(accel_offs_int32[1])) - || param_set(param_find("INAV_ACC_OFFS_2"), &(accel_offs_int32[2]))) { + || param_set(param_find("INAV_ACC_OFFS_2"), &(accel_offs_int32[2])) + || param_set(param_find("INAV_ACC_T_00"), &(accel_T[0][0])) + || param_set(param_find("INAV_ACC_T_01"), &(accel_T[0][1])) + || param_set(param_find("INAV_ACC_T_02"), &(accel_T[0][2])) + || param_set(param_find("INAV_ACC_T_10"), &(accel_T[1][0])) + || param_set(param_find("INAV_ACC_T_11"), &(accel_T[1][1])) + || param_set(param_find("INAV_ACC_T_12"), &(accel_T[1][2])) + || param_set(param_find("INAV_ACC_T_20"), &(accel_T[2][0])) + || param_set(param_find("INAV_ACC_T_21"), &(accel_T[2][1])) + || param_set(param_find("INAV_ACC_T_22"), &(accel_T[2][2]))) { printf("[position_estimator_inav] setting parameters failed\n"); exit(1); } /* auto-save to EEPROM */ int save_ret = param_save_default(); if (save_ret != 0) { - printf("[position_estimator_inav] auto-save of parameters to storage failed\n"); + printf( + "[position_estimator_inav] auto-save of parameters to storage failed\n"); exit(1); } printf("[position_estimator_inav] calibration done\n"); @@ -253,8 +280,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { static float x_est[3] = { 0.0f, 0.0f, 0.0f }; static float y_est[3] = { 0.0f, 0.0f, 0.0f }; static float z_est[3] = { 0.0f, 0.0f, 0.0f }; - const static float dT_const_120 = 1.0f / 120.0f; - const static float dT2_const_120 = 1.0f / 120.0f / 120.0f / 2.0f; bool use_gps = false; int baro_loop_cnt = 0; @@ -308,12 +333,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { .events = POLLIN } }; while (gps.fix_type < 3) { - - /* wait for GPS updates, BUT READ VEHICLE STATUS (!) - * this choice is critical, since the vehicle status might not - * actually change, if this app is started after GPS lock was - * aquired. - */ if (poll(fds_init, 1, 5000)) { /* poll only two first subscriptions */ if (fds_init[0].revents & POLLIN) { /* Wait for the GPS update to propagate (we have some time) */ @@ -345,15 +364,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { hrt_abstime last_time = 0; thread_running = true; - static int printatt_counter = 0; uint32_t accelerometer_counter = 0; uint32_t baro_counter = 0; uint16_t accelerometer_updates = 0; uint16_t baro_updates = 0; + uint16_t gps_updates = 0; + uint16_t attitude_updates = 0; hrt_abstime updates_counter_start = hrt_absolute_time(); uint32_t updates_counter_len = 1000000; hrt_abstime pub_last = hrt_absolute_time(); - uint32_t pub_interval = 5000; // limit publish rate to 200 Hz + uint32_t pub_interval = 4000; // limit publish rate to 250 Hz /* main loop */ struct pollfd fds[5] = { { .fd = parameter_update_sub, .events = POLLIN }, { @@ -368,8 +388,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { int ret = poll(fds, 5, 10); // wait maximal this 10 ms = 100 Hz minimum rate if (ret < 0) { /* poll error */ - if (verbose_mode) - printf("[position_estimator_inav] subscriptions poll error\n"); + printf("[position_estimator_inav] subscriptions poll error\n"); + thread_should_exit = true; + continue; } else if (ret > 0) { /* parameter update */ if (fds[0].revents & POLLIN) { @@ -388,6 +409,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { /* vehicle attitude */ if (fds[2].revents & POLLIN) { orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); + attitude_updates++; } /* sensor combined */ if (fds[3].revents & POLLIN) { @@ -432,6 +454,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { ((double) (gps.lon)) * 1e-7, &(local_pos_gps[0]), &(local_pos_gps[1])); local_pos_gps[2] = (float) (gps.alt * 1e-3); + gps_updates++; } } @@ -476,12 +499,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { if (t - updates_counter_start > updates_counter_len) { float updates_dt = (t - updates_counter_start) * 0.000001f; printf( - "[position_estimator_inav] accelerometer_updates_rate = %.1/s, baro_updates_rate = %.1f/s\n", + "[position_estimator_inav] updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s\n", accelerometer_updates / updates_dt, - baro_updates / updates_dt); + baro_updates / updates_dt, + gps_updates / updates_dt, + attitude_updates / updates_dt); updates_counter_start = t; accelerometer_updates = 0; baro_updates = 0; + gps_updates = 0; + attitude_updates = 0; } } if (t - pub_last > pub_interval) { From f82af140aea5ea9365cf383e0102056019c99fec Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 31 Mar 2013 20:56:43 -0700 Subject: [PATCH 031/486] Hand-merge F427 patches. --- nuttx/arch/arm/include/stm32/chip.h | 97 +++++++++ .../arch/arm/include/stm32/stm32f40xxx_irq.h | 11 +- nuttx/arch/arm/src/stm32/Kconfig | 110 +++++++++- nuttx/arch/arm/src/stm32/chip/stm32_flash.h | 25 ++- nuttx/arch/arm/src/stm32/chip/stm32_i2c.h | 20 ++ nuttx/arch/arm/src/stm32/chip/stm32_pwr.h | 11 +- nuttx/arch/arm/src/stm32/chip/stm32_syscfg.h | 5 + nuttx/arch/arm/src/stm32/chip/stm32_uart.h | 18 ++ .../arch/arm/src/stm32/chip/stm32f40xxx_dma.h | 33 ++- .../src/stm32/chip/stm32f40xxx_memorymap.h | 5 + .../arm/src/stm32/chip/stm32f40xxx_pinmap.h | 50 ++++- .../arch/arm/src/stm32/chip/stm32f40xxx_rcc.h | 50 ++++- .../arm/src/stm32/chip/stm32f40xxx_vectors.h | 20 +- nuttx/arch/arm/src/stm32/stm32_lowputc.c | 34 +++ nuttx/arch/arm/src/stm32/stm32_serial.c | 193 ++++++++++++++++- nuttx/arch/arm/src/stm32/stm32_spi.c | 194 +++++++++++++++++- nuttx/arch/arm/src/stm32/stm32_spi.h | 22 +- nuttx/arch/arm/src/stm32/stm32_uart.h | 70 ++++++- nuttx/arch/arm/src/stm32/stm32f40xxx_rcc.c | 36 ++++ nuttx/drivers/serial/Kconfig | 194 ++++++++++++++++++ 20 files changed, 1172 insertions(+), 26 deletions(-) diff --git a/nuttx/arch/arm/include/stm32/chip.h b/nuttx/arch/arm/include/stm32/chip.h index 14d92ea3d9..4738854710 100644 --- a/nuttx/arch/arm/include/stm32/chip.h +++ b/nuttx/arch/arm/include/stm32/chip.h @@ -688,6 +688,103 @@ # define STM32_NRNG 1 /* Random number generator (RNG) */ # define STM32_NDCMI 1 /* Digital camera interface (DCMI) */ +#elif defined(CONFIG_ARCH_CHIP_STM32F427I) /* BGA176; LQFP176 1024/2048KiB flash 256KiB SRAM */ +# undef CONFIG_STM32_STM32F10XX /* STM32F10xxx family */ +# undef CONFIG_STM32_LOWDENSITY /* STM32F100x, STM32F101x, STM32F102x and STM32F103x w/ 16/32 Kbytes */ +# undef CONFIG_STM32_MEDIUMDENSITY /* STM32F100x, STM32F101x, STM32F102x and STM32F103x w/ 64/128 Kbytes */ +# undef CONFIG_STM32_HIGHDENSITY /* STM32F100x, STM32F101x, and STM32F103x w/ 256/512 Kbytes */ +# undef CONFIG_STM32_VALUELINE /* STM32F100x */ +# undef CONFIG_STM32_CONNECTIVITYLINE /* STM32F105x and STM32F107x */ +# undef CONFIG_STM32_STM32F20XX /* STM32F205x and STM32F207x */ +# undef CONFIG_STM32_STM32F30XX /* STM32F30xxx family */ +# define CONFIG_STM32_STM32F40XX 1 /* STM32F405xx, STM32407xx and STM32F427/437 */ +# define STM32_NFSMC 1 /* FSMC */ +# define STM32_NATIM 2 /* Two advanced timers TIM1 and 8 */ +# define STM32_NGTIM 4 /* 16-bit general timers TIM3 and 4 with DMA + * 32-bit general timers TIM2 and 5 with DMA */ +# define STM32_NGTIMNDMA 6 /* 16-bit general timers TIM9-14 without DMA */ +# define STM32_NBTIM 2 /* Two basic timers, TIM6-7 */ +# define STM32_NDMA 2 /* DMA1-2 */ +# define STM32_NSPI 6 /* SPI1-6 */ +# define STM32_NI2S 2 /* I2S1-2 (multiplexed with SPI2-3) */ +# define STM32_NUSART 8 /* USART1-3 and 6, UART 4-5 and 7-8 */ +# define STM32_NI2C 3 /* I2C1-3 */ +# define STM32_NCAN 2 /* CAN1-2 */ +# define STM32_NSDIO 1 /* SDIO */ +# define STM32_NUSBOTG 1 /* USB OTG FS/HS */ +# define STM32_NGPIO 139 /* GPIOA-I */ +# define STM32_NADC 3 /* 12-bit ADC1-3, 24 channels */ +# define STM32_NDAC 2 /* 12-bit DAC1-2 */ +# define STM32_NCRC 1 /* CRC */ +# define STM32_NETHERNET 1 /* 100/100 Ethernet MAC */ +# define STM32_NRNG 1 /* Random number generator (RNG) */ +# define STM32_NDCMI 1 /* Digital camera interface (DCMI) */ + +#elif defined(CONFIG_ARCH_CHIP_STM32F427Z) /* LQFP144 1024/2048KiB flash 256KiB SRAM */ +# undef CONFIG_STM32_STM32F10XX /* STM32F10xxx family */ +# undef CONFIG_STM32_LOWDENSITY /* STM32F100x, STM32F101x, STM32F102x and STM32F103x w/ 16/32 Kbytes */ +# undef CONFIG_STM32_MEDIUMDENSITY /* STM32F100x, STM32F101x, STM32F102x and STM32F103x w/ 64/128 Kbytes */ +# undef CONFIG_STM32_HIGHDENSITY /* STM32F100x, STM32F101x, and STM32F103x w/ 256/512 Kbytes */ +# undef CONFIG_STM32_VALUELINE /* STM32F100x */ +# undef CONFIG_STM32_CONNECTIVITYLINE /* STM32F105x and STM32F107x */ +# undef CONFIG_STM32_STM32F20XX /* STM32F205x and STM32F207x */ +# undef CONFIG_STM32_STM32F30XX /* STM32F30xxx family */ +# define CONFIG_STM32_STM32F40XX 1 /* STM32F405xx, STM32407xx and STM32F427/437 */ +# define STM32_NFSMC 1 /* FSMC */ +# define STM32_NATIM 2 /* Two advanced timers TIM1 and 8 */ +# define STM32_NGTIM 4 /* 16-bit general timers TIM3 and 4 with DMA + * 32-bit general timers TIM2 and 5 with DMA */ +# define STM32_NGTIMNDMA 6 /* 16-bit general timers TIM9-14 without DMA */ +# define STM32_NBTIM 2 /* Two basic timers, TIM6-7 */ +# define STM32_NDMA 2 /* DMA1-2 */ +# define STM32_NSPI 6 /* SPI1-6 */ +# define STM32_NI2S 2 /* I2S1-2 (multiplexed with SPI2-3) */ +# define STM32_NUSART 8 /* USART1-3 and 6, UART 4-5 and 7-8 */ +# define STM32_NI2C 3 /* I2C1-3 */ +# define STM32_NCAN 2 /* CAN1-2 */ +# define STM32_NSDIO 1 /* SDIO */ +# define STM32_NUSBOTG 1 /* USB OTG FS/HS */ +# define STM32_NGPIO 139 /* GPIOA-I */ +# define STM32_NADC 3 /* 12-bit ADC1-3, 24 channels */ +# define STM32_NDAC 2 /* 12-bit DAC1-2 */ +# define STM32_NCRC 1 /* CRC */ +# define STM32_NETHERNET 1 /* 100/100 Ethernet MAC */ +# define STM32_NRNG 1 /* Random number generator (RNG) */ +# define STM32_NDCMI 1 /* Digital camera interface (DCMI) */ + +#elif defined(CONFIG_ARCH_CHIP_STM32F427V) /* LQFP100 1024/2048KiB flash 256KiB SRAM */ +# undef CONFIG_STM32_STM32F10XX /* STM32F10xxx family */ +# undef CONFIG_STM32_LOWDENSITY /* STM32F100x, STM32F101x, STM32F102x and STM32F103x w/ 16/32 Kbytes */ +# undef CONFIG_STM32_MEDIUMDENSITY /* STM32F100x, STM32F101x, STM32F102x and STM32F103x w/ 64/128 Kbytes */ +# undef CONFIG_STM32_HIGHDENSITY /* STM32F100x, STM32F101x, and STM32F103x w/ 256/512 Kbytes */ +# undef CONFIG_STM32_VALUELINE /* STM32F100x */ +# undef CONFIG_STM32_CONNECTIVITYLINE /* STM32F105x and STM32F107x */ +# undef CONFIG_STM32_STM32F20XX /* STM32F205x and STM32F207x */ +# undef CONFIG_STM32_STM32F30XX /* STM32F30xxx family */ +# define CONFIG_STM32_STM32F40XX 1 /* STM32F405xx, STM32407xx and STM32F427/437 */ +# define STM32_NFSMC 1 /* FSMC */ +# define STM32_NATIM 2 /* Two advanced timers TIM1 and 8 */ +# define STM32_NGTIM 4 /* 16-bit general timers TIM3 and 4 with DMA + * 32-bit general timers TIM2 and 5 with DMA */ +# define STM32_NGTIMNDMA 6 /* 16-bit general timers TIM9-14 without DMA */ +# define STM32_NBTIM 2 /* Two basic timers, TIM6-7 */ +# define STM32_NDMA 2 /* DMA1-2 */ +# define STM32_NSPI 4 /* SPI1-4 */ +# define STM32_NI2S 2 /* I2S1-2 (multiplexed with SPI2-3) */ +# define STM32_NUSART 8 /* USART1-3 and 6, UART 4-5 and 7-8 */ +# define STM32_NI2C 3 /* I2C1-3 */ +# define STM32_NCAN 2 /* CAN1-2 */ +# define STM32_NSDIO 1 /* SDIO */ +# define STM32_NUSBOTG 1 /* USB OTG FS/HS */ +# define STM32_NGPIO 139 /* GPIOA-I */ +# define STM32_NADC 3 /* 12-bit ADC1-3, 24 channels */ +# define STM32_NDAC 2 /* 12-bit DAC1-2 */ +# define STM32_NCRC 1 /* CRC */ +# define STM32_NETHERNET 1 /* 100/100 Ethernet MAC */ +# define STM32_NRNG 1 /* Random number generator (RNG) */ +# define STM32_NDCMI 1 /* Digital camera interface (DCMI) */ + + #else # error "Unsupported STM32 chip" #endif diff --git a/nuttx/arch/arm/include/stm32/stm32f40xxx_irq.h b/nuttx/arch/arm/include/stm32/stm32f40xxx_irq.h index cd97b9c9df..524568778b 100644 --- a/nuttx/arch/arm/include/stm32/stm32f40xxx_irq.h +++ b/nuttx/arch/arm/include/stm32/stm32f40xxx_irq.h @@ -153,7 +153,16 @@ #define STM32_IRQ_RNG (STM32_IRQ_INTERRUPTS+80) /* 80: Hash and Rng global interrupt */ #define STM32_IRQ_FPU (STM32_IRQ_INTERRUPTS+81) /* 81: FPU global interrupt */ -#define NR_IRQS (STM32_IRQ_INTERRUPTS+82) +#ifndef CONFIG_STM32_STM32F427 +# define NR_IRQS (STM32_IRQ_INTERRUPTS+87) +#else +# define STM32_IRQ_UART7 (STM32_IRQ_INTERRUPTS+82) /* 82: UART7 interrupt */ +# define STM32_IRQ_UART8 (STM32_IRQ_INTERRUPTS+83) /* 83: UART8 interrupt */ +# define STM32_IRQ_SPI4 (STM32_IRQ_INTERRUPTS+84) /* 84: SPI4 interrupt */ +# define STM32_IRQ_SPI5 (STM32_IRQ_INTERRUPTS+85) /* 85: SPI5 interrupt */ +# define STM32_IRQ_SPI6 (STM32_IRQ_INTERRUPTS+86) /* 86: SPI6 interrupt */ +# define NR_IRQS (STM32_IRQ_INTERRUPTS+87) +#endif /**************************************************************************************************** * Public Types diff --git a/nuttx/arch/arm/src/stm32/Kconfig b/nuttx/arch/arm/src/stm32/Kconfig index 41724be2d4..ca6a828676 100644 --- a/nuttx/arch/arm/src/stm32/Kconfig +++ b/nuttx/arch/arm/src/stm32/Kconfig @@ -173,6 +173,24 @@ config ARCH_CHIP_STM32F407IG select ARCH_CORTEXM4 select STM32_STM32F40XX +config ARCH_CHIP_STM32F427V + bool "STM32F427V" + select ARCH_CORTEXM4 + select STM32_STM32F40XX + select STM32_STM32F427 + +config ARCH_CHIP_STM32F427Z + bool "STM32F427Z" + select ARCH_CORTEXM4 + select STM32_STM32F40XX + select STM32_STM32F427 + +config ARCH_CHIP_STM32F427I + bool "STM32F427I" + select ARCH_CORTEXM4 + select STM32_STM32F40XX + select STM32_STM32F427 + endchoice config STM32_STM32F10XX @@ -193,6 +211,10 @@ config STM32_STM32F20XX config STM32_STM32F40XX bool +# This is really 427/437, but we treat the two the same. +config STM32_STM32F427 + bool + config STM32_DFU bool "DFU bootloader" default n @@ -370,6 +392,27 @@ config STM32_SPI3 select SPI select STM32_SPI +config STM32_SPI4 + bool "SPI4" + default n + depends on STM32_STM32F427 + select SPI + select STM32_SPI + +config STM32_SPI5 + bool "SPI5" + default n + depends on STM32_STM32F427 + select SPI + select STM32_SPI + +config STM32_SPI6 + bool "SPI6" + default n + depends on STM32_STM32F427 + select SPI + select STM32_SPI + config STM32_SYSCFG bool "SYSCFG" default y @@ -490,6 +533,20 @@ config STM32_USART6 select ARCH_HAVE_USART6 select STM32_USART +config STM32_UART7 + bool "UART7" + default n + depends on STM32_STM32F427 + select ARCH_HAVE_UART7 + select STM32_USART + +config STM32_UART8 + bool "UART8" + default n + depends on STM32_STM32F427 + select ARCH_HAVE_UART8 + select STM32_USART + config STM32_USB bool "USB Device" default n @@ -698,6 +755,7 @@ endmenu config STM32_FLASH_PREFETCH bool "Enable FLASH Pre-fetch" depends on STM32_STM32F20XX || STM32_STM32F40XX + default y if STM32_STM32F427 default n ---help--- Enable FLASH prefetch and F2 and F4 parts (FLASH pre-fetch is always enabled @@ -1966,9 +2024,59 @@ config USART6_RXDMA ---help--- In high data rate usage, Rx DMA may eliminate Rx overrun errors +config USART7_RS485 + bool "RS-485 on USART7" + default n + depends on STM32_USART7 + ---help--- + Enable RS-485 interface on USART7. Your board config will have to + provide GPIO_USART7_RS485_DIR pin definition. Currently it cannot be + used with USART7_RXDMA. + +config USART7_RS485_DIR_POLARITY + int "USART7 RS-485 DIR pin polarity" + default 1 + range 0 1 + depends on USART7_RS485 + ---help--- + Polarity of DIR pin for RS-485 on USART7. Set to state on DIR pin which + enables TX (0 - low / nTXEN, 1 - high / TXEN). + +config USART7_RXDMA + bool "USART7 Rx DMA" + default n + depends on STM32_STM32F40XX && STM32_DMA2 + ---help--- + In high data rate usage, Rx DMA may eliminate Rx overrun errors + +config USART8_RS485 + bool "RS-485 on USART8" + default n + depends on STM32_USART8 + ---help--- + Enable RS-485 interface on USART8. Your board config will have to + provide GPIO_USART8_RS485_DIR pin definition. Currently it cannot be + used with USART8_RXDMA. + +config USART8_RS485_DIR_POLARITY + int "USART8 RS-485 DIR pin polarity" + default 1 + range 0 1 + depends on USART8_RS485 + ---help--- + Polarity of DIR pin for RS-485 on USART8. Set to state on DIR pin which + enables TX (0 - low / nTXEN, 1 - high / TXEN). + +config USART8_RXDMA + bool "USART8 Rx DMA" + default n + depends on STM32_STM32F40XX && STM32_DMA2 + ---help--- + In high data rate usage, Rx DMA may eliminate Rx overrun errors + config SERIAL_TERMIOS bool "Serial driver TERMIOS supported" - depends on STM32_USART1 || STM32_USART2 || STM32_USART3 || STM32_UART4 || STM32_UART5 || STM32_USART6 + depends on STM32_USART1 || STM32_USART2 || STM32_USART3 || STM32_UART4 || STM32_UART5 || STM32_USART6 || STM32_USART7 || STM32_USART8 default n ---help--- Serial driver supports termios.h interfaces (tcsetattr, tcflush, etc.). diff --git a/nuttx/arch/arm/src/stm32/chip/stm32_flash.h b/nuttx/arch/arm/src/stm32/chip/stm32_flash.h index d6fcecc114..9de83893d3 100644 --- a/nuttx/arch/arm/src/stm32/chip/stm32_flash.h +++ b/nuttx/arch/arm/src/stm32/chip/stm32_flash.h @@ -55,6 +55,7 @@ #elif defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX) # define STM32_FLASH_NPAGES 8 # define STM32_FLASH_PAGESIZE (128*1024) + /* XXX this is wrong for 427, and not really right for 40x due to mixed page sizes */ #endif #define STM32_FLASH_SIZE (STM32_FLASH_NPAGES * STM32_FLASH_PAGESIZE) @@ -74,6 +75,9 @@ #elif defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX) # define STM32_FLASH_OPTCR_OFFSET 0x0014 #endif +#if defined(CONFIG_STM32_STM32F427) +# define STM32_FLASH_OPTCR1_OFFSET 0x0018 +#endif /* Register Addresses ***************************************************************/ @@ -88,7 +92,10 @@ # define STM32_FLASH_OBR (STM32_FLASHIF_BASE+STM32_FLASH_OBR_OFFSET) # define STM32_FLASH_WRPR (STM32_FLASHIF_BASE+STM32_FLASH_WRPR_OFFSET) #elif defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX) -# define STM32_FLASH_OPTCR (STM32_FLASHIF_BASE+STM32_FLASH_OPTCR_OFFSET) +# define STM32_FLASH_OPTCR (STM32_FLASHIF_BASE+STM32_FLASH_OPTCR_OFFSET) +#endif +#if defined(CONFIG_STM32_STM32F427) +# define STM32_FLASH_OPTCR1 (STM32_FLASHIF_BASE+STM32_FLASH_OPTCR1_OFFSET) #endif /* Register Bitfield Definitions ****************************************************/ @@ -150,10 +157,14 @@ #elif defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX) # define FLASH_CR_PG (1 << 0) /* Bit 0: Programming */ # define FLASH_CR_SER (1 << 1) /* Bit 1: Sector Erase */ -# define FLASH_CR_MER (1 << 2) /* Bit 2: Mass Erase */ +# define FLASH_CR_MER (1 << 2) /* Bit 2: Mass Erase sectors 0..11 */ # define FLASH_CR_SNB_SHIFT (3) /* Bits 3-6: Sector number */ # define FLASH_CR_SNB_MASK (15 << FLASH_CR_SNB_SHIFT) +#if defined(CONFIG_STM32_STM32F427) +# define FLASH_CR_SNB(n) (((n % 12) << FLASH_CR_SNB_SHIFT) | ((n / 12) << 7)) /* Sector n, n=0..23 */ +#else # define FLASH_CR_SNB(n) ((n) << FLASH_CR_SNB_SHIFT) /* Sector n, n=0..11 */ +#endif # define FLASH_CR_PSIZE_SHIFT (8) /* Bits 8-9: Program size */ # define FLASH_CR_PSIZE_MASK (3 << FLASH_CR_PSIZE_SHIFT) # define FLASH_CR_PSIZE_X8 (0 << FLASH_CR_PSIZE_SHIFT) /* 00 program x8 */ @@ -164,6 +175,9 @@ # define FLASH_CR_ERRIE (1 << 25) /* Bit 25: Error interrupt enable */ # define FLASH_CR_LOCK (1 << 31) /* Bit 31: Lock */ #endif +#if defined(CONFIG_STM32_STM32F427) +# define FLASH_CR_MER1 (1 << 15) /* Bit 15: Mass Erase sectors 12..23 */ +#endif /* Flash Option Control Register (OPTCR) */ @@ -187,5 +201,12 @@ # define FLASH_OPTCR_NWRP_MASK (0xfff << FLASH_OPTCR_NWRP_SHIFT) #endif +/* Flash Option Control Register (OPTCR1) */ + +#if defined(CONFIG_STM32_STM32F427) +# define FLASH_OPTCR1_NWRP_SHIFT (16) /* Bits 16-27: Not write protect (high bank) */ +# define FLASH_OPTCR1_NWRP_MASK (0xfff << FLASH_OPTCR_NWRP_SHIFT) +#endif + #endif /* __ARCH_ARM_SRC_STM32_CHIP_STM32_FLASH_H */ diff --git a/nuttx/arch/arm/src/stm32/chip/stm32_i2c.h b/nuttx/arch/arm/src/stm32/chip/stm32_i2c.h index f481245e0b..cb2934d101 100644 --- a/nuttx/arch/arm/src/stm32/chip/stm32_i2c.h +++ b/nuttx/arch/arm/src/stm32/chip/stm32_i2c.h @@ -51,6 +51,9 @@ #define STM32_I2C_SR2_OFFSET 0x0018 /* Status register 2 (16-bit) */ #define STM32_I2C_CCR_OFFSET 0x001c /* Clock control register (16-bit) */ #define STM32_I2C_TRISE_OFFSET 0x0020 /* TRISE Register (16-bit) */ +#ifdef CONFIG_STM32_STM32F427 +# define STM32_I2C_FLTR_OFFSET 0x0024 /* FLTR Register (16-bit) */ +#endif /* Register Addresses ***************************************************************/ @@ -64,6 +67,9 @@ # define STM32_I2C1_SR2 (STM32_I2C1_BASE+STM32_I2C_SR2_OFFSET) # define STM32_I2C1_CCR (STM32_I2C1_BASE+STM32_I2C_CCR_OFFSET) # define STM32_I2C1_TRISE (STM32_I2C1_BASE+STM32_I2C_TRISE_OFFSET) +# ifdef STM32_I2C_FLTR_OFFSET +# define STM32_I2C1_FLTR (STM32_I2C1_BASE+STM32_I2C_FLTR_OFFSET) +# endif #endif #if STM32_NI2C > 1 @@ -76,6 +82,9 @@ # define STM32_I2C2_SR2 (STM32_I2C2_BASE+STM32_I2C_SR2_OFFSET) # define STM32_I2C2_CCR (STM32_I2C2_BASE+STM32_I2C_CCR_OFFSET) # define STM32_I2C2_TRISE (STM32_I2C2_BASE+STM32_I2C_TRISE_OFFSET) +# ifdef STM32_I2C_FLTR_OFFSET +# define STM32_I2C2_FLTR (STM32_I2C2_BASE+STM32_I2C_FLTR_OFFSET) +# endif #endif #if STM32_NI2C > 2 @@ -88,6 +97,9 @@ # define STM32_I2C3_SR2 (STM32_I2C3_BASE+STM32_I2C_SR2_OFFSET) # define STM32_I2C3_CCR (STM32_I2C3_BASE+STM32_I2C_CCR_OFFSET) # define STM32_I2C3_TRISE (STM32_I2C3_BASE+STM32_I2C_TRISE_OFFSET) +# ifdef STM32_I2C_FLTR_OFFSET +# define STM32_I2C3_FLTR (STM32_I2C3_BASE+STM32_I2C_FLTR_OFFSET) +# endif #endif /* Register Bitfield Definitions ****************************************************/ @@ -188,5 +200,13 @@ #define I2C_TRISE_SHIFT (0) /* Bits 5-0: Maximum Rise Time in Fast/Standard mode (Master mode) */ #define I2C_TRISE_MASK (0x3f << I2C_TRISE_SHIFT) +/* FLTR Register */ + +#ifdef STM32_I2C_FLTR_OFFSET +# define I2C_FLTR_ANOFF (1 << 4) /* Bit 4: Analog noise filter disable */ +# define I2C_FLTR_DNF_SHIFT 0 /* Bits 0-3: Digital noise filter */ +# define I2C_FLTR_DNF_MASK (0xf << I2C_FLTR_DNF_SHIFT) +#endif + #endif /* __ARCH_ARM_SRC_STM32_CHIP_STM32_I2C_H */ diff --git a/nuttx/arch/arm/src/stm32/chip/stm32_pwr.h b/nuttx/arch/arm/src/stm32/chip/stm32_pwr.h index 2ece6a3572..72f19b6dfa 100644 --- a/nuttx/arch/arm/src/stm32/chip/stm32_pwr.h +++ b/nuttx/arch/arm/src/stm32/chip/stm32_pwr.h @@ -80,7 +80,16 @@ #if defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX) # define PWR_CR_FPDS (1 << 9) /* Bit 9: Flash power down in Stop mode */ -# define PWR_CR_VOS (1 << 14) /* Bit 14: Regulator voltage scaling output selection */ +# if defined(CONFIG_STM32_STM32F427) +# define PWR_CR_ADCDC1 (1 << 13) /* Bit 13: see AN4073 for details */ +# define PWR_CR_VOS_SCALE_1 (3 << 14) /* Fmax = 168MHz */ +# define PWR_CR_VOS_SCALE_2 (2 << 14) /* Fmax = 144MHz */ +# define PWR_CR_VOS_SCALE_3 (1 << 14) /* Fmax = 120MHz */ +# define PWR_CR_VOS_MASK (3 << 14) /* Bits 14-15: Regulator voltage scaling output selection */ +# else +# define PWR_CR_VOS (1 << 14) /* Bit 14: Regulator voltage scaling output selection */ + /* 0: Fmax = 144MHz 1: Fmax = 168MHz */ +# endif #endif /* Power control/status register */ diff --git a/nuttx/arch/arm/src/stm32/chip/stm32_syscfg.h b/nuttx/arch/arm/src/stm32/chip/stm32_syscfg.h index 6642b13059..dca3820fd1 100644 --- a/nuttx/arch/arm/src/stm32/chip/stm32_syscfg.h +++ b/nuttx/arch/arm/src/stm32/chip/stm32_syscfg.h @@ -89,6 +89,11 @@ /* SYSCFG peripheral mode configuration register */ #define SYSCFG_PMC_MII_RMII_SEL (1 << 23) /* Bit 23: Ethernet PHY interface selection */ ++#ifdef CONFIG_STM32_STM32F427 ++# define SYSCFG_PMC_ADC3DC2 (1 << 18) /* Bit 18: See AN4073 */ ++# define SYSCFG_PMC_ADC2DC2 (1 << 17) /* Bit 17: See AN4073 */ ++# define SYSCFG_PMC_ADC1DC2 (1 << 16) /* Bit 16: See AN4073 */ ++#endif /* SYSCFG external interrupt configuration register 1-4 */ diff --git a/nuttx/arch/arm/src/stm32/chip/stm32_uart.h b/nuttx/arch/arm/src/stm32/chip/stm32_uart.h index d3c1e137e6..38852553db 100644 --- a/nuttx/arch/arm/src/stm32/chip/stm32_uart.h +++ b/nuttx/arch/arm/src/stm32/chip/stm32_uart.h @@ -118,6 +118,24 @@ # define STM32_USART6_GTPR (STM32_USART6_BASE+STM32_USART_GTPR_OFFSET) #endif +#if STM32_NUSART > 6 +# define STM32_UART7_SR (STM32_UART7_BASE+STM32_USART_SR_OFFSET) +# define STM32_UART7_DR (STM32_UART7_BASE+STM32_USART_DR_OFFSET) +# define STM32_UART7_BRR (STM32_UART7_BASE+STM32_USART_BRR_OFFSET) +# define STM32_UART7_CR1 (STM32_UART7_BASE+STM32_USART_CR1_OFFSET) +# define STM32_UART7_CR2 (STM32_UART7_BASE+STM32_USART_CR2_OFFSET) +# define STM32_UART7_CR3 (STM32_UART7_BASE+STM32_USART_CR3_OFFSET) +#endif + +#if STM32_NUSART > 7 +# define STM32_UART8_SR (STM32_UART8_BASE+STM32_USART_SR_OFFSET) +# define STM32_UART8_DR (STM32_UART8_BASE+STM32_USART_DR_OFFSET) +# define STM32_UART8_BRR (STM32_UART8_BASE+STM32_USART_BRR_OFFSET) +# define STM32_UART8_CR1 (STM32_UART8_BASE+STM32_USART_CR1_OFFSET) +# define STM32_UART8_CR2 (STM32_UART8_BASE+STM32_USART_CR2_OFFSET) +# define STM32_UART8_CR3 (STM32_UART8_BASE+STM32_USART_CR3_OFFSET) +#endif + /* Register Bitfield Definitions ****************************************************/ /* Status register */ diff --git a/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_dma.h b/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_dma.h index ab0cfceaca..ddd0413a5d 100644 --- a/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_dma.h +++ b/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_dma.h @@ -438,11 +438,21 @@ #define DMAMAP_USART2_TX STM32_DMA_MAP(DMA1,DMA_STREAM6,DMA_CHAN4) #define DMAMAP_UART5_TX STM32_DMA_MAP(DMA1,DMA_STREAM7,DMA_CHAN4) +#ifdef CONFIG_STM32_STM32F427 +# define DMAMAP_UART8_TX STM32_DMA_MAP(CMA1,DMA_STREAM0,DMA_CHAN5) +# define DMAMAP_UART7_TX STM32_DMA_MAP(CMA1,DMA_STREAM1,DMA_CHAN5) +#endif #define DMAMAP_TIM3_CH4 STM32_DMA_MAP(DMA1,DMA_STREAM2,DMA_CHAN5) #define DMAMAP_TIM3_UP STM32_DMA_MAP(DMA1,DMA_STREAM2,DMA_CHAN5) +#ifdef CONFIG_STM32_STM32F427 +# define DMAMAP_UART7_RX STM32_DMA_MAP(CMA1,DMA_STREAM3,DMA_CHAN5) +#endif #define DMAMAP_TIM3_CH1 STM32_DMA_MAP(DMA1,DMA_STREAM4,DMA_CHAN5) #define DMAMAP_TIM3_TRIG STM32_DMA_MAP(DMA1,DMA_STREAM4,DMA_CHAN5) #define DMAMAP_TIM3_CH2 STM32_DMA_MAP(DMA1,DMA_STREAM5,DMA_CHAN5) +#ifdef CONFIG_STM32_STM32F427 +# define DMAMAP_UART8_RX STM32_DMA_MAP(CMA1,DMA_STREAM6,DMA_CHAN5) +#endif #define DMAMAP_TIM3_CH3 STM32_DMA_MAP(DMA1,DMA_STREAM7,DMA_CHAN5) #define DMAMAP_TIM5_CH3 STM32_DMA_MAP(DMA1,DMA_STREAM0,DMA_CHAN6) @@ -475,10 +485,18 @@ #define DMAMAP_DCMI_1 STM32_DMA_MAP(DMA2,DMA_STREAM1,DMA_CHAN1) #define DMAMAP_ADC2_1 STM32_DMA_MAP(DMA2,DMA_STREAM2,DMA_CHAN1) #define DMAMAP_ADC2_2 STM32_DMA_MAP(DMA2,DMA_STREAM3,DMA_CHAN1) +#ifdef CONFIG_STM32_STM32F427 +# define DMAMAP_SPI6_TX STM32_DMA_MAP(DMA2,DMA_STREAM5,DMA_CHAN1) +# define DMAMAP_SPI6_RX STM32_DMA_MAP(DMA2,DMA_STREAM6,DMA_CHAN1) +#endif #define DMAMAP_DCMI_2 STM32_DMA_MAP(DMA2,DMA_STREAM7,DMA_CHAN1) #define DMAMAP_ADC3_1 STM32_DMA_MAP(DMA2,DMA_STREAM0,DMA_CHAN2) #define DMAMAP_ADC3_2 STM32_DMA_MAP(DMA2,DMA_STREAM1,DMA_CHAN2) +#ifdef CONFIG_STM32_STM32F427 +# define DMAMAP_SPI5_RX_1 STM32_DMA_MAP(DMA2,DMA_STREAM3,DMA_CHAN2) +# define DMAMAP_SPI5_TX_1 STM32_DMA_MAP(DMA2,DMA_STREAM4,DMA_CHAN2) +#endif #define DMAMAP_CRYP_OUT STM32_DMA_MAP(DMA2,DMA_STREAM5,DMA_CHAN2) #define DMAMAP_CRYP_IN STM32_DMA_MAP(DMA2,DMA_STREAM6,DMA_CHAN2) #define DMAMAP_HASH_IN STM32_DMA_MAP(DMA2,DMA_STREAM7,DMA_CHAN2) @@ -488,6 +506,11 @@ #define DMAMAP_SPI1_TX_1 STM32_DMA_MAP(DMA2,DMA_STREAM3,DMA_CHAN3) #define DMAMAP_SPI1_TX_2 STM32_DMA_MAP(DMA2,DMA_STREAM5,DMA_CHAN3) +#ifdef CONFIG_STM32_STM32F427 +# define DMAMAP_SPI4_RX_1 STM32_DMA_MAP(DMA2,DMA_STREAM0,DMA_CHAN4) +# define DMAMAP_SPI4_TX_1 STM32_DMA_MAP(DMA2,DMA_STREAM1,DMA_CHAN4) +#endif +#define DMAMAP_SPI4_TX_1 STM32_DMA_MAP(DMA2,DMA_STREAM1,DMA_CHAN4) #define DMAMAP_USART1_RX_1 STM32_DMA_MAP(DMA2,DMA_STREAM2,DMA_CHAN4) #define DMAMAP_SDIO_1 STM32_DMA_MAP(DMA2,DMA_STREAM3,DMA_CHAN4) #define DMAMAP_USART1_RX_2 STM32_DMA_MAP(DMA2,DMA_STREAM5,DMA_CHAN4) @@ -496,6 +519,10 @@ #define DMAMAP_USART6_RX_1 STM32_DMA_MAP(DMA2,DMA_STREAM1,DMA_CHAN5) #define DMAMAP_USART6_RX_2 STM32_DMA_MAP(DMA2,DMA_STREAM2,DMA_CHAN5) +#ifdef CONFIG_STM32_STM32F427 +# define DMAMAP_SPI4_RX_2 STM32_DMA_MAP(DMA2,DMA_STREAM3,DMA_CHAN5) +# define DMAMAP_SPI4_TX_2 STM32_DMA_MAP(DMA2,DMA_STREAM4,DMA_CHAN5) +#endif #define DMAMAP_USART6_TX_1 STM32_DMA_MAP(DMA2,DMA_STREAM6,DMA_CHAN5) #define DMAMAP_USART6_TX_2 STM32_DMA_MAP(DMA2,DMA_STREAM7,DMA_CHAN5) @@ -512,7 +539,11 @@ #define DMAMAP_TIM8_UP STM32_DMA_MAP(DMA2,DMA_STREAM1,DMA_CHAN7) #define DMAMAP_TIM8_CH1_2 STM32_DMA_MAP(DMA2,DMA_STREAM2,DMA_CHAN7) #define DMAMAP_TIM8_CH2_2 STM32_DMA_MAP(DMA2,DMA_STREAM3,DMA_CHAN7) -#define DMAMAP_TIM8_CH3_2 STM32_DMA_MAP(DMA2,DMA_STREAM5,DMA_CHAN7) +#define DMAMAP_TIM8_CH3_2 STM32_DMA_MAP(DMA2,DMA_STREAM4,DMA_CHAN7) +#ifdef CONFIG_STM32_STM32F427 +# define DMAMAP_SPI5_RX_2 STM32_DMA_MAP(DMA2,DMA_STREAM5,DMA_CHAN7) +# define DMAMAP_SPI5_TX_2 STM32_DMA_MAP(DMA2,DMA_STREAM6,DMA_CHAN7) +#endif #define DMAMAP_TIM8_CH4 STM32_DMA_MAP(DMA2,DMA_STREAM7,DMA_CHAN7) #define DMAMAP_TIM8_TRIG STM32_DMA_MAP(DMA2,DMA_STREAM7,DMA_CHAN7) #define DMAMAP_TIM8_COM STM32_DMA_MAP(DMA2,DMA_STREAM7,DMA_CHAN7) diff --git a/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_memorymap.h b/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_memorymap.h index 6b99121216..6dc9530fb2 100644 --- a/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_memorymap.h +++ b/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_memorymap.h @@ -140,6 +140,8 @@ #define STM32_CAN2_BASE 0x40006800 /* 0x40006800-0x40006bff: bxCAN2 */ #define STM32_PWR_BASE 0x40007000 /* 0x40007000-0x400073ff: Power control PWR */ #define STM32_DAC_BASE 0x40007400 /* 0x40007400-0x400077ff: DAC */ +#define STM32_UART7_BASE 0x40007800 /* 0x40007800-0x40007bff: UART7 */ +#define STM32_UART8_BASE 0x40007c00 /* 0x40007c00-0x40007fff: UART8 */ /* APB2 Base Addresses **************************************************************/ @@ -154,11 +156,14 @@ # define STM32_ADCCMN_BASE 0x40012300 /* Common */ #define STM32_SDIO_BASE 0x40012c00 /* 0x40012c00-0x40012fff: SDIO */ #define STM32_SPI1_BASE 0x40013000 /* 0x40013000-0x400133ff: SPI1 */ +#define STM32_SPI4_BASE 0x40013400 /* 0x40013000-0x400137ff: SPI4 */ #define STM32_SYSCFG_BASE 0x40013800 /* 0x40013800-0x40013bff: SYSCFG */ #define STM32_EXTI_BASE 0x40013c00 /* 0x40013c00-0x40013fff: EXTI */ #define STM32_TIM9_BASE 0x40014000 /* 0x40014000-0x400143ff: TIM9 timer */ #define STM32_TIM10_BASE 0x40014400 /* 0x40014400-0x400147ff: TIM10 timer */ #define STM32_TIM11_BASE 0x40014800 /* 0x40014800-0x40014bff: TIM11 timer */ +#define STM32_SPI5_BASE 0x40015000 /* 0x40015000-0x400153ff: SPI5 */ +#define STM32_SPI6_BASE 0x40015400 /* 0x40015400-0x400157ff: SPI6 */ /* AHB1 Base Addresses **************************************************************/ diff --git a/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_pinmap.h b/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_pinmap.h index 31e51caf07..ed3f09c01a 100644 --- a/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_pinmap.h +++ b/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_pinmap.h @@ -331,6 +331,9 @@ #define GPIO_I2S2_CK_1 (GPIO_ALT|GPIO_AF5|GPIO_PORTB|GPIO_PIN10) #define GPIO_I2S2_CK_2 (GPIO_ALT|GPIO_AF5|GPIO_PORTB|GPIO_PIN13) #define GPIO_I2S2_CK_3 (GPIO_ALT|GPIO_AF5|GPIO_PORTI|GPIO_PIN1) +#ifdef CONFIG_STM32_STM32F427 +# define GPIO_I2S2_CK_4 (GPIO_ALT|GPIO_AF5|GPIO_PORTD|GPIO_PIN3) +#endif #define GPIO_I2S2_MCK (GPIO_ALT|GPIO_AF5|GPIO_PORTC|GPIO_PIN6) #define GPIO_I2S2_SD_1 (GPIO_ALT|GPIO_AF5|GPIO_PORTC|GPIO_PIN15) #define GPIO_I2S2_SD_2 (GPIO_ALT|GPIO_AF5|GPIO_PORTC|GPIO_PIN3) @@ -339,6 +342,9 @@ #define GPIO_I2S2_WS_2 (GPIO_ALT|GPIO_AF5|GPIO_PORTB|GPIO_PIN6) #define GPIO_I2S2_WS_3 (GPIO_ALT|GPIO_AF5|GPIO_PORTB|GPIO_PIN9) #define GPIO_I2S2_WS_4 (GPIO_ALT|GPIO_AF5|GPIO_PORTI|GPIO_PIN0) +#ifdef CONFIG_STM32_STM32F427 +# define GPIO_I2S2_WS_6 (GPIO_ALT|GPIO_AF5|GPIO_PORTB|GPIO_PIN6) +#endif #define GPIO_I2S2EXT_SD_1 (GPIO_ALT|GPIO_AF6|GPIO_PORTB|GPIO_PIN14) #define GPIO_I2S2EXT_SD_2 (GPIO_ALT|GPIO_AF6|GPIO_PORTC|GPIO_PIN2) @@ -349,6 +355,9 @@ #define GPIO_I2S3_MCK (GPIO_ALT|GPIO_AF6|GPIO_PORTC|GPIO_PIN7) #define GPIO_I2S3_SD_1 (GPIO_ALT|GPIO_AF6|GPIO_PORTB|GPIO_PIN5) #define GPIO_I2S3_SD_2 (GPIO_ALT|GPIO_AF6|GPIO_PORTC|GPIO_PIN12) +#ifdef CONFIG_STM32_STM32F427 +# define GPIO_I2S3_SD_3 (GPIO_ALT|GPIO_AF6|GPIO_PORTD|GPIO_PIN6) +#endif #define GPIO_I2S3_WS_1 (GPIO_ALT|GPIO_AF6|GPIO_PORTA|GPIO_PIN4) #define GPIO_I2S3_WS_2 (GPIO_ALT|GPIO_AF6|GPIO_PORTA|GPIO_PIN15) @@ -428,7 +437,7 @@ #define GPIO_SPI2_MISO_1 (GPIO_ALT|GPIO_AF5|GPIO_PORTB|GPIO_PIN14) #define GPIO_SPI2_MISO_2 (GPIO_ALT|GPIO_AF5|GPIO_PORTC|GPIO_PIN2) #define GPIO_SPI2_MISO_3 (GPIO_ALT|GPIO_AF5|GPIO_PORTI|GPIO_PIN2) -#define GPIO_SPI2_MOSI_1 (GPIO_ALT|GPIO_AF5|GPIO_PORTC|GPIO_PIN15) +#define GPIO_SPI2_MOSI_1 (GPIO_ALT|GPIO_AF5|GPIO_PORTB|GPIO_PIN15) #define GPIO_SPI2_MOSI_2 (GPIO_ALT|GPIO_AF5|GPIO_PORTC|GPIO_PIN3) #define GPIO_SPI2_MOSI_3 (GPIO_ALT|GPIO_AF5|GPIO_PORTI|GPIO_PIN3) #define GPIO_SPI2_NSS_1 (GPIO_ALT|GPIO_AF5|GPIO_PORTB|GPIO_PIN12) @@ -437,16 +446,45 @@ #define GPIO_SPI2_SCK_1 (GPIO_ALT|GPIO_AF5|GPIO_PORTB|GPIO_PIN10) #define GPIO_SPI2_SCK_2 (GPIO_ALT|GPIO_AF5|GPIO_PORTB|GPIO_PIN13) #define GPIO_SPI2_SCK_3 (GPIO_ALT|GPIO_AF5|GPIO_PORTI|GPIO_PIN1) +#ifdef CONFIG_STM32_STM32F427 +# define GPIO_SPI2_SCK_4 (GPIO_ALT|GPIO_AF5|GPIO_PORTD|GPIO_PIN3) +#endif #define GPIO_SPI3_MISO_1 (GPIO_ALT|GPIO_AF6|GPIO_PORTB|GPIO_PIN4) #define GPIO_SPI3_MISO_2 (GPIO_ALT|GPIO_AF6|GPIO_PORTC|GPIO_PIN11) #define GPIO_SPI3_MOSI_1 (GPIO_ALT|GPIO_AF6|GPIO_PORTB|GPIO_PIN5) #define GPIO_SPI3_MOSI_2 (GPIO_ALT|GPIO_AF6|GPIO_PORTC|GPIO_PIN12) +#ifdef CONFIG_STM32_STM32F427 +# define GPIO_SPI3_MOSI_3 (GPIO_ALT|GPIO_AF6|GPIO_PORTD|GPIO_PIN6) +#endif #define GPIO_SPI3_NSS_1 (GPIO_ALT|GPIO_AF6|GPIO_PORTA|GPIO_PIN15) #define GPIO_SPI3_NSS_2 (GPIO_ALT|GPIO_AF6|GPIO_PORTA|GPIO_PIN4) #define GPIO_SPI3_SCK_1 (GPIO_ALT|GPIO_AF6|GPIO_PORTB|GPIO_PIN3) #define GPIO_SPI3_SCK_2 (GPIO_ALT|GPIO_AF6|GPIO_PORTC|GPIO_PIN10) +#define GPIO_SPI4_MISO_1 (GPIO_ALT|GPIO_AF5|GPIO_PORTE|GPIO_PIN5) +#define GPIO_SPI4_MISO_2 (GPIO_ALT|GPIO_AF5|GPIO_PORTE|GPIO_PIN13) +#define GPIO_SPI4_MOSI_1 (GPIO_ALT|GPIO_AF5|GPIO_PORTE|GPIO_PIN6) +#define GPIO_SPI4_MOSI_2 (GPIO_ALT|GPIO_AF5|GPIO_PORTE|GPIO_PIN14) +#define GPIO_SPI4_NSS_1 (GPIO_ALT|GPIO_AF5|GPIO_PORTE|GPIO_PIN4) +#define GPIO_SPI4_NSS_2 (GPIO_ALT|GPIO_AF5|GPIO_PORTE|GPIO_PIN11) +#define GPIO_SPI4_SCK_1 (GPIO_ALT|GPIO_AF5|GPIO_PORTE|GPIO_PIN2) +#define GPIO_SPI4_SCK_2 (GPIO_ALT|GPIO_AF5|GPIO_PORTE|GPIO_PIN12) + +#define GPIO_SPI5_MISO_1 (GPIO_ALT|GPIO_AF5|GPIO_PORTF|GPIO_PIN8) +#define GPIO_SPI5_MISO_2 (GPIO_ALT|GPIO_AF5|GPIO_PORTH|GPIO_PIN7) +#define GPIO_SPI5_MOSI_1 (GPIO_ALT|GPIO_AF5|GPIO_PORTF|GPIO_PIN9) +#define GPIO_SPI5_MOSI_2 (GPIO_ALT|GPIO_AF5|GPIO_PORTF|GPIO_PIN11) +#define GPIO_SPI5_NSS_1 (GPIO_ALT|GPIO_AF5|GPIO_PORTF|GPIO_PIN6) +#define GPIO_SPI5_NSS_2 (GPIO_ALT|GPIO_AF5|GPIO_PORTH|GPIO_PIN5) +#define GPIO_SPI5_SCK_1 (GPIO_ALT|GPIO_AF5|GPIO_PORTF|GPIO_PIN7) +#define GPIO_SPI5_SCK_2 (GPIO_ALT|GPIO_AF5|GPIO_PORTH|GPIO_PIN6) + +#define GPIO_SPI6_MISO (GPIO_ALT|GPIO_AF5|GPIO_PORTG|GPIO_PIN12) +#define GPIO_SPI6_MOSI (GPIO_ALT|GPIO_AF5|GPIO_PORTG|GPIO_PIN14) +#define GPIO_SPI6_NSS (GPIO_ALT|GPIO_AF5|GPIO_PORTG|GPIO_PIN8) +#define GPIO_SPI6_SCK (GPIO_ALT|GPIO_AF5|GPIO_PORTG|GPIO_PIN13) + /* Timers */ #define GPIO_TIM1_BKIN_1 (GPIO_ALT|GPIO_AF1|GPIO_PORTA|GPIO_PIN6) @@ -691,5 +729,15 @@ #define GPIO_USART6_TX_1 (GPIO_ALT|GPIO_AF8|GPIO_PULLUP|GPIO_SPEED_100MHz|GPIO_PUSHPULL|GPIO_PORTC|GPIO_PIN6) #define GPIO_USART6_TX_2 (GPIO_ALT|GPIO_AF8|GPIO_PULLUP|GPIO_SPEED_100MHz|GPIO_PUSHPULL|GPIO_PORTG|GPIO_PIN14) +#ifdef CONFIG_STM32_STM32F427 +# define GPIO_UART7_RX_1 (GPIO_ALT|GPIO_AF8|GPIO_PULLUP|GPIO_SPEED_100MHz|GPIO_PUSHPULL|GPIO_PORTE|GPIO_PIN7) +# define GPIO_UART7_RX_2 (GPIO_ALT|GPIO_AF8|GPIO_PULLUP|GPIO_SPEED_100MHz|GPIO_PUSHPULL|GPIO_PORTF|GPIO_PIN6) +# define GPIO_UART7_TX_1 (GPIO_ALT|GPIO_AF8|GPIO_PULLUP|GPIO_SPEED_100MHz|GPIO_PUSHPULL|GPIO_PORTE|GPIO_PIN8) +# define GPIO_UART7_TX_2 (GPIO_ALT|GPIO_AF8|GPIO_PULLUP|GPIO_SPEED_100MHz|GPIO_PUSHPULL|GPIO_PORTF|GPIO_PIN7) + +# define GPIO_UART8_RX (GPIO_ALT|GPIO_AF8|GPIO_PULLUP|GPIO_SPEED_100MHz|GPIO_PUSHPULL|GPIO_PORTE|GPIO_PIN0) +# define GPIO_UART8_TX (GPIO_ALT|GPIO_AF8|GPIO_PULLUP|GPIO_SPEED_100MHz|GPIO_PUSHPULL|GPIO_PORTE|GPIO_PIN1) +#endif + #endif /* __ARCH_ARM_SRC_STM32_CHIP_STM32F40XXX_PINMAP_H */ diff --git a/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_rcc.h b/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_rcc.h index 04cb057417..8ab09c478f 100644 --- a/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_rcc.h +++ b/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_rcc.h @@ -65,6 +65,9 @@ #define STM32_RCC_CSR_OFFSET 0x0074 /* Control/status register */ #define STM32_RCC_SSCGR_OFFSET 0x0080 /* Spread spectrum clock generation register */ #define STM32_RCC_PLLI2SCFGR_OFFSET 0x0084 /* PLLI2S configuration register */ +#ifdef CONFIG_STM32_STM32F427 +# define STM32_RCC_DCKCFGR_OFFSET 0x008c /* Dedicated clocks configuration register */ +#endif /* Register Addresses *******************************************************************************/ @@ -91,6 +94,9 @@ #define STM32_RCC_CSR (STM32_RCC_BASE+STM32_RCC_CSR_OFFSET) #define STM32_RCC_SSCGR (STM32_RCC_BASE+STM32_RCC_SSCGR_OFFSET) #define STM32_RCC_PLLI2SCFGR (STM32_RCC_BASE+STM32_RCC_PLLI2SCFGR_OFFSET) +#ifdef CONFIG_STM32_STM32F427 +# define STM32_RCC_DCKCFGR (STM32_RCC_BASE+STM32_RCC_DCKCFGR_OFFSET) +#endif /* Register Bitfield Definitions ********************************************************************/ @@ -282,6 +288,10 @@ #define RCC_APB1RSTR_CAN2RST (1 << 26) /* Bit 26: CAN2 reset */ #define RCC_APB1RSTR_PWRRST (1 << 28) /* Bit 28: Power interface reset */ #define RCC_APB1RSTR_DACRST (1 << 29) /* Bit 29: DAC reset */ +#ifdef CONFIG_STM32_STM32F427 +# define RCC_APB1RSTR_UART7RST (1 << 30) /* Bit 30: USART 7 reset */ +# define RCC_APB1RSTR_UART8RST (1 << 31) /* Bit 31: USART 8 reset */ +#endif /* APB2 Peripheral reset register */ @@ -291,11 +301,18 @@ #define RCC_APB2RSTR_USART6RST (1 << 5) /* Bit 5: USART6 reset */ #define RCC_APB2RSTR_ADCRST (1 << 8) /* Bit 8: ADC interface reset (common to all ADCs) */ #define RCC_APB2RSTR_SDIORST (1 << 11) /* Bit 11: SDIO reset */ -#define RCC_APB2RSTR_SPI1RST (1 << 12) /* Bit 12: SPI 1 reset */ +#define RCC_APB2RSTR_SPI1RST (1 << 12) /* Bit 12: SPI1 reset */ +#ifdef CONFIG_STM32_STM32F427 +# define RCC_APB2RSTR_SPI4RST (1 << 13) /* Bit 13: SPI4 reset */ +#endif #define RCC_APB2RSTR_SYSCFGRST (1 << 14) /* Bit 14: System configuration controller reset */ #define RCC_APB2RSTR_TIM9RST (1 << 16) /* Bit 16: TIM9 reset */ #define RCC_APB2RSTR_TIM10RST (1 << 17) /* Bit 17: TIM10 reset */ #define RCC_APB2RSTR_TIM11RST (1 << 18) /* Bit 18: TIM11 reset */ +#ifdef CONFIG_STM32_STM32F427 +# define RCC_APB2RSTR_SPI5RST (1 << 20) /* Bit 20: SPI 5 reset */ +# define RCC_APB2RSTR_SPI6RST (1 << 21) /* Bit 21: SPI 6 reset */ +#endif /* AHB1 Peripheral Clock enable register */ @@ -358,6 +375,10 @@ #define RCC_APB1ENR_CAN2EN (1 << 26) /* Bit 26: CAN 2 clock enable */ #define RCC_APB1ENR_PWREN (1 << 28) /* Bit 28: Power interface clock enable */ #define RCC_APB1ENR_DACEN (1 << 29) /* Bit 29: DAC interface clock enable */ +#ifdef CONFIG_STM32_STM32F427 +# define RCC_APB1ENR_UART7EN (1 << 30) /* Bit 30: UART7 clock enable */ +# define RCC_APB1ENR_UART8EN (1 << 31) /* Bit 31: UART8 clock enable */ +#endif /* APB2 Peripheral Clock enable register */ @@ -370,10 +391,17 @@ #define RCC_APB2ENR_ADC3EN (1 << 10) /* Bit 10: ADC3 clock enable */ #define RCC_APB2ENR_SDIOEN (1 << 11) /* Bit 11: SDIO clock enable */ #define RCC_APB2ENR_SPI1EN (1 << 12) /* Bit 12: SPI1 clock enable */ +#ifdef CONFIG_STM32_STM32F427 +# define RCC_APB2ENR_SPI4EN (1 << 13) /* Bit 13: SPI4 clock enable */ +#endif #define RCC_APB2ENR_SYSCFGEN (1 << 14) /* Bit 14: System configuration controller clock enable */ #define RCC_APB2ENR_TIM9EN (1 << 16) /* Bit 16: TIM9 clock enable */ #define RCC_APB2ENR_TIM10EN (1 << 17) /* Bit 17: TIM10 clock enable */ #define RCC_APB2ENR_TIM11EN (1 << 18) /* Bit 18: TIM11 clock enable */ +#ifdef CONFIG_STM32_STM32F427 +# define RCC_APB2ENR_SPI5EN (1 << 20) /* Bit 20: SPI5 clock enable */ +# define RCC_APB2ENR_SPI6EN (1 << 21) /* Bit 21: SPI6 clock enable */ +#endif /* RCC AHB1 low power modeperipheral clock enable register */ @@ -392,6 +420,9 @@ #define RCC_AHB1LPENR_SRAM1LPEN (1 << 16) /* Bit 16: SRAM 1 interface clock enable during Sleep mode */ #define RCC_AHB1LPENR_SRAM2LPEN (1 << 17) /* Bit 17: SRAM 2 interface clock enable during Sleep mode */ #define RCC_AHB1LPENR_BKPSRAMLPEN (1 << 18) /* Bit 18: Backup SRAM interface clock enable during Sleep mode */ +#ifdef CONFIG_STM32_STM32F427 +# define RCC_AHB1LPENR_SRAM3LPEN (1 << 19) /* Bit 19: SRAM 3 interface clock enable during Sleep mode */ +#endif #define RCC_AHB1LPENR_CCMDATARAMLPEN (1 << 20) /* Bit 20: CCM data RAM clock enable during Sleep mode */ #define RCC_AHB1LPENR_DMA1LPEN (1 << 21) /* Bit 21: DMA1 clock enable during Sleep mode */ #define RCC_AHB1LPENR_DMA2LPEN (1 << 22) /* Bit 22: DMA2 clock enable during Sleep mode */ @@ -440,6 +471,10 @@ #define RCC_APB1LPENR_CAN2LPEN (1 << 26) /* Bit 26: CAN 2 clock enable during Sleep mode */ #define RCC_APB1LPENR_PWRLPEN (1 << 28) /* Bit 28: Power interface clock enable during Sleep mode */ #define RCC_APB1LPENR_DACLPEN (1 << 29) /* Bit 29: DAC interface clock enable during Sleep mode */ +#ifdef CONFIG_STM32_STM32F427 +# define RCC_APB1LPENR_UART7LPEN (1 << 30) /* Bit 30: UART7 clock enable during Sleep mode */ +# define RCC_APB1LPENR_UART8LPEN (1 << 31) /* Bit 31: UART8 clock enable during Sleep mode */ +#endif /* RCC APB2 low power modeperipheral clock enable register */ @@ -452,10 +487,17 @@ #define RCC_APB2LPENR_ADC3LPEN (1 << 10) /* Bit 10: ADC3 clock enable during Sleep mode */ #define RCC_APB2LPENR_SDIOLPEN (1 << 11) /* Bit 11: SDIO clock enable during Sleep mode */ #define RCC_APB2LPENR_SPI1LPEN (1 << 12) /* Bit 12: SPI1 clock enable during Sleep mode */ +#ifdef CONFIG_STM32_STM32F427 +# define RCC_APB2LPENR_SPI4LPEN (1 << 13) /* Bit 13: SPI4 clock enable during Sleep mode */ +#endif #define RCC_APB2LPENR_SYSCFGLPEN (1 << 14) /* Bit 14: System configuration controller clock enable during Sleep mode */ #define RCC_APB2LPENR_TIM9LPEN (1 << 16) /* Bit 16: TIM9 clock enable during Sleep mode */ #define RCC_APB2LPENR_TIM10LPEN (1 << 17) /* Bit 17: TIM10 clock enable during Sleep mode */ #define RCC_APB2LPENR_TIM11LPEN (1 << 18) /* Bit 18: TIM11 clock enable during Sleep mode */ +#ifdef CONFIG_STM32_STM32F427 +# define RCC_APB2LPENR_SPI5LPEN (1 << 20) /* Bit 20: SPI5 clock enable during Sleep mode */ +# define RCC_APB2LPENR_SPI6LPEN (1 << 21) /* Bit 21: SPI6 clock enable during Sleep mode */ +#endif /* Backup domain control register */ @@ -502,5 +544,11 @@ #define RCC_PLLI2SCFGR_PLLI2SR_SHIFT (28) /* Bits 28-30: PLLI2S division factor for I2S clocks */ #define RCC_PLLI2SCFGR_PLLI2SR_MASK (7 << RCC_PLLI2SCFGR_PLLI2SR_SHIFT) +/* Dedicated clocks configuration register */ + +#ifdef CONFIG_STM32_STM32F427 +# define RCC_DCKCFGR_TIMPRE (1 << 24) /* Bit 24: Timer clock prescaler selection */ +#endif + #endif /* __ARCH_ARM_SRC_STM32_CHIP_STM32F40XXX_RCC_H */ diff --git a/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_vectors.h b/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_vectors.h index 8db1bd19eb..31453c6a41 100644 --- a/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_vectors.h +++ b/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_vectors.h @@ -40,7 +40,7 @@ /* This file is included by stm32_vectors.S. It provides the macro VECTOR that * supplies ach STM32F40xxx vector in terms of a (lower-case) ISR label and an * (upper-case) IRQ number as defined in arch/arm/include/stm32/stm32f40xxx_irq.h. - * stm32_vectors.S will defined the VECTOR in different ways in order to generate + * stm32_vectors.S will define the VECTOR macro in different ways in order to generate * the interrupt vectors and handlers in their final form. */ @@ -50,9 +50,13 @@ #ifdef CONFIG_ARMV7M_CMNVECTOR -/* Reserve 82 interrupt table entries for I/O interrupts. */ +/* Reserve interrupt table entries for I/O interrupts. */ -# define ARMV7M_PERIPHERAL_INTERRUPTS 82 +# ifdef CONFIG_STM32_STM32F427 +# define ARMV7M_PERIPHERAL_INTERRUPTS 87 +# else +# define ARMV7M_PERIPHERAL_INTERRUPTS 82 +# endif #else @@ -133,10 +137,18 @@ VECTOR(stm32_i2c3er, STM32_IRQ_I2C3ER) /* Vector 16+73: I2C3 error int VECTOR(stm32_otghsep1out, STM32_IRQ_OTGHSEP1OUT) /* Vector 16+74: USB On The Go HS End Point 1 Out global interrupt */ VECTOR(stm32_otghsep1in, STM32_IRQ_OTGHSEP1IN) /* Vector 16+75: USB On The Go HS End Point 1 In global interrupt */ VECTOR(stm32_otghswkup, STM32_IRQ_OTGHSWKUP) /* Vector 16+76: USB On The Go HS Wakeup through EXTI interrupt */ -VECTOR(stm32_otghs, STM32_IRQ_OTGHS ) /* Vector 16+77: USB On The Go HS global interrupt */ +VECTOR(stm32_otghs, STM32_IRQ_OTGHS) /* Vector 16+77: USB On The Go HS global interrupt */ VECTOR(stm32_dcmi, STM32_IRQ_DCMI) /* Vector 16+78: DCMI global interrupt */ VECTOR(stm32_cryp, STM32_IRQ_CRYP) /* Vector 16+79: CRYP crypto global interrupt */ VECTOR(stm32_hash, STM32_IRQ_HASH) /* Vector 16+80: Hash and Rng global interrupt */ VECTOR(stm32_fpu, STM32_IRQ_FPU) /* Vector 16+81: FPU global interrupt */ +#ifdef CONFIG_STM32_STM32F427 +VECTOR(stm32_uart7, STM32_IRQ_UART7) /* Vector 16+82: UART7 interrupt */ +VECTOR(stm32_uart8, STM32_IRQ_UART8) /* Vector 16+83: UART8 interrupt */ +VECTOR(stm32_spi4, STM32_IRQ_SPI4) /* Vector 16+84: SPI4 interrupt */ +VECTOR(stm32_spi5, STM32_IRQ_SPI5) /* Vector 16+85: SPI5 interrupt */ +VECTOR(stm32_spi6, STM32_IRQ_SPI6) /* Vector 16+86: SPI6 interrupt */ +#endif + #endif /* CONFIG_ARMV7M_CMNVECTOR */ diff --git a/nuttx/arch/arm/src/stm32/stm32_lowputc.c b/nuttx/arch/arm/src/stm32/stm32_lowputc.c index 6cb07dad93..5ad0ce7d31 100644 --- a/nuttx/arch/arm/src/stm32/stm32_lowputc.c +++ b/nuttx/arch/arm/src/stm32/stm32_lowputc.c @@ -160,6 +160,40 @@ # define STM32_CONSOLE_RS485_DIR_POLARITY true # endif # endif +#elif defined(CONFIG_UART7_SERIAL_CONSOLE) +# define STM32_CONSOLE_BASE STM32_UART7_BASE +# define STM32_APBCLOCK STM32_PCLK1_FREQUENCY +# define STM32_CONSOLE_BAUD CONFIG_UART7_BAUD +# define STM32_CONSOLE_BITS CONFIG_UART7_BITS +# define STM32_CONSOLE_PARITY CONFIG_UART7_PARITY +# define STM32_CONSOLE_2STOP CONFIG_UART7_2STOP +# define STM32_CONSOLE_TX GPIO_UART7_TX +# define STM32_CONSOLE_RX GPIO_UART7_RX +# ifdef CONFIG_UART7_RS485 +# define STM32_CONSOLE_RS485_DIR GPIO_UART7_RS485_DIR +# if (CONFIG_UART7_RS485_DIR_POLARITY == 0) +# define STM32_CONSOLE_RS485_DIR_POLARITY false +# else +# define STM32_CONSOLE_RS485_DIR_POLARITY true +# endif +# endif +#elif defined(CONFIG_UART8_SERIAL_CONSOLE) +# define STM32_CONSOLE_BASE STM32_UART8_BASE +# define STM32_APBCLOCK STM32_PCLK1_FREQUENCY +# define STM32_CONSOLE_BAUD CONFIG_UART8_BAUD +# define STM32_CONSOLE_BITS CONFIG_UART8_BITS +# define STM32_CONSOLE_PARITY CONFIG_UART8_PARITY +# define STM32_CONSOLE_2STOP CONFIG_UART8_2STOP +# define STM32_CONSOLE_TX GPIO_UART8_TX +# define STM32_CONSOLE_RX GPIO_UART8_RX +# ifdef CONFIG_UART8_RS485 +# define STM32_CONSOLE_RS485_DIR GPIO_UART8_RS485_DIR +# if (CONFIG_UART8_RS485_DIR_POLARITY == 0) +# define STM32_CONSOLE_RS485_DIR_POLARITY false +# else +# define STM32_CONSOLE_RS485_DIR_POLARITY true +# endif +# endif #endif /* CR1 settings */ diff --git a/nuttx/arch/arm/src/stm32/stm32_serial.c b/nuttx/arch/arm/src/stm32/stm32_serial.c index c91f6a6d75..3273e52daf 100644 --- a/nuttx/arch/arm/src/stm32/stm32_serial.c +++ b/nuttx/arch/arm/src/stm32/stm32_serial.c @@ -90,9 +90,10 @@ # endif # if defined(CONFIG_USART2_RXDMA) || defined(CONFIG_USART3_RXDMA) || \ - defined(CONFIG_UART4_RXDMA) || defined(CONFIG_UART5_RXDMA) + defined(CONFIG_UART4_RXDMA) || defined(CONFIG_UART5_RXDMA) || \ + defined(CONFIG_UART7_RXDMA) || defined(CONFIG_UART8_RXDMA) # ifndef CONFIG_STM32_DMA1 -# error STM32 USART2/3/4/5 receive DMA requires CONFIG_STM32_DMA1 +# error STM32 USART2/3/4/5/7/8 receive DMA requires CONFIG_STM32_DMA1 # endif # endif @@ -105,7 +106,9 @@ (defined(CONFIG_USART3_RXDMA) && defined(CONFIG_USART3_RS485)) || \ (defined(CONFIG_UART4_RXDMA) && defined(CONFIG_UART4_RS485)) || \ (defined(CONFIG_UART5_RXDMA) && defined(CONFIG_UART5_RS485)) || \ - (defined(CONFIG_USART6_RXDMA) && defined(CONFIG_USART6_RS485)) + (defined(CONFIG_USART6_RXDMA) && defined(CONFIG_USART6_RS485)) || \ + (defined(CONFIG_UART7_RXDMA) && defined(CONFIG_UART7_RS485)) || \ + (defined(CONFIG_UART8_RXDMA) && defined(CONFIG_UART8_RS485)) # error "RXDMA and RS-485 cannot be enabled at the same time for the same U[S]ART" # endif @@ -138,6 +141,14 @@ # error "USART6 DMA channel not defined (DMAMAP_USART6_RX)" # endif +# if defined(CONFIG_UART7_RXDMA) && !defined(DMAMAP_UART7_RX) +# error "UART7 DMA channel not defined (DMAMAP_UART7_RX)" +# endif + +# if defined(CONFIG_UART8_RXDMA) && !defined(DMAMAP_UART8_RX) +# error "UART8 DMA channel not defined (DMAMAP_UART8_RX)" +# endif + # elif defined(CONFIG_STM32_STM32F10XX) # if defined(CONFIG_USART1_RXDMA) || defined(CONFIG_USART2_RXDMA) || \ @@ -337,6 +348,12 @@ static int up_interrupt_uart5(int irq, void *context); #ifdef CONFIG_STM32_USART6 static int up_interrupt_usart6(int irq, void *context); #endif +#ifdef CONFIG_STM32_UART7 +static int up_interrupt_uart7(int irq, void *context); +#endif +#ifdef CONFIG_STM32_UART8 +static int up_interrupt_uart8(int irq, void *context); +#endif /**************************************************************************** * Private Variables @@ -428,6 +445,22 @@ static char g_usart6rxfifo[RXDMA_BUFFER_SIZE]; # endif #endif +#ifdef CONFIG_STM32_UART7 +static char g_uart7rxbuffer[CONFIG_UART7_RXBUFSIZE]; +static char g_uart7txbuffer[CONFIG_UART7_TXBUFSIZE]; +# ifdef CONFIG_UART7_RXDMA +static char g_uart7rxfifo[RXDMA_BUFFER_SIZE]; +# endif +#endif + +#ifdef CONFIG_STM32_UART8 +static char g_uart8rxbuffer[CONFIG_UART8_RXBUFSIZE]; +static char g_uart8txbuffer[CONFIG_UART8_TXBUFSIZE]; +# ifdef CONFIG_UART8_RXDMA +static char g_uart8rxfifo[RXDMA_BUFFER_SIZE]; +# endif +#endif + /* This describes the state of the STM32 USART1 ports. */ #ifdef CONFIG_STM32_USART1 @@ -792,6 +825,126 @@ static struct up_dev_s g_usart6priv = }; #endif +/* This describes the state of the STM32 UART7 port. */ + +#ifdef CONFIG_STM32_UART7 +static struct up_dev_s g_uart7priv = +{ + .dev = + { +#if CONSOLE_UART == 7 + .isconsole = true, +#endif + .recv = + { + .size = CONFIG_UART7_RXBUFSIZE, + .buffer = g_uart7rxbuffer, + }, + .xmit = + { + .size = CONFIG_UART7_TXBUFSIZE, + .buffer = g_uart7txbuffer, + }, +#ifdef CONFIG_UART7_RXDMA + .ops = &g_uart_dma_ops, +#else + .ops = &g_uart_ops, +#endif + .priv = &g_uart7priv, + }, + + .irq = STM32_IRQ_UART7, + .parity = CONFIG_UART7_PARITY, + .bits = CONFIG_UART7_BITS, + .stopbits2 = CONFIG_UART7_2STOP, + .baud = CONFIG_UART7_BAUD, + .apbclock = STM32_PCLK1_FREQUENCY, + .usartbase = STM32_UART7_BASE, + .tx_gpio = GPIO_UART7_TX, + .rx_gpio = GPIO_UART7_RX, +#ifdef GPIO_UART7_CTS + .cts_gpio = GPIO_UART7_CTS, +#endif +#ifdef GPIO_UART7_RTS + .rts_gpio = GPIO_UART7_RTS, +#endif +#ifdef CONFIG_UART7_RXDMA + .rxdma_channel = DMAMAP_UART7_RX, + .rxfifo = g_uart7rxfifo, +#endif + .vector = up_interrupt_uart7, + +#ifdef CONFIG_UART7_RS485 + .rs485_dir_gpio = GPIO_UART7_RS485_DIR, +# if (CONFIG_UART7_RS485_DIR_POLARITY == 0) + .rs485_dir_polarity = false, +# else + .rs485_dir_polarity = true, +# endif +#endif +}; +#endif + +/* This describes the state of the STM32 UART8 port. */ + +#ifdef CONFIG_STM32_UART8 +static struct up_dev_s g_uart8priv = +{ + .dev = + { +#if CONSOLE_UART == 8 + .isconsole = true, +#endif + .recv = + { + .size = CONFIG_UART8_RXBUFSIZE, + .buffer = g_uart8rxbuffer, + }, + .xmit = + { + .size = CONFIG_UART8_TXBUFSIZE, + .buffer = g_uart8txbuffer, + }, +#ifdef CONFIG_UART8_RXDMA + .ops = &g_uart_dma_ops, +#else + .ops = &g_uart_ops, +#endif + .priv = &g_uart8priv, + }, + + .irq = STM32_IRQ_UART8, + .parity = CONFIG_UART8_PARITY, + .bits = CONFIG_UART8_BITS, + .stopbits2 = CONFIG_UART8_2STOP, + .baud = CONFIG_UART8_BAUD, + .apbclock = STM32_PCLK1_FREQUENCY, + .usartbase = STM32_UART8_BASE, + .tx_gpio = GPIO_UART8_TX, + .rx_gpio = GPIO_UART8_RX, +#ifdef GPIO_UART8_CTS + .cts_gpio = GPIO_UART8_CTS, +#endif +#ifdef GPIO_UART8_RTS + .rts_gpio = GPIO_UART8_RTS, +#endif +#ifdef CONFIG_UART8_RXDMA + .rxdma_channel = DMAMAP_UART8_RX, + .rxfifo = g_uart8rxfifo, +#endif + .vector = up_interrupt_uart8, + +#ifdef CONFIG_UART8_RS485 + .rs485_dir_gpio = GPIO_UART8_RS485_DIR, +# if (CONFIG_UART8_RS485_DIR_POLARITY == 0) + .rs485_dir_polarity = false, +# else + .rs485_dir_polarity = true, +# endif +#endif +}; +#endif + /* This table lets us iterate over the configured USARTs */ static struct up_dev_s *uart_devs[STM32_NUSART] = @@ -814,6 +967,12 @@ static struct up_dev_s *uart_devs[STM32_NUSART] = #ifdef CONFIG_STM32_USART6 [5] = &g_usart6priv, #endif +#ifdef CONFIG_STM32_UART7 + [6] = &g_uart7priv, +#endif +#ifdef CONFIG_STM32_UART8 + [7] = &g_uart8priv, +#endif }; #ifdef CONFIG_PM @@ -1899,6 +2058,20 @@ static int up_interrupt_usart6(int irq, void *context) } #endif +#ifdef CONFIG_STM32_UART7 +static int up_interrupt_uart7(int irq, void *context) +{ + return up_interrupt_common(&g_uart7priv); +} +#endif + +#ifdef CONFIG_STM32_UART8 +static int up_interrupt_uart8(int irq, void *context) +{ + return up_interrupt_common(&g_uart8priv); +} +#endif + /**************************************************************************** * Name: up_dma_rxcallback * @@ -2187,6 +2360,20 @@ void stm32_serial_dma_poll(void) } #endif +#ifdef CONFIG_UART7_RXDMA + if (g_uart7priv.rxdma != NULL) + { + up_dma_rxcallback(g_uart7priv.rxdma, 0, &g_uart7priv); + } +#endif + +#ifdef CONFIG_UART8_RXDMA + if (g_uart8priv.rxdma != NULL) + { + up_dma_rxcallback(g_uart8priv.rxdma, 0, &g_uart8priv); + } +#endif + irqrestore(flags); } #endif diff --git a/nuttx/arch/arm/src/stm32/stm32_spi.c b/nuttx/arch/arm/src/stm32/stm32_spi.c index b4a4f36ab7..c8a8faa662 100644 --- a/nuttx/arch/arm/src/stm32/stm32_spi.c +++ b/nuttx/arch/arm/src/stm32/stm32_spi.c @@ -82,7 +82,8 @@ #include "stm32_dma.h" #include "stm32_spi.h" -#if defined(CONFIG_STM32_SPI1) || defined(CONFIG_STM32_SPI2) || defined(CONFIG_STM32_SPI3) +#if defined(CONFIG_STM32_SPI1) || defined(CONFIG_STM32_SPI2) || defined(CONFIG_STM32_SPI3) || \ + defined(CONFIG_STM32_SPI4) || defined(CONFIG_STM32_SPI5) || defined(CONFIG_STM32_SPI6) /************************************************************************************ * Definitions @@ -377,6 +378,123 @@ static struct stm32_spidev_s g_spi3dev = }; #endif +#ifdef CONFIG_STM32_SPI4 +static const struct spi_ops_s g_sp4iops = +{ +#ifndef CONFIG_SPI_OWNBUS + .lock = spi_lock, +#endif + .select = stm32_spi4select, + .setfrequency = spi_setfrequency, + .setmode = spi_setmode, + .setbits = spi_setbits, + .status = stm32_spi4status, +#ifdef CONFIG_SPI_CMDDATA + .cmddata = stm32_spi4cmddata, +#endif + .send = spi_send, +#ifdef CONFIG_SPI_EXCHANGE + .exchange = spi_exchange, +#else + .sndblock = spi_sndblock, + .recvblock = spi_recvblock, +#endif + .registercallback = 0, +}; + +static struct stm32_spidev_s g_spi4dev = +{ + .spidev = { &g_sp4iops }, + .spibase = STM32_SPI4_BASE, + .spiclock = STM32_PCLK1_FREQUENCY, +#ifdef CONFIG_STM32_SPI_INTERRUPTS + .spiirq = STM32_IRQ_SPI4, +#endif +#ifdef CONFIG_STM32_SPI_DMA + .rxch = DMACHAN_SPI4_RX, + .txch = DMACHAN_SPI4_TX, +#endif +}; +#endif + +#ifdef CONFIG_STM32_SPI5 +static const struct spi_ops_s g_sp5iops = +{ +#ifndef CONFIG_SPI_OWNBUS + .lock = spi_lock, +#endif + .select = stm32_spi5select, + .setfrequency = spi_setfrequency, + .setmode = spi_setmode, + .setbits = spi_setbits, + .status = stm32_spi5status, +#ifdef CONFIG_SPI_CMDDATA + .cmddata = stm32_spi5cmddata, +#endif + .send = spi_send, +#ifdef CONFIG_SPI_EXCHANGE + .exchange = spi_exchange, +#else + .sndblock = spi_sndblock, + .recvblock = spi_recvblock, +#endif + .registercallback = 0, +}; + +static struct stm32_spidev_s g_spi5dev = +{ + .spidev = { &g_sp5iops }, + .spibase = STM32_SPI5_BASE, + .spiclock = STM32_PCLK1_FREQUENCY, +#ifdef CONFIG_STM32_SPI_INTERRUPTS + .spiirq = STM32_IRQ_SPI5, +#endif +#ifdef CONFIG_STM32_SPI_DMA + .rxch = DMACHAN_SPI5_RX, + .txch = DMACHAN_SPI5_TX, +#endif +}; +#endif + +#ifdef CONFIG_STM32_SPI6 +static const struct spi_ops_s g_sp6iops = +{ +#ifndef CONFIG_SPI_OWNBUS + .lock = spi_lock, +#endif + .select = stm32_spi6select, + .setfrequency = spi_setfrequency, + .setmode = spi_setmode, + .setbits = spi_setbits, + .status = stm32_spi6status, +#ifdef CONFIG_SPI_CMDDATA + .cmddata = stm32_spi3cmddata, +#endif + .send = spi_send, +#ifdef CONFIG_SPI_EXCHANGE + .exchange = spi_exchange, +#else + .sndblock = spi_sndblock, + .recvblock = spi_recvblock, +#endif + .registercallback = 0, +}; + +static struct stm32_spidev_s g_spi6dev = +{ + .spidev = { &g_sp6iops }, + .spibase = STM32_SPI6_BASE, + .spiclock = STM32_PCLK1_FREQUENCY, +#ifdef CONFIG_STM32_SPI_INTERRUPTS + .spiirq = STM32_IRQ_SPI6, +#endif +#ifdef CONFIG_STM32_SPI_DMA + .rxch = DMACHAN_SPI6_RX, + .txch = DMACHAN_SPI6_TX, +#endif +}; +#endif + /************************************************************************************ * Public Data ************************************************************************************/ @@ -1464,6 +1582,78 @@ FAR struct spi_dev_s *up_spiinitialize(int port) } else #endif +#ifdef CONFIG_STM32_SPI4 + if (port == 4) + { + /* Select SPI4 */ + + priv = &g_spi4dev; + + /* Only configure if the port is not already configured */ + + if ((spi_getreg(priv, STM32_SPI_CR1_OFFSET) & SPI_CR1_SPE) == 0) + { + /* Configure SPI4 pins: SCK, MISO, and MOSI */ + + stm32_configgpio(GPIO_SPI4_SCK); + stm32_configgpio(GPIO_SPI4_MISO); + stm32_configgpio(GPIO_SPI4_MOSI); + + /* Set up default configuration: Master, 8-bit, etc. */ + + spi_portinitialize(priv); + } + } + else +#endif +#ifdef CONFIG_STM32_SPI5 + if (port == 5) + { + /* Select SPI5 */ + + priv = &g_spi5dev; + + /* Only configure if the port is not already configured */ + + if ((spi_getreg(priv, STM32_SPI_CR1_OFFSET) & SPI_CR1_SPE) == 0) + { + /* Configure SPI5 pins: SCK, MISO, and MOSI */ + + stm32_configgpio(GPIO_SPI5_SCK); + stm32_configgpio(GPIO_SPI5_MISO); + stm32_configgpio(GPIO_SPI5_MOSI); + + /* Set up default configuration: Master, 8-bit, etc. */ + + spi_portinitialize(priv); + } + } + else +#endif +#ifdef CONFIG_STM32_SPI6 + if (port == 6) + { + /* Select SPI6 */ + + priv = &g_spi6dev; + + /* Only configure if the port is not already configured */ + + if ((spi_getreg(priv, STM32_SPI_CR1_OFFSET) & SPI_CR1_SPE) == 0) + { + /* Configure SPI6 pins: SCK, MISO, and MOSI */ + + stm32_configgpio(GPIO_SPI6_SCK); + stm32_configgpio(GPIO_SPI6_MISO); + stm32_configgpio(GPIO_SPI6_MOSI); + + /* Set up default configuration: Master, 8-bit, etc. */ + + spi_portinitialize(priv); + } + } + else +#endif { spidbg("ERROR: Unsupported SPI port: %d\n", port); return NULL; @@ -1473,4 +1663,4 @@ FAR struct spi_dev_s *up_spiinitialize(int port) return (FAR struct spi_dev_s *)priv; } -#endif /* CONFIG_STM32_SPI1 || CONFIG_STM32_SPI2 || CONFIG_STM32_SPI3 */ +#endif /* CONFIG_STM32_SPI1 || CONFIG_STM32_SPI2 || CONFIG_STM32_SPI3 || CONFIG_STM32_SPI4 || CONFIG_STM32_SPI5 || CONFIG_STM32_SPI6 */ diff --git a/nuttx/arch/arm/src/stm32/stm32_spi.h b/nuttx/arch/arm/src/stm32/stm32_spi.h index 6030ddfdd2..58c3f0da75 100644 --- a/nuttx/arch/arm/src/stm32/stm32_spi.h +++ b/nuttx/arch/arm/src/stm32/stm32_spi.h @@ -71,11 +71,11 @@ enum spi_dev_e; ************************************************************************************/ /************************************************************************************ - * Name: stm32_spi1/2/3select and stm32_spi1/2/3status + * Name: stm32_spi1/2/...select and stm32_spi1/2/...status * * Description: - * The external functions, stm32_spi1/2/3select, stm32_spi1/2/3status, and - * stm32_spi1/2/3cmddata must be provided by board-specific logic. These are + * The external functions, stm32_spi1/2/...select, stm32_spi1/2/...status, and + * stm32_spi1/2/...cmddata must be provided by board-specific logic. These are * implementations of the select, status, and cmddata methods of the SPI interface * defined by struct spi_ops_s (see include/nuttx/spi.h). All other methods * (including up_spiinitialize()) are provided by common STM32 logic. To use this @@ -83,11 +83,11 @@ enum spi_dev_e; * * 1. Provide logic in stm32_boardinitialize() to configure SPI chip select * pins. - * 2. Provide stm32_spi1/2/3select() and stm32_spi1/2/3status() functions in your + * 2. Provide stm32_spi1/2/...select() and stm32_spi1/2/...status() functions in your * board-specific logic. These functions will perform chip selection and * status operations using GPIOs in the way your board is configured. * 3. If CONFIG_SPI_CMDDATA is defined in your NuttX configuration file, then - * provide stm32_spi1/2/3cmddata() functions in your board-specific logic. + * provide stm32_spi1/2/...cmddata() functions in your board-specific logic. * These functions will perform cmd/data selection operations using GPIOs in the * way your board is configured. * 4. Add a calls to up_spiinitialize() in your low level application @@ -111,6 +111,18 @@ EXTERN void stm32_spi3select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, b EXTERN uint8_t stm32_spi3status(FAR struct spi_dev_s *dev, enum spi_dev_e devid); EXTERN int stm32_spi3cmddata(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool cmd); +EXTERN void stm32_spi4select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected); +EXTERN uint8_t stm32_spi4status(FAR struct spi_dev_s *dev, enum spi_dev_e devid); +EXTERN int stm32_spi4cmddata(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool cmd); + +EXTERN void stm32_spi5select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected); +EXTERN uint8_t stm32_spi5status(FAR struct spi_dev_s *dev, enum spi_dev_e devid); +EXTERN int stm32_spi5cmddata(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool cmd); + +EXTERN void stm32_spi6select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected); +EXTERN uint8_t stm32_spi6status(FAR struct spi_dev_s *dev, enum spi_dev_e devid); +EXTERN int stm32_spi6cmddata(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool cmd); + #undef EXTERN #if defined(__cplusplus) } diff --git a/nuttx/arch/arm/src/stm32/stm32_uart.h b/nuttx/arch/arm/src/stm32/stm32_uart.h index a26ea20099..e77122d828 100644 --- a/nuttx/arch/arm/src/stm32/stm32_uart.h +++ b/nuttx/arch/arm/src/stm32/stm32_uart.h @@ -52,6 +52,12 @@ * the device. */ +#if STM32_NUSART < 8 +# undef CONFIG_STM32_UART8 +#endif +#if STM32_NUSART < 7 +# undef CONFIG_STM32_UART7 +#endif #if STM32_NUSART < 6 # undef CONFIG_STM32_USART6 #endif @@ -75,7 +81,8 @@ #if defined(CONFIG_STM32_USART1) || defined(CONFIG_STM32_USART2) || \ defined(CONFIG_STM32_USART3) || defined(CONFIG_STM32_UART4) || \ - defined(CONFIG_STM32_UART5) || defined(CONFIG_STM32_USART6) + defined(CONFIG_STM32_UART5) || defined(CONFIG_STM32_USART6) || \ + defined(CONFIG_STM32_UART7) || defined(CONFIG_STM32_UART8) # define HAVE_UART 1 #endif @@ -87,6 +94,8 @@ # undef CONFIG_UART4_SERIAL_CONSOLE # undef CONFIG_UART5_SERIAL_CONSOLE # undef CONFIG_USART6_SERIAL_CONSOLE +# undef CONFIG_UART7_SERIAL_CONSOLE +# undef CONFIG_UART8_SERIAL_CONSOLE # define CONSOLE_UART 1 # define HAVE_CONSOLE 1 #elif defined(CONFIG_USART2_SERIAL_CONSOLE) && defined(CONFIG_STM32_USART2) @@ -95,6 +104,8 @@ # undef CONFIG_UART4_SERIAL_CONSOLE # undef CONFIG_UART5_SERIAL_CONSOLE # undef CONFIG_USART6_SERIAL_CONSOLE +# undef CONFIG_UART7_SERIAL_CONSOLE +# undef CONFIG_UART8_SERIAL_CONSOLE # define CONSOLE_UART 2 # define HAVE_CONSOLE 1 #elif defined(CONFIG_USART3_SERIAL_CONSOLE) && defined(CONFIG_STM32_USART3) @@ -103,6 +114,8 @@ # undef CONFIG_UART4_SERIAL_CONSOLE # undef CONFIG_UART5_SERIAL_CONSOLE # undef CONFIG_USART6_SERIAL_CONSOLE +# undef CONFIG_UART7_SERIAL_CONSOLE +# undef CONFIG_UART8_SERIAL_CONSOLE # define CONSOLE_UART 3 # define HAVE_CONSOLE 1 #elif defined(CONFIG_UART4_SERIAL_CONSOLE) && defined(CONFIG_STM32_UART4) @@ -111,6 +124,8 @@ # undef CONFIG_USART3_SERIAL_CONSOLE # undef CONFIG_UART5_SERIAL_CONSOLE # undef CONFIG_USART6_SERIAL_CONSOLE +# undef CONFIG_UART7_SERIAL_CONSOLE +# undef CONFIG_UART8_SERIAL_CONSOLE # define CONSOLE_UART 4 # define HAVE_CONSOLE 1 #elif defined(CONFIG_UART5_SERIAL_CONSOLE) && defined(CONFIG_STM32_UART5) @@ -119,6 +134,8 @@ # undef CONFIG_USART3_SERIAL_CONSOLE # undef CONFIG_UART4_SERIAL_CONSOLE # undef CONFIG_USART6_SERIAL_CONSOLE +# undef CONFIG_UART7_SERIAL_CONSOLE +# undef CONFIG_UART8_SERIAL_CONSOLE # define CONSOLE_UART 5 # define HAVE_CONSOLE 1 #elif defined(CONFIG_USART6_SERIAL_CONSOLE) && defined(CONFIG_STM32_USART6) @@ -127,8 +144,31 @@ # undef CONFIG_USART3_SERIAL_CONSOLE # undef CONFIG_UART4_SERIAL_CONSOLE # undef CONFIG_UART5_SERIAL_CONSOLE +# undef CONFIG_UART7_SERIAL_CONSOLE +# undef CONFIG_UART8_SERIAL_CONSOLE # define CONSOLE_UART 6 # define HAVE_CONSOLE 1 +#elif defined(CONFIG_UART7_SERIAL_CONSOLE) && defined(CONFIG_STM32_UART7) +# undef CONFIG_USART1_SERIAL_CONSOLE +# undef CONFIG_USART2_SERIAL_CONSOLE +# undef CONFIG_USART3_SERIAL_CONSOLE +# undef CONFIG_UART4_SERIAL_CONSOLE +# undef CONFIG_UART5_SERIAL_CONSOLE +# undef CONFIG_USART6_SERIAL_CONSOLE +# undef CONFIG_UART5_SERIAL_CONSOLE +# undef CONFIG_UART8_SERIAL_CONSOLE +# define CONSOLE_UART 7 +# define HAVE_CONSOLE 1 +#elif defined(CONFIG_UART8_SERIAL_CONSOLE) && defined(CONFIG_STM32_UART8) +# undef CONFIG_USART1_SERIAL_CONSOLE +# undef CONFIG_USART2_SERIAL_CONSOLE +# undef CONFIG_USART3_SERIAL_CONSOLE +# undef CONFIG_UART4_SERIAL_CONSOLE +# undef CONFIG_UART6_SERIAL_CONSOLE +# undef CONFIG_USART6_SERIAL_CONSOLE +# undef CONFIG_UART7_SERIAL_CONSOLE +# define CONSOLE_UART 8 +# define HAVE_CONSOLE 1 #else # undef CONFIG_USART1_SERIAL_CONSOLE # undef CONFIG_USART2_SERIAL_CONSOLE @@ -136,6 +176,8 @@ # undef CONFIG_UART4_SERIAL_CONSOLE # undef CONFIG_UART5_SERIAL_CONSOLE # undef CONFIG_USART6_SERIAL_CONSOLE +# undef CONFIG_UART7_SERIAL_CONSOLE +# undef CONFIG_UART8_SERIAL_CONSOLE # define CONSOLE_UART 0 # undef HAVE_CONSOLE #endif @@ -149,6 +191,8 @@ # undef CONFIG_UART4_RXDMA # undef CONFIG_UART5_RXDMA # undef CONFIG_USART6_RXDMA +# undef CONFIG_UART7_RXDMA +# undef CONFIG_UART8_RXDMA #endif /* Disable the DMA configuration on all unused USARTs */ @@ -177,12 +221,21 @@ # undef CONFIG_USART6_RXDMA #endif +#ifndef CONFIG_STM32_UART7 +# undef CONFIG_UART7_RXDMA +#endif + +#ifndef CONFIG_STM32_UART8 +# undef CONFIG_UART8_RXDMA +#endif + /* Is DMA available on any (enabled) USART? */ #undef SERIAL_HAVE_DMA #if defined(CONFIG_USART1_RXDMA) || defined(CONFIG_USART2_RXDMA) || \ - defined(CONFIG_USART3_RXDMA) || defined(CONFIG_UART4_RXDMA) || \ - defined(CONFIG_UART5_RXDMA) || defined(CONFIG_USART6_RXDMA) + defined(CONFIG_USART3_RXDMA) || defined(CONFIG_UART4_RXDMA) || \ + defined(CONFIG_UART5_RXDMA) || defined(CONFIG_USART6_RXDMA) || \ + defined(CONFIG_UART7_RXDMA) || defined(CONFIG_UART8_RXDMA) # define SERIAL_HAVE_DMA 1 #endif @@ -201,6 +254,10 @@ # define SERIAL_HAVE_CONSOLE_DMA 1 #elif defined(CONFIG_USART6_SERIAL_CONSOLE) && defined(CONFIG_USART6_RXDMA) # define SERIAL_HAVE_CONSOLE_DMA 1 +#elif defined(CONFIG_UART7_SERIAL_CONSOLE) && defined(CONFIG_UART7_RXDMA) +# define SERIAL_HAVE_CONSOLE_DMA 1 +#elif defined(CONFIG_UART8_SERIAL_CONSOLE) && defined(CONFIG_UART8_RXDMA) +# define SERIAL_HAVE_CONSOLE_DMA 1 #endif /* Is DMA used on all (enabled) USARTs */ @@ -218,13 +275,18 @@ # undef SERIAL_HAVE_ONLY_DMA #elif defined(CONFIG_STM32_USART6) && !defined(CONFIG_USART6_RXDMA) # undef SERIAL_HAVE_ONLY_DMA +#elif defined(CONFIG_STM32_UART7) && !defined(CONFIG_UART7_RXDMA) +# undef SERIAL_HAVE_ONLY_DMA +#elif defined(CONFIG_STM32_UART8) && !defined(CONFIG_UART8_RXDMA) +# undef SERIAL_HAVE_ONLY_DMA #endif /* Is RS-485 used? */ #if defined(CONFIG_USART1_RS485) || defined(CONFIG_USART2_RS485) || \ defined(CONFIG_USART3_RS485) || defined(CONFIG_UART4_RS485) || \ - defined(CONFIG_UART5_RS485) || defined(CONFIG_USART6_RS485) + defined(CONFIG_UART5_RS485) || defined(CONFIG_USART6_RS485) || \ + defined(CONFIG_UART7_RS485) || defined(CONFIG_UART8_RS485) # define HAVE_RS485 1 #endif diff --git a/nuttx/arch/arm/src/stm32/stm32f40xxx_rcc.c b/nuttx/arch/arm/src/stm32/stm32f40xxx_rcc.c index c6c0b23827..fc7fe1697d 100644 --- a/nuttx/arch/arm/src/stm32/stm32f40xxx_rcc.c +++ b/nuttx/arch/arm/src/stm32/stm32f40xxx_rcc.c @@ -437,6 +437,19 @@ static inline void rcc_enableapb1(void) regval |= RCC_APB1ENR_DACEN; #endif +#ifdef CONFIG_STM32_UART7 + /* UART7 clock enable */ + + regval |= RCC_APB1ENR_UART7EN; +#endif + +#ifdef CONFIG_STM32_UART8 + /* UART8 clock enable */ + + regval |= RCC_APB1ENR_UART8EN; +#endif + + putreg32(regval, STM32_RCC_APB1ENR); /* Enable peripherals */ } @@ -512,6 +525,12 @@ static inline void rcc_enableapb2(void) regval |= RCC_APB2ENR_SPI1EN; #endif +#ifdef CONFIG_STM32_SPI4 + /* SPI4 clock enable */ + + regval |= RCC_APB2ENR_SPI4EN; +#endif + #ifdef CONFIG_STM32_SYSCFG /* System configuration controller clock enable */ @@ -536,6 +555,18 @@ static inline void rcc_enableapb2(void) regval |= RCC_APB2ENR_TIM11EN; #endif +#ifdef CONFIG_STM32_SPI5 + /* SPI5 clock enable */ + + regval |= RCC_APB2ENR_SPI5EN; +#endif + +#ifdef CONFIG_STM32_SPI6 + /* SPI6 clock enable */ + + regval |= RCC_APB2ENR_SPI6EN; +#endif + putreg32(regval, STM32_RCC_APB2ENR); /* Enable peripherals */ } @@ -591,7 +622,12 @@ static void stm32_stdclockconfig(void) putreg32(regval, STM32_RCC_APB1ENR); regval = getreg32(STM32_PWR_CR); +#if defined(CONFIG_STM32_STM32F427) + regval &= ~PWR_CR_VOS_MASK; + regval |= PWR_CR_VOS_SCALE_1; +#else regval |= PWR_CR_VOS; +#endif putreg32(regval, STM32_PWR_CR); /* Set the HCLK source/divider */ diff --git a/nuttx/drivers/serial/Kconfig b/nuttx/drivers/serial/Kconfig index 119923a690..e91246612c 100644 --- a/nuttx/drivers/serial/Kconfig +++ b/nuttx/drivers/serial/Kconfig @@ -292,6 +292,10 @@ config ARCH_HAVE_UART5 bool config ARCH_HAVE_UART6 bool +config ARCH_HAVE_UART7 + bool +config ARCH_HAVE_UART8 + bool config ARCH_HAVE_USART0 bool @@ -307,6 +311,10 @@ config ARCH_HAVE_USART5 bool config ARCH_HAVE_USART6 bool +config ARCH_HAVE_USART7 + bool +config ARCH_HAVE_USART8 + bool config MCU_SERIAL bool @@ -403,6 +411,22 @@ config USART6_SERIAL_CONSOLE bool "USART6" depends on ARCH_HAVE_USART6 +config UART7_SERIAL_CONSOLE + bool "UART7" + depends on ARCH_HAVE_UART7 + +config USART7_SERIAL_CONSOLE + bool "USART7" + depends on ARCH_HAVE_USART7 + +config UART8_SERIAL_CONSOLE + bool "UART8" + depends on ARCH_HAVE_UART8 + +config USART8_SERIAL_CONSOLE + bool "USART8" + depends on ARCH_HAVE_USART8 + config NO_SERIAL_CONSOLE bool "No serial console" @@ -1052,3 +1076,173 @@ config UART6_2STOP 1=Two stop bits endmenu + +menu "USART7 Configuration" + depends on ARCH_HAVE_USART7 + +config USART7_RXBUFSIZE + int "Receive buffer size" + default 256 + help + Characters are buffered as they are received. This specifies + the size of the receive buffer. + +config USART7_TXBUFSIZE + int "Transmit buffer size" + default 256 + help + Characters are buffered before being sent. This specifies + the size of the transmit buffer. + +config USART7_BAUD + int "BAUD rate" + default 115200 + help + The configured BAUD of the USART. + +config USART7_BITS + int "Character size" + default 8 + help + The number of bits. Must be either 7 or 8. + +config USART7_PARITY + int "Parity setting" + default 0 + help + 0=no parity, 1=odd parity, 2=even parity + +config USART7_2STOP + int "Uses 2 stop bits" + default 0 + help + 1=Two stop bits + +endmenu + +menu "UART7 Configuration" + depends on ARCH_HAVE_UART7 + +config UART7_RXBUFSIZE + int "Receive buffer size" + default 256 + help + Characters are buffered as they are received. This specifies + the size of the receive buffer. + +config UART7_TXBUFSIZE + int "Transmit buffer size" + default 256 + help + Characters are buffered before being sent. This specifies + the size of the transmit buffer. + +config UART7_BAUD + int "BAUD rate" + default 115200 + help + The configured BAUD of the UART. + +config UART7_BITS + int "Character size" + default 8 + help + The number of bits. Must be either 7 or 8. + +config UART7_PARITY + int "Parity setting" + default 0 + help + 0=no parity, 1=odd parity, 2=even parity + +config UART7_2STOP + int "Uses 2 stop bits" + default 0 + help + 1=Two stop bits + +menu "USART8 Configuration" + depends on ARCH_HAVE_USART8 + +config USART8_RXBUFSIZE + int "Receive buffer size" + default 256 + help + Characters are buffered as they are received. This specifies + the size of the receive buffer. + +config USART8_TXBUFSIZE + int "Transmit buffer size" + default 256 + help + Characters are buffered before being sent. This specifies + the size of the transmit buffer. + +config USART8_BAUD + int "BAUD rate" + default 115200 + help + The configured BAUD of the USART. + +config USART8_BITS + int "Character size" + default 8 + help + The number of bits. Must be either 7 or 8. + +config USART8_PARITY + int "Parity setting" + default 0 + help + 0=no parity, 1=odd parity, 2=even parity + +config USART8_2STOP + int "Uses 2 stop bits" + default 0 + help + 1=Two stop bits + +endmenu + +menu "UART8 Configuration" + depends on ARCH_HAVE_UART8 + +config UART8_RXBUFSIZE + int "Receive buffer size" + default 256 + help + Characters are buffered as they are received. This specifies + the size of the receive buffer. + +config UART8_TXBUFSIZE + int "Transmit buffer size" + default 256 + help + Characters are buffered before being sent. This specifies + the size of the transmit buffer. + +config UART8_BAUD + int "BAUD rate" + default 115200 + help + The configured BAUD of the UART. + +config UART8_BITS + int "Character size" + default 8 + help + The number of bits. Must be either 7 or 8. + +config UART8_PARITY + int "Parity setting" + default 0 + help + 0=no parity, 1=odd parity, 2=even parity + +config UART8_2STOP + int "Uses 2 stop bits" + default 0 + help + 1=Two stop bits + +endmenu From f243f6ef66cb17882a0ee8645a8a7c639272c753 Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 1 Apr 2013 01:23:05 -0700 Subject: [PATCH 032/486] Scratch in a mostly-building board config for fmuv2 --- apps/drivers/boards/px4fmuv2/Makefile | 41 + apps/drivers/boards/px4fmuv2/px4fmu_can.c | 144 +++ apps/drivers/boards/px4fmuv2/px4fmu_init.c | 224 ++++ .../drivers/boards/px4fmuv2/px4fmu_internal.h | 106 ++ .../boards/px4fmuv2/px4fmu_pwm_servo.c | 105 ++ apps/drivers/boards/px4fmuv2/px4fmu_spi.c | 141 +++ apps/drivers/boards/px4fmuv2/px4fmu_usb.c | 108 ++ apps/drivers/drv_gpio.h | 5 + apps/drivers/stm32/drv_hrt.c | 2 +- apps/sensors/sensors.cpp | 2 + makefiles/config_px4fmuv2_default.mk | 62 + nuttx/arch/arm/src/stm32/chip/stm32_syscfg.h | 10 +- nuttx/configs/px4fmuv2/Kconfig | 7 + nuttx/configs/px4fmuv2/common/Make.defs | 184 +++ nuttx/configs/px4fmuv2/common/ld.script | 150 +++ nuttx/configs/px4fmuv2/include/board.h | 351 ++++++ nuttx/configs/px4fmuv2/include/nsh_romfsimg.h | 42 + nuttx/configs/px4fmuv2/nsh/Make.defs | 3 + nuttx/configs/px4fmuv2/nsh/appconfig | 143 +++ nuttx/configs/px4fmuv2/nsh/defconfig | 1050 +++++++++++++++++ nuttx/configs/px4fmuv2/nsh/setenv.sh | 67 ++ nuttx/configs/px4fmuv2/src/Makefile | 84 ++ nuttx/configs/px4fmuv2/src/empty.c | 4 + 23 files changed, 3029 insertions(+), 6 deletions(-) create mode 100644 apps/drivers/boards/px4fmuv2/Makefile create mode 100644 apps/drivers/boards/px4fmuv2/px4fmu_can.c create mode 100644 apps/drivers/boards/px4fmuv2/px4fmu_init.c create mode 100644 apps/drivers/boards/px4fmuv2/px4fmu_internal.h create mode 100644 apps/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c create mode 100644 apps/drivers/boards/px4fmuv2/px4fmu_spi.c create mode 100644 apps/drivers/boards/px4fmuv2/px4fmu_usb.c create mode 100644 makefiles/config_px4fmuv2_default.mk create mode 100644 nuttx/configs/px4fmuv2/Kconfig create mode 100644 nuttx/configs/px4fmuv2/common/Make.defs create mode 100644 nuttx/configs/px4fmuv2/common/ld.script create mode 100755 nuttx/configs/px4fmuv2/include/board.h create mode 100644 nuttx/configs/px4fmuv2/include/nsh_romfsimg.h create mode 100644 nuttx/configs/px4fmuv2/nsh/Make.defs create mode 100644 nuttx/configs/px4fmuv2/nsh/appconfig create mode 100755 nuttx/configs/px4fmuv2/nsh/defconfig create mode 100755 nuttx/configs/px4fmuv2/nsh/setenv.sh create mode 100644 nuttx/configs/px4fmuv2/src/Makefile create mode 100644 nuttx/configs/px4fmuv2/src/empty.c diff --git a/apps/drivers/boards/px4fmuv2/Makefile b/apps/drivers/boards/px4fmuv2/Makefile new file mode 100644 index 0000000000..9c04a8c42a --- /dev/null +++ b/apps/drivers/boards/px4fmuv2/Makefile @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (C) 2012 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. +# +############################################################################ + +# +# Board-specific startup code for the PX4FMUv2 +# + +INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common +LIBNAME = brd_px4fmuv2 + +include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/boards/px4fmuv2/px4fmu_can.c b/apps/drivers/boards/px4fmuv2/px4fmu_can.c new file mode 100644 index 0000000000..86ba183b83 --- /dev/null +++ b/apps/drivers/boards/px4fmuv2/px4fmu_can.c @@ -0,0 +1,144 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 px4fmu_can.c + * + * Board-specific CAN functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include + +#include +#include + +#include "chip.h" +#include "up_arch.h" + +#include "stm32.h" +#include "stm32_can.h" +#include "px4fmu_internal.h" + +#ifdef CONFIG_CAN + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ +/* Configuration ********************************************************************/ + +#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2) +# warning "Both CAN1 and CAN2 are enabled. Assuming only CAN1." +# undef CONFIG_STM32_CAN2 +#endif + +#ifdef CONFIG_STM32_CAN1 +# define CAN_PORT 1 +#else +# define CAN_PORT 2 +#endif + +/* Debug ***************************************************************************/ +/* Non-standard debug that may be enabled just for testing CAN */ + +#ifdef CONFIG_DEBUG_CAN +# define candbg dbg +# define canvdbg vdbg +# define canlldbg lldbg +# define canllvdbg llvdbg +#else +# define candbg(x...) +# define canvdbg(x...) +# define canlldbg(x...) +# define canllvdbg(x...) +#endif + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: can_devinit + * + * Description: + * All STM32 architectures must provide the following interface to work with + * examples/can. + * + ************************************************************************************/ + +int can_devinit(void) +{ + static bool initialized = false; + struct can_dev_s *can; + int ret; + + /* Check if we have already initialized */ + + if (!initialized) { + /* Call stm32_caninitialize() to get an instance of the CAN interface */ + + can = stm32_caninitialize(CAN_PORT); + + if (can == NULL) { + candbg("ERROR: Failed to get CAN interface\n"); + return -ENODEV; + } + + /* Register the CAN driver at "/dev/can0" */ + + ret = can_register("/dev/can0", can); + + if (ret < 0) { + candbg("ERROR: can_register failed: %d\n", ret); + return ret; + } + + /* Now we are initialized */ + + initialized = true; + } + + return OK; +} + +#endif \ No newline at end of file diff --git a/apps/drivers/boards/px4fmuv2/px4fmu_init.c b/apps/drivers/boards/px4fmuv2/px4fmu_init.c new file mode 100644 index 0000000000..c9364c2a49 --- /dev/null +++ b/apps/drivers/boards/px4fmuv2/px4fmu_init.c @@ -0,0 +1,224 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 px4fmu_init.c + * + * PX4FMU-specific early startup code. This file implements the + * nsh_archinitialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialisation. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "stm32_internal.h" +#include "px4fmu_internal.h" +#include "stm32_uart.h" + +#include + +#include +#include + +#include + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +/* Debug ********************************************************************/ + +#ifdef CONFIG_CPP_HAVE_VARARGS +# ifdef CONFIG_DEBUG +# define message(...) lowsyslog(__VA_ARGS__) +# else +# define message(...) printf(__VA_ARGS__) +# endif +#else +# ifdef CONFIG_DEBUG +# define message lowsyslog +# else +# define message printf +# endif +#endif + +/**************************************************************************** + * Protected Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void stm32_boardinitialize(void) +{ + /* configure SPI interfaces */ + stm32_spiinitialize(); + + /* configure LEDs */ + up_ledinit(); +} + +/**************************************************************************** + * Name: nsh_archinitialize + * + * Description: + * Perform architecture specific initialization + * + ****************************************************************************/ + +static struct spi_dev_s *spi1; +static struct spi_dev_s *spi2; + +#include + +#ifdef __cplusplus +__EXPORT int matherr(struct __exception *e) +{ + return 1; +} +#else +__EXPORT int matherr(struct exception *e) +{ + return 1; +} +#endif + +__EXPORT int nsh_archinitialize(void) +{ + int result; + + /* configure the high-resolution time/callout interface */ + hrt_init(); + + /* configure CPU load estimation */ +#ifdef CONFIG_SCHED_INSTRUMENTATION + cpuload_initialize_once(); +#endif + + /* set up the serial DMA polling */ + static struct hrt_call serial_dma_call; + struct timespec ts; + + /* + * Poll at 1ms intervals for received bytes that have not triggered + * a DMA event. + */ + ts.tv_sec = 0; + ts.tv_nsec = 1000000; + + hrt_call_every(&serial_dma_call, + ts_to_abstime(&ts), + ts_to_abstime(&ts), + (hrt_callout)stm32_serial_dma_poll, + NULL); + + // initial LED state + // XXX need to make this work again +// drv_led_start(); + up_ledoff(LED_AMBER); + + /* Configure SPI-based devices */ + + spi1 = up_spiinitialize(1); + + if (!spi1) { + message("[boot] FAILED to initialize SPI port 1\r\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + + // Default SPI1 to 1MHz and de-assert the known chip selects. + SPI_SETFREQUENCY(spi1, 10000000); + SPI_SETBITS(spi1, 8); + SPI_SETMODE(spi1, SPIDEV_MODE3); + SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false); + SPI_SELECT(spi1, PX4_SPIDEV_ACCEL_MAG, false); + SPI_SELECT(spi1, PX4_SPIDEV_BARO, false); + up_udelay(20); + + message("[boot] Successfully initialized SPI port 1\r\n"); + + /* Get the SPI port for the FRAM */ + + message("[boot] Initializing SPI port 2\n"); + spi2 = up_spiinitialize(3); + + if (!spi2) { + message("[boot] FAILED to initialize SPI port 2\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + + message("[boot] Successfully initialized SPI port 2\n"); + + /* XXX need a driver to bind the FRAM to */ + + //message("[boot] Successfully bound SPI port 2 to the FRAM driver\n"); + + /* configure ADC pins */ + stm32_configgpio(GPIO_ADC1_IN10); + stm32_configgpio(GPIO_ADC1_IN11); + stm32_configgpio(GPIO_ADC1_IN12); + + return OK; +} diff --git a/apps/drivers/boards/px4fmuv2/px4fmu_internal.h b/apps/drivers/boards/px4fmuv2/px4fmu_internal.h new file mode 100644 index 0000000000..001b23cb2f --- /dev/null +++ b/apps/drivers/boards/px4fmuv2/px4fmu_internal.h @@ -0,0 +1,106 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 px4fmu_internal.h + * + * PX4FMU internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + +__BEGIN_DECLS + +/* these headers are not C++ safe */ +#include + + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ +/* Configuration ************************************************************************************/ + +/* PX4FMU GPIOs ***********************************************************************************/ +/* LEDs */ + +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN12) + +/* External interrupts */ + +/* SPI chip selects */ +#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) +#define GPIO_SPI_CS_ACCEL_MAG (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) +#define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7) +#define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) + +/* USB OTG FS + * + * PA9 OTG_FS_VBUS VBUS sensing (also connected to the green LED) + */ +#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9) + +/**************************************************************************************************** + * Public Types + ****************************************************************************************************/ + +/**************************************************************************************************** + * Public data + ****************************************************************************************************/ + +#ifndef __ASSEMBLY__ + +/**************************************************************************************************** + * Public Functions + ****************************************************************************************************/ + +/**************************************************************************************************** + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ****************************************************************************************************/ + +extern void stm32_spiinitialize(void); + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/apps/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c b/apps/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c new file mode 100644 index 0000000000..14e4052be1 --- /dev/null +++ b/apps/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 px4fmu_pwm_servo.c + * + * Configuration data for the stm32 pwm_servo driver. + * + * Note that these arrays must always be fully-sized. + */ + +#include + +#include + +#include +#include + +#include +#include +#include + +__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = { + { + .base = STM32_TIM1_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB2ENR_TIM1EN, + .clock_freq = STM32_APB2_TIM1_CLKIN + }, + { + .base = STM32_TIM4_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM4EN, + .clock_freq = STM32_APB1_TIM4_CLKIN + } +}; + +__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = { + { + .gpio = GPIO_TIM1_CH4OUT, + .timer_index = 0, + .timer_channel = 4, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM1_CH3OUT, + .timer_index = 0, + .timer_channel = 3, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM1_CH2OUT, + .timer_index = 0, + .timer_channel = 2, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM1_CH1OUT, + .timer_index = 0, + .timer_channel = 1, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM4_CH2OUT, + .timer_index = 1, + .timer_channel = 2, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM4_CH3OUT, + .timer_index = 1, + .timer_channel = 3, + .default_value = 1000, + } +}; diff --git a/apps/drivers/boards/px4fmuv2/px4fmu_spi.c b/apps/drivers/boards/px4fmuv2/px4fmu_spi.c new file mode 100644 index 0000000000..f90f250716 --- /dev/null +++ b/apps/drivers/boards/px4fmuv2/px4fmu_spi.c @@ -0,0 +1,141 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 px4fmu_spi.c + * + * Board-specific SPI functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include + +#include +#include + +#include "up_arch.h" +#include "chip.h" +#include "stm32_internal.h" +#include "px4fmu_internal.h" + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ************************************************************************************/ + +__EXPORT void weak_function stm32_spiinitialize(void) +{ +#ifdef CONFIG_STM32_SPI1 + stm32_configgpio(GPIO_SPI_CS_GYRO); + stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG); + stm32_configgpio(GPIO_SPI_CS_BARO); + + /* De-activate all peripherals, + * required for some peripheral + * state machines + */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); +#endif + +#ifdef CONFIG_STM32_SPI2 + stm32_configgpio(GPIO_SPI_CS_FRAM); + stm32_gpiowrite(GPIO_SPI_CS_FRAM, 1); +#endif +} + +__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* SPI select is active low, so write !selected to select the device */ + + switch (devid) { + case PX4_SPIDEV_GYRO: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + break; + + case PX4_SPIDEV_ACCEL_MAG: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected); + stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + break; + + case PX4_SPIDEV_BARO: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + stm32_gpiowrite(GPIO_SPI_CS_BARO, !selected); + break; + + default: + break; + } +} + +__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + return SPI_STATUS_PRESENT; +} + + +#ifdef CONFIG_STM32_SPI2 +__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* there can only be one device on this bus, so always select it */ + stm32_gpiowrite(GPIO_SPI_CS_FRAM, !selected); +} + +__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + /* FRAM is always present */ + return SPI_STATUS_PRESENT; +} +#endif diff --git a/apps/drivers/boards/px4fmuv2/px4fmu_usb.c b/apps/drivers/boards/px4fmuv2/px4fmu_usb.c new file mode 100644 index 0000000000..b0b669fbed --- /dev/null +++ b/apps/drivers/boards/px4fmuv2/px4fmu_usb.c @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 px4fmu_usb.c + * + * Board-specific USB functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include + +#include "up_arch.h" +#include "stm32_internal.h" +#include "px4fmu_internal.h" + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: stm32_usbinitialize + * + * Description: + * Called to setup USB-related GPIO pins for the PX4FMU board. + * + ************************************************************************************/ + +__EXPORT void stm32_usbinitialize(void) +{ + /* The OTG FS has an internal soft pull-up */ + + /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ + +#ifdef CONFIG_STM32_OTGFS + stm32_configgpio(GPIO_OTGFS_VBUS); + /* XXX We only support device mode + stm32_configgpio(GPIO_OTGFS_PWRON); + stm32_configgpio(GPIO_OTGFS_OVER); + */ +#endif +} + +/************************************************************************************ + * Name: stm32_usbsuspend + * + * Description: + * Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is + * used. This function is called whenever the USB enters or leaves suspend mode. + * This is an opportunity for the board logic to shutdown clocks, power, etc. + * while the USB is suspended. + * + ************************************************************************************/ + +__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume) +{ + ulldbg("resume: %d\n", resume); +} + diff --git a/apps/drivers/drv_gpio.h b/apps/drivers/drv_gpio.h index 92d184326b..2fa6d8b8e6 100644 --- a/apps/drivers/drv_gpio.h +++ b/apps/drivers/drv_gpio.h @@ -42,6 +42,11 @@ #include +#ifdef CONFIG_ARCH_BOARD_PX4FMUV2 +#warning No GPIOs on this board. +#define GPIO_DEVICE_PATH "/dev/null" +#endif + #ifdef CONFIG_ARCH_BOARD_PX4FMU /* * PX4FMU GPIO numbers. diff --git a/apps/drivers/stm32/drv_hrt.c b/apps/drivers/stm32/drv_hrt.c index bb67d5e6d2..cec0c49ffd 100644 --- a/apps/drivers/stm32/drv_hrt.c +++ b/apps/drivers/stm32/drv_hrt.c @@ -125,7 +125,7 @@ # define HRT_TIMER_VECTOR STM32_IRQ_TIM8CC # define HRT_TIMER_CLOCK STM32_APB2_TIM8_CLKIN # if CONFIG_STM32_TIM8 -# error must not set CONFIG_STM32_TIM8=y and HRT_TIMER=6 +# error must not set CONFIG_STM32_TIM8=y and HRT_TIMER=8 # endif #elif HRT_TIMER == 9 # define HRT_TIMER_BASE STM32_TIM9_BASE diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index 1f3f7707e2..123bbb1203 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -372,7 +372,9 @@ Sensors *g_sensors; } Sensors::Sensors() : +#ifdef CONFIG_HRT_PPM _ppm_last_valid(0), +#endif _fd_adc(-1), _last_adc(0), diff --git a/makefiles/config_px4fmuv2_default.mk b/makefiles/config_px4fmuv2_default.mk new file mode 100644 index 0000000000..d5bc758b85 --- /dev/null +++ b/makefiles/config_px4fmuv2_default.mk @@ -0,0 +1,62 @@ +# +# Makefile for the px4fmu_default configuration +# + +# +# Use the configuration's ROMFS. +# +ROMFS_ROOT = $(PX4_BASE)/ROMFS/$(CONFIG) + +# +# Transitional support - add commands from the NuttX export archive. +# +# In general, these should move to modules over time. +# +# Each entry here is ... but we use a helper macro +# to make the table a bit more readable. +# +define _B + $(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4) +endef + +# command priority stack entrypoint +BUILTIN_COMMANDS := \ + $(call _B, adc, , 2048, adc_main ) \ + $(call _B, ardrone_interface, SCHED_PRIORITY_MAX-15, 2048, ardrone_interface_main ) \ + $(call _B, attitude_estimator_ekf, , 2048, attitude_estimator_ekf_main) \ + $(call _B, bl_update, , 4096, bl_update_main ) \ + $(call _B, blinkm, , 2048, blinkm_main ) \ + $(call _B, boardinfo, , 2048, boardinfo_main ) \ + $(call _B, commander, SCHED_PRIORITY_MAX-30, 2048, commander_main ) \ + $(call _B, control_demo, , 2048, control_demo_main ) \ + $(call _B, delay_test, , 2048, delay_test_main ) \ + $(call _B, eeprom, , 4096, eeprom_main ) \ + $(call _B, fixedwing_att_control, SCHED_PRIORITY_MAX-30, 2048, fixedwing_att_control_main ) \ + $(call _B, fixedwing_pos_control, SCHED_PRIORITY_MAX-30, 2048, fixedwing_pos_control_main ) \ + $(call _B, fmu, , 2048, fmu_main ) \ + $(call _B, gps, , 2048, gps_main ) \ + $(call _B, hil, , 2048, hil_main ) \ + $(call _B, hott_telemetry, , 2048, hott_telemetry_main ) \ + $(call _B, kalman_demo, SCHED_PRIORITY_MAX-30, 2048, kalman_demo_main ) \ + $(call _B, l3gd20, , 2048, l3gd20_main ) \ + $(call _B, math_demo, , 8192, math_demo_main ) \ + $(call _B, mavlink, , 2048, mavlink_main ) \ + $(call _B, mavlink_onboard, , 2048, mavlink_onboard_main ) \ + $(call _B, mixer, , 4096, mixer_main ) \ + $(call _B, ms5611, , 2048, ms5611_main ) \ + $(call _B, multirotor_att_control, SCHED_PRIORITY_MAX-15, 2048, multirotor_att_control_main) \ + $(call _B, multirotor_pos_control, SCHED_PRIORITY_MAX-25, 2048, multirotor_pos_control_main) \ + $(call _B, param, , 4096, param_main ) \ + $(call _B, perf, , 2048, perf_main ) \ + $(call _B, position_estimator, , 4096, position_estimator_main ) \ + $(call _B, preflight_check, , 2048, preflight_check_main ) \ + $(call _B, px4io, , 2048, px4io_main ) \ + $(call _B, reboot, , 2048, reboot_main ) \ + $(call _B, sdlog, SCHED_PRIORITY_MAX-30, 2048, sdlog_main ) \ + $(call _B, sensors, SCHED_PRIORITY_MAX-5, 4096, sensors_main ) \ + $(call _B, sercon, , 2048, sercon_main ) \ + $(call _B, serdis, , 2048, serdis_main ) \ + $(call _B, tests, , 12000, tests_main ) \ + $(call _B, tone_alarm, , 2048, tone_alarm_main ) \ + $(call _B, top, SCHED_PRIORITY_DEFAULT-10, 3000, top_main ) \ + $(call _B, uorb, , 4096, uorb_main ) diff --git a/nuttx/arch/arm/src/stm32/chip/stm32_syscfg.h b/nuttx/arch/arm/src/stm32/chip/stm32_syscfg.h index dca3820fd1..55133b3daa 100644 --- a/nuttx/arch/arm/src/stm32/chip/stm32_syscfg.h +++ b/nuttx/arch/arm/src/stm32/chip/stm32_syscfg.h @@ -89,11 +89,11 @@ /* SYSCFG peripheral mode configuration register */ #define SYSCFG_PMC_MII_RMII_SEL (1 << 23) /* Bit 23: Ethernet PHY interface selection */ -+#ifdef CONFIG_STM32_STM32F427 -+# define SYSCFG_PMC_ADC3DC2 (1 << 18) /* Bit 18: See AN4073 */ -+# define SYSCFG_PMC_ADC2DC2 (1 << 17) /* Bit 17: See AN4073 */ -+# define SYSCFG_PMC_ADC1DC2 (1 << 16) /* Bit 16: See AN4073 */ -+#endif +#ifdef CONFIG_STM32_STM32F427 +# define SYSCFG_PMC_ADC3DC2 (1 << 18) /* Bit 18: See AN4073 */ +# define SYSCFG_PMC_ADC2DC2 (1 << 17) /* Bit 17: See AN4073 */ +# define SYSCFG_PMC_ADC1DC2 (1 << 16) /* Bit 16: See AN4073 */ +#endif /* SYSCFG external interrupt configuration register 1-4 */ diff --git a/nuttx/configs/px4fmuv2/Kconfig b/nuttx/configs/px4fmuv2/Kconfig new file mode 100644 index 0000000000..a10a02ba47 --- /dev/null +++ b/nuttx/configs/px4fmuv2/Kconfig @@ -0,0 +1,7 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# + +if ARCH_BOARD_PX4FMUV2 +endif diff --git a/nuttx/configs/px4fmuv2/common/Make.defs b/nuttx/configs/px4fmuv2/common/Make.defs new file mode 100644 index 0000000000..be387dce14 --- /dev/null +++ b/nuttx/configs/px4fmuv2/common/Make.defs @@ -0,0 +1,184 @@ +############################################################################ +# configs/px4fmu/common/Make.defs +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# +############################################################################ + +# +# Generic Make.defs for the PX4FMUV2 +# Do not specify/use this file directly - it is included by config-specific +# Make.defs in the per-config directories. +# + +include ${TOPDIR}/tools/Config.mk + +# +# We only support building with the ARM bare-metal toolchain from +# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS. +# +CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI + +include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs + +CC = $(CROSSDEV)gcc +CXX = $(CROSSDEV)g++ +CPP = $(CROSSDEV)gcc -E +LD = $(CROSSDEV)ld +AR = $(CROSSDEV)ar rcs +NM = $(CROSSDEV)nm +OBJCOPY = $(CROSSDEV)objcopy +OBJDUMP = $(CROSSDEV)objdump + +MAXOPTIMIZATION = -O3 +ARCHCPUFLAGS = -mcpu=cortex-m4 \ + -mthumb \ + -march=armv7e-m \ + -mfpu=fpv4-sp-d16 \ + -mfloat-abi=hard + + +# enable precise stack overflow tracking +INSTRUMENTATIONDEFINES = -finstrument-functions \ + -ffixed-r10 + +# pull in *just* libm from the toolchain ... this is grody +LIBM = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}" +EXTRA_LIBS += $(LIBM) + +# use our linker script +LDSCRIPT = ld.script + +ifeq ($(WINTOOL),y) + # Windows-native toolchains + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" + ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" + ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT)}" +else + ifeq ($(PX4_WINTOOL),y) + # Windows-native toolchains (MSYS) + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT) + else + # Linux/Cygwin-native toolchain + MKDEP = $(TOPDIR)/tools/mkdeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT) + endif +endif + +# tool versions +ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} +ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} + +# optimisation flags +ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ + -fno-strict-aliasing \ + -fno-strength-reduce \ + -fomit-frame-pointer \ + -funsafe-math-optimizations \ + -fno-builtin-printf \ + -ffunction-sections \ + -fdata-sections + +ifeq ("${CONFIG_DEBUG_SYMBOLS}","y") +ARCHOPTIMIZATION += -g +endif + +ARCHCFLAGS = -std=gnu99 +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x +ARCHWARNINGS = -Wall \ + -Wextra \ + -Wdouble-promotion \ + -Wshadow \ + -Wfloat-equal \ + -Wframe-larger-than=1024 \ + -Wpointer-arith \ + -Wlogical-op \ + -Wmissing-declarations \ + -Wpacked \ + -Wno-unused-parameter +# -Wcast-qual - generates spurious noreturn attribute warnings, try again later +# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code +# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives + +ARCHCWARNINGS = $(ARCHWARNINGS) \ + -Wbad-function-cast \ + -Wstrict-prototypes \ + -Wold-style-declaration \ + -Wmissing-parameter-type \ + -Wmissing-prototypes \ + -Wnested-externs \ + -Wunsuffixed-float-constants +ARCHWARNINGSXX = $(ARCHWARNINGS) \ + -Wno-psabi +ARCHDEFINES = +ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 + +# this seems to be the only way to add linker flags +EXTRA_LIBS += --warn-common \ + --gc-sections + +CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common +CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) +CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe +CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) +CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) +AFLAGS = $(CFLAGS) -D__ASSEMBLY__ + +NXFLATLDFLAGS1 = -r -d -warn-common +NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections +LDNXFLATFLAGS = -e main -s 2048 + +OBJEXT = .o +LIBEXT = .a +EXEEXT = + + +# produce partially-linked $1 from files in $2 +define PRELINK + @echo "PRELINK: $1" + $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 +endef + +HOSTCC = gcc +HOSTINCLUDES = -I. +HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe +HOSTLDFLAGS = + diff --git a/nuttx/configs/px4fmuv2/common/ld.script b/nuttx/configs/px4fmuv2/common/ld.script new file mode 100644 index 0000000000..1be39fb87f --- /dev/null +++ b/nuttx/configs/px4fmuv2/common/ld.script @@ -0,0 +1,150 @@ +/**************************************************************************** + * configs/px4fmu/common/ld.script + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/* The STM32F427 has 2048Kb of FLASH beginning at address 0x0800:0000 and + * 256Kb of SRAM. SRAM is split up into three blocks: + * + * 1) 112Kb of SRAM beginning at address 0x2000:0000 + * 2) 16Kb of SRAM beginning at address 0x2001:c000 + * 3) 64Kb of SRAM beginning at address 0x2002:0000 + * 4) 64Kb of TCM SRAM beginning at address 0x1000:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + * + * The first 0x4000 of flash is reserved for the bootloader. + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x08004000, LENGTH = 2032K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K + ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K +} + +OUTPUT_ARCH(arm) + +ENTRY(__start) /* treat __start as the anchor for dead code stripping */ +EXTERN(_vectors) /* force the vectors to be included in the output */ + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + /* + * This is a hack to make the newlib libm __errno() call + * use the NuttX get_errno_ptr() function. + */ + __errno = get_errno_ptr; + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + /* + * Construction data for parameters. + */ + __param ALIGN(4): { + __param_start = ABSOLUTE(.); + KEEP(*(__param*)) + __param_end = ABSOLUTE(.); + } > flash + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/nuttx/configs/px4fmuv2/include/board.h b/nuttx/configs/px4fmuv2/include/board.h new file mode 100755 index 0000000000..286dedab05 --- /dev/null +++ b/nuttx/configs/px4fmuv2/include/board.h @@ -0,0 +1,351 @@ +/************************************************************************************ + * configs/px4fmu/include/board.h + * include/arch/board/board.h + * + * Copyright (C) 2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ************************************************************************************/ + +#ifndef __ARCH_BOARD_BOARD_H +#define __ARCH_BOARD_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include +#ifndef __ASSEMBLY__ +# include +#endif + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ +/* The PX4FMUV2 uses a 24MHz crystal connected to the HSE. + * + * This is the "standard" configuration as set up by arch/arm/src/stm32f40xx_rcc.c: + * System Clock source : PLL (HSE) + * SYSCLK(Hz) : 168000000 Determined by PLL configuration + * HCLK(Hz) : 168000000 (STM32_RCC_CFGR_HPRE) + * AHB Prescaler : 1 (STM32_RCC_CFGR_HPRE) + * APB1 Prescaler : 4 (STM32_RCC_CFGR_PPRE1) + * APB2 Prescaler : 2 (STM32_RCC_CFGR_PPRE2) + * HSE Frequency(Hz) : 24000000 (STM32_BOARD_XTAL) + * PLLM : 24 (STM32_PLLCFG_PLLM) + * PLLN : 336 (STM32_PLLCFG_PLLN) + * PLLP : 2 (STM32_PLLCFG_PLLP) + * PLLQ : 7 (STM32_PLLCFG_PPQ) + * Main regulator output voltage : Scale1 mode Needed for high speed SYSCLK + * Flash Latency(WS) : 5 + * Prefetch Buffer : OFF + * Instruction cache : ON + * Data cache : ON + * Require 48MHz for USB OTG FS, : Enabled + * SDIO and RNG clock + */ + +/* HSI - 16 MHz RC factory-trimmed + * LSI - 32 KHz RC + * HSE - On-board crystal frequency is 24MHz + * LSE - not installed + */ + +#define STM32_BOARD_XTAL 24000000ul + +#define STM32_HSI_FREQUENCY 16000000ul +#define STM32_LSI_FREQUENCY 32000 +#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL +//#define STM32_LSE_FREQUENCY 32768 + +/* Main PLL Configuration. + * + * PLL source is HSE + * PLL_VCO = (STM32_HSE_FREQUENCY / PLLM) * PLLN + * = (25,000,000 / 25) * 336 + * = 336,000,000 + * SYSCLK = PLL_VCO / PLLP + * = 336,000,000 / 2 = 168,000,000 + * USB OTG FS, SDIO and RNG Clock + * = PLL_VCO / PLLQ + * = 48,000,000 + */ + +#define STM32_PLLCFG_PLLM RCC_PLLCFG_PLLM(24) +#define STM32_PLLCFG_PLLN RCC_PLLCFG_PLLN(336) +#define STM32_PLLCFG_PLLP RCC_PLLCFG_PLLP_2 +#define STM32_PLLCFG_PLLQ RCC_PLLCFG_PLLQ(7) + +#define STM32_SYSCLK_FREQUENCY 168000000ul + +/* AHB clock (HCLK) is SYSCLK (168MHz) */ + +#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK /* HCLK = SYSCLK / 1 */ +#define STM32_HCLK_FREQUENCY STM32_SYSCLK_FREQUENCY +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ + +/* APB1 clock (PCLK1) is HCLK/4 (42MHz) */ + +#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLKd4 /* PCLK1 = HCLK / 4 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/4) + +/* Timers driven from APB1 will be twice PCLK1 */ + +#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM6_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM7_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* APB2 clock (PCLK2) is HCLK/2 (84MHz) */ + +#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* Timers driven from APB2 will be twice PCLK2 */ + +#define STM32_APB2_TIM1_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM9_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB2_TIM10_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx + * otherwise frequency is 2xAPBx. + * Note: TIM1,8 are on APB2, others on APB1 + */ + +#define STM32_TIM18_FREQUENCY (2*STM32_PCLK2_FREQUENCY) +#define STM32_TIM27_FREQUENCY (2*STM32_PCLK1_FREQUENCY) + +/* SDIO dividers. Note that slower clocking is required when DMA is disabled + * in order to avoid RX overrun/TX underrun errors due to delayed responses + * to service FIFOs in interrupt driven mode. These values have not been + * tuned!!! + * + * HCLK=72MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(178+2)=400 KHz + */ + +#define SDIO_INIT_CLKDIV (178 << SDIO_CLKCR_CLKDIV_SHIFT) + +/* DMA ON: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(2+2)=18 MHz + * DMA OFF: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(3+2)=14.4 MHz + */ + +#ifdef CONFIG_SDIO_DMA +# define SDIO_MMCXFR_CLKDIV (2 << SDIO_CLKCR_CLKDIV_SHIFT) +#else +# define SDIO_MMCXFR_CLKDIV (3 << SDIO_CLKCR_CLKDIV_SHIFT) +#endif + +/* DMA ON: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(1+2)=24 MHz + * DMA OFF: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(3+2)=14.4 MHz + */ + +#ifdef CONFIG_SDIO_DMA +# define SDIO_SDXFR_CLKDIV (1 << SDIO_CLKCR_CLKDIV_SHIFT) +#else +# define SDIO_SDXFR_CLKDIV (3 << SDIO_CLKCR_CLKDIV_SHIFT) +#endif + +/* High-resolution timer + */ +#ifdef CONFIG_HRT_TIMER +# define HRT_TIMER 8 /* use timer8 for the HRT */ +# define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */ +#endif + +/* Alternate function pin selections ************************************************/ + +/* + * UARTs. + */ +#define GPIO_USART1_RX GPIO_USART1_RX_1 /* console in from IO */ +#define GPIO_USART1_TX 0 /* USART1 is RX-only */ + +#define GPIO_USART2_RX GPIO_USART2_RX_2 +#define GPIO_USART2_TX GPIO_USART2_TX_2 +#define GPIO_USART2_RTS GPIO_USART2_RTS_2 +#define GPIO_USART2_CTS GPIO_USART2_CTS_2 + +#define GPIO_USART3_RX GPIO_USART3_RX_3 +#define GPIO_USART3_TX GPIO_USART3_TX_3 +#define GPIO_USART2_RTS GPIO_USART2_RTS_2 +#define GPIO_USART2_CTS GPIO_USART2_CTS_2 + +#define GPIO_UART4_RX GPIO_UART4_RX_1 +#define GPIO_UART4_TX GPIO_UART4_TX_1 + +#define GPIO_USART6_RX GPIO_USART6_RX_1 +#define GPIO_USART6_TX GPIO_USART6_TX_1 + +#define GPIO_UART7_RX GPIO_UART7_RX_1 +#define GPIO_UART7_TX GPIO_UART7_TX_1 + +/* UART8 has no alternate pin config */ + +/* UART RX DMA configurations */ +#define DMAMAP_USART1_RX DMAMAP_USART1_RX_2 +#define DMAMAP_USART6_RX DMAMAP_USART6_RX_2 + +/* + * PWM + * + * Six PWM outputs are configured. + * + * Pins: + * + * CH1 : PE14 : TIM1_CH4 + * CH2 : PE13 : TIM1_CH3 + * CH3 : PE11 : TIM1_CH2 + * CH4 : PE9 : TIM1_CH1 + * CH5 : PD13 : TIM4_CH2 + * CH6 : PD14 : TIM4_CH3 + * + */ +#define GPIO_TIM1_CH1OUT GPIO_TIM1_CH1OUT_2 +#define GPIO_TIM1_CH2OUT GPIO_TIM1_CH2OUT_2 +#define GPIO_TIM1_CH3OUT GPIO_TIM1_CH3OUT_2 +#define GPIO_TIM1_CH4OUT GPIO_TIM1_CH4OUT_2 +#define GPIO_TIM4_CH2OUT GPIO_TIM4_CH2OUT_2 +#define GPIO_TIM4_CH3OUT GPIO_TIM4_CH3OUT_2 + +/* + * CAN + * + * CAN1 is routed to the onboard transceiver. + * CAN2 is routed to the expansion connector. + */ + +#define GPIO_CAN1_RX GPIO_CAN1_RX_3 +#define GPIO_CAN1_TX GPIO_CAN1_TX_3 +#define GPIO_CAN2_RX GPIO_CAN2_RX_1 +#define GPIO_CAN2_TX GPIO_CAN2_TX_2 + +/* + * I2C + * + * The optional _GPIO configurations allow the I2C driver to manually + * reset the bus to clear stuck slaves. They match the pin configuration, + * but are normally-high GPIOs. + */ +#define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 +#define GPIO_I2C1_SDA GPIO_I2C1_SDA_2 +#define GPIO_I2C1_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN8) +#define GPIO_I2C1_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN9) + +#define GPIO_I2C2_SCL GPIO_I2C2_SCL_1 +#define GPIO_I2C2_SDA GPIO_I2C2_SDA_1 +#define GPIO_I2C2_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN10) +#define GPIO_I2C2_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11) + +/* + * I2C busses + */ +#define PX4_I2C_BUS_EXPANSION 1 +#define PX4_I2C_BUS_LED 2 + +/* + * Devices on the onboard bus. + * + * Note that these are unshifted addresses. + */ +#define PX4_I2C_OBDEV_LED 0x5a + +/* + * SPI + * + * There are sensors on SPI1, and SPI2 is connected to the FRAM. + */ +#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 +#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 +#define GPIO_SPI1_SCK GPIO_SPI1_SCK_1 + +#define GPIO_SPI2_MISO GPIO_SPI2_MISO_1 +#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_1 +#define GPIO_SPI2_SCK GPIO_SPI2_SCK_2 + +/* + * Use these in place of the spi_dev_e enumeration to + * select a specific SPI device on SPI1 + */ +#define PX4_SPIDEV_GYRO 1 +#define PX4_SPIDEV_ACCEL_MAG 2 +#define PX4_SPIDEV_BARO 3 + +/* + * Tone alarm output + */ +#define TONE_ALARM_TIMER 2 /* timer 3 */ +#define TONE_ALARM_CHANNEL 1 /* channel 3 */ +#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF2|GPIO_SPEED_2MHz|GPIO_FLOAT|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN15) + +/************************************************************************************ + * Public Data + ************************************************************************************/ + +#ifndef __ASSEMBLY__ + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +/************************************************************************************ + * Public Function Prototypes + ************************************************************************************/ +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +EXTERN void stm32_boardinitialize(void); + +#undef EXTERN +#if defined(__cplusplus) +} +#endif + +#endif /* __ASSEMBLY__ */ +#endif /* __ARCH_BOARD_BOARD_H */ diff --git a/nuttx/configs/px4fmuv2/include/nsh_romfsimg.h b/nuttx/configs/px4fmuv2/include/nsh_romfsimg.h new file mode 100644 index 0000000000..15e4e7a8d5 --- /dev/null +++ b/nuttx/configs/px4fmuv2/include/nsh_romfsimg.h @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * nsh_romfsetc.h + * + * This file is a stub for 'make export' purposes; the actual ROMFS + * must be supplied by the library client. + */ + +extern unsigned char romfs_img[]; +extern unsigned int romfs_img_len; diff --git a/nuttx/configs/px4fmuv2/nsh/Make.defs b/nuttx/configs/px4fmuv2/nsh/Make.defs new file mode 100644 index 0000000000..3e6f88bd3a --- /dev/null +++ b/nuttx/configs/px4fmuv2/nsh/Make.defs @@ -0,0 +1,3 @@ +include ${TOPDIR}/.config + +include $(TOPDIR)/configs/px4fmu/common/Make.defs diff --git a/nuttx/configs/px4fmuv2/nsh/appconfig b/nuttx/configs/px4fmuv2/nsh/appconfig new file mode 100644 index 0000000000..fbcb67dcff --- /dev/null +++ b/nuttx/configs/px4fmuv2/nsh/appconfig @@ -0,0 +1,143 @@ +############################################################################ +# configs/px4fmu/nsh/appconfig +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# +############################################################################ + +# Path to example in apps/examples containing the user_start entry point + +CONFIGURED_APPS += examples/nsh + +# The NSH application library +CONFIGURED_APPS += nshlib +CONFIGURED_APPS += system/readline + +# System library - utility functions +CONFIGURED_APPS += systemlib +CONFIGURED_APPS += systemlib/mixer + +# Math library +ifneq ($(CONFIG_APM),y) +CONFIGURED_APPS += mathlib +CONFIGURED_APPS += mathlib/CMSIS +CONFIGURED_APPS += examples/math_demo +endif + +# Control library +ifneq ($(CONFIG_APM),y) +CONFIGURED_APPS += controllib +CONFIGURED_APPS += examples/control_demo +CONFIGURED_APPS += examples/kalman_demo +endif + +# System utility commands +CONFIGURED_APPS += systemcmds/reboot +CONFIGURED_APPS += systemcmds/perf +CONFIGURED_APPS += systemcmds/top +CONFIGURED_APPS += systemcmds/boardinfo +CONFIGURED_APPS += systemcmds/mixer +# No I2C EEPROM - need new param interface +#CONFIGURED_APPS += systemcmds/eeprom +#CONFIGURED_APPS += systemcmds/param +CONFIGURED_APPS += systemcmds/pwm +CONFIGURED_APPS += systemcmds/bl_update +CONFIGURED_APPS += systemcmds/preflight_check +CONFIGURED_APPS += systemcmds/delay_test + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/hello_sky +# CONFIGURED_APPS += examples/px4_simple_app + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/deamon +# CONFIGURED_APPS += examples/px4_deamon_app + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/debug_values +# CONFIGURED_APPS += examples/px4_mavlink_debug + +# Shared object broker; required by many parts of the system. +CONFIGURED_APPS += uORB + +CONFIGURED_APPS += mavlink +CONFIGURED_APPS += mavlink_onboard +CONFIGURED_APPS += commander +CONFIGURED_APPS += sdlog +CONFIGURED_APPS += sensors + +ifneq ($(CONFIG_APM),y) +#CONFIGURED_APPS += ardrone_interface +CONFIGURED_APPS += multirotor_att_control +CONFIGURED_APPS += multirotor_pos_control +#CONFIGURED_APPS += fixedwing_control +CONFIGURED_APPS += fixedwing_att_control +CONFIGURED_APPS += fixedwing_pos_control +CONFIGURED_APPS += position_estimator +CONFIGURED_APPS += attitude_estimator_ekf +CONFIGURED_APPS += hott_telemetry +endif + +# Hacking tools +# XXX needs updating for new i2c config +#CONFIGURED_APPS += systemcmds/i2c + +# Communication and Drivers +CONFIGURED_APPS += drivers/boards/px4fmuv2 +CONFIGURED_APPS += drivers/device +# XXX needs conversion to SPI +#CONFIGURED_APPS += drivers/ms5611 +CONFIGURED_APPS += drivers/l3gd20 +# XXX needs conversion to serial +#CONFIGURED_APPS += drivers/px4io +CONFIGURED_APPS += drivers/stm32 +#CONFIGURED_APPS += drivers/led +CONFIGURED_APPS += drivers/blinkm +CONFIGURED_APPS += drivers/stm32/tone_alarm +CONFIGURED_APPS += drivers/stm32/adc +#CONFIGURED_APPS += drivers/px4fmu +CONFIGURED_APPS += drivers/hil +CONFIGURED_APPS += drivers/gps +CONFIGURED_APPS += drivers/mb12xx + +# Testing stuff +CONFIGURED_APPS += px4/sensors_bringup +CONFIGURED_APPS += px4/tests + +ifeq ($(CONFIG_CAN),y) +#CONFIGURED_APPS += examples/can +endif + +#ifeq ($(CONFIG_USBDEV),y) +#ifeq ($(CONFIG_CDCACM),y) +CONFIGURED_APPS += examples/cdcacm +#endif +#endif diff --git a/nuttx/configs/px4fmuv2/nsh/defconfig b/nuttx/configs/px4fmuv2/nsh/defconfig new file mode 100755 index 0000000000..8e9feef65d --- /dev/null +++ b/nuttx/configs/px4fmuv2/nsh/defconfig @@ -0,0 +1,1050 @@ +############################################################################ +# configs/px4fmu/nsh/defconfig +# +# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# +############################################################################ +# +# architecture selection +# +# CONFIG_ARCH - identifies the arch subdirectory and, hence, the +# processor architecture. +# CONFIG_ARCH_family - for use in C code. This identifies the +# particular chip family that the architecture is implemented +# in. +# CONFIG_ARCH_architecture - for use in C code. This identifies the +# specific architecture within the chip family. +# CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory +# CONFIG_ARCH_CHIP_name - For use in C code +# CONFIG_ARCH_BOARD - identifies the configs subdirectory and, hence, +# the board that supports the particular chip or SoC. +# CONFIG_ARCH_BOARD_name - for use in C code +# CONFIG_ENDIAN_BIG - define if big endian (default is little endian) +# CONFIG_BOARD_LOOPSPERMSEC - for delay loops +# CONFIG_DRAM_SIZE - Describes the installed DRAM. +# CONFIG_DRAM_START - The start address of DRAM (physical) +# CONFIG_ARCH_IRQPRIO - The STM3240xxx supports interrupt prioritization +# CONFIG_ARCH_FPU - The STM3240xxx supports a floating point unit (FPU). +# CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt +# stack. If defined, this symbol is the size of the interrupt +# stack in bytes. If not defined, the user task stacks will be +# used during interrupt handling. +# CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions +# CONFIG_ARCH_BOOTLOADER - Set if you are using a bootloader. +# CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to board architecture. +# CONFIG_ARCH_BUTTONS - Enable support for buttons. Unique to board architecture. +# CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that +# cause a 100 second delay during boot-up. This 100 second delay +# serves no purpose other than it allows you to calibrate +# CONFIG_BOARD_LOOPSPERMSEC. You simply use a stop watch to measure +# the 100 second delay then adjust CONFIG_BOARD_LOOPSPERMSEC until +# the delay actually is 100 seconds. +# CONFIG_ARCH_DMA - Support DMA initialization +# +CONFIG_ARCH="arm" +CONFIG_ARCH_ARM=y +CONFIG_ARCH_CORTEXM4=y +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARCH_CHIP_STM32F427V=y +CONFIG_ARCH_BOARD="px4fmuv2" +CONFIG_ARCH_BOARD_PX4FMUV2=y +CONFIG_BOARD_LOOPSPERMSEC=16717 +CONFIG_DRAM_SIZE=0x00040000 +CONFIG_DRAM_START=0x20000000 +CONFIG_ARCH_IRQPRIO=y +CONFIG_ARCH_FPU=y +CONFIG_ARCH_INTERRUPTSTACK=n +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARCH_BOOTLOADER=n +CONFIG_ARCH_LEDS=n +CONFIG_ARCH_BUTTONS=n +CONFIG_ARCH_CALIBRATION=n +CONFIG_ARCH_DMA=y +CONFIG_ARCH_MATH_H=y + +CONFIG_ARMV7M_CMNVECTOR=y +CONFIG_STM32_STM32F427=y + +# +# JTAG Enable settings (by default JTAG-DP and SW-DP are enabled): +# +# CONFIG_STM32_DFU - Use the DFU bootloader, not JTAG (ignored) +# +# JTAG Enable options: +# +# CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) +# CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) +# but without JNTRST. +# CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled +# +CONFIG_STM32_DFU=n +CONFIG_STM32_JTAG_FULL_ENABLE=y +CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n +CONFIG_STM32_JTAG_SW_ENABLE=n + +# +# On-chip CCM SRAM configuration +# +# CONFIG_STM32_CCMEXCLUDE - Exclude CCM SRAM from the HEAP. You would need +# to do this if DMA is enabled to prevent non-DMA-able CCM memory from +# being a part of the stack. +# + +# +# On-board FSMC SRAM configuration +# +# CONFIG_STM32_FSMC - Required. See below +# CONFIG_MM_REGIONS - Required. Must be 2 or 3 (see above) +# +# CONFIG_STM32_FSMC_SRAM=y - Indicates that SRAM is available via the +# FSMC (as opposed to an LCD or FLASH). +# CONFIG_HEAP2_BASE - The base address of the SRAM in the FSMC address space +# CONFIG_HEAP2_END - The end (+1) of the SRAM in the FSMC address space +# +#CONFIG_STM32_FSMC_SRAM=n +#CONFIG_HEAP2_BASE=0x64000000 +#CONFIG_HEAP2_END=(0x64000000+(2*1024*1024)) + +# +# Individual subsystems can be enabled: +# +# This set is exhaustive for PX4FMU and should be safe to cut and +# paste into any other config. +# +# AHB1: +CONFIG_STM32_CRC=n +CONFIG_STM32_BKPSRAM=y +CONFIG_STM32_CCMDATARAM=y +CONFIG_STM32_DMA1=y +CONFIG_STM32_DMA2=y +CONFIG_STM32_ETHMAC=n +CONFIG_STM32_OTGHS=n +# AHB2: +CONFIG_STM32_DCMI=n +CONFIG_STM32_CRYP=n +CONFIG_STM32_HASH=n +CONFIG_STM32_RNG=n +CONFIG_STM32_OTGFS=y +# AHB3: +CONFIG_STM32_FSMC=n +# APB1: +CONFIG_STM32_TIM2=n +CONFIG_STM32_TIM3=y +CONFIG_STM32_TIM4=y +CONFIG_STM32_TIM5=n +CONFIG_STM32_TIM6=n +CONFIG_STM32_TIM7=n +CONFIG_STM32_TIM12=n +CONFIG_STM32_TIM13=n +CONFIG_STM32_TIM14=n +CONFIG_STM32_WWDG=y +CONFIG_STM32_IWDG=n +CONFIG_STM32_SPI2=y +CONFIG_STM32_SPI3=n +CONFIG_STM32_USART2=y +CONFIG_STM32_USART3=y +CONFIG_STM32_UART4=y +CONFIG_STM32_UART5=n +CONFIG_STM32_UART7=y +CONFIG_STM32_UART8=y +CONFIG_STM32_I2C1=y +CONFIG_STM32_I2C2=y +CONFIG_STM32_I2C3=n +CONFIG_STM32_CAN1=n +CONFIG_STM32_CAN2=n +CONFIG_STM32_DAC=n +CONFIG_STM32_PWR=y +# APB2: +CONFIG_STM32_TIM1=y +CONFIG_STM32_TIM8=n +CONFIG_STM32_USART1=y +CONFIG_STM32_USART6=y +# We use our own driver, but leave this on. +CONFIG_STM32_ADC1=y +CONFIG_STM32_ADC2=n +CONFIG_STM32_ADC3=n +CONFIG_STM32_SDIO=n +CONFIG_STM32_SPI1=y +CONFIG_STM32_SYSCFG=y +CONFIG_STM32_TIM9=y +CONFIG_STM32_TIM10=y +CONFIG_STM32_TIM11=y + +# +# Enable single wire support. If this is not defined, then this mode cannot +# be enabled. +# +CONFIG_STM32_USART_SINGLEWIRE=y + +# +# We want the flash prefetch on for max performance. +# +STM32_FLASH_PREFETCH=y + +# +# STM32F40xxx specific serial device driver settings +# +# CONFIG_SERIAL_TERMIOS - Serial driver supports termios.h interfaces (tcsetattr, +# tcflush, etc.). If this is not defined, then the terminal settings (baud, +# parity, etc.) are not configurable at runtime; serial streams cannot be +# flushed, etc. +# CONFIG_SERIAL_CONSOLE_REINIT - re-initializes the console serial port +# immediately after creating the /dev/console device. This is required +# if the console serial port has RX DMA enabled. +# +# CONFIG_USARTn_SERIAL_CONSOLE - selects the USARTn for the +# console and ttys0 (default is the USART1). +# CONFIG_USARTn_RXBUFSIZE - Characters are buffered as received. +# This specific the size of the receive buffer +# CONFIG_USARTn_TXBUFSIZE - Characters are buffered before +# being sent. This specific the size of the transmit buffer +# CONFIG_USARTn_BAUD - The configure BAUD of the UART. Must be +# CONFIG_USARTn_BITS - The number of bits. Must be either 7 or 8. +# CONFIG_USARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity +# CONFIG_USARTn_2STOP - Two stop bits +# +CONFIG_SERIAL_TERMIOS=y +CONFIG_SERIAL_CONSOLE_REINIT=y +CONFIG_STANDARD_SERIAL=y + +CONFIG_UART8_SERIAL_CONSOLE=y + +#Mavlink messages can be bigger than 128 +CONFIG_USART1_TXBUFSIZE=512 +CONFIG_USART2_TXBUFSIZE=256 +CONFIG_USART3_TXBUFSIZE=256 +CONFIG_UART4_TXBUFSIZE=256 +CONFIG_USART6_TXBUFSIZE=128 +CONFIG_UART7_TXBUFSIZE=256 +CONFIG_UART8_TXBUFSIZE=256 + +CONFIG_USART1_RXBUFSIZE=512 +CONFIG_USART2_RXBUFSIZE=256 +CONFIG_USART3_RXBUFSIZE=256 +CONFIG_UART4_RXBUFSIZE=256 +CONFIG_USART6_RXBUFSIZE=256 +CONFIG_UART7_RXBUFSIZE=256 +CONFIG_UART8_RXBUFSIZE=256 + +CONFIG_USART1_BAUD=115200 +CONFIG_USART2_BAUD=115200 +CONFIG_USART3_BAUD=115200 +CONFIG_UART4_BAUD=115200 +CONFIG_USART6_BAUD=9600 +CONFIG_UART7_BAUD=115200 +CONFIG_UART8_BAUD=57600 + +CONFIG_USART1_BITS=8 +CONFIG_USART2_BITS=8 +CONFIG_USART3_BITS=8 +CONFIG_UART4_BITS=8 +CONFIG_USART6_BITS=8 +CONFIG_UART7_BITS=8 +CONFIG_UART8_BITS=8 + +CONFIG_USART1_PARITY=0 +CONFIG_USART2_PARITY=0 +CONFIG_USART3_PARITY=0 +CONFIG_UART4_PARITY=0 +CONFIG_USART6_PARITY=0 +CONFIG_UART7_PARITY=0 +CONFIG_UART8_PARITY=0 + +CONFIG_USART1_2STOP=0 +CONFIG_USART2_2STOP=0 +CONFIG_USART3_2STOP=0 +CONFIG_UART4_2STOP=0 +CONFIG_USART6_2STOP=0 +CONFIG_UART7_2STOP=0 +CONFIG_UART8_2STOP=0 + +CONFIG_USART1_RXDMA=y +CONFIG_USART2_RXDMA=y +CONFIG_USART3_RXDMA=n +CONFIG_UART4_RXDMA=n +CONFIG_USART6_RXDMA=y +CONFIG_UART7_RXDMA=n +CONFIG_UART8_RXDMA=n + +# +# PX4FMU specific driver settings +# +# CONFIG_HRT_TIMER +# Enables the high-resolution timer. The board definition must +# set HRT_TIMER and HRT_TIMER_CHANNEL to the timer and capture/ +# compare channels to be used. +# CONFIG_HRT_PPM +# Enables R/C PPM input using the HRT. The board definition must +# set HRT_PPM_CHANNEL to the timer capture/compare channel to be +# used, and define GPIO_PPM_IN to configure the appropriate timer +# GPIO. +# +CONFIG_HRT_TIMER=y +CONFIG_HRT_PPM=n + +# +# STM32F40xxx specific SPI device driver settings +# +CONFIG_SPI_EXCHANGE=y +# DMA needs more work, not implemented on STM32F4x yet +#CONFIG_STM32_SPI_DMA=y + +# +# STM32F40xxx specific CAN device driver settings +# +# CONFIG_CAN - Enables CAN support (one or both of CONFIG_STM32_CAN1 or +# CONFIG_STM32_CAN2 must also be defined) +# CONFIG_CAN_EXTID - Enables support for the 29-bit extended ID. Default +# Standard 11-bit IDs. +# CONFIG_CAN_FIFOSIZE - The size of the circular buffer of CAN messages. +# Default: 8 +# CONFIG_CAN_NPENDINGRTR - The size of the list of pending RTR requests. +# Default: 4 +# CONFIG_CAN_LOOPBACK - A CAN driver may or may not support a loopback +# mode for testing. The STM32 CAN driver does support loopback mode. +# CONFIG_CAN1_BAUD - CAN1 BAUD rate. Required if CONFIG_STM32_CAN1 is defined. +# CONFIG_CAN2_BAUD - CAN1 BAUD rate. Required if CONFIG_STM32_CAN2 is defined. +# CONFIG_CAN_TSEG1 - The number of CAN time quanta in segment 1. Default: 6 +# CONFIG_CAN_TSEG2 - the number of CAN time quanta in segment 2. Default: 7 +# +CONFIG_CAN=n +CONFIG_CAN_EXTID=n +#CONFIG_CAN_FIFOSIZE +#CONFIG_CAN_NPENDINGRTR +CONFIG_CAN_LOOPBACK=n +CONFIG_CAN1_BAUD=700000 +CONFIG_CAN2_BAUD=700000 + + +# XXX remove after integration testing +# Allow 180 us per byte, a wide margin for the 400 KHz clock we're using +# e.g. 9.6 ms for an EEPROM page write, 0.9 ms for a MAG update +CONFIG_STM32_I2CTIMEOUS_PER_BYTE=200 +# Constant overhead for generating I2C start / stop conditions +CONFIG_STM32_I2CTIMEOUS_START_STOP=700 +# XXX this is bad and we want it gone +CONFIG_I2C_WRITEREAD=y + +# +# I2C configuration +# +CONFIG_I2C=y +CONFIG_I2C_POLLED=n +CONFIG_I2C_TRANSFER=y +CONFIG_I2C_TRACE=n +CONFIG_I2C_RESET=y +# XXX fixed per-transaction timeout +CONFIG_STM32_I2CTIMEOMS=10 + + +# XXX re-enable after integration testing + +# +# I2C configuration +# +#CONFIG_I2C=y +#CONFIG_I2C_POLLED=y +#CONFIG_I2C_TRANSFER=y +#CONFIG_I2C_TRACE=n +#CONFIG_I2C_RESET=y + +# Dynamic timeout +#CONFIG_STM32_I2C_DYNTIMEO=y +#CONFIG_STM32_I2C_DYNTIMEO_STARTSTOP=500 +#CONFIG_STM32_I2C_DYNTIMEO_USECPERBYTE=200 + +# Fixed per-transaction timeout +#CONFIG_STM32_I2CTIMEOSEC=0 +#CONFIG_STM32_I2CTIMEOMS=10 + + + + + + +# +# General build options +# +# CONFIG_RRLOAD_BINARY - make the rrload binary format used with +# BSPs from www.ridgerun.com using the tools/mkimage.sh script +# CONFIG_INTELHEX_BINARY - make the Intel HEX binary format +# used with many different loaders using the GNU objcopy program +# Should not be selected if you are not using the GNU toolchain. +# CONFIG_MOTOROLA_SREC - make the Motorola S-Record binary format +# used with many different loaders using the GNU objcopy program +# Should not be selected if you are not using the GNU toolchain. +# CONFIG_RAW_BINARY - make a raw binary format file used with many +# different loaders using the GNU objcopy program. This option +# should not be selected if you are not using the GNU toolchain. +# CONFIG_HAVE_LIBM - toolchain supports libm.a +# +CONFIG_RRLOAD_BINARY=n +CONFIG_INTELHEX_BINARY=n +CONFIG_MOTOROLA_SREC=n +CONFIG_RAW_BINARY=y +CONFIG_HAVE_LIBM=y + +# +# General OS setup +# +# CONFIG_APPS_DIR - Identifies the relative path to the directory +# that builds the application to link with NuttX. Default: ../apps +# CONFIG_DEBUG - enables built-in debug options +# CONFIG_DEBUG_VERBOSE - enables verbose debug output +# CONFIG_DEBUG_SYMBOLS - build without optimization and with +# debug symbols (needed for use with a debugger). +# CONFIG_HAVE_CXX - Enable support for C++ +# CONFIG_HAVE_CXXINITIALIZE - The platform-specific logic includes support +# for initialization of static C++ instances for this architecture +# and for the selected toolchain (via up_cxxinitialize()). +# CONFIG_MM_REGIONS - If the architecture includes multiple +# regions of memory to allocate from, this specifies the +# number of memory regions that the memory manager must +# handle and enables the API mm_addregion(start, end); +# CONFIG_ARCH_LOWPUTC - architecture supports low-level, boot +# time console output +# CONFIG_MSEC_PER_TICK - The default system timer is 100Hz +# or MSEC_PER_TICK=10. This setting may be defined to +# inform NuttX that the processor hardware is providing +# system timer interrupts at some interrupt interval other +# than 10 msec. +# CONFIG_RR_INTERVAL - The round robin timeslice will be set +# this number of milliseconds; Round robin scheduling can +# be disabled by setting this value to zero. +# CONFIG_SCHED_INSTRUMENTATION - enables instrumentation in +# scheduler to monitor system performance +# CONFIG_TASK_NAME_SIZE - Spcifies that maximum size of a +# task name to save in the TCB. Useful if scheduler +# instrumentation is selected. Set to zero to disable. +# CONFIG_START_YEAR, CONFIG_START_MONTH, CONFIG_START_DAY - +# Used to initialize the internal time logic. +# CONFIG_GREGORIAN_TIME - Enables Gregorian time conversions. +# You would only need this if you are concerned about accurate +# time conversions in the past or in the distant future. +# CONFIG_JULIAN_TIME - Enables Julian time conversions. You +# would only need this if you are concerned about accurate +# time conversion in the distand past. You must also define +# CONFIG_GREGORIAN_TIME in order to use Julian time. +# CONFIG_DEV_CONSOLE - Set if architecture-specific logic +# provides /dev/console. Enables stdout, stderr, stdin. +# CONFIG_DEV_LOWCONSOLE - Use the simple, low-level serial console +# driver (minimul support) +# CONFIG_MUTEX_TYPES: Set to enable support for recursive and +# errorcheck mutexes. Enables pthread_mutexattr_settype(). +# CONFIG_PRIORITY_INHERITANCE : Set to enable support for priority +# inheritance on mutexes and semaphores. +# CONFIG_SEM_PREALLOCHOLDERS: This setting is only used if priority +# inheritance is enabled. It defines the maximum number of +# different threads (minus one) that can take counts on a +# semaphore with priority inheritance support. This may be +# set to zero if priority inheritance is disabled OR if you +# are only using semaphores as mutexes (only one holder) OR +# if no more than two threads participate using a counting +# semaphore. +# CONFIG_SEM_NNESTPRIO. If priority inheritance is enabled, +# then this setting is the maximum number of higher priority +# threads (minus 1) than can be waiting for another thread +# to release a count on a semaphore. This value may be set +# to zero if no more than one thread is expected to wait for +# a semaphore. +# CONFIG_FDCLONE_DISABLE. Disable cloning of all file descriptors +# by task_create() when a new task is started. If set, all +# files/drivers will appear to be closed in the new task. +# CONFIG_FDCLONE_STDIO. Disable cloning of all but the first +# three file descriptors (stdin, stdout, stderr) by task_create() +# when a new task is started. If set, all files/drivers will +# appear to be closed in the new task except for stdin, stdout, +# and stderr. +# CONFIG_SDCLONE_DISABLE. Disable cloning of all socket +# desciptors by task_create() when a new task is started. If +# set, all sockets will appear to be closed in the new task. +# CONFIG_SCHED_WORKQUEUE. Create a dedicated "worker" thread to +# handle delayed processing from interrupt handlers. This feature +# is required for some drivers but, if there are not complaints, +# can be safely disabled. The worker thread also performs +# garbage collection -- completing any delayed memory deallocations +# from interrupt handlers. If the worker thread is disabled, +# then that clean will be performed by the IDLE thread instead +# (which runs at the lowest of priority and may not be appropriate +# if memory reclamation is of high priority). If CONFIG_SCHED_WORKQUEUE +# is enabled, then the following options can also be used: +# CONFIG_SCHED_WORKPRIORITY - The execution priority of the worker +# thread. Default: 192 +# CONFIG_SCHED_WORKPERIOD - How often the worker thread checks for +# work in units of microseconds. Default: 50*1000 (50 MS). +# CONFIG_SCHED_WORKSTACKSIZE - The stack size allocated for the worker +# thread. Default: CONFIG_IDLETHREAD_STACKSIZE. +# CONFIG_SIG_SIGWORK - The signal number that will be used to wake-up +# the worker thread. Default: 4 +# +# CONFIG_SCHED_LPWORK. If CONFIG_SCHED_WORKQUEUE is defined, then a single +# work queue is created by default. If CONFIG_SCHED_LPWORK is also defined +# then an additional, lower-priority work queue will also be created. This +# lower priority work queue is better suited for more extended processing +# (such as file system clean-up operations) +# CONFIG_SCHED_LPWORKPRIORITY - The execution priority of the lower priority +# worker thread. Default: 50 +# CONFIG_SCHED_LPWORKPERIOD - How often the lower priority worker thread +# checks for work in units of microseconds. Default: 50*1000 (50 MS). +# CONFIG_SCHED_LPWORKSTACKSIZE - The stack size allocated for the lower +# priority worker thread. Default: CONFIG_IDLETHREAD_STACKSIZE. +# CONFIG_SCHED_WAITPID - Enable the waitpid() API +# CONFIG_SCHED_ATEXIT - Enabled the atexit() API +# +CONFIG_USER_ENTRYPOINT="nsh_main" +#CONFIG_APPS_DIR= +CONFIG_DEBUG=y +CONFIG_DEBUG_VERBOSE=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_FS=n +CONFIG_DEBUG_GRAPHICS=n +CONFIG_DEBUG_LCD=n +CONFIG_DEBUG_USB=n +CONFIG_DEBUG_NET=n +CONFIG_DEBUG_RTC=n +CONFIG_DEBUG_ANALOG=n +CONFIG_DEBUG_PWM=n +CONFIG_DEBUG_CAN=n +CONFIG_DEBUG_I2C=n +CONFIG_DEBUG_INPUT=n + +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_MM_REGIONS=2 +CONFIG_ARCH_LOWPUTC=y +CONFIG_MSEC_PER_TICK=1 +CONFIG_RR_INTERVAL=0 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_START_YEAR=1970 +CONFIG_START_MONTH=1 +CONFIG_START_DAY=1 +CONFIG_GREGORIAN_TIME=n +CONFIG_JULIAN_TIME=n +CONFIG_DEV_CONSOLE=y +CONFIG_DEV_LOWCONSOLE=n +CONFIG_MUTEX_TYPES=n +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_NNESTPRIO=8 +CONFIG_FDCLONE_DISABLE=n +CONFIG_FDCLONE_STDIO=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SCHED_WORKQUEUE=y +CONFIG_SCHED_WORKPRIORITY=192 +CONFIG_SCHED_WORKPERIOD=5000 +CONFIG_SCHED_WORKSTACKSIZE=2048 +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKPERIOD=50000 +CONFIG_SCHED_LPWORKSTACKSIZE=2048 +CONFIG_SIG_SIGWORK=4 +CONFIG_SCHED_WAITPID=y +CONFIG_SCHED_ATEXIT=n + +# +# System Logging +# +# CONFIG_SYSLOG - Enables the System Logging feature. +# CONFIG_RAMLOG - Enables the RAM logging feature +# CONFIG_RAMLOG_CONSOLE - Use the RAM logging device as a system console. +# If this feature is enabled (along with CONFIG_DEV_CONSOLE), then all +# console output will be re-directed to a circular buffer in RAM. This +# is useful, for example, if the only console is a Telnet console. Then +# in that case, console output from non-Telnet threads will go to the +# circular buffer and can be viewed using the NSH 'dmesg' command. +# CONFIG_RAMLOG_SYSLOG - Use the RAM logging device for the syslogging +# interface. If this feature is enabled (along with CONFIG_SYSLOG), +# then all debug output (only) will be re-directed to the circular +# buffer in RAM. This RAM log can be view from NSH using the 'dmesg' +# command. +# CONFIG_RAMLOG_NPOLLWAITERS - The number of threads than can be waiting +# for this driver on poll(). Default: 4 +# +# If CONFIG_RAMLOG_CONSOLE or CONFIG_RAMLOG_SYSLOG is selected, then the +# following may also be provided: +# +# CONFIG_RAMLOG_CONSOLE_BUFSIZE - Size of the console RAM log. Default: 1024 +# + +CONFIG_SYSLOG=n +CONFIG_RAMLOG=n +CONFIG_RAMLOG_CONSOLE=n +CONFIG_RAMLOG_SYSLOG=n +#CONFIG_RAMLOG_NPOLLWAITERS +#CONFIG_RAMLOG_CONSOLE_BUFSIZE + +# +# The following can be used to disable categories of +# APIs supported by the OS. If the compiler supports +# weak functions, then it should not be necessary to +# disable functions unless you want to restrict usage +# of those APIs. +# +# There are certain dependency relationships in these +# features. +# +# o mq_notify logic depends on signals to awaken tasks +# waiting for queues to become full or empty. +# o pthread_condtimedwait() depends on signals to wake +# up waiting tasks. +# +CONFIG_DISABLE_CLOCK=n +CONFIG_DISABLE_POSIX_TIMERS=n +CONFIG_DISABLE_PTHREAD=n +CONFIG_DISABLE_SIGNALS=n +CONFIG_DISABLE_MQUEUE=n +CONFIG_DISABLE_MOUNTPOINT=n +CONFIG_DISABLE_ENVIRON=n +CONFIG_DISABLE_POLL=n + +# +# Misc libc settings +# +# CONFIG_NOPRINTF_FIELDWIDTH - sprintf-related logic is a +# little smaller if we do not support fieldwidthes +# CONFIG_LIBC_FLOATINGPOINT - Enables printf("%f") +# CONFIG_LIBC_FIXEDPRECISION - Sets 7 digits after dot for printing: +# 5.1234567 +# CONFIG_HAVE_LONG_LONG - Enabled printf("%llu) +# +CONFIG_NOPRINTF_FIELDWIDTH=n +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_HAVE_LONG_LONG=y + +# +# Allow for architecture optimized implementations +# +# The architecture can provide optimized versions of the +# following to improve system performance +# +CONFIG_ARCH_MEMCPY=y +CONFIG_ARCH_MEMCMP=n +CONFIG_ARCH_MEMMOVE=n +CONFIG_ARCH_MEMSET=n +CONFIG_ARCH_STRCMP=n +CONFIG_ARCH_STRCPY=n +CONFIG_ARCH_STRNCPY=n +CONFIG_ARCH_STRLEN=n +CONFIG_ARCH_STRNLEN=n +CONFIG_ARCH_BZERO=n + +# +# Sizes of configurable things (0 disables) +# +# CONFIG_MAX_TASKS - The maximum number of simultaneously +# active tasks. This value must be a power of two. +# CONFIG_MAX_TASK_ARGS - This controls the maximum number of +# of parameters that a task may receive (i.e., maxmum value +# of 'argc') +# CONFIG_NPTHREAD_KEYS - The number of items of thread- +# specific data that can be retained +# CONFIG_NFILE_DESCRIPTORS - The maximum number of file +# descriptors (one for each open) +# CONFIG_NFILE_STREAMS - The maximum number of streams that +# can be fopen'ed +# CONFIG_NAME_MAX - The maximum size of a file name. +# CONFIG_STDIO_BUFFER_SIZE - Size of the buffer to allocate +# on fopen. (Only if CONFIG_NFILE_STREAMS > 0) +# CONFIG_STDIO_LINEBUFFER - If standard C buffered I/O is enabled +# (CONFIG_STDIO_BUFFER_SIZE > 0), then this option may be added +# to force automatic, line-oriented flushing the output buffer +# for putc(), fputc(), putchar(), puts(), fputs(), printf(), +# fprintf(), and vfprintf(). When a newline is encountered in +# the output string, the output buffer will be flushed. This +# (slightly) increases the NuttX footprint but supports the kind +# of behavior that people expect for printf(). +# CONFIG_NUNGET_CHARS - Number of characters that can be +# buffered by ungetc() (Only if CONFIG_NFILE_STREAMS > 0) +# CONFIG_PREALLOC_MQ_MSGS - The number of pre-allocated message +# structures. The system manages a pool of preallocated +# message structures to minimize dynamic allocations +# CONFIG_MQ_MAXMSGSIZE - Message structures are allocated with +# a fixed payload size given by this settin (does not include +# other message structure overhead. +# CONFIG_MAX_WDOGPARMS - Maximum number of parameters that +# can be passed to a watchdog handler +# CONFIG_PREALLOC_WDOGS - The number of pre-allocated watchdog +# structures. The system manages a pool of preallocated +# watchdog structures to minimize dynamic allocations +# CONFIG_PREALLOC_TIMERS - The number of pre-allocated POSIX +# timer structures. The system manages a pool of preallocated +# timer structures to minimize dynamic allocations. Set to +# zero for all dynamic allocations. +# +CONFIG_MAX_TASKS=32 +CONFIG_MAX_TASK_ARGS=8 +CONFIG_NPTHREAD_KEYS=4 +CONFIG_NFILE_DESCRIPTORS=32 +CONFIG_NFILE_STREAMS=25 +CONFIG_NAME_MAX=32 +CONFIG_STDIO_BUFFER_SIZE=256 +CONFIG_STDIO_LINEBUFFER=y +CONFIG_NUNGET_CHARS=2 +CONFIG_PREALLOC_MQ_MSGS=4 +CONFIG_MQ_MAXMSGSIZE=32 +CONFIG_MAX_WDOGPARMS=2 +CONFIG_PREALLOC_WDOGS=50 +CONFIG_PREALLOC_TIMERS=50 + +# +# Filesystem configuration +# +# CONFIG_FS_FAT - Enable FAT filesystem support +# CONFIG_FAT_SECTORSIZE - Max supported sector size +# CONFIG_FAT_LCNAMES - Enable use of the NT-style upper/lower case 8.3 +# file name support. +# CONFIG_FAT_LFN - Enable FAT long file names. NOTE: Microsoft claims +# patents on FAT long file name technology. Please read the +# disclaimer in the top-level COPYING file and only enable this +# feature if you understand these issues. +# CONFIG_FAT_MAXFNAME - If CONFIG_FAT_LFN is defined, then the +# default, maximum long file name is 255 bytes. This can eat up +# a lot of memory (especially stack space). If you are willing +# to live with some non-standard, short long file names, then +# define this value. A good choice would be the same value as +# selected for CONFIG_NAME_MAX which will limit the visibility +# of longer file names anyway. +# CONFIG_FS_NXFFS: Enable NuttX FLASH file system (NXFF) support. +# CONFIG_NXFFS_ERASEDSTATE: The erased state of FLASH. +# This must have one of the values of 0xff or 0x00. +# Default: 0xff. +# CONFIG_NXFFS_PACKTHRESHOLD: When packing flash file data, +# don't both with file chunks smaller than this number of data bytes. +# CONFIG_NXFFS_MAXNAMLEN: The maximum size of an NXFFS file name. +# Default: 255. +# CONFIG_NXFFS_PACKTHRESHOLD: When packing flash file data, +# don't both with file chunks smaller than this number of data bytes. +# Default: 32. +# CONFIG_NXFFS_TAILTHRESHOLD: clean-up can either mean +# packing files together toward the end of the file or, if file are +# deleted at the end of the file, clean up can simply mean erasing +# the end of FLASH memory so that it can be re-used again. However, +# doing this can also harm the life of the FLASH part because it can +# mean that the tail end of the FLASH is re-used too often. This +# threshold determines if/when it is worth erased the tail end of FLASH +# and making it available for re-use (and possible over-wear). +# Default: 8192. +# CONFIG_FS_ROMFS - Enable ROMFS filesystem support +# CONFIG_FS_RAMMAP - For file systems that do not support XIP, this +# option will enable a limited form of memory mapping that is +# implemented by copying whole files into memory. +# +CONFIG_FS_FAT=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_MAXFNAME=32 +CONFIG_FS_NXFFS=y +CONFIG_NXFFS_MAXNAMLEN=32 +CONFIG_NXFFS_TAILTHRESHOLD=2048 +CONFIG_NXFFS_PREALLOCATED=y +CONFIG_FS_ROMFS=y +CONFIG_FS_BINFS=y + +# +# SPI-based MMC/SD driver +# +# CONFIG_MMCSD_NSLOTS +# Number of MMC/SD slots supported by the driver +# CONFIG_MMCSD_READONLY +# Provide read-only access (default is read/write) +# CONFIG_MMCSD_SPICLOCK - Maximum SPI clock to drive MMC/SD card. +# Default is 20MHz, current setting 24 MHz +# +CONFIG_MMCSD=n +# XXX need to rejig this for SDIO +#CONFIG_MMCSD_SPI=y +#CONFIG_MMCSD_NSLOTS=1 +#CONFIG_MMCSD_READONLY=n +#CONFIG_MMCSD_SPICLOCK=24000000 + +# +# Block driver buffering +# +# CONFIG_FS_READAHEAD +# Enable read-ahead buffering +# CONFIG_FS_WRITEBUFFER +# Enable write buffering +# +CONFIG_FS_READAHEAD=n +CONFIG_FS_WRITEBUFFER=n + +# +# RTC Configuration +# +# CONFIG_RTC - Enables general support for a hardware RTC. Specific +# architectures may require other specific settings. +# CONFIG_RTC_DATETIME - There are two general types of RTC: (1) A simple +# battery backed counter that keeps the time when power is down, and (2) +# A full date / time RTC the provides the date and time information, often +# in BCD format. If CONFIG_RTC_DATETIME is selected, it specifies this +# second kind of RTC. In this case, the RTC is used to "seed" the normal +# NuttX timer and the NuttX system timer provides for higher resoution +# time. +# CONFIG_RTC_HIRES - If CONFIG_RTC_DATETIME not selected, then the simple, +# battery backed counter is used. There are two different implementations +# of such simple counters based on the time resolution of the counter: +# The typical RTC keeps time to resolution of 1 second, usually +# supporting a 32-bit time_t value. In this case, the RTC is used to +# "seed" the normal NuttX timer and the NuttX timer provides for higher +# resoution time. If CONFIG_RTC_HIRES is enabled in the NuttX configuration, +# then the RTC provides higher resolution time and completely replaces the +# system timer for purpose of date and time. +# CONFIG_RTC_FREQUENCY - If CONFIG_RTC_HIRES is defined, then the frequency +# of the high resolution RTC must be provided. If CONFIG_RTC_HIRES is +# not defined, CONFIG_RTC_FREQUENCY is assumed to be one. +# CONFIG_RTC_ALARM - Enable if the RTC hardware supports setting of an +# alarm. A callback function will be executed when the alarm goes off +# +CONFIG_RTC=n +CONFIG_RTC_DATETIME=y +CONFIG_RTC_HIRES=n +CONFIG_RTC_FREQUENCY=n +CONFIG_RTC_ALARM=n + +# +# USB Device Configuration +# +# CONFIG_USBDEV +# Enables USB device support +# CONFIG_USBDEV_ISOCHRONOUS +# Build in extra support for isochronous endpoints +# CONFIG_USBDEV_DUALSPEED +# Hardware handles high and full speed operation (USB 2.0) +# CONFIG_USBDEV_SELFPOWERED +# Will cause USB features to indicate that the device is +# self-powered +# CONFIG_USBDEV_MAXPOWER +# Maximum power consumption in mA +# CONFIG_USBDEV_TRACE +# Enables USB tracing for debug +# CONFIG_USBDEV_TRACE_NRECORDS +# Number of trace entries to remember +# +CONFIG_USBDEV=y +CONFIG_USBDEV_ISOCHRONOUS=n +CONFIG_USBDEV_DUALSPEED=n +CONFIG_USBDEV_SELFPOWERED=y +CONFIG_USBDEV_REMOTEWAKEUP=n +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USBDEV_TRACE=n +CONFIG_USBDEV_TRACE_NRECORDS=512 + +# +# USB serial device class driver (Standard CDC ACM class) +# +# CONFIG_CDCACM +# Enable compilation of the USB serial driver +# CONFIG_CDCACM_CONSOLE +# Configures the CDC/ACM serial port as the console device. +# CONFIG_CDCACM_EP0MAXPACKET +# Endpoint 0 max packet size. Default 64 +# CONFIG_CDCACM_EPINTIN +# The logical 7-bit address of a hardware endpoint that supports +# interrupt IN operation. Default 2. +# CONFIG_CDCACM_EPINTIN_FSSIZE +# Max package size for the interrupt IN endpoint if full speed mode. +# Default 64. +# CONFIG_CDCACM_EPINTIN_HSSIZE +# Max package size for the interrupt IN endpoint if high speed mode. +# Default 64 +# CONFIG_CDCACM_EPBULKOUT +# The logical 7-bit address of a hardware endpoint that supports +# bulk OUT operation. Default 4. +# CONFIG_CDCACM_EPBULKOUT_FSSIZE +# Max package size for the bulk OUT endpoint if full speed mode. +# Default 64. +# CONFIG_CDCACM_EPBULKOUT_HSSIZE +# Max package size for the bulk OUT endpoint if high speed mode. +# Default 512. +# CONFIG_CDCACM_EPBULKIN +# The logical 7-bit address of a hardware endpoint that supports +# bulk IN operation. Default 3. +# CONFIG_CDCACM_EPBULKIN_FSSIZE +# Max package size for the bulk IN endpoint if full speed mode. +# Default 64. +# CONFIG_CDCACM_EPBULKIN_HSSIZE +# Max package size for the bulk IN endpoint if high speed mode. +# Default 512. +# CONFIG_CDCACM_NWRREQS and CONFIG_CDCACM_NRDREQS +# The number of write/read requests that can be in flight. +# Default 256. +# CONFIG_CDCACM_VENDORID and CONFIG_CDCACM_VENDORSTR +# The vendor ID code/string. Default 0x0525 and "NuttX" +# 0x0525 is the Netchip vendor and should not be used in any +# products. This default VID was selected for compatibility with +# the Linux CDC ACM default VID. +# CONFIG_CDCACM_PRODUCTID and CONFIG_CDCACM_PRODUCTSTR +# The product ID code/string. Default 0xa4a7 and "CDC/ACM Serial" +# 0xa4a7 was selected for compatibility with the Linux CDC ACM +# default PID. +# CONFIG_CDCACM_RXBUFSIZE and CONFIG_CDCACM_TXBUFSIZE +# Size of the serial receive/transmit buffers. Default 256. +# +CONFIG_CDCACM=y +CONFIG_CDCACM_CONSOLE=n +#CONFIG_CDCACM_EP0MAXPACKET +CONFIG_CDCACM_EPINTIN=1 +#CONFIG_CDCACM_EPINTIN_FSSIZE +#CONFIG_CDCACM_EPINTIN_HSSIZE +CONFIG_CDCACM_EPBULKOUT=3 +#CONFIG_CDCACM_EPBULKOUT_FSSIZE +#CONFIG_CDCACM_EPBULKOUT_HSSIZE +CONFIG_CDCACM_EPBULKIN=2 +#CONFIG_CDCACM_EPBULKIN_FSSIZE +#CONFIG_CDCACM_EPBULKIN_HSSIZE +#CONFIG_CDCACM_NWRREQS +#CONFIG_CDCACM_NRDREQS +CONFIG_CDCACM_VENDORID=0x26AC +CONFIG_CDCACM_VENDORSTR="3D Robotics" +CONFIG_CDCACM_PRODUCTID=0x0010 +CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v1.6" +#CONFIG_CDCACM_RXBUFSIZE +#CONFIG_CDCACM_TXBUFSIZE + + +# +# Settings for apps/nshlib +# +# CONFIG_NSH_BUILTIN_APPS - Support external registered, +# "named" applications that can be executed from the NSH +# command line (see apps/README.txt for more information). +# CONFIG_NSH_FILEIOSIZE - Size of a static I/O buffer +# CONFIG_NSH_STRERROR - Use strerror(errno) +# CONFIG_NSH_LINELEN - Maximum length of one command line +# CONFIG_NSH_MAX_ARGUMENTS - Maximum number of arguments for command line +# CONFIG_NSH_NESTDEPTH - Max number of nested if-then[-else]-fi +# CONFIG_NSH_DISABLESCRIPT - Disable scripting support +# CONFIG_NSH_DISABLEBG - Disable background commands +# CONFIG_NSH_ROMFSETC - Use startup script in /etc +# CONFIG_NSH_CONSOLE - Use serial console front end +# CONFIG_NSH_TELNET - Use telnetd console front end +# CONFIG_NSH_ARCHINIT - Platform provides architecture +# specific initialization (nsh_archinitialize()). +# +# If CONFIG_NSH_TELNET is selected: +# CONFIG_NSH_IOBUFFER_SIZE -- Telnetd I/O buffer size +# CONFIG_NSH_DHCPC - Obtain address using DHCP +# CONFIG_NSH_IPADDR - Provides static IP address +# CONFIG_NSH_DRIPADDR - Provides static router IP address +# CONFIG_NSH_NETMASK - Provides static network mask +# CONFIG_NSH_NOMAC - Use a bogus MAC address +# +# If CONFIG_NSH_ROMFSETC is selected: +# CONFIG_NSH_ROMFSMOUNTPT - ROMFS mountpoint +# CONFIG_NSH_INITSCRIPT - Relative path to init script +# CONFIG_NSH_ROMFSDEVNO - ROMFS RAM device minor +# CONFIG_NSH_ROMFSSECTSIZE - ROMF sector size +# CONFIG_NSH_FATDEVNO - FAT FS RAM device minor +# CONFIG_NSH_FATSECTSIZE - FAT FS sector size +# CONFIG_NSH_FATNSECTORS - FAT FS number of sectors +# CONFIG_NSH_FATMOUNTPT - FAT FS mountpoint +# +CONFIG_BUILTIN=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_FILEIOSIZE=512 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAX_ARGUMENTS=12 +CONFIG_NSH_NESTDEPTH=8 +CONFIG_NSH_DISABLESCRIPT=n +CONFIG_NSH_DISABLEBG=n +CONFIG_NSH_ROMFSETC=y +CONFIG_NSH_ARCHROMFS=y +CONFIG_NSH_CONSOLE=y +CONFIG_NSH_USBCONSOLE=n +CONFIG_NSH_USBCONDEV="/dev/ttyACM0" +CONFIG_NSH_TELNET=n +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_IOBUFFER_SIZE=512 +CONFIG_NSH_DHCPC=n +CONFIG_NSH_NOMAC=y +CONFIG_NSH_IPADDR=0x0a000002 +CONFIG_NSH_DRIPADDR=0x0a000001 +CONFIG_NSH_NETMASK=0xffffff00 +CONFIG_NSH_ROMFSMOUNTPT="/etc" +CONFIG_NSH_INITSCRIPT="init.d/rcS" +CONFIG_NSH_ROMFSDEVNO=0 +CONFIG_NSH_ROMFSSECTSIZE=128 # Default 64, increased to allow for more than 64 folders on the sdcard +CONFIG_NSH_FATDEVNO=1 +CONFIG_NSH_FATSECTSIZE=512 +CONFIG_NSH_FATNSECTORS=1024 +CONFIG_NSH_FATMOUNTPT=/tmp + +# +# Architecture-specific NSH options +# +#CONFIG_NSH_MMCSDSPIPORTNO=3 +#CONFIG_NSH_MMCSDSLOTNO=0 +#CONFIG_NSH_MMCSDMINOR=0 + + +# +# Stack and heap information +# +# CONFIG_BOOT_RUNFROMFLASH - Some configurations support XIP +# operation from FLASH but must copy initialized .data sections to RAM. +# (should also be =n for the STM3240G-EVAL which always runs from flash) +# CONFIG_BOOT_COPYTORAM - Some configurations boot in FLASH +# but copy themselves entirely into RAM for better performance. +# CONFIG_CUSTOM_STACK - The up_ implementation will handle +# all stack operations outside of the nuttx model. +# CONFIG_STACK_POINTER - The initial stack pointer (arm7tdmi only) +# CONFIG_IDLETHREAD_STACKSIZE - The size of the initial stack. +# This is the thread that (1) performs the inital boot of the system up +# to the point where user_start() is spawned, and (2) there after is the +# IDLE thread that executes only when there is no other thread ready to +# run. +# CONFIG_USERMAIN_STACKSIZE - The size of the stack to allocate +# for the main user thread that begins at the user_start() entry point. +# CONFIG_PTHREAD_STACK_MIN - Minimum pthread stack size +# CONFIG_PTHREAD_STACK_DEFAULT - Default pthread stack size +# CONFIG_HEAP_BASE - The beginning of the heap +# CONFIG_HEAP_SIZE - The size of the heap +# +CONFIG_BOOT_RUNFROMFLASH=n +CONFIG_BOOT_COPYTORAM=n +CONFIG_CUSTOM_STACK=n +CONFIG_STACK_POINTER= +# Idle thread needs 4096 bytes +# default 1 KB is not enough +# 4096 bytes +CONFIG_IDLETHREAD_STACKSIZE=6000 +# USERMAIN stack size probably needs to be around 4096 bytes +CONFIG_USERMAIN_STACKSIZE=4096 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_PTHREAD_STACK_DEFAULT=2048 +CONFIG_HEAP_BASE= +CONFIG_HEAP_SIZE= + +# enable bindir +CONFIG_APPS_BINDIR=y diff --git a/nuttx/configs/px4fmuv2/nsh/setenv.sh b/nuttx/configs/px4fmuv2/nsh/setenv.sh new file mode 100755 index 0000000000..265520997d --- /dev/null +++ b/nuttx/configs/px4fmuv2/nsh/setenv.sh @@ -0,0 +1,67 @@ +#!/bin/bash +# configs/stm3240g-eval/nsh/setenv.sh +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# + +if [ "$_" = "$0" ] ; then + echo "You must source this script, not run it!" 1>&2 + exit 1 +fi + +WD=`pwd` +if [ ! -x "setenv.sh" ]; then + echo "This script must be executed from the top-level NuttX build directory" + exit 1 +fi + +if [ -z "${PATH_ORIG}" ]; then + export PATH_ORIG="${PATH}" +fi + +# This the Cygwin path to the location where I installed the RIDE +# toolchain under windows. You will also have to edit this if you install +# the RIDE toolchain in any other location +#export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/Raisonance/Ride/arm-gcc/bin" + +# This the Cygwin path to the location where I installed the CodeSourcery +# toolchain under windows. You will also have to edit this if you install +# the CodeSourcery toolchain in any other location +export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/CodeSourcery/Sourcery G++ Lite/bin" + +# This the Cygwin path to the location where I build the buildroot +# toolchain. +#export TOOLCHAIN_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin" + +# Add the path to the toolchain to the PATH varialble +export PATH="${TOOLCHAIN_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" + +echo "PATH : ${PATH}" diff --git a/nuttx/configs/px4fmuv2/src/Makefile b/nuttx/configs/px4fmuv2/src/Makefile new file mode 100644 index 0000000000..d4276f7fc5 --- /dev/null +++ b/nuttx/configs/px4fmuv2/src/Makefile @@ -0,0 +1,84 @@ +############################################################################ +# configs/px4fmu/src/Makefile +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# +############################################################################ + +-include $(TOPDIR)/Make.defs + +CFLAGS += -I$(TOPDIR)/sched + +ASRCS = +AOBJS = $(ASRCS:.S=$(OBJEXT)) + +CSRCS = empty.c +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ARCH_SRCDIR = $(TOPDIR)/arch/$(CONFIG_ARCH)/src +ifeq ($(WINTOOL),y) + CFLAGS += -I "${shell cygpath -w $(ARCH_SRCDIR)/chip}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/common}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/armv7-m}" +else + CFLAGS += -I$(ARCH_SRCDIR)/chip -I$(ARCH_SRCDIR)/common -I$(ARCH_SRCDIR)/armv7-m +endif + +all: libboard$(LIBEXT) + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS) $(LINKOBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +libboard$(LIBEXT): $(OBJS) + $(call ARCHIVE, $@, $(OBJS)) + +.depend: Makefile $(SRCS) + $(Q) $(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + $(Q) touch $@ + +depend: .depend + +clean: + $(call DELFILE, libboard$(LIBEXT)) + $(call CLEAN) + +distclean: clean + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) + +-include Make.dep + diff --git a/nuttx/configs/px4fmuv2/src/empty.c b/nuttx/configs/px4fmuv2/src/empty.c new file mode 100644 index 0000000000..ace900866c --- /dev/null +++ b/nuttx/configs/px4fmuv2/src/empty.c @@ -0,0 +1,4 @@ +/* + * There are no source files here, but libboard.a can't be empty, so + * we have this empty source file to keep it company. + */ From 976f1334efbb1218a0cd5af6e9f1d344b067eb13 Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 4 Apr 2013 23:17:21 -0700 Subject: [PATCH 033/486] More config for fmuv2 --- Makefile | 7 +++---- makefiles/board_px4fmu.mk | 2 +- makefiles/board_px4fmuv2.mk | 10 ++++++++++ makefiles/board_px4io.mk | 2 +- 4 files changed, 15 insertions(+), 6 deletions(-) create mode 100644 makefiles/board_px4fmuv2.mk diff --git a/Makefile b/Makefile index d0ffeb7402..e87513e785 100644 --- a/Makefile +++ b/Makefile @@ -42,7 +42,8 @@ include $(PX4_BASE)makefiles/setup.mk # # Canned firmware configurations that we build. # -CONFIGS ?= px4fmu_default px4io_default +CONFIGS ?= $(subst config_,,$(basename $(notdir $(wildcard $(PX4_MK_DIR)config_*.mk)))) +#CONFIGS ?= px4fmu_default px4io_default # # Boards that we build NuttX export kits for. @@ -162,9 +163,7 @@ help: @echo "" @echo " all" @echo " Build all firmware configs: $(CONFIGS)" - @echo " A limited set of configs can be built with:" - @echo "" - @echo " CONFIGS=" + @echo " A limited set of configs can be built with CONFIGS=" @echo "" @for config in $(CONFIGS); do \ echo " $$config"; \ diff --git a/makefiles/board_px4fmu.mk b/makefiles/board_px4fmu.mk index 959e4ed263..8370696446 100644 --- a/makefiles/board_px4fmu.mk +++ b/makefiles/board_px4fmu.mk @@ -1,5 +1,5 @@ # -# Platform-specific definitions for the PX4FMU +# Board-specific definitions for the PX4FMU # # diff --git a/makefiles/board_px4fmuv2.mk b/makefiles/board_px4fmuv2.mk new file mode 100644 index 0000000000..4b3b7e5854 --- /dev/null +++ b/makefiles/board_px4fmuv2.mk @@ -0,0 +1,10 @@ +# +# Board-specific definitions for the PX4FMUv2 +# + +# +# Configure the toolchain +# +CONFIG_ARCH = CORTEXM4F + +include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk diff --git a/makefiles/board_px4io.mk b/makefiles/board_px4io.mk index 275014aba5..b0eb2dae79 100644 --- a/makefiles/board_px4io.mk +++ b/makefiles/board_px4io.mk @@ -1,5 +1,5 @@ # -# Platform-specific definitions for the PX4IO +# Board-specific definitions for the PX4IO # # From 2e5809d051121cbb73d92bf98be0a2952f1dbd2e Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Apr 2013 00:04:59 -0700 Subject: [PATCH 034/486] Fix the remaining pieces so that we can build a firmware image for FMUv2 --- Images/px4fmuv2.prototype | 12 ++++++++++++ Makefile | 2 +- makefiles/config_px4fmuv2_default.mk | 6 ------ 3 files changed, 13 insertions(+), 7 deletions(-) create mode 100644 Images/px4fmuv2.prototype diff --git a/Images/px4fmuv2.prototype b/Images/px4fmuv2.prototype new file mode 100644 index 0000000000..5109b77d1e --- /dev/null +++ b/Images/px4fmuv2.prototype @@ -0,0 +1,12 @@ +{ + "board_id": 9, + "magic": "PX4FWv1", + "description": "Firmware for the PX4FMUv2 board", + "image": "", + "build_time": 0, + "summary": "PX4FMUv2", + "version": "0.1", + "image_size": 0, + "git_identity": "", + "board_revision": 0 +} diff --git a/Makefile b/Makefile index e87513e785..0b8a65d94a 100644 --- a/Makefile +++ b/Makefile @@ -48,7 +48,7 @@ CONFIGS ?= $(subst config_,,$(basename $(notdir $(wildcard $(PX4_MK_DIR)config # # Boards that we build NuttX export kits for. # -BOARDS = px4fmu px4io +BOARDS := $(subst board_,,$(basename $(notdir $(wildcard $(PX4_MK_DIR)board_*.mk)))) # # Debugging diff --git a/makefiles/config_px4fmuv2_default.mk b/makefiles/config_px4fmuv2_default.mk index d5bc758b85..2f104a5e4a 100644 --- a/makefiles/config_px4fmuv2_default.mk +++ b/makefiles/config_px4fmuv2_default.mk @@ -22,7 +22,6 @@ endef # command priority stack entrypoint BUILTIN_COMMANDS := \ $(call _B, adc, , 2048, adc_main ) \ - $(call _B, ardrone_interface, SCHED_PRIORITY_MAX-15, 2048, ardrone_interface_main ) \ $(call _B, attitude_estimator_ekf, , 2048, attitude_estimator_ekf_main) \ $(call _B, bl_update, , 4096, bl_update_main ) \ $(call _B, blinkm, , 2048, blinkm_main ) \ @@ -30,10 +29,8 @@ BUILTIN_COMMANDS := \ $(call _B, commander, SCHED_PRIORITY_MAX-30, 2048, commander_main ) \ $(call _B, control_demo, , 2048, control_demo_main ) \ $(call _B, delay_test, , 2048, delay_test_main ) \ - $(call _B, eeprom, , 4096, eeprom_main ) \ $(call _B, fixedwing_att_control, SCHED_PRIORITY_MAX-30, 2048, fixedwing_att_control_main ) \ $(call _B, fixedwing_pos_control, SCHED_PRIORITY_MAX-30, 2048, fixedwing_pos_control_main ) \ - $(call _B, fmu, , 2048, fmu_main ) \ $(call _B, gps, , 2048, gps_main ) \ $(call _B, hil, , 2048, hil_main ) \ $(call _B, hott_telemetry, , 2048, hott_telemetry_main ) \ @@ -43,14 +40,11 @@ BUILTIN_COMMANDS := \ $(call _B, mavlink, , 2048, mavlink_main ) \ $(call _B, mavlink_onboard, , 2048, mavlink_onboard_main ) \ $(call _B, mixer, , 4096, mixer_main ) \ - $(call _B, ms5611, , 2048, ms5611_main ) \ $(call _B, multirotor_att_control, SCHED_PRIORITY_MAX-15, 2048, multirotor_att_control_main) \ $(call _B, multirotor_pos_control, SCHED_PRIORITY_MAX-25, 2048, multirotor_pos_control_main) \ - $(call _B, param, , 4096, param_main ) \ $(call _B, perf, , 2048, perf_main ) \ $(call _B, position_estimator, , 4096, position_estimator_main ) \ $(call _B, preflight_check, , 2048, preflight_check_main ) \ - $(call _B, px4io, , 2048, px4io_main ) \ $(call _B, reboot, , 2048, reboot_main ) \ $(call _B, sdlog, SCHED_PRIORITY_MAX-30, 2048, sdlog_main ) \ $(call _B, sensors, SCHED_PRIORITY_MAX-5, 4096, sensors_main ) \ From 8b9b41fd507b9d2a62e2b0e3c5df398a489f7606 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Apr 2013 00:51:59 -0700 Subject: [PATCH 035/486] Populate INCLUDE_DIRS with some likely candidates. Implement __EXPORT and such for modules, as well as symbol visibility. Don't use UNZIP to point to unzip, as it looks there for arguments. --- makefiles/module.mk | 24 ++++++++++++---- makefiles/nuttx.mk | 2 +- makefiles/setup.mk | 32 ++++++++++++++------- src/include/visibility.h | 62 ++++++++++++++++++++++++++++++++++++++++ 4 files changed, 103 insertions(+), 17 deletions(-) create mode 100644 src/include/visibility.h diff --git a/makefiles/module.mk b/makefiles/module.mk index 8b01d0a126..154d37cc2b 100644 --- a/makefiles/module.mk +++ b/makefiles/module.mk @@ -75,6 +75,12 @@ # the list should be formatted as: # ... # +# DEFAULT_VISIBILITY (optional) +# +# If not set, global symbols defined in a module will not be visible +# outside the module. Symbols that should be globally visible must be +# marked __EXPORT. +# If set, global symbols defined in a module will be globally visible. # # @@ -98,11 +104,6 @@ $(error No module makefile specified) endif $(info % MODULE_MK = $(MODULE_MK)) -# -# Get path and tool config -# -include $(PX4_BASE)/makefiles/setup.mk - # # Get the board/toolchain config # @@ -150,6 +151,19 @@ $(MODULE_COMMAND_FILES): $(GLOBAL_DEPS) $(Q) $(TOUCH) $@ endif +################################################################################ +# Adjust compilation flags to implement EXPORT +################################################################################ + +ifeq ($(DEFAULT_VISIBILITY),) +DEFAULT_VISIBILITY = hidden +else +DEFAULT_VISIBILITY = default +endif + +CFLAGS += -fvisibility=$(DEFAULT_VISIBILITY) -include $(PX4_INCLUDE_DIR)visibility.h +CXXFLAGS += -fvisibility=$(DEFAULT_VISIBILITY) -include $(PX4_INCLUDE_DIR)visibility.h + ################################################################################ # Build rules ################################################################################ diff --git a/makefiles/nuttx.mk b/makefiles/nuttx.mk index c6e2c86d1b..e86f1370b9 100644 --- a/makefiles/nuttx.mk +++ b/makefiles/nuttx.mk @@ -71,5 +71,5 @@ LINK_DEPS += $(NUTTX_EXPORT_DIR)libs/libapps.a \ $(NUTTX_CONFIG_HEADER): $(NUTTX_ARCHIVE) @$(ECHO) %% Unpacking $(NUTTX_ARCHIVE) - $(Q) $(UNZIP) -q -o -d $(WORK_DIR) $(NUTTX_ARCHIVE) + $(Q) $(UNZIP_CMD) -q -o -d $(WORK_DIR) $(NUTTX_ARCHIVE) $(Q) $(TOUCH) $@ diff --git a/makefiles/setup.mk b/makefiles/setup.mk index 8e7a00ef4c..7655872e5f 100644 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -41,6 +41,7 @@ # the number of duplicate slashes we have lying around in paths, # and is consistent with joining the results of $(dir) and $(notdir). # +export PX4_INCLUDE_DIR = $(abspath $(PX4_BASE)/src/include)/ export PX4_MODULE_SRC = $(abspath $(PX4_BASE)/src/modules)/ export PX4_MK_DIR = $(abspath $(PX4_BASE)/makefiles)/ export NUTTX_SRC = $(abspath $(PX4_BASE)/nuttx)/ @@ -51,24 +52,33 @@ export IMAGE_DIR = $(abspath $(PX4_BASE)/Images)/ export BUILD_DIR = $(abspath $(PX4_BASE)/Build)/ export ARCHIVE_DIR = $(abspath $(PX4_BASE)/Archives)/ +# +# Default include paths +# +export INCLUDE_DIRS := $(PX4_MODULE_SRC) \ + $(PX4_INCLUDE_DIR) + +# Include from legacy app/library path +export INCLUDE_DIRS += $(NUTTX_APP_SRC) + # # Tools # -MKFW = $(PX4_BASE)/Tools/px_mkfw.py -UPLOADER = $(PX4_BASE)/Tools/px_uploader.py -COPY = cp -REMOVE = rm -f -RMDIR = rm -rf -GENROMFS = genromfs -TOUCH = touch -MKDIR = mkdir -ECHO = echo -UNZIP = unzip +export MKFW = $(PX4_BASE)/Tools/px_mkfw.py +export UPLOADER = $(PX4_BASE)/Tools/px_uploader.py +export COPY = cp +export REMOVE = rm -f +export RMDIR = rm -rf +export GENROMFS = genromfs +export TOUCH = touch +export MKDIR = mkdir +export ECHO = echo +export UNZIP_CMD = unzip # # Host-specific paths, hacks and fixups # -SYSTYPE := $(shell uname -s) +export SYSTYPE := $(shell uname -s) ifeq ($(SYSTYPE),Darwin) # Eclipse may not have the toolchain on its path. diff --git a/src/include/visibility.h b/src/include/visibility.h new file mode 100644 index 0000000000..2c6adc062e --- /dev/null +++ b/src/include/visibility.h @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 visibility.h + * + * Definitions controlling symbol naming and visibility. + * + * This file is normally included automatically by the build system. + */ + +#ifndef __SYSTEMLIB_VISIBILITY_H +#define __SYSTEMLIB_VISIBILITY_H + +#ifdef __EXPORT +# undef __EXPORT +#endif +#define __EXPORT __attribute__ ((visibility ("default"))) + +#ifdef __PRIVATE +# undef __PRIVATE +#endif +#define __PRIVATE __attribute__ ((visibility ("hidden"))) + +#ifdef __cplusplus +# define __BEGIN_DECLS extern "C" { +# define __END_DECLS } +#else +# define __BEGIN_DECLS +# define __END_DECLS +#endif +#endif \ No newline at end of file From c1f6f81e9d964f464cc474b36ce0b0b2935f1ab8 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Apr 2013 00:56:26 -0700 Subject: [PATCH 036/486] wchar_t is indeed a builtin type that should not be overridden. --- makefiles/toolchain_gnu-arm-eabi.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index d214006bef..5e6a5bf04b 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -157,6 +157,7 @@ CXXFLAGS = $(ARCHCXXFLAGS) \ $(INSTRUMENTATIONDEFINES) \ $(ARCHDEFINES) \ $(EXTRADEFINES) \ + -DCONFIG_WCHAR_BUILTIN \ $(addprefix -I,$(INCLUDE_DIRS)) # Flags we pass to the assembler From c558ad15abaab45ba1810f0f990f8ece87bed501 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Apr 2013 01:00:07 -0700 Subject: [PATCH 037/486] Add the RGB LED driver as an example. --- makefiles/config_px4fmuv2_default.mk | 5 + makefiles/setup.mk | 2 +- src/device/rgbled/module.mk | 6 + src/device/rgbled/rgbled.cpp | 490 +++++++++++++++++++++++++++ 4 files changed, 502 insertions(+), 1 deletion(-) create mode 100644 src/device/rgbled/module.mk create mode 100644 src/device/rgbled/rgbled.cpp diff --git a/makefiles/config_px4fmuv2_default.mk b/makefiles/config_px4fmuv2_default.mk index 2f104a5e4a..0ea0a90479 100644 --- a/makefiles/config_px4fmuv2_default.mk +++ b/makefiles/config_px4fmuv2_default.mk @@ -7,6 +7,11 @@ # ROMFS_ROOT = $(PX4_BASE)/ROMFS/$(CONFIG) +# +# Board support modules +# +MODULES += device/rgbled + # # Transitional support - add commands from the NuttX export archive. # diff --git a/makefiles/setup.mk b/makefiles/setup.mk index 7655872e5f..b798f7cabe 100644 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -42,7 +42,7 @@ # and is consistent with joining the results of $(dir) and $(notdir). # export PX4_INCLUDE_DIR = $(abspath $(PX4_BASE)/src/include)/ -export PX4_MODULE_SRC = $(abspath $(PX4_BASE)/src/modules)/ +export PX4_MODULE_SRC = $(abspath $(PX4_BASE)/src)/ export PX4_MK_DIR = $(abspath $(PX4_BASE)/makefiles)/ export NUTTX_SRC = $(abspath $(PX4_BASE)/nuttx)/ export NUTTX_APP_SRC = $(abspath $(PX4_BASE)/apps)/ diff --git a/src/device/rgbled/module.mk b/src/device/rgbled/module.mk new file mode 100644 index 0000000000..39b53ec9ed --- /dev/null +++ b/src/device/rgbled/module.mk @@ -0,0 +1,6 @@ +# +# TCA62724FMG driver for RGB LED +# + +MODULE_COMMAND = rgbled +SRCS = rgbled.cpp diff --git a/src/device/rgbled/rgbled.cpp b/src/device/rgbled/rgbled.cpp new file mode 100644 index 0000000000..c3b92ba7eb --- /dev/null +++ b/src/device/rgbled/rgbled.cpp @@ -0,0 +1,490 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 rgbled.cpp + * + * Driver for the onboard RGB LED controller (TCA62724FMG) connected via I2C. + * + * + */ + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#define LED_ONTIME 120 +#define LED_OFFTIME 120 + +#define RGBLED_DEVICE_PATH "/dev/rgbled" + +#define ADDR 0x55 /**< I2C adress of TCA62724FMG */ +#define SUB_ADDR_START 0x01 /**< write everything (with auto-increment) */ +#define SUB_ADDR_PWM0 0x81 /**< blue (without auto-increment) */ +#define SUB_ADDR_PWM1 0x82 /**< green (without auto-increment) */ +#define SUB_ADDR_PWM2 0x83 /**< red (without auto-increment) */ +#define SUB_ADDR_SETTINGS 0x84 /**< settings (without auto-increment)*/ + +#define SETTING_NOT_POWERSAVE 0x01 /**< power-save mode not off */ +#define SETTING_ENABLE 0x02 /**< on */ + + +enum ledModes { + LED_MODE_TEST, + LED_MODE_SYSTEMSTATE, + LED_MODE_OFF +}; + +class RGBLED : public device::I2C +{ +public: + RGBLED(int bus, int rgbled); + virtual ~RGBLED(); + + + virtual int init(); + virtual int probe(); + virtual int info(); + virtual int setMode(enum ledModes mode); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + +private: + + enum ledColors { + LED_COLOR_OFF, + LED_COLOR_RED, + LED_COLOR_YELLOW, + LED_COLOR_PURPLE, + LED_COLOR_GREEN, + LED_COLOR_BLUE, + LED_COLOR_WHITE, + LED_COLOR_AMBER, + }; + + enum ledBlinkModes { + LED_BLINK_ON, + LED_BLINK_OFF + }; + + work_s _work; + + int led_colors[8]; + int led_blink; + + int mode; + int running; + + void setLEDColor(int ledcolor); + static void led_trampoline(void *arg); + void led(); + + int set(bool on, uint8_t r, uint8_t g, uint8_t b); + int set_on(bool on); + int set_rgb(uint8_t r, uint8_t g, uint8_t b); + int get(bool &on, bool ¬_powersave, uint8_t &r, uint8_t &g, uint8_t &b); +}; + +/* for now, we only support one RGBLED */ +namespace +{ + RGBLED *g_rgbled; +} + + +extern "C" __EXPORT int rgbled_main(int argc, char *argv[]); + +RGBLED::RGBLED(int bus, int rgbled) : + I2C("rgbled", RGBLED_DEVICE_PATH, bus, rgbled, 100000), + led_colors({0,0,0,0,0,0,0,0}), + led_blink(LED_BLINK_OFF), + mode(LED_MODE_OFF), + running(false) +{ + memset(&_work, 0, sizeof(_work)); +} + +RGBLED::~RGBLED() +{ +} + +int +RGBLED::init() +{ + int ret; + ret = I2C::init(); + + if (ret != OK) { + warnx("I2C init failed"); + return ret; + } + + /* start off */ + set(false, 0, 0, 0); + + return OK; +} + +int +RGBLED::setMode(enum ledModes new_mode) +{ + switch (new_mode) { + case LED_MODE_SYSTEMSTATE: + case LED_MODE_TEST: + mode = new_mode; + if (!running) { + running = true; + set_on(true); + work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1); + } + break; + case LED_MODE_OFF: + + default: + if (running) { + running = false; + set_on(false); + } + mode = LED_MODE_OFF; + break; + } + + return OK; +} + +int +RGBLED::probe() +{ + int ret; + bool on, not_powersave; + uint8_t r, g, b; + + ret = get(on, not_powersave, r, g, b); + + return ret; +} + +int +RGBLED::info() +{ + int ret; + bool on, not_powersave; + uint8_t r, g, b; + + ret = get(on, not_powersave, r, g, b); + + /* we don't care about power-save mode */ + log("State: %s", on ? "ON" : "OFF"); + log("Red: %d, Green: %d, Blue: %d", r, g, b); + + return ret; +} + +int +RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + int ret = ENOTTY; + + switch (cmd) { + + default: + break; + } + + return ret; +} + + +void +RGBLED::led_trampoline(void *arg) +{ + RGBLED *rgbl = (RGBLED *)arg; + + rgbl->led(); +} + + + +void +RGBLED::led() +{ + static int led_thread_runcount=0; + static int led_interval = 1000; + + switch (mode) { + case LED_MODE_TEST: + /* Demo LED pattern for now */ + led_colors[0] = LED_COLOR_YELLOW; + led_colors[1] = LED_COLOR_AMBER; + led_colors[2] = LED_COLOR_RED; + led_colors[3] = LED_COLOR_PURPLE; + led_colors[4] = LED_COLOR_BLUE; + led_colors[5] = LED_COLOR_GREEN; + led_colors[6] = LED_COLOR_WHITE; + led_colors[7] = LED_COLOR_OFF; + led_blink = LED_BLINK_ON; + break; + + case LED_MODE_SYSTEMSTATE: + /* XXX TODO set pattern */ + led_colors[0] = LED_COLOR_OFF; + led_colors[1] = LED_COLOR_OFF; + led_colors[2] = LED_COLOR_OFF; + led_colors[3] = LED_COLOR_OFF; + led_colors[4] = LED_COLOR_OFF; + led_colors[5] = LED_COLOR_OFF; + led_colors[6] = LED_COLOR_OFF; + led_colors[7] = LED_COLOR_OFF; + led_blink = LED_BLINK_OFF; + + break; + + case LED_MODE_OFF: + default: + return; + break; + } + + + if (led_thread_runcount & 1) { + if (led_blink == LED_BLINK_ON) + setLEDColor(LED_COLOR_OFF); + led_interval = LED_OFFTIME; + } else { + setLEDColor(led_colors[(led_thread_runcount/2) % 8]); + led_interval = LED_ONTIME; + } + + led_thread_runcount++; + + + if(running) { + /* re-queue ourselves to run again later */ + work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, led_interval); + } else { + set_on(false); + } +} + +void RGBLED::setLEDColor(int ledcolor) { + switch (ledcolor) { + case LED_COLOR_OFF: // off + set_rgb(0,0,0); + break; + case LED_COLOR_RED: // red + set_rgb(255,0,0); + break; + case LED_COLOR_YELLOW: // yellow + set_rgb(255,70,0); + break; + case LED_COLOR_PURPLE: // purple + set_rgb(255,0,255); + break; + case LED_COLOR_GREEN: // green + set_rgb(0,255,0); + break; + case LED_COLOR_BLUE: // blue + set_rgb(0,0,255); + break; + case LED_COLOR_WHITE: // white + set_rgb(255,255,255); + break; + case LED_COLOR_AMBER: // amber + set_rgb(255,20,0); + break; + } +} + +int +RGBLED::set(bool on, uint8_t r, uint8_t g, uint8_t b) +{ + uint8_t settings_byte = 0; + + if (on) + settings_byte |= SETTING_ENABLE; +/* powersave not used */ +// if (not_powersave) + settings_byte |= SETTING_NOT_POWERSAVE; + + const uint8_t msg[5] = { SUB_ADDR_START, (uint8_t)(b*15/255), (uint8_t)(g*15/255), (uint8_t)(r*15/255), settings_byte}; + + return transfer(msg, sizeof(msg), nullptr, 0); +} + +int +RGBLED::set_on(bool on) +{ + uint8_t settings_byte = 0; + + if (on) + settings_byte |= SETTING_ENABLE; + +/* powersave not used */ +// if (not_powersave) + settings_byte |= SETTING_NOT_POWERSAVE; + + const uint8_t msg[2] = { SUB_ADDR_SETTINGS, settings_byte}; + + return transfer(msg, sizeof(msg), nullptr, 0); +} + +int +RGBLED::set_rgb(uint8_t r, uint8_t g, uint8_t b) +{ + const uint8_t msg[6] = { SUB_ADDR_PWM0, (uint8_t)(b*15/255), SUB_ADDR_PWM1, (uint8_t)(g*15/255), SUB_ADDR_PWM2, (uint8_t)(r*15/255)}; + + return transfer(msg, sizeof(msg), nullptr, 0); +} + + +int +RGBLED::get(bool &on, bool ¬_powersave, uint8_t &r, uint8_t &g, uint8_t &b) +{ + uint8_t result[2]; + int ret; + + ret = transfer(nullptr, 0, &result[0], 2); + + if (ret == OK) { + on = result[0] & SETTING_ENABLE; + not_powersave = result[0] & SETTING_NOT_POWERSAVE; + r = (result[0] & 0x0f)*255/15; + g = (result[1] & 0xf0)*255/15; + b = (result[1] & 0x0f)*255/15; + } + + return ret; +} + + +void rgbled_usage() { + fprintf(stderr, "missing command: try 'start', 'systemstate', 'test', 'info', 'off'\n"); + fprintf(stderr, "options:\n"); + fprintf(stderr, "\t-b --bus i2cbus (3)\n"); + fprintf(stderr, "\t-a --ddr addr (9)\n"); +} + +int +rgbled_main(int argc, char *argv[]) +{ + + int i2cdevice = PX4_I2C_BUS_LED; + int rgbledadr = ADDR; /* 7bit */ + + int x; + + for (x = 1; x < argc; x++) { + if (strcmp(argv[x], "-b") == 0 || strcmp(argv[x], "--bus") == 0) { + if (argc > x + 1) { + i2cdevice = atoi(argv[x + 1]); + } + } + + if (strcmp(argv[x], "-a") == 0 || strcmp(argv[x], "--addr") == 0) { + if (argc > x + 1) { + rgbledadr = atoi(argv[x + 1]); + } + } + + } + + if (!strcmp(argv[1], "start")) { + if (g_rgbled != nullptr) + errx(1, "already started"); + + g_rgbled = new RGBLED(i2cdevice, rgbledadr); + + if (g_rgbled == nullptr) + errx(1, "new failed"); + + if (OK != g_rgbled->init()) { + delete g_rgbled; + g_rgbled = nullptr; + errx(1, "init failed"); + } + + exit(0); + } + + + if (g_rgbled == nullptr) { + fprintf(stderr, "not started\n"); + rgbled_usage(); + exit(0); + } + + if (!strcmp(argv[1], "test")) { + g_rgbled->setMode(LED_MODE_TEST); + exit(0); + } + + if (!strcmp(argv[1], "systemstate")) { + g_rgbled->setMode(LED_MODE_SYSTEMSTATE); + exit(0); + } + + if (!strcmp(argv[1], "info")) { + g_rgbled->info(); + exit(0); + } + + if (!strcmp(argv[1], "off")) { + g_rgbled->setMode(LED_MODE_OFF); + exit(0); + } + + + /* things that require access to the device */ + int fd = open(RGBLED_DEVICE_PATH, 0); + if (fd < 0) + err(1, "can't open RGBLED device"); + + rgbled_usage(); + exit(0); +} From 4ccf252b76cd4e52ecd11adffa63ee1e0b67ab37 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 3 Apr 2013 16:38:16 -0700 Subject: [PATCH 038/486] Changed I2C bus for blinkm, tested on fmuv2 --- apps/drivers/blinkm/blinkm.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/apps/drivers/blinkm/blinkm.cpp b/apps/drivers/blinkm/blinkm.cpp index 56265995f3..3fabfd9a54 100644 --- a/apps/drivers/blinkm/blinkm.cpp +++ b/apps/drivers/blinkm/blinkm.cpp @@ -92,6 +92,7 @@ #include +#include #include #include @@ -841,7 +842,7 @@ int blinkm_main(int argc, char *argv[]) { - int i2cdevice = 3; + int i2cdevice = PX4_I2C_BUS_EXPANSION; int blinkmadr = 9; int x; From 4a5505b044a079d692ca631b6c3f268626ed2860 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 4 Apr 2013 23:12:05 +0200 Subject: [PATCH 039/486] Added LSM303D driver skeleton --- apps/drivers/lsm303d/Makefile | 42 ++ apps/drivers/lsm303d/lsm303d.cpp | 833 +++++++++++++++++++++++++++ nuttx/configs/px4fmuv2/nsh/appconfig | 1 + 3 files changed, 876 insertions(+) create mode 100644 apps/drivers/lsm303d/Makefile create mode 100644 apps/drivers/lsm303d/lsm303d.cpp diff --git a/apps/drivers/lsm303d/Makefile b/apps/drivers/lsm303d/Makefile new file mode 100644 index 0000000000..542a9bd40a --- /dev/null +++ b/apps/drivers/lsm303d/Makefile @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (C) 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Makefile to build the LSM303D driver. +# + +APPNAME = lsm303d +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/lsm303d/lsm303d.cpp b/apps/drivers/lsm303d/lsm303d.cpp new file mode 100644 index 0000000000..b0f9001024 --- /dev/null +++ b/apps/drivers/lsm303d/lsm303d.cpp @@ -0,0 +1,833 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file lsm303d.cpp + * Driver for the ST LSM303D MEMS accel / mag connected via SPI. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include + + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +/* SPI protocol address bits */ +#define DIR_READ (1<<7) +#define DIR_WRITE (0<<7) +#define ADDR_INCREMENT (1<<6) + +/* register addresses */ +#define ADDR_TEMP_OUT_L 0x05 +#define ADDR_TEMP_OUT_H 0x06 +#define ADDR_STATUS_M 0x07 +#define ADDR_OUT_X_L_M 0x08 +#define ADDR_OUT_X_H_M 0x09 +#define ADDR_OUT_Y_L_M 0x08 +#define ADDR_OUT_Y_H_M 0x09 +#define ADDR_OUT_Z_L_M 0x0A +#define ADDR_OUT_Z_H_M 0x0B + +#define ADDR_WHO_AM_I 0x0F +#define WHO_I_AM 0x49 + +#define INT_CTRL_M 0x12 +#define INT_SRC_M 0x13 + +extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); } + +class LSM303D : public device::SPI +{ +public: + LSM303D(int bus, const char* path, spi_dev_e device); + virtual ~LSM303D(); + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + virtual int probe(); + +private: + + struct hrt_call _call; + unsigned _call_interval; + + unsigned _num_reports; + volatile unsigned _next_report; + volatile unsigned _oldest_report; + struct gyro_report *_reports; + + struct gyro_scale _gyro_scale; + float _gyro_range_scale; + float _gyro_range_rad_s; + orb_advert_t _gyro_topic; + + unsigned _current_rate; + unsigned _current_range; + + perf_counter_t _sample_perf; + + /** + * Start automatic measurement. + */ + void start(); + + /** + * Stop automatic measurement. + */ + void stop(); + + /** + * Static trampoline from the hrt_call context; because we don't have a + * generic hrt wrapper yet. + * + * Called by the HRT in interrupt context at the specified rate if + * automatic polling is enabled. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void measure_trampoline(void *arg); + + /** + * Fetch measurements from the sensor and update the report ring. + */ + void measure(); + + /** + * Read a register from the LSM303D + * + * @param The register to read. + * @return The value that was read. + */ + uint8_t read_reg(unsigned reg); + + /** + * Write a register in the LSM303D + * + * @param reg The register to write. + * @param value The new value to write. + */ + void write_reg(unsigned reg, uint8_t value); + + /** + * Modify a register in the LSM303D + * + * Bits are cleared before bits are set. + * + * @param reg The register to modify. + * @param clearbits Bits in the register to clear. + * @param setbits Bits in the register to set. + */ + void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); + + /** + * Set the LSM303D measurement range. + * + * @param max_dps The measurement range is set to permit reading at least + * this rate in degrees per second. + * Zero selects the maximum supported range. + * @return OK if the value can be supported, -ERANGE otherwise. + */ + int set_range(unsigned max_dps); + + /** + * Set the LSM303D internal sampling frequency. + * + * @param frequency The internal sampling frequency is set to not less than + * this value. + * Zero selects the maximum rate supported. + * @return OK if the value can be supported. + */ + int set_samplerate(unsigned frequency); +}; + +/* helper macro for handling report buffer indices */ +#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) + + +LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : + SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 8000000), + _call_interval(0), + _num_reports(0), + _next_report(0), + _oldest_report(0), + _reports(nullptr), + _gyro_range_scale(0.0f), + _gyro_range_rad_s(0.0f), + _gyro_topic(-1), + _current_rate(0), + _current_range(0), + _sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_read")) +{ + // enable debug() calls + _debug_enabled = true; + + // default scale factors + _gyro_scale.x_offset = 0; + _gyro_scale.x_scale = 1.0f; + _gyro_scale.y_offset = 0; + _gyro_scale.y_scale = 1.0f; + _gyro_scale.z_offset = 0; + _gyro_scale.z_scale = 1.0f; +} + +LSM303D::~LSM303D() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_reports != nullptr) + delete[] _reports; + + /* delete the perf counter */ + perf_free(_sample_perf); +} + +int +LSM303D::init() +{ + int ret = ERROR; + + /* do SPI init (and probe) first */ + if (SPI::init() != OK) + goto out; + + /* allocate basic report buffers */ + _num_reports = 2; + _oldest_report = _next_report = 0; + _reports = new struct accel_report[_num_reports]; + + if (_reports == nullptr) + goto out; + + /* advertise sensor topic */ + memset(&_reports[0], 0, sizeof(_reports[0])); + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &_reports[0]); + +// /* set default configuration */ +// write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); +// write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ +// write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */ +// write_reg(ADDR_CTRL_REG4, REG4_BDU); +// write_reg(ADDR_CTRL_REG5, 0); +// +// write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */ +// write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_STREAM_MODE); /* Enable FIFO, old data is overwritten */ + + set_range(500); /* default to 500dps */ + set_samplerate(0); /* max sample rate */ + + ret = OK; +out: + return ret; +} + +int +LSM303D::probe() +{ + /* read dummy value to void to clear SPI statemachine on sensor */ + (void)read_reg(ADDR_WHO_AM_I); + + /* verify that the device is attached and functioning */ + if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) + return OK; + + return -EIO; +} + +ssize_t +LSM303D::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct gyro_report); + int ret = 0; + +// /* buffer must be large enough */ +// if (count < 1) +// return -ENOSPC; +// +// /* if automatic measurement is enabled */ +// if (_call_interval > 0) { +// +// /* +// * While there is space in the caller's buffer, and reports, copy them. +// * Note that we may be pre-empted by the measurement code while we are doing this; +// * we are careful to avoid racing with it. +// */ +// while (count--) { +// if (_oldest_report != _next_report) { +// memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); +// ret += sizeof(_reports[0]); +// INCREMENT(_oldest_report, _num_reports); +// } +// } +// +// /* if there was no data, warn the caller */ +// return ret ? ret : -EAGAIN; +// } +// +// /* manual measurement */ +// _oldest_report = _next_report = 0; +// measure(); +// +// /* measurement will have generated a report, copy it out */ +// memcpy(buffer, _reports, sizeof(*_reports)); +// ret = sizeof(*_reports); + + return ret; +} + +int +LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + +// case SENSORIOCSPOLLRATE: { +// switch (arg) { +// +// /* switching to manual polling */ +// case SENSOR_POLLRATE_MANUAL: +// stop(); +// _call_interval = 0; +// return OK; +// +// /* external signalling not supported */ +// case SENSOR_POLLRATE_EXTERNAL: +// +// /* zero would be bad */ +// case 0: +// return -EINVAL; +// +// /* set default/max polling rate */ +// case SENSOR_POLLRATE_MAX: +// case SENSOR_POLLRATE_DEFAULT: +// /* With internal low pass filters enabled, 250 Hz is sufficient */ +// return ioctl(filp, SENSORIOCSPOLLRATE, 250); +// +// /* adjust to a legal polling interval in Hz */ +// default: { +// /* do we need to start internal polling? */ +// bool want_start = (_call_interval == 0); +// +// /* convert hz to hrt interval via microseconds */ +// unsigned ticks = 1000000 / arg; +// +// /* check against maximum sane rate */ +// if (ticks < 1000) +// return -EINVAL; +// +// /* update interval for next measurement */ +// /* XXX this is a bit shady, but no other way to adjust... */ +// _call.period = _call_interval = ticks; +// +// /* if we need to start the poll state machine, do it */ +// if (want_start) +// start(); +// +// return OK; +// } +// } +// } +// +// case SENSORIOCGPOLLRATE: +// if (_call_interval == 0) +// return SENSOR_POLLRATE_MANUAL; +// +// return 1000000 / _call_interval; +// +// case SENSORIOCSQUEUEDEPTH: { +// /* account for sentinel in the ring */ +// arg++; +// +// /* lower bound is mandatory, upper bound is a sanity check */ +// if ((arg < 2) || (arg > 100)) +// return -EINVAL; +// +// /* allocate new buffer */ +// struct gyro_report *buf = new struct gyro_report[arg]; +// +// if (nullptr == buf) +// return -ENOMEM; +// +// /* reset the measurement state machine with the new buffer, free the old */ +// stop(); +// delete[] _reports; +// _num_reports = arg; +// _reports = buf; +// start(); +// +// return OK; +// } +// +// case SENSORIOCGQUEUEDEPTH: +// return _num_reports - 1; +// +// case SENSORIOCRESET: +// /* XXX implement */ +// return -EINVAL; +// +// case GYROIOCSSAMPLERATE: +// return set_samplerate(arg); +// +// case GYROIOCGSAMPLERATE: +// return _current_rate; +// +// case GYROIOCSLOWPASS: +// case GYROIOCGLOWPASS: +// /* XXX not implemented due to wacky interaction with samplerate */ +// return -EINVAL; +// +// case GYROIOCSSCALE: +// /* copy scale in */ +// memcpy(&_gyro_scale, (struct gyro_scale *) arg, sizeof(_gyro_scale)); +// return OK; +// +// case GYROIOCGSCALE: +// /* copy scale out */ +// memcpy((struct gyro_scale *) arg, &_gyro_scale, sizeof(_gyro_scale)); +// return OK; +// +// case GYROIOCSRANGE: +// return set_range(arg); +// +// case GYROIOCGRANGE: +// return _current_range; + + default: + /* give it to the superclass */ + return SPI::ioctl(filp, cmd, arg); + } +} + +uint8_t +LSM303D::read_reg(unsigned reg) +{ + uint8_t cmd[2]; + + cmd[0] = reg | DIR_READ; + + transfer(cmd, cmd, sizeof(cmd)); + + return cmd[1]; +} + +void +LSM303D::write_reg(unsigned reg, uint8_t value) +{ + uint8_t cmd[2]; + + cmd[0] = reg | DIR_WRITE; + cmd[1] = value; + + transfer(cmd, nullptr, sizeof(cmd)); +} + +void +LSM303D::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) +{ + uint8_t val; + + val = read_reg(reg); + val &= ~clearbits; + val |= setbits; + write_reg(reg, val); +} + +int +LSM303D::set_range(unsigned max_dps) +{ +// uint8_t bits = REG4_BDU; +// +// if (max_dps == 0) +// max_dps = 2000; +// +// if (max_dps <= 250) { +// _current_range = 250; +// bits |= RANGE_250DPS; +// +// } else if (max_dps <= 500) { +// _current_range = 500; +// bits |= RANGE_500DPS; +// +// } else if (max_dps <= 2000) { +// _current_range = 2000; +// bits |= RANGE_2000DPS; +// +// } else { +// return -EINVAL; +// } +// +// _gyro_range_rad_s = _current_range / 180.0f * M_PI_F; +// _gyro_range_scale = _gyro_range_rad_s / 32768.0f; +// write_reg(ADDR_CTRL_REG4, bits); + + return OK; +} + +int +LSM303D::set_samplerate(unsigned frequency) +{ +// uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE; +// +// if (frequency == 0) +// frequency = 760; +// +// if (frequency <= 95) { +// _current_rate = 95; +// bits |= RATE_95HZ_LP_25HZ; +// +// } else if (frequency <= 190) { +// _current_rate = 190; +// bits |= RATE_190HZ_LP_25HZ; +// +// } else if (frequency <= 380) { +// _current_rate = 380; +// bits |= RATE_380HZ_LP_30HZ; +// +// } else if (frequency <= 760) { +// _current_rate = 760; +// bits |= RATE_760HZ_LP_30HZ; +// +// } else { +// return -EINVAL; +// } +// +// write_reg(ADDR_CTRL_REG1, bits); + + return OK; +} + +void +LSM303D::start() +{ + /* make sure we are stopped first */ + stop(); + + /* reset the report ring */ + _oldest_report = _next_report = 0; + + /* start polling at the specified rate */ + hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&LSM303D::measure_trampoline, this); +} + +void +LSM303D::stop() +{ + hrt_cancel(&_call); +} + +void +LSM303D::measure_trampoline(void *arg) +{ + LSM303D *dev = (LSM303D *)arg; + + /* make another measurement */ + dev->measure(); +} + +void +LSM303D::measure() +{ +// /* status register and data as read back from the device */ +//#pragma pack(push, 1) +// struct { +// uint8_t cmd; +// uint8_t temp; +// uint8_t status; +// int16_t x; +// int16_t y; +// int16_t z; +// } raw_report; +//#pragma pack(pop) +// +// gyro_report *report = &_reports[_next_report]; +// +// /* start the performance counter */ +// perf_begin(_sample_perf); +// +// /* fetch data from the sensor */ +// raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT; +// transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report)); +// +// /* +// * 1) Scale raw value to SI units using scaling from datasheet. +// * 2) Subtract static offset (in SI units) +// * 3) Scale the statically calibrated values with a linear +// * dynamically obtained factor +// * +// * Note: the static sensor offset is the number the sensor outputs +// * at a nominally 'zero' input. Therefore the offset has to +// * be subtracted. +// * +// * Example: A gyro outputs a value of 74 at zero angular rate +// * the offset is 74 from the origin and subtracting +// * 74 from all measurements centers them around zero. +// */ +// report->timestamp = hrt_absolute_time(); +// /* XXX adjust for sensor alignment to board here */ +// report->x_raw = raw_report.x; +// report->y_raw = raw_report.y; +// report->z_raw = raw_report.z; +// +// report->x = ((report->x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; +// report->y = ((report->y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; +// report->z = ((report->z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; +// report->scaling = _gyro_range_scale; +// report->range_rad_s = _gyro_range_rad_s; +// +// /* post a report to the ring - note, not locked */ +// INCREMENT(_next_report, _num_reports); +// +// /* if we are running up against the oldest report, fix it */ +// if (_next_report == _oldest_report) +// INCREMENT(_oldest_report, _num_reports); +// +// /* notify anyone waiting for data */ +// poll_notify(POLLIN); +// +// /* publish for subscribers */ +// orb_publish(ORB_ID(sensor_gyro), _gyro_topic, report); + + /* stop the perf counter */ + perf_end(_sample_perf); +} + +void +LSM303D::print_info() +{ + perf_print_counter(_sample_perf); + printf("report queue: %u (%u/%u @ %p)\n", + _num_reports, _oldest_report, _next_report, _reports); +} + +/** + * Local functions in support of the shell command. + */ +namespace lsm303d +{ + +LSM303D *g_dev; + +void start(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. + */ +void +start() +{ + int fd; + + if (g_dev != nullptr) + errx(1, "already started"); + + /* create the driver */ + g_dev = new LSM303D(1 /* XXX magic number */, ACCEL_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_ACCEL); + + if (g_dev == nullptr) + goto fail; + + if (OK != g_dev->init()) + goto fail; + + /* set the poll rate to default, starts automatic data collection */ + fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + goto fail; + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + goto fail; + + exit(0); +fail: + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + errx(1, "driver start failed"); +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ + int fd_accel = -1; + struct accel_report a_report; + ssize_t sz; + + /* get the driver */ + fd_accel = open(ACCEL_DEVICE_PATH, O_RDONLY); + + if (fd_accel < 0) + err(1, "%s open failed", ACCEL_DEVICE_PATH); + + /* reset to manual polling */ + if (ioctl(fd_accel, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) + err(1, "reset to manual polling"); + + /* do a simple demand read */ + sz = read(fd_accel, &a_report, sizeof(a_report)); + + if (sz != sizeof(a_report)) + err(1, "immediate gyro read failed"); + + warnx("accel x: \t% 9.5f\tm/s^2", (double)a_report.x); + warnx("accel y: \t% 9.5f\tm/s^2", (double)a_report.y); + warnx("accel z: \t% 9.5f\tm/s^2", (double)a_report.z); + warnx("accel x: \t%d\traw", (int)a_report.x_raw); + warnx("accel y: \t%d\traw", (int)a_report.y_raw); + warnx("accel z: \t%d\traw", (int)a_report.z_raw); + warnx("accel range: %8.4f m/s^2", (double)a_report.range_m_s2); + + /* XXX add poll-rate tests here too */ + + reset(); + errx(0, "PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + err(1, "failed "); + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) + err(1, "driver reset failed"); + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + err(1, "driver poll restart failed"); + + exit(0); +} + +/** + * Print a little info about the driver. + */ +void +info() +{ + if (g_dev == nullptr) + errx(1, "driver not running\n"); + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + exit(0); +} + + +} // namespace + +int +lsm303d_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + + */ + if (!strcmp(argv[1], "start")) + LSM303D::start(); + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) + lsm303d::test(); + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) + lsm303d::reset(); + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info")) + lsm303d::info(); + + errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); +} diff --git a/nuttx/configs/px4fmuv2/nsh/appconfig b/nuttx/configs/px4fmuv2/nsh/appconfig index fbcb67dcff..fff1406733 100644 --- a/nuttx/configs/px4fmuv2/nsh/appconfig +++ b/nuttx/configs/px4fmuv2/nsh/appconfig @@ -116,6 +116,7 @@ CONFIGURED_APPS += drivers/device # XXX needs conversion to SPI #CONFIGURED_APPS += drivers/ms5611 CONFIGURED_APPS += drivers/l3gd20 +CONFIGURED_APPS += drivers/lsm303d # XXX needs conversion to serial #CONFIGURED_APPS += drivers/px4io CONFIGURED_APPS += drivers/stm32 From b4483a09b2886a6c0ecd78703e1f06e776d3a6d4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 4 Apr 2013 23:22:39 +0200 Subject: [PATCH 040/486] Added LSM303D driver --- apps/drivers/lsm303d/lsm303d.cpp | 48 ++++++++++++++++---------------- 1 file changed, 24 insertions(+), 24 deletions(-) diff --git a/apps/drivers/lsm303d/lsm303d.cpp b/apps/drivers/lsm303d/lsm303d.cpp index b0f9001024..c00726b4e9 100644 --- a/apps/drivers/lsm303d/lsm303d.cpp +++ b/apps/drivers/lsm303d/lsm303d.cpp @@ -123,12 +123,12 @@ private: unsigned _num_reports; volatile unsigned _next_report; volatile unsigned _oldest_report; - struct gyro_report *_reports; + struct accel_report *_reports; - struct gyro_scale _gyro_scale; - float _gyro_range_scale; - float _gyro_range_rad_s; - orb_advert_t _gyro_topic; + struct accel_scale _accel_scale; + float _accel_range_scale; + float _accel_range_m_s2; + orb_advert_t _accel_topic; unsigned _current_rate; unsigned _current_range; @@ -220,9 +220,9 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _next_report(0), _oldest_report(0), _reports(nullptr), - _gyro_range_scale(0.0f), - _gyro_range_rad_s(0.0f), - _gyro_topic(-1), + _accel_range_scale(0.0f), + _accel_range_m_s2(0.0f), + _accel_topic(-1), _current_rate(0), _current_range(0), _sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_read")) @@ -231,12 +231,12 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _debug_enabled = true; // default scale factors - _gyro_scale.x_offset = 0; - _gyro_scale.x_scale = 1.0f; - _gyro_scale.y_offset = 0; - _gyro_scale.y_scale = 1.0f; - _gyro_scale.z_offset = 0; - _gyro_scale.z_scale = 1.0f; + _accel_scale.x_offset = 0; + _accel_scale.x_scale = 1.0f; + _accel_scale.y_offset = 0; + _accel_scale.y_scale = 1.0f; + _accel_scale.z_offset = 0; + _accel_scale.z_scale = 1.0f; } LSM303D::~LSM303D() @@ -307,7 +307,7 @@ LSM303D::probe() ssize_t LSM303D::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(struct gyro_report); + unsigned count = buflen / sizeof(struct accel_report); int ret = 0; // /* buffer must be large enough */ @@ -412,7 +412,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) // return -EINVAL; // // /* allocate new buffer */ -// struct gyro_report *buf = new struct gyro_report[arg]; +// struct accel_report *buf = new struct accel_report[arg]; // // if (nullptr == buf) // return -ENOMEM; @@ -447,12 +447,12 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) // // case GYROIOCSSCALE: // /* copy scale in */ -// memcpy(&_gyro_scale, (struct gyro_scale *) arg, sizeof(_gyro_scale)); +// memcpy(&_accel_scale, (struct accel_scale *) arg, sizeof(_accel_scale)); // return OK; // // case GYROIOCGSCALE: // /* copy scale out */ -// memcpy((struct gyro_scale *) arg, &_gyro_scale, sizeof(_gyro_scale)); +// memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale)); // return OK; // // case GYROIOCSRANGE: @@ -608,7 +608,7 @@ LSM303D::measure() // } raw_report; //#pragma pack(pop) // -// gyro_report *report = &_reports[_next_report]; +// accel_report *report = &_reports[_next_report]; // // /* start the performance counter */ // perf_begin(_sample_perf); @@ -637,9 +637,9 @@ LSM303D::measure() // report->y_raw = raw_report.y; // report->z_raw = raw_report.z; // -// report->x = ((report->x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; -// report->y = ((report->y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; -// report->z = ((report->z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; +// report->x = ((report->x_raw * _gyro_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; +// report->y = ((report->y_raw * _gyro_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; +// report->z = ((report->z_raw * _gyro_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; // report->scaling = _gyro_range_scale; // report->range_rad_s = _gyro_range_rad_s; // @@ -693,7 +693,7 @@ start() errx(1, "already started"); /* create the driver */ - g_dev = new LSM303D(1 /* XXX magic number */, ACCEL_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_ACCEL); + g_dev = new LSM303D(1 /* XXX magic number */, ACCEL_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); if (g_dev == nullptr) goto fail; @@ -809,7 +809,7 @@ lsm303d_main(int argc, char *argv[]) */ if (!strcmp(argv[1], "start")) - LSM303D::start(); + lsm303d::start(); /* * Test the driver/device. From 2187dc8e9abced1ab88e08feaf24e026f728ea5c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 4 Apr 2013 19:46:55 -0700 Subject: [PATCH 041/486] LSM303D accel raw values look ok (work in progress) --- apps/drivers/lsm303d/lsm303d.cpp | 405 ++++++++++++++++--------------- 1 file changed, 206 insertions(+), 199 deletions(-) diff --git a/apps/drivers/lsm303d/lsm303d.cpp b/apps/drivers/lsm303d/lsm303d.cpp index c00726b4e9..b10c39f8f8 100644 --- a/apps/drivers/lsm303d/lsm303d.cpp +++ b/apps/drivers/lsm303d/lsm303d.cpp @@ -83,10 +83,31 @@ static const int ERROR = -1; #define ADDR_STATUS_M 0x07 #define ADDR_OUT_X_L_M 0x08 #define ADDR_OUT_X_H_M 0x09 -#define ADDR_OUT_Y_L_M 0x08 -#define ADDR_OUT_Y_H_M 0x09 -#define ADDR_OUT_Z_L_M 0x0A -#define ADDR_OUT_Z_H_M 0x0B +#define ADDR_OUT_Y_L_M 0x0A +#define ADDR_OUT_Y_H_M 0x0B +#define ADDR_OUT_Z_L_M 0x0C +#define ADDR_OUT_Z_H_M 0x0D + +#define ADDR_OUT_TEMP_A 0x26 +#define ADDR_STATUS_A 0x27 +#define ADDR_OUT_X_L_A 0x28 +#define ADDR_OUT_X_H_A 0x29 +#define ADDR_OUT_Y_L_A 0x2A +#define ADDR_OUT_Y_H_A 0x2B +#define ADDR_OUT_Z_L_A 0x2C +#define ADDR_OUT_Z_H_A 0x2D + +#define ADDR_CTRL_REG1 0x20 + +#define REG1_RATE_50HZ_A ((0<<7) | (1<<6) | (0<<5) | (1<<4)) +#define REG1_RATE_100HZ_A ((0<<7) | (1<<6) | (1<<5) | (0<<4)) +#define REG1_RATE_200HZ_A ((0<<7) | (1<<6) | (1<<5) | (1<<4)) +#define REG1_RATE_400HZ_A ((1<<7) | (0<<6) | (0<<5) | (0<<4)) + +#define REG1_CONT_UPDATE_A (0<<3) +#define REG1_Z_ENABLE_A (1<<2) +#define REG1_Y_ENABLE_A (1<<1) +#define REG1_X_ENABLE_A (1<<0) #define ADDR_WHO_AM_I 0x0F #define WHO_I_AM 0x49 @@ -274,7 +295,7 @@ LSM303D::init() _accel_topic = orb_advertise(ORB_ID(sensor_accel), &_reports[0]); // /* set default configuration */ -// write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); + write_reg(ADDR_CTRL_REG1, REG1_RATE_100HZ_A | REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A); // write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ // write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */ // write_reg(ADDR_CTRL_REG4, REG4_BDU); @@ -310,37 +331,37 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen) unsigned count = buflen / sizeof(struct accel_report); int ret = 0; -// /* buffer must be large enough */ -// if (count < 1) -// return -ENOSPC; -// -// /* if automatic measurement is enabled */ -// if (_call_interval > 0) { -// -// /* -// * While there is space in the caller's buffer, and reports, copy them. -// * Note that we may be pre-empted by the measurement code while we are doing this; -// * we are careful to avoid racing with it. -// */ -// while (count--) { -// if (_oldest_report != _next_report) { -// memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); -// ret += sizeof(_reports[0]); -// INCREMENT(_oldest_report, _num_reports); -// } -// } -// -// /* if there was no data, warn the caller */ -// return ret ? ret : -EAGAIN; -// } -// -// /* manual measurement */ -// _oldest_report = _next_report = 0; -// measure(); -// -// /* measurement will have generated a report, copy it out */ -// memcpy(buffer, _reports, sizeof(*_reports)); -// ret = sizeof(*_reports); + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + /* if automatic measurement is enabled */ + if (_call_interval > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the measurement code while we are doing this; + * we are careful to avoid racing with it. + */ + while (count--) { + if (_oldest_report != _next_report) { + memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); + ret += sizeof(_reports[0]); + INCREMENT(_oldest_report, _num_reports); + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement */ + _oldest_report = _next_report = 0; + measure(); + + /* measurement will have generated a report, copy it out */ + memcpy(buffer, _reports, sizeof(*_reports)); + ret = sizeof(*_reports); return ret; } @@ -350,116 +371,89 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { -// case SENSORIOCSPOLLRATE: { -// switch (arg) { -// -// /* switching to manual polling */ -// case SENSOR_POLLRATE_MANUAL: -// stop(); -// _call_interval = 0; -// return OK; -// -// /* external signalling not supported */ -// case SENSOR_POLLRATE_EXTERNAL: -// -// /* zero would be bad */ -// case 0: -// return -EINVAL; -// -// /* set default/max polling rate */ -// case SENSOR_POLLRATE_MAX: -// case SENSOR_POLLRATE_DEFAULT: -// /* With internal low pass filters enabled, 250 Hz is sufficient */ -// return ioctl(filp, SENSORIOCSPOLLRATE, 250); -// -// /* adjust to a legal polling interval in Hz */ -// default: { -// /* do we need to start internal polling? */ -// bool want_start = (_call_interval == 0); -// -// /* convert hz to hrt interval via microseconds */ -// unsigned ticks = 1000000 / arg; -// -// /* check against maximum sane rate */ -// if (ticks < 1000) -// return -EINVAL; -// -// /* update interval for next measurement */ -// /* XXX this is a bit shady, but no other way to adjust... */ -// _call.period = _call_interval = ticks; -// -// /* if we need to start the poll state machine, do it */ -// if (want_start) -// start(); -// -// return OK; -// } -// } -// } -// -// case SENSORIOCGPOLLRATE: -// if (_call_interval == 0) -// return SENSOR_POLLRATE_MANUAL; -// -// return 1000000 / _call_interval; -// -// case SENSORIOCSQUEUEDEPTH: { -// /* account for sentinel in the ring */ -// arg++; -// -// /* lower bound is mandatory, upper bound is a sanity check */ -// if ((arg < 2) || (arg > 100)) -// return -EINVAL; -// -// /* allocate new buffer */ -// struct accel_report *buf = new struct accel_report[arg]; -// -// if (nullptr == buf) -// return -ENOMEM; -// -// /* reset the measurement state machine with the new buffer, free the old */ -// stop(); -// delete[] _reports; -// _num_reports = arg; -// _reports = buf; -// start(); -// -// return OK; -// } -// -// case SENSORIOCGQUEUEDEPTH: -// return _num_reports - 1; -// -// case SENSORIOCRESET: -// /* XXX implement */ -// return -EINVAL; -// -// case GYROIOCSSAMPLERATE: -// return set_samplerate(arg); -// -// case GYROIOCGSAMPLERATE: -// return _current_rate; -// -// case GYROIOCSLOWPASS: -// case GYROIOCGLOWPASS: -// /* XXX not implemented due to wacky interaction with samplerate */ -// return -EINVAL; -// -// case GYROIOCSSCALE: -// /* copy scale in */ -// memcpy(&_accel_scale, (struct accel_scale *) arg, sizeof(_accel_scale)); -// return OK; -// -// case GYROIOCGSCALE: -// /* copy scale out */ -// memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale)); -// return OK; -// -// case GYROIOCSRANGE: -// return set_range(arg); -// -// case GYROIOCGRANGE: -// return _current_range; + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _call_interval = 0; + return OK; + + /* external signalling not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: + /* With internal low pass filters enabled, 250 Hz is sufficient */ + return ioctl(filp, SENSORIOCSPOLLRATE, 250); + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_call_interval == 0); + + /* convert hz to hrt interval via microseconds */ + unsigned ticks = 1000000 / arg; + + /* check against maximum sane rate */ + if (ticks < 1000) + return -EINVAL; + + /* update interval for next measurement */ + /* XXX this is a bit shady, but no other way to adjust... */ + _call.period = _call_interval = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_call_interval == 0) + return SENSOR_POLLRATE_MANUAL; + + return 1000000 / _call_interval; + + case SENSORIOCSQUEUEDEPTH: { + /* account for sentinel in the ring */ + arg++; + + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 2) || (arg > 100)) + return -EINVAL; + + /* allocate new buffer */ + struct accel_report *buf = new struct accel_report[arg]; + + if (nullptr == buf) + return -ENOMEM; + + /* reset the measurement state machine with the new buffer, free the old */ + stop(); + delete[] _reports; + _num_reports = arg; + _reports = buf; + start(); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _num_reports - 1; + + case SENSORIOCRESET: + /* XXX implement */ + return -EINVAL; default: /* give it to the superclass */ @@ -596,65 +590,78 @@ LSM303D::measure_trampoline(void *arg) void LSM303D::measure() { -// /* status register and data as read back from the device */ + /* status register and data as read back from the device */ //#pragma pack(push, 1) // struct { // uint8_t cmd; -// uint8_t temp; +// uint16_t temp; // uint8_t status; // int16_t x; // int16_t y; // int16_t z; -// } raw_report; +// } raw_report_mag; //#pragma pack(pop) -// -// accel_report *report = &_reports[_next_report]; -// -// /* start the performance counter */ -// perf_begin(_sample_perf); -// -// /* fetch data from the sensor */ -// raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT; -// transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report)); -// -// /* -// * 1) Scale raw value to SI units using scaling from datasheet. -// * 2) Subtract static offset (in SI units) -// * 3) Scale the statically calibrated values with a linear -// * dynamically obtained factor -// * -// * Note: the static sensor offset is the number the sensor outputs -// * at a nominally 'zero' input. Therefore the offset has to -// * be subtracted. -// * -// * Example: A gyro outputs a value of 74 at zero angular rate -// * the offset is 74 from the origin and subtracting -// * 74 from all measurements centers them around zero. -// */ -// report->timestamp = hrt_absolute_time(); -// /* XXX adjust for sensor alignment to board here */ -// report->x_raw = raw_report.x; -// report->y_raw = raw_report.y; -// report->z_raw = raw_report.z; -// + +#pragma pack(push, 1) + struct { + uint8_t cmd; + uint8_t status; + int16_t x; + int16_t y; + int16_t z; + } raw_report_accel; +#pragma pack(pop) + + accel_report *report = &_reports[_next_report]; + + /* start the performance counter */ + perf_begin(_sample_perf); + + /* fetch data from the sensor */ + raw_report_accel.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT; + transfer((uint8_t *)&raw_report_accel, (uint8_t *)&raw_report_accel, sizeof(raw_report_accel)); + + /* + * 1) Scale raw value to SI units using scaling from datasheet. + * 2) Subtract static offset (in SI units) + * 3) Scale the statically calibrated values with a linear + * dynamically obtained factor + * + * Note: the static sensor offset is the number the sensor outputs + * at a nominally 'zero' input. Therefore the offset has to + * be subtracted. + * + * Example: A gyro outputs a value of 74 at zero angular rate + * the offset is 74 from the origin and subtracting + * 74 from all measurements centers them around zero. + */ + + + report->timestamp = hrt_absolute_time(); + /* XXX adjust for sensor alignment to board here */ + report->x_raw = raw_report_accel.x; + report->y_raw = raw_report_accel.y; + report->z_raw = raw_report_accel.z; + + // report->x = ((report->x_raw * _gyro_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; // report->y = ((report->y_raw * _gyro_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; // report->z = ((report->z_raw * _gyro_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; // report->scaling = _gyro_range_scale; // report->range_rad_s = _gyro_range_rad_s; -// -// /* post a report to the ring - note, not locked */ -// INCREMENT(_next_report, _num_reports); -// -// /* if we are running up against the oldest report, fix it */ -// if (_next_report == _oldest_report) -// INCREMENT(_oldest_report, _num_reports); -// -// /* notify anyone waiting for data */ -// poll_notify(POLLIN); -// -// /* publish for subscribers */ -// orb_publish(ORB_ID(sensor_gyro), _gyro_topic, report); + + /* post a report to the ring - note, not locked */ + INCREMENT(_next_report, _num_reports); + + /* if we are running up against the oldest report, fix it */ + if (_next_report == _oldest_report) + INCREMENT(_oldest_report, _num_reports); + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + /* publish for subscribers */ + orb_publish(ORB_ID(sensor_accel), _accel_topic, report); /* stop the perf counter */ perf_end(_sample_perf); @@ -740,22 +747,22 @@ test() err(1, "%s open failed", ACCEL_DEVICE_PATH); /* reset to manual polling */ - if (ioctl(fd_accel, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) - err(1, "reset to manual polling"); +// if (ioctl(fd_accel, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) +// err(1, "reset to manual polling"); /* do a simple demand read */ sz = read(fd_accel, &a_report, sizeof(a_report)); if (sz != sizeof(a_report)) - err(1, "immediate gyro read failed"); + err(1, "immediate read failed"); - warnx("accel x: \t% 9.5f\tm/s^2", (double)a_report.x); - warnx("accel y: \t% 9.5f\tm/s^2", (double)a_report.y); - warnx("accel z: \t% 9.5f\tm/s^2", (double)a_report.z); +// warnx("accel x: \t% 9.5f\tm/s^2", (double)a_report.x); +// warnx("accel y: \t% 9.5f\tm/s^2", (double)a_report.y); +// warnx("accel z: \t% 9.5f\tm/s^2", (double)a_report.z); warnx("accel x: \t%d\traw", (int)a_report.x_raw); warnx("accel y: \t%d\traw", (int)a_report.y_raw); warnx("accel z: \t%d\traw", (int)a_report.z_raw); - warnx("accel range: %8.4f m/s^2", (double)a_report.range_m_s2); +// warnx("accel range: %8.4f m/s^2", (double)a_report.range_m_s2); /* XXX add poll-rate tests here too */ From f288c65baa9d75d657bebf5029f557c7c0d8044c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 5 Apr 2013 16:36:42 -0700 Subject: [PATCH 042/486] LSM303D accel and mag working (still WIP) --- apps/drivers/lsm303d/lsm303d.cpp | 564 +++++++++++++++++++++++++++---- 1 file changed, 489 insertions(+), 75 deletions(-) diff --git a/apps/drivers/lsm303d/lsm303d.cpp b/apps/drivers/lsm303d/lsm303d.cpp index b10c39f8f8..49f2f29eeb 100644 --- a/apps/drivers/lsm303d/lsm303d.cpp +++ b/apps/drivers/lsm303d/lsm303d.cpp @@ -98,17 +98,30 @@ static const int ERROR = -1; #define ADDR_OUT_Z_H_A 0x2D #define ADDR_CTRL_REG1 0x20 +#define ADDR_CTRL_REG5 0x24 +#define ADDR_CTRL_REG7 0x26 #define REG1_RATE_50HZ_A ((0<<7) | (1<<6) | (0<<5) | (1<<4)) #define REG1_RATE_100HZ_A ((0<<7) | (1<<6) | (1<<5) | (0<<4)) #define REG1_RATE_200HZ_A ((0<<7) | (1<<6) | (1<<5) | (1<<4)) #define REG1_RATE_400HZ_A ((1<<7) | (0<<6) | (0<<5) | (0<<4)) +#define REG1_RATE_800HZ_A ((1<<7) | (0<<6) | (0<<5) | (1<<4)) +#define REG1_RATE_1600HZ_A ((1<<7) | (0<<6) | (1<<5) | (0<<4)) #define REG1_CONT_UPDATE_A (0<<3) #define REG1_Z_ENABLE_A (1<<2) #define REG1_Y_ENABLE_A (1<<1) #define REG1_X_ENABLE_A (1<<0) +#define REG5_TEMP_ENABLE (1<<7) +#define REG5_M_RES_HIGH ((1<<6) | (1<<5)) + +#define REG5_RATE_50HZ_M ((1<<4) | (0<<3) | (0<<2)) +#define REG5_RATE_100HZ_M ((1<<4) | (0<<3) | (1<<2)) + + +#define REG7_CONTINUOUS_MODE_M ((0<<1) | (0<<0)) + #define ADDR_WHO_AM_I 0x0F #define WHO_I_AM 0x49 @@ -117,6 +130,8 @@ static const int ERROR = -1; extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); } +class LSM303D_mag; + class LSM303D : public device::SPI { public: @@ -136,25 +151,49 @@ public: protected: virtual int probe(); + friend class LSM303D_mag; + + virtual ssize_t mag_read(struct file *filp, char *buffer, size_t buflen); + virtual int mag_ioctl(struct file *filp, int cmd, unsigned long arg); + private: - struct hrt_call _call; - unsigned _call_interval; + LSM303D_mag *_mag; - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; - struct accel_report *_reports; + struct hrt_call _accel_call; + struct hrt_call _mag_call; + + unsigned _call_accel_interval; + unsigned _call_mag_interval; + + unsigned _num_accel_reports; + volatile unsigned _next_accel_report; + volatile unsigned _oldest_accel_report; + struct accel_report *_accel_reports; struct accel_scale _accel_scale; float _accel_range_scale; float _accel_range_m_s2; orb_advert_t _accel_topic; - unsigned _current_rate; - unsigned _current_range; + unsigned _num_mag_reports; + volatile unsigned _next_mag_report; + volatile unsigned _oldest_mag_report; + struct mag_report *_mag_reports; - perf_counter_t _sample_perf; + struct mag_scale _mag_scale; + float _mag_range_scale; + float _mag_range_ga; + orb_advert_t _mag_topic; + + unsigned _current_accel_rate; + unsigned _current_accel_range; + + unsigned _current_mag_rate; + unsigned _current_mag_range; + + perf_counter_t _accel_sample_perf; + perf_counter_t _mag_sample_perf; /** * Start automatic measurement. @@ -177,11 +216,15 @@ private: */ static void measure_trampoline(void *arg); + static void mag_measure_trampoline(void *arg); + /** * Fetch measurements from the sensor and update the report ring. */ void measure(); + void mag_measure(); + /** * Read a register from the LSM303D * @@ -230,27 +273,66 @@ private: int set_samplerate(unsigned frequency); }; +/** + * Helper class implementing the mag driver node. + */ +class LSM303D_mag : public device::CDev +{ +public: + LSM303D_mag(LSM303D *parent); + ~LSM303D_mag(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + +protected: + friend class LSM303D; + + void parent_poll_notify(); +private: + LSM303D *_parent; + + void measure(); + + void measure_trampoline(void *arg); +}; + + /* helper macro for handling report buffer indices */ #define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 8000000), - _call_interval(0), - _num_reports(0), - _next_report(0), - _oldest_report(0), - _reports(nullptr), + _mag(new LSM303D_mag(this)), + _call_accel_interval(0), + _call_mag_interval(0), + _num_accel_reports(0), + _next_accel_report(0), + _oldest_accel_report(0), + _accel_reports(nullptr), _accel_range_scale(0.0f), _accel_range_m_s2(0.0f), _accel_topic(-1), - _current_rate(0), - _current_range(0), - _sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_read")) + _num_mag_reports(0), + _next_mag_report(0), + _oldest_mag_report(0), + _mag_reports(nullptr), + _mag_range_scale(0.0f), + _mag_range_ga(0.0f), + _current_accel_rate(0), + _current_accel_range(0), + _current_mag_rate(0), + _current_mag_range(0), + _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), + _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")) { // enable debug() calls _debug_enabled = true; + _accel_range_scale = 1.0f; + _mag_range_scale = 1.0f; + // default scale factors _accel_scale.x_offset = 0; _accel_scale.x_scale = 1.0f; @@ -258,6 +340,13 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _accel_scale.y_scale = 1.0f; _accel_scale.z_offset = 0; _accel_scale.z_scale = 1.0f; + + _mag_scale.x_offset = 0; + _mag_scale.x_scale = 1.0f; + _mag_scale.y_offset = 0; + _mag_scale.y_scale = 1.0f; + _mag_scale.z_offset = 0; + _mag_scale.z_scale = 1.0f; } LSM303D::~LSM303D() @@ -266,36 +355,57 @@ LSM303D::~LSM303D() stop(); /* free any existing reports */ - if (_reports != nullptr) - delete[] _reports; + if (_accel_reports != nullptr) + delete[] _accel_reports; + if (_mag_reports != nullptr) + delete[] _mag_reports; + + delete _mag; /* delete the perf counter */ - perf_free(_sample_perf); + perf_free(_accel_sample_perf); + perf_free(_mag_sample_perf); } int LSM303D::init() { int ret = ERROR; + int mag_ret; + int fd_mag; /* do SPI init (and probe) first */ if (SPI::init() != OK) goto out; /* allocate basic report buffers */ - _num_reports = 2; - _oldest_report = _next_report = 0; - _reports = new struct accel_report[_num_reports]; + _num_accel_reports = 2; + _oldest_accel_report = _next_accel_report = 0; + _accel_reports = new struct accel_report[_num_accel_reports]; - if (_reports == nullptr) + if (_accel_reports == nullptr) goto out; - /* advertise sensor topic */ - memset(&_reports[0], 0, sizeof(_reports[0])); - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &_reports[0]); + /* advertise accel topic */ + memset(&_accel_reports[0], 0, sizeof(_accel_reports[0])); + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &_accel_reports[0]); + + _num_mag_reports = 2; + _oldest_mag_report = _next_mag_report = 0; + _mag_reports = new struct mag_report[_num_mag_reports]; + + if (_mag_reports == nullptr) + goto out; + + /* advertise mag topic */ + memset(&_mag_reports[0], 0, sizeof(_mag_reports[0])); + _mag_topic = orb_advertise(ORB_ID(sensor_mag), &_mag_reports[0]); + + /* set default configuration */ + write_reg(ADDR_CTRL_REG1, REG1_RATE_400HZ_A | REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A); + write_reg(ADDR_CTRL_REG7, REG7_CONTINUOUS_MODE_M); + write_reg(ADDR_CTRL_REG5, REG5_TEMP_ENABLE | REG5_RATE_50HZ_M | REG5_M_RES_HIGH); -// /* set default configuration */ - write_reg(ADDR_CTRL_REG1, REG1_RATE_100HZ_A | REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A); // write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ // write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */ // write_reg(ADDR_CTRL_REG4, REG4_BDU); @@ -307,6 +417,15 @@ LSM303D::init() set_range(500); /* default to 500dps */ set_samplerate(0); /* max sample rate */ +// _current_accel_rate = 100; + + /* do CDev init for the gyro device node, keep it optional */ + mag_ret = _mag->init(); + + if (mag_ret != OK) { + _mag_topic = -1; + } + ret = OK; out: return ret; @@ -336,7 +455,7 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen) return -ENOSPC; /* if automatic measurement is enabled */ - if (_call_interval > 0) { + if (_call_accel_interval > 0) { /* * While there is space in the caller's buffer, and reports, copy them. @@ -344,10 +463,10 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen) * we are careful to avoid racing with it. */ while (count--) { - if (_oldest_report != _next_report) { - memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); - ret += sizeof(_reports[0]); - INCREMENT(_oldest_report, _num_reports); + if (_oldest_accel_report != _next_accel_report) { + memcpy(buffer, _accel_reports + _oldest_accel_report, sizeof(*_accel_reports)); + ret += sizeof(_accel_reports[0]); + INCREMENT(_oldest_accel_report, _num_accel_reports); } } @@ -356,12 +475,53 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen) } /* manual measurement */ - _oldest_report = _next_report = 0; + _oldest_accel_report = _next_accel_report = 0; measure(); /* measurement will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); + memcpy(buffer, _accel_reports, sizeof(*_accel_reports)); + ret = sizeof(*_accel_reports); + + return ret; +} + +ssize_t +LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct mag_report); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + /* if automatic measurement is enabled */ + if (_call_mag_interval > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the measurement code while we are doing this; + * we are careful to avoid racing with it. + */ + while (count--) { + if (_oldest_mag_report != _next_mag_report) { + memcpy(buffer, _mag_reports + _oldest_mag_report, sizeof(*_mag_reports)); + ret += sizeof(_mag_reports[0]); + INCREMENT(_oldest_mag_report, _num_mag_reports); + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement */ + _oldest_mag_report = _next_mag_report = 0; + measure(); + + /* measurement will have generated a report, copy it out */ + memcpy(buffer, _mag_reports, sizeof(*_mag_reports)); + ret = sizeof(*_mag_reports); return ret; } @@ -377,7 +537,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); - _call_interval = 0; + _call_accel_interval = 0; return OK; /* external signalling not supported */ @@ -396,7 +556,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ - bool want_start = (_call_interval == 0); + bool want_start = (_call_accel_interval == 0); /* convert hz to hrt interval via microseconds */ unsigned ticks = 1000000 / arg; @@ -407,7 +567,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ - _call.period = _call_interval = ticks; + _accel_call.period = _call_accel_interval = ticks; /* if we need to start the poll state machine, do it */ if (want_start) @@ -419,10 +579,10 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_call_interval == 0) + if (_call_accel_interval == 0) return SENSOR_POLLRATE_MANUAL; - return 1000000 / _call_interval; + return 1000000 / _call_accel_interval; case SENSORIOCSQUEUEDEPTH: { /* account for sentinel in the ring */ @@ -440,16 +600,16 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) /* reset the measurement state machine with the new buffer, free the old */ stop(); - delete[] _reports; - _num_reports = arg; - _reports = buf; + delete[] _accel_reports; + _num_accel_reports = arg; + _accel_reports = buf; start(); return OK; } case SENSORIOCGQUEUEDEPTH: - return _num_reports - 1; + return _num_accel_reports - 1; case SENSORIOCRESET: /* XXX implement */ @@ -461,6 +621,110 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) } } +int +LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _call_mag_interval = 0; + return OK; + + /* external signalling not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: + /* 50 Hz is max for mag */ + return mag_ioctl(filp, SENSORIOCSPOLLRATE, 50); + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_call_mag_interval == 0); + + /* convert hz to hrt interval via microseconds */ + unsigned ticks = 1000000 / arg; + + /* check against maximum sane rate */ + if (ticks < 1000) + return -EINVAL; + + /* update interval for next measurement */ + /* XXX this is a bit shady, but no other way to adjust... */ + _mag_call.period = _call_mag_interval = ticks; + + + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_call_mag_interval == 0) + return SENSOR_POLLRATE_MANUAL; + + return 1000000 / _call_mag_interval; + case SENSORIOCSQUEUEDEPTH: + case SENSORIOCGQUEUEDEPTH: + case SENSORIOCRESET: + return ioctl(filp, cmd, arg); + + case MAGIOCSSAMPLERATE: +// case MAGIOCGSAMPLERATE: + /* XXX not implemented */ + return -EINVAL; + + case MAGIOCSLOWPASS: +// case MAGIOCGLOWPASS: + /* XXX not implemented */ +// _set_dlpf_filter((uint16_t)arg); + return -EINVAL; + + case MAGIOCSSCALE: + /* copy scale in */ + memcpy(&_mag_scale, (struct mag_scale *) arg, sizeof(_mag_scale)); + return OK; + + case MAGIOCGSCALE: + /* copy scale out */ + memcpy((struct mag_scale *) arg, &_mag_scale, sizeof(_mag_scale)); + return OK; + + case MAGIOCSRANGE: +// case MAGIOCGRANGE: + /* XXX not implemented */ + // XXX change these two values on set: + // _mag_range_scale = xx + // _mag_range_ga = xx + return -EINVAL; + + case MAGIOCSELFTEST: + /* XXX not implemented */ +// return self_test(); + return -EINVAL; + + default: + /* give it to the superclass */ + return SPI::ioctl(filp, cmd, arg); + } +} + uint8_t LSM303D::read_reg(unsigned reg) { @@ -529,6 +793,8 @@ LSM303D::set_range(unsigned max_dps) int LSM303D::set_samplerate(unsigned frequency) { + _current_accel_rate = 100; + // uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE; // // if (frequency == 0) @@ -566,16 +832,19 @@ LSM303D::start() stop(); /* reset the report ring */ - _oldest_report = _next_report = 0; + _oldest_accel_report = _next_accel_report = 0; + _oldest_mag_report = _next_mag_report = 0; /* start polling at the specified rate */ - hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&LSM303D::measure_trampoline, this); + hrt_call_every(&_accel_call, 1000, _call_accel_interval, (hrt_callout)&LSM303D::measure_trampoline, this); + hrt_call_every(&_mag_call, 1000, _call_mag_interval, (hrt_callout)&LSM303D::mag_measure_trampoline, this); } void LSM303D::stop() { - hrt_cancel(&_call); + hrt_cancel(&_accel_call); + hrt_cancel(&_mag_call); } void @@ -587,6 +856,15 @@ LSM303D::measure_trampoline(void *arg) dev->measure(); } +void +LSM303D::mag_measure_trampoline(void *arg) +{ + LSM303D *dev = (LSM303D *)arg; + + /* make another measurement */ + dev->mag_measure(); +} + void LSM303D::measure() { @@ -609,17 +887,17 @@ LSM303D::measure() int16_t x; int16_t y; int16_t z; - } raw_report_accel; + } raw_accel_report; #pragma pack(pop) - accel_report *report = &_reports[_next_report]; + accel_report *accel_report = &_accel_reports[_next_accel_report]; /* start the performance counter */ - perf_begin(_sample_perf); + perf_begin(_accel_sample_perf); /* fetch data from the sensor */ - raw_report_accel.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT; - transfer((uint8_t *)&raw_report_accel, (uint8_t *)&raw_report_accel, sizeof(raw_report_accel)); + raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT; + transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report)); /* * 1) Scale raw value to SI units using scaling from datasheet. @@ -637,42 +915,151 @@ LSM303D::measure() */ - report->timestamp = hrt_absolute_time(); + accel_report->timestamp = hrt_absolute_time(); /* XXX adjust for sensor alignment to board here */ - report->x_raw = raw_report_accel.x; - report->y_raw = raw_report_accel.y; - report->z_raw = raw_report_accel.z; + accel_report->x_raw = raw_accel_report.x; + accel_report->y_raw = raw_accel_report.y; + accel_report->z_raw = raw_accel_report.z; - -// report->x = ((report->x_raw * _gyro_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; -// report->y = ((report->y_raw * _gyro_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; -// report->z = ((report->z_raw * _gyro_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + accel_report->x = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + accel_report->y = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + accel_report->z = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; // report->scaling = _gyro_range_scale; // report->range_rad_s = _gyro_range_rad_s; /* post a report to the ring - note, not locked */ - INCREMENT(_next_report, _num_reports); + INCREMENT(_next_accel_report, _num_accel_reports); /* if we are running up against the oldest report, fix it */ - if (_next_report == _oldest_report) - INCREMENT(_oldest_report, _num_reports); + if (_next_accel_report == _oldest_accel_report) + INCREMENT(_oldest_accel_report, _num_accel_reports); /* notify anyone waiting for data */ poll_notify(POLLIN); /* publish for subscribers */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, report); + orb_publish(ORB_ID(sensor_accel), _accel_topic, accel_report); /* stop the perf counter */ - perf_end(_sample_perf); + perf_end(_accel_sample_perf); +} + +void +LSM303D::mag_measure() +{ + /* status register and data as read back from the device */ +#pragma pack(push, 1) + struct { + uint8_t cmd; + uint8_t status; + int16_t x; + int16_t y; + int16_t z; + } raw_mag_report; +#pragma pack(pop) + + mag_report *mag_report = &_mag_reports[_next_mag_report]; + + /* start the performance counter */ + perf_begin(_mag_sample_perf); + + /* fetch data from the sensor */ + raw_mag_report.cmd = ADDR_STATUS_M | DIR_READ | ADDR_INCREMENT; + transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report)); + + /* + * 1) Scale raw value to SI units using scaling from datasheet. + * 2) Subtract static offset (in SI units) + * 3) Scale the statically calibrated values with a linear + * dynamically obtained factor + * + * Note: the static sensor offset is the number the sensor outputs + * at a nominally 'zero' input. Therefore the offset has to + * be subtracted. + * + * Example: A gyro outputs a value of 74 at zero angular rate + * the offset is 74 from the origin and subtracting + * 74 from all measurements centers them around zero. + */ + + + mag_report->timestamp = hrt_absolute_time(); + /* XXX adjust for sensor alignment to board here */ + mag_report->x_raw = raw_mag_report.x; + mag_report->y_raw = raw_mag_report.y; + mag_report->z_raw = raw_mag_report.z; + mag_report->x = ((mag_report->x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; + mag_report->y = ((mag_report->y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; + mag_report->z = ((mag_report->z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; +// report->scaling = _gyro_range_scale; +// report->range_rad_s = _gyro_range_rad_s; + + /* post a report to the ring - note, not locked */ + INCREMENT(_next_mag_report, _num_mag_reports); + + /* if we are running up against the oldest report, fix it */ + if (_next_mag_report == _oldest_mag_report) + INCREMENT(_oldest_mag_report, _num_mag_reports); + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + /* publish for subscribers */ + orb_publish(ORB_ID(sensor_mag), _mag_topic, mag_report); + + /* stop the perf counter */ + perf_end(_mag_sample_perf); } void LSM303D::print_info() { - perf_print_counter(_sample_perf); + perf_print_counter(_accel_sample_perf); printf("report queue: %u (%u/%u @ %p)\n", - _num_reports, _oldest_report, _next_report, _reports); + _num_accel_reports, _oldest_accel_report, _next_accel_report, _accel_reports); + perf_print_counter(_mag_sample_perf); + printf("report queue: %u (%u/%u @ %p)\n", + _num_mag_reports, _oldest_mag_report, _next_mag_report, _mag_reports); +} + +LSM303D_mag::LSM303D_mag(LSM303D *parent) : + CDev("LSM303D_mag", MAG_DEVICE_PATH), + _parent(parent) +{ +} + +LSM303D_mag::~LSM303D_mag() +{ +} + +void +LSM303D_mag::parent_poll_notify() +{ + poll_notify(POLLIN); +} + +ssize_t +LSM303D_mag::read(struct file *filp, char *buffer, size_t buflen) +{ + return _parent->mag_read(filp, buffer, buflen); +} + +int +LSM303D_mag::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + return _parent->mag_ioctl(filp, cmd, arg); +} + +void +LSM303D_mag::measure() +{ + _parent->mag_measure(); +} + +void +LSM303D_mag::measure_trampoline(void *arg) +{ + _parent->mag_measure_trampoline(arg); } /** @@ -694,7 +1081,7 @@ void info(); void start() { - int fd; + int fd, fd_mag; if (g_dev != nullptr) errx(1, "already started"); @@ -717,6 +1104,16 @@ start() if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) goto fail; + fd_mag = open(MAG_DEVICE_PATH, O_RDONLY); + + /* don't fail if open cannot be opened */ + if (0 <= fd_mag) { + if (ioctl(fd_mag, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + goto fail; + } + } + + exit(0); fail: @@ -746,10 +1143,6 @@ test() if (fd_accel < 0) err(1, "%s open failed", ACCEL_DEVICE_PATH); - /* reset to manual polling */ -// if (ioctl(fd_accel, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) -// err(1, "reset to manual polling"); - /* do a simple demand read */ sz = read(fd_accel, &a_report, sizeof(a_report)); @@ -764,9 +1157,30 @@ test() warnx("accel z: \t%d\traw", (int)a_report.z_raw); // warnx("accel range: %8.4f m/s^2", (double)a_report.range_m_s2); + + + int fd_mag = -1; + struct mag_report m_report; + + /* get the driver */ + fd_mag = open(MAG_DEVICE_PATH, O_RDONLY); + + if (fd_mag < 0) + err(1, "%s open failed", MAG_DEVICE_PATH); + + /* do a simple demand read */ + sz = read(fd_mag, &m_report, sizeof(m_report)); + + if (sz != sizeof(m_report)) + err(1, "immediate read failed"); + + warnx("mag x: \t%d\traw", (int)m_report.x_raw); + warnx("mag y: \t%d\traw", (int)m_report.y_raw); + warnx("mag z: \t%d\traw", (int)m_report.z_raw); + /* XXX add poll-rate tests here too */ - reset(); +// reset(); errx(0, "PASS"); } From d1d4d1d1e2f29ef540caec51d1a6b1244437d756 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 5 Apr 2013 17:31:53 -0700 Subject: [PATCH 043/486] Some defines and comments (still WIP) --- apps/drivers/lsm303d/lsm303d.cpp | 114 ++++++++++++++++++++----------- 1 file changed, 75 insertions(+), 39 deletions(-) diff --git a/apps/drivers/lsm303d/lsm303d.cpp b/apps/drivers/lsm303d/lsm303d.cpp index 49f2f29eeb..32030a1f77 100644 --- a/apps/drivers/lsm303d/lsm303d.cpp +++ b/apps/drivers/lsm303d/lsm303d.cpp @@ -33,7 +33,7 @@ /** * @file lsm303d.cpp - * Driver for the ST LSM303D MEMS accel / mag connected via SPI. + * Driver for the ST LSM303D MEMS accelerometer / magnetometer connected via SPI. */ #include @@ -75,11 +75,16 @@ static const int ERROR = -1; /* SPI protocol address bits */ #define DIR_READ (1<<7) #define DIR_WRITE (0<<7) -#define ADDR_INCREMENT (1<<6) +#define ADDR_INCREMENT (1<<6) -/* register addresses */ -#define ADDR_TEMP_OUT_L 0x05 -#define ADDR_TEMP_OUT_H 0x06 + + +/* register addresses: A: accel, M: mag, T: temp */ +#define ADDR_WHO_AM_I 0x0F +#define WHO_I_AM 0x49 + +#define ADDR_OUT_L_T 0x05 +#define ADDR_OUT_H_T 0x06 #define ADDR_STATUS_M 0x07 #define ADDR_OUT_X_L_M 0x08 #define ADDR_OUT_X_H_M 0x09 @@ -97,39 +102,70 @@ static const int ERROR = -1; #define ADDR_OUT_Z_L_A 0x2C #define ADDR_OUT_Z_H_A 0x2D +#define ADDR_CTRL_REG0 0x1F #define ADDR_CTRL_REG1 0x20 +#define ADDR_CTRL_REG2 0x21 +#define ADDR_CTRL_REG3 0x22 +#define ADDR_CTRL_REG4 0x23 #define ADDR_CTRL_REG5 0x24 #define ADDR_CTRL_REG7 0x26 -#define REG1_RATE_50HZ_A ((0<<7) | (1<<6) | (0<<5) | (1<<4)) -#define REG1_RATE_100HZ_A ((0<<7) | (1<<6) | (1<<5) | (0<<4)) -#define REG1_RATE_200HZ_A ((0<<7) | (1<<6) | (1<<5) | (1<<4)) -#define REG1_RATE_400HZ_A ((1<<7) | (0<<6) | (0<<5) | (0<<4)) -#define REG1_RATE_800HZ_A ((1<<7) | (0<<6) | (0<<5) | (1<<4)) -#define REG1_RATE_1600HZ_A ((1<<7) | (0<<6) | (1<<5) | (0<<4)) +#define REG1_POWERDOWN_A ((0<<7) | (0<<6) | (0<<5) | (0<<4)) +#define REG1_RATE_3_125HZ_A ((0<<7) | (0<<6) | (0<<5) | (1<<4)) +#define REG1_RATE_6_25HZ_A ((0<<7) | (0<<6) | (1<<5) | (0<<4)) +#define REG1_RATE_12_5HZ_A ((0<<7) | (0<<6) | (1<<5) | (1<<4)) +#define REG1_RATE_25HZ_A ((0<<7) | (1<<6) | (0<<5) | (0<<4)) +#define REG1_RATE_50HZ_A ((0<<7) | (1<<6) | (0<<5) | (1<<4)) +#define REG1_RATE_100HZ_A ((0<<7) | (1<<6) | (1<<5) | (0<<4)) +#define REG1_RATE_200HZ_A ((0<<7) | (1<<6) | (1<<5) | (1<<4)) +#define REG1_RATE_400HZ_A ((1<<7) | (0<<6) | (0<<5) | (0<<4)) +#define REG1_RATE_800HZ_A ((1<<7) | (0<<6) | (0<<5) | (1<<4)) +#define REG1_RATE_1600HZ_A ((1<<7) | (0<<6) | (1<<5) | (0<<4)) #define REG1_CONT_UPDATE_A (0<<3) #define REG1_Z_ENABLE_A (1<<2) #define REG1_Y_ENABLE_A (1<<1) #define REG1_X_ENABLE_A (1<<0) -#define REG5_TEMP_ENABLE (1<<7) -#define REG5_M_RES_HIGH ((1<<6) | (1<<5)) +#define REG2_AA_FILTER_BW_773HZ_A ((0<<7) | (0<<6)) +#define REG2_AA_FILTER_BW_194HZ_A ((0<<7) | (1<<6)) +#define REG2_AA_FILTER_BW_362HZ_A ((1<<7) | (0<<6)) +#define REG2_AA_FILTER_BW_50HZ_A ((1<<7) | (1<<6)) +#define REG2_FULL_SCALE_2G_A ((0<<5) | (0<<4) | (0<<3)) +#define REG2_FULL_SCALE_4G_A ((0<<5) | (0<<4) | (1<<3)) +#define REG2_FULL_SCALE_6G_A ((0<<5) | (1<<4) | (0<<3)) +#define REG2_FULL_SCALE_8G_A ((0<<5) | (1<<4) | (1<<3)) +#define REG2_FULL_SCALE_16G_A ((1<<5) | (0<<4) | (0<<3)) + +#define REG5_ENABLE_T (1<<7) + +#define REG5_RES_HIGH_M ((1<<6) | (1<<5)) +#define REG5_RES_LOW_M ((0<<6) | (0<<5)) + +#define REG5_RATE_3_125HZ_M ((0<<4) | (0<<3) | (0<<2)) +#define REG5_RATE_6_25HZ_M ((0<<4) | (0<<3) | (1<<2)) +#define REG5_RATE_12_5HZ_M ((0<<4) | (1<<3) | (0<<2)) +#define REG5_RATE_25HZ_M ((0<<4) | (1<<3) | (1<<2)) #define REG5_RATE_50HZ_M ((1<<4) | (0<<3) | (0<<2)) #define REG5_RATE_100HZ_M ((1<<4) | (0<<3) | (1<<2)) +#define REG5_RATE_DO_NOT_USE_M ((1<<4) | (1<<3) | (0<<2)) +#define REG6_FULL_SCALE_2GA_M ((0<<7) | (0<<6)) +#define REG6_FULL_SCALE_4GA_M ((0<<7) | (1<<6)) +#define REG6_FULL_SCALE_8GA_M ((1<<7) | (0<<6)) +#define REG6_FULL_SCALE_12GA_M ((1<<7) | (1<<6)) -#define REG7_CONTINUOUS_MODE_M ((0<<1) | (0<<0)) +#define REG7_CONT_MODE_M ((0<<1) | (0<<0)) -#define ADDR_WHO_AM_I 0x0F -#define WHO_I_AM 0x49 #define INT_CTRL_M 0x12 #define INT_SRC_M 0x13 + extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); } + class LSM303D_mag; class LSM303D : public device::SPI @@ -216,13 +252,21 @@ private: */ static void measure_trampoline(void *arg); + /** + * Static trampoline for the mag because it runs at a lower rate + * + * @param arg Instance pointer for the driver that is polling. + */ static void mag_measure_trampoline(void *arg); /** - * Fetch measurements from the sensor and update the report ring. + * Fetch accel measurements from the sensor and update the report ring. */ void measure(); + /** + * Fetch mag measurements from the sensor and update the report ring. + */ void mag_measure(); /** @@ -330,6 +374,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : // enable debug() calls _debug_enabled = true; + /* XXX fix this default values */ _accel_range_scale = 1.0f; _mag_range_scale = 1.0f; @@ -401,25 +446,21 @@ LSM303D::init() memset(&_mag_reports[0], 0, sizeof(_mag_reports[0])); _mag_topic = orb_advertise(ORB_ID(sensor_mag), &_mag_reports[0]); + /* XXX do this with ioctls */ /* set default configuration */ write_reg(ADDR_CTRL_REG1, REG1_RATE_400HZ_A | REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A); - write_reg(ADDR_CTRL_REG7, REG7_CONTINUOUS_MODE_M); - write_reg(ADDR_CTRL_REG5, REG5_TEMP_ENABLE | REG5_RATE_50HZ_M | REG5_M_RES_HIGH); + write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); + write_reg(ADDR_CTRL_REG5, REG5_RATE_100HZ_M | REG5_RES_HIGH_M); -// write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ -// write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */ -// write_reg(ADDR_CTRL_REG4, REG4_BDU); -// write_reg(ADDR_CTRL_REG5, 0); -// -// write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */ -// write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_STREAM_MODE); /* Enable FIFO, old data is overwritten */ + /* XXX should we enable FIFO */ set_range(500); /* default to 500dps */ set_samplerate(0); /* max sample rate */ // _current_accel_rate = 100; - /* do CDev init for the gyro device node, keep it optional */ + /* XXX test this when another mag is used */ + /* do CDev init for the mag device node, keep it optional */ mag_ret = _mag->init(); if (mag_ret != OK) { @@ -762,6 +803,7 @@ LSM303D::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) int LSM303D::set_range(unsigned max_dps) { + /* XXX implement this */ // uint8_t bits = REG4_BDU; // // if (max_dps == 0) @@ -793,8 +835,7 @@ LSM303D::set_range(unsigned max_dps) int LSM303D::set_samplerate(unsigned frequency) { - _current_accel_rate = 100; - + /* XXX implement this */ // uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE; // // if (frequency == 0) @@ -869,16 +910,6 @@ void LSM303D::measure() { /* status register and data as read back from the device */ -//#pragma pack(push, 1) -// struct { -// uint8_t cmd; -// uint16_t temp; -// uint8_t status; -// int16_t x; -// int16_t y; -// int16_t z; -// } raw_report_mag; -//#pragma pack(pop) #pragma pack(push, 1) struct { @@ -899,6 +930,7 @@ LSM303D::measure() raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT; transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report)); + /* XXX adapt the comment to specs */ /* * 1) Scale raw value to SI units using scaling from datasheet. * 2) Subtract static offset (in SI units) @@ -967,6 +999,7 @@ LSM303D::mag_measure() raw_mag_report.cmd = ADDR_STATUS_M | DIR_READ | ADDR_INCREMENT; transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report)); + /* XXX adapt the comment to specs */ /* * 1) Scale raw value to SI units using scaling from datasheet. * 2) Subtract static offset (in SI units) @@ -1001,6 +1034,7 @@ LSM303D::mag_measure() if (_next_mag_report == _oldest_mag_report) INCREMENT(_oldest_mag_report, _num_mag_reports); + /* XXX please check this poll_notify, is it the right one? */ /* notify anyone waiting for data */ poll_notify(POLLIN); @@ -1149,6 +1183,7 @@ test() if (sz != sizeof(a_report)) err(1, "immediate read failed"); + /* XXX fix the test output */ // warnx("accel x: \t% 9.5f\tm/s^2", (double)a_report.x); // warnx("accel y: \t% 9.5f\tm/s^2", (double)a_report.y); // warnx("accel z: \t% 9.5f\tm/s^2", (double)a_report.z); @@ -1174,6 +1209,7 @@ test() if (sz != sizeof(m_report)) err(1, "immediate read failed"); + /* XXX fix the test output */ warnx("mag x: \t%d\traw", (int)m_report.x_raw); warnx("mag y: \t%d\traw", (int)m_report.y_raw); warnx("mag z: \t%d\traw", (int)m_report.z_raw); From c25248f1af8d6c4d12b3d7d0f9d42e58e28a6c22 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Apr 2013 12:00:51 +0200 Subject: [PATCH 044/486] Fixed RGB led warnings and error handling --- src/device/rgbled/rgbled.cpp | 21 ++++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/src/device/rgbled/rgbled.cpp b/src/device/rgbled/rgbled.cpp index c3b92ba7eb..a0db30f480 100644 --- a/src/device/rgbled/rgbled.cpp +++ b/src/device/rgbled/rgbled.cpp @@ -219,9 +219,13 @@ RGBLED::info() ret = get(on, not_powersave, r, g, b); - /* we don't care about power-save mode */ - log("State: %s", on ? "ON" : "OFF"); - log("Red: %d, Green: %d, Blue: %d", r, g, b); + if (ret == OK) { + /* we don't care about power-save mode */ + log("state: %s", on ? "ON" : "OFF"); + log("red: %u, green: %u, blue: %u", (unsigned)r, (unsigned)g, (unsigned)b); + } else { + warnx("failed to read led"); + } return ret; } @@ -394,6 +398,7 @@ RGBLED::get(bool &on, bool ¬_powersave, uint8_t &r, uint8_t &g, uint8_t &b) if (ret == OK) { on = result[0] & SETTING_ENABLE; not_powersave = result[0] & SETTING_NOT_POWERSAVE; + /* XXX check, looks wrong */ r = (result[0] & 0x0f)*255/15; g = (result[1] & 0xf0)*255/15; b = (result[1] & 0x0f)*255/15; @@ -402,12 +407,14 @@ RGBLED::get(bool &on, bool ¬_powersave, uint8_t &r, uint8_t &g, uint8_t &b) return ret; } +void rgbled_usage(); + void rgbled_usage() { - fprintf(stderr, "missing command: try 'start', 'systemstate', 'test', 'info', 'off'\n"); - fprintf(stderr, "options:\n"); - fprintf(stderr, "\t-b --bus i2cbus (3)\n"); - fprintf(stderr, "\t-a --ddr addr (9)\n"); + warnx("missing command: try 'start', 'systemstate', 'test', 'info', 'off'"); + warnx("options:"); + warnx("\t-b --bus i2cbus (3)"); + warnx("\t-a --ddr addr (9)"); } int From e2c30d7c1de93be33d700aa0dc2ffba23df33cdd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Apr 2013 12:12:39 +0200 Subject: [PATCH 045/486] Added include dir for RGB led --- src/device/rgbled/rgbled.cpp | 4 +-- src/include/device/rgbled.h | 67 ++++++++++++++++++++++++++++++++++++ 2 files changed, 69 insertions(+), 2 deletions(-) create mode 100644 src/include/device/rgbled.h diff --git a/src/device/rgbled/rgbled.cpp b/src/device/rgbled/rgbled.cpp index a0db30f480..923e0a14f9 100644 --- a/src/device/rgbled/rgbled.cpp +++ b/src/device/rgbled/rgbled.cpp @@ -60,11 +60,11 @@ #include #include +#include "device/rgbled.h" + #define LED_ONTIME 120 #define LED_OFFTIME 120 -#define RGBLED_DEVICE_PATH "/dev/rgbled" - #define ADDR 0x55 /**< I2C adress of TCA62724FMG */ #define SUB_ADDR_START 0x01 /**< write everything (with auto-increment) */ #define SUB_ADDR_PWM0 0x81 /**< blue (without auto-increment) */ diff --git a/src/include/device/rgbled.h b/src/include/device/rgbled.h new file mode 100644 index 0000000000..a18920892b --- /dev/null +++ b/src/include/device/rgbled.h @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file rgbled.h + * + * RGB led device API + */ + +#pragma once + +#include +#include + +/* more devices will be 1, 2, etc */ +#define RGBLED_DEVICE_PATH "/dev/rgbled0" + +/* + * ioctl() definitions + */ + +#define _RGBLEDIOCBASE (0x2900) +#define _RGBLEDIOC(_n) (_IOC(_RGBLEDIOCBASE, _n)) + +/** play the named script in *(char *)arg, repeating forever */ +#define RGBLED_PLAY_SCRIPT_NAMED _RGBLEDIOC(1) + +/** play the numbered script in (arg), repeating forever */ +#define RGBLED_PLAY_SCRIPT _RGBLEDIOC(2) + +/** + * Set the user script; (arg) is a pointer to an array of script lines, + * where each line is an array of four bytes giving , , arg[0-2] + * + * The script is terminated by a zero command. + */ +#define RGBLED_SET_USER_SCRIPT _RGBLEDIOC(3) From dc11bcfb425b8bbe2860f585e296f53e08f619ab Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Apr 2013 12:56:22 +0200 Subject: [PATCH 046/486] Disabled full JTAG port to free PA15 for tone alarm --- nuttx/configs/px4fmuv2/nsh/defconfig | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx/configs/px4fmuv2/nsh/defconfig b/nuttx/configs/px4fmuv2/nsh/defconfig index 8e9feef65d..beeb475517 100755 --- a/nuttx/configs/px4fmuv2/nsh/defconfig +++ b/nuttx/configs/px4fmuv2/nsh/defconfig @@ -107,9 +107,9 @@ CONFIG_STM32_STM32F427=y # CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled # CONFIG_STM32_DFU=n -CONFIG_STM32_JTAG_FULL_ENABLE=y +CONFIG_STM32_JTAG_FULL_ENABLE=n CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n -CONFIG_STM32_JTAG_SW_ENABLE=n +CONFIG_STM32_JTAG_SW_ENABLE=y # # On-chip CCM SRAM configuration From aa369dfef167f0fd1250350dbf73c8208691fd67 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Apr 2013 10:20:03 -0700 Subject: [PATCH 047/486] Fix command registration for modules. 'rgbled test' works now. --- makefiles/firmware.mk | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index dad57d531e..c34382ae0b 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -298,10 +298,12 @@ endif # BUILTIN_CSRC = $(WORK_DIR)builtin_commands.c -# add command definitions from modules -BUILTIN_COMMAND_FILES := $(wildcard $(WORK_DIR)builtin_commands/COMMAND.*) -BUILTIN_COMMANDS += $(subst COMMAND.,,$(notdir $(BUILTIN_COMMAND_FILES))) +# command definitions from modules (may be empty at Makefile parsing time...) +MODULE_COMMANDS = $(subst COMMAND.,,$(notdir $(wildcard $(WORK_DIR)builtin_commands/COMMAND.*))) +# We must have at least one pre-defined builtin command in order to generate +# any of this. +# ifneq ($(BUILTIN_COMMANDS),) # (BUILTIN_PROTO,,) @@ -315,17 +317,19 @@ define BUILTIN_DEF endef # Don't generate until modules have updated their command files -$(BUILTIN_CSRC): $(GLOBAL_DEPS) $(BUILTIN_COMMAND_FILES) +$(BUILTIN_CSRC): $(GLOBAL_DEPS) $(MODULE_OBJS) $(BUILTIN_COMMAND_FILES) @$(ECHO) %% generating $@ $(Q) $(ECHO) '/* builtin command list - automatically generated, do not edit */' > $@ $(Q) $(ECHO) '#include ' >> $@ $(Q) $(ECHO) '#include ' >> $@ $(Q) $(foreach spec,$(BUILTIN_COMMANDS),$(call BUILTIN_PROTO,$(subst ., ,$(spec)),$@)) + $(Q) $(foreach spec,$(MODULE_COMMANDS),$(call BUILTIN_PROTO,$(subst ., ,$(spec)),$@)) $(Q) $(ECHO) 'const struct builtin_s g_builtins[] = {' >> $@ $(Q) $(foreach spec,$(BUILTIN_COMMANDS),$(call BUILTIN_DEF,$(subst ., ,$(spec)),$@)) + $(Q) $(foreach spec,$(MODULE_COMMANDS),$(call BUILTIN_DEF,$(subst ., ,$(spec)),$@)) $(Q) $(ECHO) ' {NULL, 0, 0, NULL}' >> $@ $(Q) $(ECHO) '};' >> $@ - $(Q) $(ECHO) 'const int g_builtin_count = $(words $(BUILTIN_COMMANDS));' >> $@ + $(Q) $(ECHO) 'const int g_builtin_count = $(words $(BUILTIN_COMMANDS) $(MODULE_COMMANDS));' >> $@ SRCS += $(BUILTIN_CSRC) From e5fa9dbcea5defb49506dc60a35e134b3736bbb6 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Apr 2013 11:16:24 -0700 Subject: [PATCH 048/486] Move the LSM303D driver over into the new world. --- apps/drivers/lsm303d/Makefile | 42 ------------------- makefiles/config_px4fmuv2_default.mk | 1 + .../device}/lsm303d/lsm303d.cpp | 0 src/device/lsm303d/module.mk | 6 +++ 4 files changed, 7 insertions(+), 42 deletions(-) delete mode 100644 apps/drivers/lsm303d/Makefile rename {apps/drivers => src/device}/lsm303d/lsm303d.cpp (100%) create mode 100644 src/device/lsm303d/module.mk diff --git a/apps/drivers/lsm303d/Makefile b/apps/drivers/lsm303d/Makefile deleted file mode 100644 index 542a9bd40a..0000000000 --- a/apps/drivers/lsm303d/Makefile +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (C) 2013 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Makefile to build the LSM303D driver. -# - -APPNAME = lsm303d -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 - -include $(APPDIR)/mk/app.mk diff --git a/makefiles/config_px4fmuv2_default.mk b/makefiles/config_px4fmuv2_default.mk index 0ea0a90479..374c0cfe98 100644 --- a/makefiles/config_px4fmuv2_default.mk +++ b/makefiles/config_px4fmuv2_default.mk @@ -11,6 +11,7 @@ ROMFS_ROOT = $(PX4_BASE)/ROMFS/$(CONFIG) # Board support modules # MODULES += device/rgbled +MODULES += device/lsm303d # # Transitional support - add commands from the NuttX export archive. diff --git a/apps/drivers/lsm303d/lsm303d.cpp b/src/device/lsm303d/lsm303d.cpp similarity index 100% rename from apps/drivers/lsm303d/lsm303d.cpp rename to src/device/lsm303d/lsm303d.cpp diff --git a/src/device/lsm303d/module.mk b/src/device/lsm303d/module.mk new file mode 100644 index 0000000000..e93b1e3318 --- /dev/null +++ b/src/device/lsm303d/module.mk @@ -0,0 +1,6 @@ +# +# LSM303D accel/mag driver +# + +MODULE_COMMAND = lsm303d +SRCS = lsm303d.cpp From aa09ebd7d3cb0d06781470e400e61c75b449985b Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Apr 2013 11:16:54 -0700 Subject: [PATCH 049/486] Share the ROMFS prototype betwen FMUv1 and v2 --- ROMFS/{px4fmu_default => px4fmu_common}/logging/logconv.m | 0 .../{px4fmu_default => px4fmu_common}/mixers/FMU_AERT.mix | 0 ROMFS/{px4fmu_default => px4fmu_common}/mixers/FMU_AET.mix | 0 ROMFS/{px4fmu_default => px4fmu_common}/mixers/FMU_Q.mix | 0 ROMFS/{px4fmu_default => px4fmu_common}/mixers/FMU_RET.mix | 0 ROMFS/{px4fmu_default => px4fmu_common}/mixers/FMU_X5.mix | 0 .../{px4fmu_default => px4fmu_common}/mixers/FMU_delta.mix | 0 .../{px4fmu_default => px4fmu_common}/mixers/FMU_hex_+.mix | 0 .../{px4fmu_default => px4fmu_common}/mixers/FMU_hex_x.mix | 0 .../mixers/FMU_octo_+.mix | 0 .../mixers/FMU_octo_x.mix | 0 .../{px4fmu_default => px4fmu_common}/mixers/FMU_pass.mix | 0 .../mixers/FMU_quad_+.mix | 0 .../mixers/FMU_quad_x.mix | 0 ROMFS/{px4fmu_default => px4fmu_common}/mixers/README | 0 makefiles/config_px4fmu_default.mk | 2 +- makefiles/config_px4fmuv2_default.mk | 2 +- makefiles/firmware.mk | 7 +++++-- makefiles/module.mk | 1 - 19 files changed, 7 insertions(+), 5 deletions(-) rename ROMFS/{px4fmu_default => px4fmu_common}/logging/logconv.m (100%) rename ROMFS/{px4fmu_default => px4fmu_common}/mixers/FMU_AERT.mix (100%) rename ROMFS/{px4fmu_default => px4fmu_common}/mixers/FMU_AET.mix (100%) rename ROMFS/{px4fmu_default => px4fmu_common}/mixers/FMU_Q.mix (100%) rename ROMFS/{px4fmu_default => px4fmu_common}/mixers/FMU_RET.mix (100%) rename ROMFS/{px4fmu_default => px4fmu_common}/mixers/FMU_X5.mix (100%) rename ROMFS/{px4fmu_default => px4fmu_common}/mixers/FMU_delta.mix (100%) rename ROMFS/{px4fmu_default => px4fmu_common}/mixers/FMU_hex_+.mix (100%) rename ROMFS/{px4fmu_default => px4fmu_common}/mixers/FMU_hex_x.mix (100%) rename ROMFS/{px4fmu_default => px4fmu_common}/mixers/FMU_octo_+.mix (100%) rename ROMFS/{px4fmu_default => px4fmu_common}/mixers/FMU_octo_x.mix (100%) rename ROMFS/{px4fmu_default => px4fmu_common}/mixers/FMU_pass.mix (100%) rename ROMFS/{px4fmu_default => px4fmu_common}/mixers/FMU_quad_+.mix (100%) rename ROMFS/{px4fmu_default => px4fmu_common}/mixers/FMU_quad_x.mix (100%) rename ROMFS/{px4fmu_default => px4fmu_common}/mixers/README (100%) diff --git a/ROMFS/px4fmu_default/logging/logconv.m b/ROMFS/px4fmu_common/logging/logconv.m similarity index 100% rename from ROMFS/px4fmu_default/logging/logconv.m rename to ROMFS/px4fmu_common/logging/logconv.m diff --git a/ROMFS/px4fmu_default/mixers/FMU_AERT.mix b/ROMFS/px4fmu_common/mixers/FMU_AERT.mix similarity index 100% rename from ROMFS/px4fmu_default/mixers/FMU_AERT.mix rename to ROMFS/px4fmu_common/mixers/FMU_AERT.mix diff --git a/ROMFS/px4fmu_default/mixers/FMU_AET.mix b/ROMFS/px4fmu_common/mixers/FMU_AET.mix similarity index 100% rename from ROMFS/px4fmu_default/mixers/FMU_AET.mix rename to ROMFS/px4fmu_common/mixers/FMU_AET.mix diff --git a/ROMFS/px4fmu_default/mixers/FMU_Q.mix b/ROMFS/px4fmu_common/mixers/FMU_Q.mix similarity index 100% rename from ROMFS/px4fmu_default/mixers/FMU_Q.mix rename to ROMFS/px4fmu_common/mixers/FMU_Q.mix diff --git a/ROMFS/px4fmu_default/mixers/FMU_RET.mix b/ROMFS/px4fmu_common/mixers/FMU_RET.mix similarity index 100% rename from ROMFS/px4fmu_default/mixers/FMU_RET.mix rename to ROMFS/px4fmu_common/mixers/FMU_RET.mix diff --git a/ROMFS/px4fmu_default/mixers/FMU_X5.mix b/ROMFS/px4fmu_common/mixers/FMU_X5.mix similarity index 100% rename from ROMFS/px4fmu_default/mixers/FMU_X5.mix rename to ROMFS/px4fmu_common/mixers/FMU_X5.mix diff --git a/ROMFS/px4fmu_default/mixers/FMU_delta.mix b/ROMFS/px4fmu_common/mixers/FMU_delta.mix similarity index 100% rename from ROMFS/px4fmu_default/mixers/FMU_delta.mix rename to ROMFS/px4fmu_common/mixers/FMU_delta.mix diff --git a/ROMFS/px4fmu_default/mixers/FMU_hex_+.mix b/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix similarity index 100% rename from ROMFS/px4fmu_default/mixers/FMU_hex_+.mix rename to ROMFS/px4fmu_common/mixers/FMU_hex_+.mix diff --git a/ROMFS/px4fmu_default/mixers/FMU_hex_x.mix b/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix similarity index 100% rename from ROMFS/px4fmu_default/mixers/FMU_hex_x.mix rename to ROMFS/px4fmu_common/mixers/FMU_hex_x.mix diff --git a/ROMFS/px4fmu_default/mixers/FMU_octo_+.mix b/ROMFS/px4fmu_common/mixers/FMU_octo_+.mix similarity index 100% rename from ROMFS/px4fmu_default/mixers/FMU_octo_+.mix rename to ROMFS/px4fmu_common/mixers/FMU_octo_+.mix diff --git a/ROMFS/px4fmu_default/mixers/FMU_octo_x.mix b/ROMFS/px4fmu_common/mixers/FMU_octo_x.mix similarity index 100% rename from ROMFS/px4fmu_default/mixers/FMU_octo_x.mix rename to ROMFS/px4fmu_common/mixers/FMU_octo_x.mix diff --git a/ROMFS/px4fmu_default/mixers/FMU_pass.mix b/ROMFS/px4fmu_common/mixers/FMU_pass.mix similarity index 100% rename from ROMFS/px4fmu_default/mixers/FMU_pass.mix rename to ROMFS/px4fmu_common/mixers/FMU_pass.mix diff --git a/ROMFS/px4fmu_default/mixers/FMU_quad_+.mix b/ROMFS/px4fmu_common/mixers/FMU_quad_+.mix similarity index 100% rename from ROMFS/px4fmu_default/mixers/FMU_quad_+.mix rename to ROMFS/px4fmu_common/mixers/FMU_quad_+.mix diff --git a/ROMFS/px4fmu_default/mixers/FMU_quad_x.mix b/ROMFS/px4fmu_common/mixers/FMU_quad_x.mix similarity index 100% rename from ROMFS/px4fmu_default/mixers/FMU_quad_x.mix rename to ROMFS/px4fmu_common/mixers/FMU_quad_x.mix diff --git a/ROMFS/px4fmu_default/mixers/README b/ROMFS/px4fmu_common/mixers/README similarity index 100% rename from ROMFS/px4fmu_default/mixers/README rename to ROMFS/px4fmu_common/mixers/README diff --git a/makefiles/config_px4fmu_default.mk b/makefiles/config_px4fmu_default.mk index a6d766511c..b9ce1123f1 100644 --- a/makefiles/config_px4fmu_default.mk +++ b/makefiles/config_px4fmu_default.mk @@ -5,7 +5,7 @@ # # Use the configuration's ROMFS. # -ROMFS_ROOT = $(PX4_BASE)/ROMFS/$(CONFIG) +ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common # # Transitional support - add commands from the NuttX export archive. diff --git a/makefiles/config_px4fmuv2_default.mk b/makefiles/config_px4fmuv2_default.mk index 374c0cfe98..bd324d7d0d 100644 --- a/makefiles/config_px4fmuv2_default.mk +++ b/makefiles/config_px4fmuv2_default.mk @@ -5,7 +5,7 @@ # # Use the configuration's ROMFS. # -ROMFS_ROOT = $(PX4_BASE)/ROMFS/$(CONFIG) +ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common # # Board support modules diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index c34382ae0b..a2227b5c45 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -246,6 +246,9 @@ include $(PX4_MK_DIR)/nuttx.mk ################################################################################ ifneq ($(ROMFS_ROOT),) +ifeq ($(wildcard $(ROMFS_ROOT)),) +$(error ROMFS_ROOT specifies a directory that does not exist) +endif # # Note that there is no support for more than one root directory or constructing @@ -272,7 +275,7 @@ $(ROMFS_OBJ): $(ROMFS_IMG) $(GLOBAL_DEPS) # Generate the ROMFS image from the root $(ROMFS_IMG): $(ROMFS_DEPS) $(GLOBAL_DEPS) - @$(ECHO) %% generating $@ + @$(ECHO) "ROMFS: $@" $(Q) $(GENROMFS) -f $@ -d $(ROMFS_ROOT) -V "NSHInitVol" EXTRA_CLEANS += $(ROMGS_OBJ) $(ROMFS_IMG) @@ -318,7 +321,7 @@ endef # Don't generate until modules have updated their command files $(BUILTIN_CSRC): $(GLOBAL_DEPS) $(MODULE_OBJS) $(BUILTIN_COMMAND_FILES) - @$(ECHO) %% generating $@ + @$(ECHO) "CMDS: $@" $(Q) $(ECHO) '/* builtin command list - automatically generated, do not edit */' > $@ $(Q) $(ECHO) '#include ' >> $@ $(Q) $(ECHO) '#include ' >> $@ diff --git a/makefiles/module.mk b/makefiles/module.mk index 154d37cc2b..1db0f6fee6 100644 --- a/makefiles/module.mk +++ b/makefiles/module.mk @@ -145,7 +145,6 @@ MODULE_COMMAND_FILES := $(addprefix $(WORK_DIR)/builtin_commands/COMMAND.,$(MODU $(MODULE_COMMAND_FILES): command = $(word 2,$(subst ., ,$(notdir $(@)))) $(MODULE_COMMAND_FILES): exclude = $(dir $@)COMMAND.$(command).* $(MODULE_COMMAND_FILES): $(GLOBAL_DEPS) - @$(ECHO) COMMAND: $(command) @$(REMOVE) -f $(exclude) @$(MKDIR) -p $(dir $@) $(Q) $(TOUCH) $@ From 8f1070bb42dbc597c3d2fa7bf9a5931896d651f9 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Apr 2013 12:28:36 -0700 Subject: [PATCH 050/486] Fix a misleading comment about the tone_alarm timers. --- nuttx/configs/px4fmuv2/include/board.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/nuttx/configs/px4fmuv2/include/board.h b/nuttx/configs/px4fmuv2/include/board.h index 286dedab05..5eae244772 100755 --- a/nuttx/configs/px4fmuv2/include/board.h +++ b/nuttx/configs/px4fmuv2/include/board.h @@ -309,9 +309,9 @@ /* * Tone alarm output */ -#define TONE_ALARM_TIMER 2 /* timer 3 */ -#define TONE_ALARM_CHANNEL 1 /* channel 3 */ -#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF2|GPIO_SPEED_2MHz|GPIO_FLOAT|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN15) +#define TONE_ALARM_TIMER 2 /* timer 2 */ +#define TONE_ALARM_CHANNEL 1 /* channel 1 */ +#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF2|GPIO_SPEED_2MHz|GPIO_PULLUP|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN15) /************************************************************************************ * Public Data From 52bb5e561c2407937d80545c127e37da6d2c3a04 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Apr 2013 12:57:53 -0700 Subject: [PATCH 051/486] Fix memory sizing so that we get the extra 64K we promised. --- nuttx/arch/arm/src/stm32/stm32_allocateheap.c | 26 ++++++++++++------- 1 file changed, 17 insertions(+), 9 deletions(-) diff --git a/nuttx/arch/arm/src/stm32/stm32_allocateheap.c b/nuttx/arch/arm/src/stm32/stm32_allocateheap.c index 4b8707a2b7..c53ff31372 100644 --- a/nuttx/arch/arm/src/stm32/stm32_allocateheap.c +++ b/nuttx/arch/arm/src/stm32/stm32_allocateheap.c @@ -118,19 +118,23 @@ # undef CONFIG_STM32_CCMEXCLUDE # define CONFIG_STM32_CCMEXCLUDE 1 -/* All members of the STM32F20xxx and STM32F40xxx families have 192Kb in three banks: +/* All members of the STM32F20xxx and STM32F40xxx families have 192KiB in three banks: * - * 1) 112Kb of System SRAM beginning at address 0x2000:0000 - * 2) 16Kb of System SRAM beginning at address 0x2001:c000 - * 3) 64Kb of CCM SRAM beginning at address 0x1000:0000 + * 1) 112KiB of System SRAM beginning at address 0x2000:0000 + * 2) 16KiB of System SRAM beginning at address 0x2001:c000 + * 3) 64KiB of CCM SRAM beginning at address 0x1000:0000 * - * As determined by ld.script, g_heapbase lies in the 112Kb memory + * The STM32F427/437 have an additional 64KiB of System SRAM beginning at + * address 0x2002:0000 for a total of 256KiB. + * + * As determined by ld.script, g_heapbase lies in the 112KiB memory * region and that extends to 0x2001:0000. But the first and second memory * regions are contiguous and treated as one in this logic that extends to - * 0x2002:0000. + * 0x2002:0000 (or 0x2003:0000 for the F427/F437). * - * As a complication, it appears that CCM SRAM cannot be used for DMA. So, if - * STM32 DMA is enabled, CCM SRAM should probably be excluded from the heap. + * As a complication, CCM SRAM cannot be used for DMA. So, if STM32 DMA is enabled, + * CCM SRAM should probably be excluded from the heap or the application must take + * extra care to ensure that DMA buffers are not allocated in CCM SRAM. * * In addition, external FSMC SRAM may be available. */ @@ -147,7 +151,11 @@ /* Set the end of system SRAM */ -# define SRAM1_END 0x20020000 +# if defined(CONFIG_STM32_STM32F427) +# define SRAM1_END 0x20030000 +# else +# define SRAM1_END 0x20020000 +# endif /* Set the range of CCM SRAM as well (although we may not use it) */ From d8e8e6cd209e688de51a800123e513f4885655cd Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Apr 2013 14:07:00 -0700 Subject: [PATCH 052/486] Fix alt function selector for tone_alarm GPIO. --- nuttx/configs/px4fmuv2/include/board.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx/configs/px4fmuv2/include/board.h b/nuttx/configs/px4fmuv2/include/board.h index 5eae244772..8158e618da 100755 --- a/nuttx/configs/px4fmuv2/include/board.h +++ b/nuttx/configs/px4fmuv2/include/board.h @@ -311,7 +311,7 @@ */ #define TONE_ALARM_TIMER 2 /* timer 2 */ #define TONE_ALARM_CHANNEL 1 /* channel 1 */ -#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF2|GPIO_SPEED_2MHz|GPIO_PULLUP|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN15) +#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_FLOAT|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN15) /************************************************************************************ * Public Data From e7e35616c01119eae7a436c260d88cdc6fec6ce5 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Apr 2013 15:47:34 -0700 Subject: [PATCH 053/486] Fix the way that tone_alarm idles the GPIO and make it idle safely for v2 boards. --- apps/drivers/stm32/tone_alarm/tone_alarm.cpp | 8 ++++---- nuttx/configs/px4fmu/include/board.h | 1 + nuttx/configs/px4fmuv2/include/board.h | 3 ++- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/apps/drivers/stm32/tone_alarm/tone_alarm.cpp b/apps/drivers/stm32/tone_alarm/tone_alarm.cpp index baa652d8ad..ac5511e60a 100644 --- a/apps/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/apps/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -494,7 +494,7 @@ ToneAlarm::init() return ret; /* configure the GPIO to the idle state */ - stm32_configgpio(GPIO_TONE_ALARM); + stm32_configgpio(GPIO_TONE_ALARM_IDLE); /* clock/power on our timer */ modifyreg32(STM32_RCC_APB1ENR, 0, TONE_ALARM_CLOCK_ENABLE); @@ -606,6 +606,8 @@ ToneAlarm::start_note(unsigned note) rEGR = GTIM_EGR_UG; // force a reload of the period rCCER |= TONE_CCER; // enable the output + // configure the GPIO to enable timer output + stm32_configgpio(GPIO_TONE_ALARM); } void @@ -616,10 +618,8 @@ ToneAlarm::stop_note() /* * Make sure the GPIO is not driving the speaker. - * - * XXX this presumes PX4FMU and the onboard speaker driver FET. */ - stm32_gpiowrite(GPIO_TONE_ALARM, 0); + stm32_configgpio(GPIO_TONE_ALARM_IDLE); } void diff --git a/nuttx/configs/px4fmu/include/board.h b/nuttx/configs/px4fmu/include/board.h index 8ad56a4c6a..0bc0b30216 100755 --- a/nuttx/configs/px4fmu/include/board.h +++ b/nuttx/configs/px4fmu/include/board.h @@ -326,6 +326,7 @@ */ #define TONE_ALARM_TIMER 3 /* timer 3 */ #define TONE_ALARM_CHANNEL 3 /* channel 3 */ +#define GPIO_TONE_ALARM_IDLE (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN8) #define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF2|GPIO_SPEED_2MHz|GPIO_FLOAT|GPIO_PUSHPULL|GPIO_PORTC|GPIO_PIN8) /************************************************************************************ diff --git a/nuttx/configs/px4fmuv2/include/board.h b/nuttx/configs/px4fmuv2/include/board.h index 8158e618da..973aab0cef 100755 --- a/nuttx/configs/px4fmuv2/include/board.h +++ b/nuttx/configs/px4fmuv2/include/board.h @@ -311,7 +311,8 @@ */ #define TONE_ALARM_TIMER 2 /* timer 2 */ #define TONE_ALARM_CHANNEL 1 /* channel 1 */ -#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_FLOAT|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN15) +#define GPIO_TONE_ALARM_IDLE (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15) +#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN15) /************************************************************************************ * Public Data From d1a2e9a9c12ae5f4b52299f57c6704a03304b766 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Apr 2013 18:18:49 -0700 Subject: [PATCH 054/486] Fix the v2 RGB LED ID --- nuttx/configs/px4fmuv2/include/board.h | 2 +- src/device/rgbled/rgbled.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx/configs/px4fmuv2/include/board.h b/nuttx/configs/px4fmuv2/include/board.h index 973aab0cef..fd8f78b80b 100755 --- a/nuttx/configs/px4fmuv2/include/board.h +++ b/nuttx/configs/px4fmuv2/include/board.h @@ -283,7 +283,7 @@ * * Note that these are unshifted addresses. */ -#define PX4_I2C_OBDEV_LED 0x5a +#define PX4_I2C_OBDEV_LED 0x55 /* * SPI diff --git a/src/device/rgbled/rgbled.cpp b/src/device/rgbled/rgbled.cpp index 923e0a14f9..dc1e469c0a 100644 --- a/src/device/rgbled/rgbled.cpp +++ b/src/device/rgbled/rgbled.cpp @@ -65,7 +65,7 @@ #define LED_ONTIME 120 #define LED_OFFTIME 120 -#define ADDR 0x55 /**< I2C adress of TCA62724FMG */ +#define ADDR PX4_I2C_OBDEV_LED /**< I2C adress of TCA62724FMG */ #define SUB_ADDR_START 0x01 /**< write everything (with auto-increment) */ #define SUB_ADDR_PWM0 0x81 /**< blue (without auto-increment) */ #define SUB_ADDR_PWM1 0x82 /**< green (without auto-increment) */ From 29324cc97f06519a3e74b292ccd53474936afd5a Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Apr 2013 18:33:32 -0700 Subject: [PATCH 055/486] Add the SoC chip and common directories to the NuttX-related include paths. --- makefiles/nuttx.mk | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/makefiles/nuttx.mk b/makefiles/nuttx.mk index e86f1370b9..5186dc3efe 100644 --- a/makefiles/nuttx.mk +++ b/makefiles/nuttx.mk @@ -63,7 +63,10 @@ LDSCRIPT += $(NUTTX_EXPORT_DIR)build/ld.script # # Add directories from the NuttX export to the relevant search paths # -INCLUDE_DIRS += $(NUTTX_EXPORT_DIR)include +INCLUDE_DIRS += $(NUTTX_EXPORT_DIR)include \ + += $(NUTTX_EXPORT_DIR)arch/chip \ + += $(NUTTX_EXPORT_DIR)arch/common + LIB_DIRS += $(NUTTX_EXPORT_DIR)libs LIBS += -lapps -lnuttx LINK_DEPS += $(NUTTX_EXPORT_DIR)libs/libapps.a \ From 706dcb6a53cc0163572541b856902616b30258ae Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Apr 2013 18:34:29 -0700 Subject: [PATCH 056/486] Move the FMU driver from the old universe to the new universe so that we can teach it about v2. --- apps/drivers/px4fmu/Makefile | 44 --------------------- makefiles/config_px4fmu_default.mk | 6 ++- nuttx/configs/px4fmu/nsh/appconfig | 1 - {apps/drivers => src/device}/px4fmu/fmu.cpp | 0 src/device/px4fmu/module.mk | 6 +++ 5 files changed, 11 insertions(+), 46 deletions(-) delete mode 100644 apps/drivers/px4fmu/Makefile rename {apps/drivers => src/device}/px4fmu/fmu.cpp (100%) create mode 100644 src/device/px4fmu/module.mk diff --git a/apps/drivers/px4fmu/Makefile b/apps/drivers/px4fmu/Makefile deleted file mode 100644 index 7f7c2a7328..0000000000 --- a/apps/drivers/px4fmu/Makefile +++ /dev/null @@ -1,44 +0,0 @@ -############################################################################ -# -# Copyright (C) 2012 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. -# -############################################################################ - -# -# Interface driver for the PX4FMU board -# - -APPNAME = fmu -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 - -INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common - -include $(APPDIR)/mk/app.mk diff --git a/makefiles/config_px4fmu_default.mk b/makefiles/config_px4fmu_default.mk index b9ce1123f1..39b47d8174 100644 --- a/makefiles/config_px4fmu_default.mk +++ b/makefiles/config_px4fmu_default.mk @@ -7,6 +7,11 @@ # ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common +# +# Board support modules +# +MODULES += device/px4fmu + # # Transitional support - add commands from the NuttX export archive. # @@ -34,7 +39,6 @@ BUILTIN_COMMANDS := \ $(call _B, eeprom, , 4096, eeprom_main ) \ $(call _B, fixedwing_att_control, SCHED_PRIORITY_MAX-30, 2048, fixedwing_att_control_main ) \ $(call _B, fixedwing_pos_control, SCHED_PRIORITY_MAX-30, 2048, fixedwing_pos_control_main ) \ - $(call _B, fmu, , 2048, fmu_main ) \ $(call _B, gps, , 2048, gps_main ) \ $(call _B, hil, , 2048, hil_main ) \ $(call _B, hmc5883, , 4096, hmc5883_main ) \ diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index 80cf6f312c..d642b46923 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -123,7 +123,6 @@ CONFIGURED_APPS += drivers/led CONFIGURED_APPS += drivers/blinkm CONFIGURED_APPS += drivers/stm32/tone_alarm CONFIGURED_APPS += drivers/stm32/adc -CONFIGURED_APPS += drivers/px4fmu CONFIGURED_APPS += drivers/hil CONFIGURED_APPS += drivers/gps CONFIGURED_APPS += drivers/mb12xx diff --git a/apps/drivers/px4fmu/fmu.cpp b/src/device/px4fmu/fmu.cpp similarity index 100% rename from apps/drivers/px4fmu/fmu.cpp rename to src/device/px4fmu/fmu.cpp diff --git a/src/device/px4fmu/module.mk b/src/device/px4fmu/module.mk new file mode 100644 index 0000000000..05bc7a5b33 --- /dev/null +++ b/src/device/px4fmu/module.mk @@ -0,0 +1,6 @@ +# +# Interface driver for the PX4FMU board +# + +MODULE_COMMAND = fmu +SRCS = fmu.cpp From c355275669378c0d6f2e372afa370446525c66ee Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Apr 2013 19:20:08 -0700 Subject: [PATCH 057/486] Make the 'fmu' command build for v2. Should be enough to get the FMU-side PWM outputs working, but untested. --- .../drivers/boards/px4fmuv2/px4fmu_internal.h | 17 ++ apps/drivers/drv_gpio.h | 27 ++- makefiles/config_px4fmuv2_default.mk | 5 +- src/device/px4fmu/fmu.cpp | 198 ++++++++++++++---- 4 files changed, 203 insertions(+), 44 deletions(-) diff --git a/apps/drivers/boards/px4fmuv2/px4fmu_internal.h b/apps/drivers/boards/px4fmuv2/px4fmu_internal.h index 001b23cb2f..235b193f3f 100644 --- a/apps/drivers/boards/px4fmuv2/px4fmu_internal.h +++ b/apps/drivers/boards/px4fmuv2/px4fmu_internal.h @@ -71,6 +71,23 @@ __BEGIN_DECLS #define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7) #define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) +/* User GPIOs + * + * GPIO0-5 are the PWM servo outputs. + */ +#define GPIO_GPIO0_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN14) +#define GPIO_GPIO1_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN13) +#define GPIO_GPIO2_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN11) +#define GPIO_GPIO3_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN9) +#define GPIO_GPIO4_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN13) +#define GPIO_GPIO5_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN14) +#define GPIO_GPIO0_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN14) +#define GPIO_GPIO1_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN13) +#define GPIO_GPIO2_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11) +#define GPIO_GPIO3_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN9) +#define GPIO_GPIO4_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13) +#define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14) + /* USB OTG FS * * PA9 OTG_FS_VBUS VBUS sensing (also connected to the green LED) diff --git a/apps/drivers/drv_gpio.h b/apps/drivers/drv_gpio.h index 2fa6d8b8e6..d21fc5c33d 100644 --- a/apps/drivers/drv_gpio.h +++ b/apps/drivers/drv_gpio.h @@ -42,11 +42,6 @@ #include -#ifdef CONFIG_ARCH_BOARD_PX4FMUV2 -#warning No GPIOs on this board. -#define GPIO_DEVICE_PATH "/dev/null" -#endif - #ifdef CONFIG_ARCH_BOARD_PX4FMU /* * PX4FMU GPIO numbers. @@ -72,6 +67,28 @@ #endif +#ifdef CONFIG_ARCH_BOARD_PX4FMUV2 +/* + * PX4FMUv2 GPIO numbers. + * + * There are no alternate functions on this board. + */ +# define GPIO_SERVO_1 (1<<0) /**< servo 1 output */ +# define GPIO_SERVO_2 (1<<1) /**< servo 2 output */ +# define GPIO_SERVO_3 (1<<2) /**< servo 3 output */ +# define GPIO_SERVO_4 (1<<3) /**< servo 4 output */ +# define GPIO_SERVO_5 (1<<4) /**< servo 5 output */ +# define GPIO_SERVO_6 (1<<5) /**< servo 6 output */ + +/** + * Default GPIO device - other devices may also support this protocol if + * they also export GPIO-like things. This is always the GPIOs on the + * main board. + */ +# define GPIO_DEVICE_PATH "/dev/px4fmu" + +#endif + #ifdef CONFIG_ARCH_BOARD_PX4IO /* * PX4IO GPIO numbers. diff --git a/makefiles/config_px4fmuv2_default.mk b/makefiles/config_px4fmuv2_default.mk index bd324d7d0d..11ca5227e9 100644 --- a/makefiles/config_px4fmuv2_default.mk +++ b/makefiles/config_px4fmuv2_default.mk @@ -10,8 +10,9 @@ ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common # # Board support modules # -MODULES += device/rgbled -MODULES += device/lsm303d +MODULES += device/lsm303d +MODULES += device/px4fmu +MODULES += device/rgbled # # Transitional support - add commands from the NuttX export archive. diff --git a/src/device/px4fmu/fmu.cpp b/src/device/px4fmu/fmu.cpp index e547245367..7b8ca9bbe6 100644 --- a/src/device/px4fmu/fmu.cpp +++ b/src/device/px4fmu/fmu.cpp @@ -34,7 +34,7 @@ /** * @file fmu.cpp * - * Driver/configurator for the PX4 FMU multi-purpose port. + * Driver/configurator for the PX4 FMU multi-purpose port on v1 and v2 boards. */ #include @@ -57,9 +57,18 @@ #include #include #include -#include #include +#if defined(CONFIG_ARCH_BOARD_PX4FMU) +# include +# define FMU_HAVE_PPM +#elif defined(CONFIG_ARCH_BOARD_PX4FMUV2) +# include +# undef FMU_HAVE_PPM +#else +# error Unrecognised FMU board. +#endif + #include #include #include @@ -71,15 +80,18 @@ #include #include -#include +#ifdef FMU_HAVE_PPM +# include +#endif class PX4FMU : public device::CDev { public: enum Mode { + MODE_NONE, MODE_2PWM, MODE_4PWM, - MODE_NONE + MODE_6PWM, }; PX4FMU(); virtual ~PX4FMU(); @@ -146,6 +158,7 @@ private: }; const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { +#if defined(CONFIG_ARCH_BOARD_PX4FMU) {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, GPIO_USART2_CTS_1}, @@ -154,6 +167,15 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, GPIO_USART2_RX_1}, {GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, GPIO_CAN2_TX_2}, {GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, GPIO_CAN2_RX_2}, +#endif +#if defined(CONFIG_ARCH_BOARD_PX4FMUV2) + {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, + {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, + {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, 0}, + {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, 0}, + {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, 0}, + {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, 0}, +#endif }; const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]); @@ -271,9 +293,8 @@ PX4FMU::set_mode(Mode mode) * are presented on the output pins. */ switch (mode) { - case MODE_2PWM: // multi-port with flow control lines as PWM - case MODE_4PWM: // multi-port as 4 PWM outs - debug("MODE_%dPWM", (mode == MODE_2PWM) ? 2 : 4); + case MODE_2PWM: // v1 multi-port with flow control lines as PWM + debug("MODE_2PWM"); /* default output rates */ _pwm_default_rate = 50; @@ -281,7 +302,35 @@ PX4FMU::set_mode(Mode mode) _pwm_alt_rate_channels = 0; /* XXX magic numbers */ - up_pwm_servo_init((mode == MODE_2PWM) ? 0x3 : 0xf); + up_pwm_servo_init(0x3); + set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); + + break; + + case MODE_4PWM: // v1 multi-port as 4 PWM outs + debug("MODE_4PWM"); + + /* default output rates */ + _pwm_default_rate = 50; + _pwm_alt_rate = 50; + _pwm_alt_rate_channels = 0; + + /* XXX magic numbers */ + up_pwm_servo_init(0xf); + set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); + + break; + + case MODE_6PWM: // v2 PWMs as 6 PWM outs + debug("MODE_6PWM"); + + /* default output rates */ + _pwm_default_rate = 50; + _pwm_alt_rate = 50; + _pwm_alt_rate_channels = 0; + + /* XXX magic numbers */ + up_pwm_servo_init(0x3f); set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); break; @@ -399,14 +448,14 @@ PX4FMU::task_main() fds[1].fd = _t_armed; fds[1].events = POLLIN; - unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4; - +#ifdef FMU_HAVE_PPM // rc input, published to ORB struct rc_input_values rc_in; orb_advert_t to_input_rc = 0; memset(&rc_in, 0, sizeof(rc_in)); rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM; +#endif log("starting"); @@ -460,6 +509,23 @@ PX4FMU::task_main() /* can we mix? */ if (_mixers != nullptr) { + unsigned num_outputs; + + switch (_mode) { + case MODE_2PWM: + num_outputs = 2; + break; + case MODE_4PWM: + num_outputs = 4; + break; + case MODE_6PWM: + num_outputs = 6; + break; + default: + num_outputs = 0; + break; + } + /* do mixing */ outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); outputs.timestamp = hrt_absolute_time(); @@ -508,6 +574,7 @@ PX4FMU::task_main() up_pwm_servo_arm(aa.armed && !aa.lockdown); } +#ifdef FMU_HAVE_PPM // see if we have new PPM input data if (ppm_last_valid_decode != rc_in.timestamp) { // we have a new PPM frame. Publish it. @@ -527,6 +594,8 @@ PX4FMU::task_main() orb_publish(ORB_ID(input_rc), to_input_rc, &rc_in); } } +#endif + } ::close(_t_actuators); @@ -575,6 +644,7 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg) switch (_mode) { case MODE_2PWM: case MODE_4PWM: + case MODE_6PWM: ret = pwm_ioctl(filp, cmd, arg); break; @@ -614,16 +684,24 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) ret = set_pwm_rate(arg, _pwm_default_rate, _pwm_alt_rate); break; - case PWM_SERVO_SET(2): - case PWM_SERVO_SET(3): - if (_mode != MODE_4PWM) { + case PWM_SERVO_SET(5): + case PWM_SERVO_SET(4): + if (_mode < MODE_6PWM) { + ret = -EINVAL; + break; + } + + /* FALLTHROUGH */ + case PWM_SERVO_SET(3): + case PWM_SERVO_SET(2): + if (_mode < MODE_4PWM) { ret = -EINVAL; break; } /* FALLTHROUGH */ - case PWM_SERVO_SET(0): case PWM_SERVO_SET(1): + case PWM_SERVO_SET(0): if (arg < 2100) { up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg); } else { @@ -632,16 +710,24 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; - case PWM_SERVO_GET(2): - case PWM_SERVO_GET(3): - if (_mode != MODE_4PWM) { + case PWM_SERVO_GET(5): + case PWM_SERVO_GET(4): + if (_mode < MODE_6PWM) { + ret = -EINVAL; + break; + } + + /* FALLTHROUGH */ + case PWM_SERVO_GET(3): + case PWM_SERVO_GET(2): + if (_mode < MODE_4PWM) { ret = -EINVAL; break; } /* FALLTHROUGH */ - case PWM_SERVO_GET(0): case PWM_SERVO_GET(1): + case PWM_SERVO_GET(0): *(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0)); break; @@ -649,16 +735,26 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) case PWM_SERVO_GET_RATEGROUP(1): case PWM_SERVO_GET_RATEGROUP(2): case PWM_SERVO_GET_RATEGROUP(3): + case PWM_SERVO_GET_RATEGROUP(4): + case PWM_SERVO_GET_RATEGROUP(5): *(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0)); break; case PWM_SERVO_GET_COUNT: case MIXERIOCGETOUTPUTCOUNT: - if (_mode == MODE_4PWM) { + switch (_mode) { + case MODE_6PWM: + *(unsigned *)arg = 6; + break; + case MODE_4PWM: *(unsigned *)arg = 4; - - } else { + break; + case MODE_2PWM: *(unsigned *)arg = 2; + break; + default: + ret = -EINVAL; + break; } break; @@ -734,17 +830,17 @@ ssize_t PX4FMU::write(file *filp, const char *buffer, size_t len) { unsigned count = len / 2; - uint16_t values[4]; + uint16_t values[6]; - if (count > 4) { - // we only have 4 PWM outputs on the FMU - count = 4; + if (count > 6) { + // we have at most 6 outputs + count = 6; } // allow for misaligned values - memcpy(values, buffer, count*2); + memcpy(values, buffer, count * 2); - for (uint8_t i=0; i Date: Sat, 6 Apr 2013 22:46:50 -0700 Subject: [PATCH 058/486] Add GPIO driver access to the power supply control/monitoring GPIOs for FMUv2 --- apps/drivers/boards/px4fmuv2/px4fmu_init.c | 19 ++++++++++++------- .../drivers/boards/px4fmuv2/px4fmu_internal.h | 6 ++++++ apps/drivers/drv_gpio.h | 4 ++++ src/device/px4fmu/fmu.cpp | 15 ++++++++++++--- 4 files changed, 34 insertions(+), 10 deletions(-) diff --git a/apps/drivers/boards/px4fmuv2/px4fmu_init.c b/apps/drivers/boards/px4fmuv2/px4fmu_init.c index c9364c2a49..1d99f15bfc 100644 --- a/apps/drivers/boards/px4fmuv2/px4fmu_init.c +++ b/apps/drivers/boards/px4fmuv2/px4fmu_init.c @@ -145,7 +145,17 @@ __EXPORT int matherr(struct exception *e) __EXPORT int nsh_archinitialize(void) { - int result; + + /* configure ADC pins */ + stm32_configgpio(GPIO_ADC1_IN10); + stm32_configgpio(GPIO_ADC1_IN11); + stm32_configgpio(GPIO_ADC1_IN12); + + /* configure power supply control pins */ + stm32_configgpio(GPIO_VDD_5V_PERIPH_EN); + stm32_configgpio(GPIO_VDD_2V8_SENSORS_EN); + stm32_configgpio(GPIO_VDD_5V_HIPOWER_OC); + stm32_configgpio(GPIO_VDD_5V_PERIPH_OC); /* configure the high-resolution time/callout interface */ hrt_init(); @@ -201,7 +211,7 @@ __EXPORT int nsh_archinitialize(void) /* Get the SPI port for the FRAM */ message("[boot] Initializing SPI port 2\n"); - spi2 = up_spiinitialize(3); + spi2 = up_spiinitialize(2); if (!spi2) { message("[boot] FAILED to initialize SPI port 2\n"); @@ -215,10 +225,5 @@ __EXPORT int nsh_archinitialize(void) //message("[boot] Successfully bound SPI port 2 to the FRAM driver\n"); - /* configure ADC pins */ - stm32_configgpio(GPIO_ADC1_IN10); - stm32_configgpio(GPIO_ADC1_IN11); - stm32_configgpio(GPIO_ADC1_IN12); - return OK; } diff --git a/apps/drivers/boards/px4fmuv2/px4fmu_internal.h b/apps/drivers/boards/px4fmuv2/px4fmu_internal.h index 235b193f3f..cc4e9529dc 100644 --- a/apps/drivers/boards/px4fmuv2/px4fmu_internal.h +++ b/apps/drivers/boards/px4fmuv2/px4fmu_internal.h @@ -88,6 +88,12 @@ __BEGIN_DECLS #define GPIO_GPIO4_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13) #define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14) +/* Power supply control and monitoring GPIOs */ +#define GPIO_VDD_5V_PERIPH_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN8) +#define GPIO_VDD_2V8_SENSORS_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3) +#define GPIO_VDD_5V_HIPOWER_OC (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN10) +#define GPIO_VDD_5V_PERIPH_OC (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN15) + /* USB OTG FS * * PA9 OTG_FS_VBUS VBUS sensing (also connected to the green LED) diff --git a/apps/drivers/drv_gpio.h b/apps/drivers/drv_gpio.h index d21fc5c33d..a4c59d2180 100644 --- a/apps/drivers/drv_gpio.h +++ b/apps/drivers/drv_gpio.h @@ -80,6 +80,10 @@ # define GPIO_SERVO_5 (1<<4) /**< servo 5 output */ # define GPIO_SERVO_6 (1<<5) /**< servo 6 output */ +# define GPIO_5V_PERIPH_EN (1<<6) /**< PA8 - VDD_5V_PERIPH_EN */ +# define GPIO_5V_HIPOWER_OC (1<<7) /**< PE10 - !VDD_5V_HIPOWER_OC */ +# define GPIO_5V_PERIPH_OC (1<<8) /**< PE15 - !VDD_5V_PERIPH_OC */ + /** * Default GPIO device - other devices may also support this protocol if * they also export GPIO-like things. This is always the GPIOs on the diff --git a/src/device/px4fmu/fmu.cpp b/src/device/px4fmu/fmu.cpp index 7b8ca9bbe6..d3865f053e 100644 --- a/src/device/px4fmu/fmu.cpp +++ b/src/device/px4fmu/fmu.cpp @@ -175,6 +175,9 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, 0}, {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, 0}, {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, 0}, + {0, GPIO_VDD_5V_PERIPH_EN, 0}, + {GPIO_5V_HIPOWER_OC, 0, 0}, + {GPIO_5V_PERIPH_OC, 0, 0}, #endif }; @@ -850,10 +853,16 @@ void PX4FMU::gpio_reset(void) { /* - * Setup default GPIO config - all pins as GPIOs. + * Setup default GPIO config - all pins as GPIOs, input if + * possible otherwise output if possible. */ - for (unsigned i = 0; i < _ngpio; i++) - stm32_configgpio(_gpio_tab[i].input); + for (unsigned i = 0; i < _ngpio; i++) { + if (_gpio_tab[i].input != 0) { + stm32_configgpio(_gpio_tab[i].input); + } else if (_gpio_tab[i].output != 0) { + stm32_configgpio(_gpio_tab[i].output); + } + } #if defined(CONFIG_ARCH_BOARD_PX4FMU) /* if we have a GPIO direction control, set it to zero (input) */ From 2557f0d2de80e2df690b40b60f8ec79de799657e Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Apr 2013 23:04:32 -0700 Subject: [PATCH 059/486] Rename the 'device' directory back to 'drivers', it's less confusing that way. Move the fmuv2 board driver out into the new world. --- apps/drivers/boards/px4fmuv2/Makefile | 41 ------------------- makefiles/config_px4fmu_default.mk | 2 +- makefiles/config_px4fmuv2_default.mk | 7 ++-- src/drivers/boards/px4fmuv2/module.mk | 9 ++++ .../drivers/boards/px4fmuv2/px4fmu_can.c | 0 .../drivers/boards/px4fmuv2/px4fmu_init.c | 0 .../drivers/boards/px4fmuv2/px4fmu_internal.h | 0 .../boards/px4fmuv2/px4fmu_pwm_servo.c | 0 .../drivers/boards/px4fmuv2/px4fmu_spi.c | 0 .../drivers/boards/px4fmuv2/px4fmu_usb.c | 0 src/{device => drivers}/lsm303d/lsm303d.cpp | 0 src/{device => drivers}/lsm303d/module.mk | 0 src/{device => drivers}/px4fmu/fmu.cpp | 0 src/{device => drivers}/px4fmu/module.mk | 0 src/{device => drivers}/rgbled/module.mk | 0 src/{device => drivers}/rgbled/rgbled.cpp | 0 16 files changed, 14 insertions(+), 45 deletions(-) delete mode 100644 apps/drivers/boards/px4fmuv2/Makefile create mode 100644 src/drivers/boards/px4fmuv2/module.mk rename {apps => src}/drivers/boards/px4fmuv2/px4fmu_can.c (100%) rename {apps => src}/drivers/boards/px4fmuv2/px4fmu_init.c (100%) rename {apps => src}/drivers/boards/px4fmuv2/px4fmu_internal.h (100%) rename {apps => src}/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c (100%) rename {apps => src}/drivers/boards/px4fmuv2/px4fmu_spi.c (100%) rename {apps => src}/drivers/boards/px4fmuv2/px4fmu_usb.c (100%) rename src/{device => drivers}/lsm303d/lsm303d.cpp (100%) rename src/{device => drivers}/lsm303d/module.mk (100%) rename src/{device => drivers}/px4fmu/fmu.cpp (100%) rename src/{device => drivers}/px4fmu/module.mk (100%) rename src/{device => drivers}/rgbled/module.mk (100%) rename src/{device => drivers}/rgbled/rgbled.cpp (100%) diff --git a/apps/drivers/boards/px4fmuv2/Makefile b/apps/drivers/boards/px4fmuv2/Makefile deleted file mode 100644 index 9c04a8c42a..0000000000 --- a/apps/drivers/boards/px4fmuv2/Makefile +++ /dev/null @@ -1,41 +0,0 @@ -############################################################################ -# -# Copyright (C) 2012 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. -# -############################################################################ - -# -# Board-specific startup code for the PX4FMUv2 -# - -INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common -LIBNAME = brd_px4fmuv2 - -include $(APPDIR)/mk/app.mk diff --git a/makefiles/config_px4fmu_default.mk b/makefiles/config_px4fmu_default.mk index 39b47d8174..1a6c91b269 100644 --- a/makefiles/config_px4fmu_default.mk +++ b/makefiles/config_px4fmu_default.mk @@ -10,7 +10,7 @@ ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common # # Board support modules # -MODULES += device/px4fmu +MODULES += drivers/px4fmu # # Transitional support - add commands from the NuttX export archive. diff --git a/makefiles/config_px4fmuv2_default.mk b/makefiles/config_px4fmuv2_default.mk index 11ca5227e9..d296c5379c 100644 --- a/makefiles/config_px4fmuv2_default.mk +++ b/makefiles/config_px4fmuv2_default.mk @@ -10,9 +10,10 @@ ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common # # Board support modules # -MODULES += device/lsm303d -MODULES += device/px4fmu -MODULES += device/rgbled +MODULES += drivers/boards/px4fmuv2 +MODULES += drivers/lsm303d +MODULES += drivers/px4fmu +MODULES += drivers/rgbled # # Transitional support - add commands from the NuttX export archive. diff --git a/src/drivers/boards/px4fmuv2/module.mk b/src/drivers/boards/px4fmuv2/module.mk new file mode 100644 index 0000000000..eb8841e4d1 --- /dev/null +++ b/src/drivers/boards/px4fmuv2/module.mk @@ -0,0 +1,9 @@ +# +# Board-specific startup code for the PX4FMUv2 +# + +SRCS = px4fmu_can.c \ + px4fmu_init.c \ + px4fmu_pwm_servo.c \ + px4fmu_spi.c \ + px4fmu_usb.c diff --git a/apps/drivers/boards/px4fmuv2/px4fmu_can.c b/src/drivers/boards/px4fmuv2/px4fmu_can.c similarity index 100% rename from apps/drivers/boards/px4fmuv2/px4fmu_can.c rename to src/drivers/boards/px4fmuv2/px4fmu_can.c diff --git a/apps/drivers/boards/px4fmuv2/px4fmu_init.c b/src/drivers/boards/px4fmuv2/px4fmu_init.c similarity index 100% rename from apps/drivers/boards/px4fmuv2/px4fmu_init.c rename to src/drivers/boards/px4fmuv2/px4fmu_init.c diff --git a/apps/drivers/boards/px4fmuv2/px4fmu_internal.h b/src/drivers/boards/px4fmuv2/px4fmu_internal.h similarity index 100% rename from apps/drivers/boards/px4fmuv2/px4fmu_internal.h rename to src/drivers/boards/px4fmuv2/px4fmu_internal.h diff --git a/apps/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c similarity index 100% rename from apps/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c rename to src/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c diff --git a/apps/drivers/boards/px4fmuv2/px4fmu_spi.c b/src/drivers/boards/px4fmuv2/px4fmu_spi.c similarity index 100% rename from apps/drivers/boards/px4fmuv2/px4fmu_spi.c rename to src/drivers/boards/px4fmuv2/px4fmu_spi.c diff --git a/apps/drivers/boards/px4fmuv2/px4fmu_usb.c b/src/drivers/boards/px4fmuv2/px4fmu_usb.c similarity index 100% rename from apps/drivers/boards/px4fmuv2/px4fmu_usb.c rename to src/drivers/boards/px4fmuv2/px4fmu_usb.c diff --git a/src/device/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp similarity index 100% rename from src/device/lsm303d/lsm303d.cpp rename to src/drivers/lsm303d/lsm303d.cpp diff --git a/src/device/lsm303d/module.mk b/src/drivers/lsm303d/module.mk similarity index 100% rename from src/device/lsm303d/module.mk rename to src/drivers/lsm303d/module.mk diff --git a/src/device/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp similarity index 100% rename from src/device/px4fmu/fmu.cpp rename to src/drivers/px4fmu/fmu.cpp diff --git a/src/device/px4fmu/module.mk b/src/drivers/px4fmu/module.mk similarity index 100% rename from src/device/px4fmu/module.mk rename to src/drivers/px4fmu/module.mk diff --git a/src/device/rgbled/module.mk b/src/drivers/rgbled/module.mk similarity index 100% rename from src/device/rgbled/module.mk rename to src/drivers/rgbled/module.mk diff --git a/src/device/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp similarity index 100% rename from src/device/rgbled/rgbled.cpp rename to src/drivers/rgbled/rgbled.cpp From 4703a689798e2a520dfb69c0ef9146f3ecb956f2 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 7 Apr 2013 22:00:23 -0700 Subject: [PATCH 060/486] Fix the default state of the peripheral power control. --- src/drivers/boards/px4fmuv2/px4fmu_internal.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/boards/px4fmuv2/px4fmu_internal.h b/src/drivers/boards/px4fmuv2/px4fmu_internal.h index cc4e9529dc..78f6a2e65d 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu_internal.h +++ b/src/drivers/boards/px4fmuv2/px4fmu_internal.h @@ -89,7 +89,7 @@ __BEGIN_DECLS #define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14) /* Power supply control and monitoring GPIOs */ -#define GPIO_VDD_5V_PERIPH_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN8) +#define GPIO_VDD_5V_PERIPH_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN8) #define GPIO_VDD_2V8_SENSORS_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3) #define GPIO_VDD_5V_HIPOWER_OC (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN10) #define GPIO_VDD_5V_PERIPH_OC (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN15) From 4a062086aefd4fcd25f40d0cfa65ab3a1c3c3984 Mon Sep 17 00:00:00 2001 From: Anton Date: Mon, 8 Apr 2013 17:04:04 +0400 Subject: [PATCH 061/486] position_estimator_inav: bugfixes, some refactoring --- .../position_estimator_inav_main.c | 80 ++++++++++--------- .../position_estimator_inav_params.c | 23 ++++-- .../position_estimator_inav_params.h | 3 + 3 files changed, 63 insertions(+), 43 deletions(-) diff --git a/apps/position_estimator_inav/position_estimator_inav_main.c b/apps/position_estimator_inav/position_estimator_inav_main.c index 292cf7f215..97d669612b 100644 --- a/apps/position_estimator_inav/position_estimator_inav_main.c +++ b/apps/position_estimator_inav/position_estimator_inav_main.c @@ -281,7 +281,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { static float y_est[3] = { 0.0f, 0.0f, 0.0f }; static float z_est[3] = { 0.0f, 0.0f, 0.0f }; - bool use_gps = false; int baro_loop_cnt = 0; int baro_loop_end = 70; /* measurement for 1 second */ float baro_alt0 = 0.0f; /* to determin while start up */ @@ -300,8 +299,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { memset(&gps, 0, sizeof(gps)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); - struct vehicle_local_position_s local_pos_est; - memset(&local_pos_est, 0, sizeof(local_pos_est)); + struct vehicle_local_position_s pos; + memset(&pos, 0, sizeof(pos)); /* subscribe */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -311,10 +310,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); /* advertise */ - orb_advert_t local_pos_est_pub = orb_advertise( - ORB_ID(vehicle_local_position), &local_pos_est); + orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &pos); - struct position_estimator_inav_params pos_inav_params; + struct position_estimator_inav_params params; struct position_estimator_inav_param_handles pos_inav_param_handles; /* initialize parameter handles */ parameters_init(&pos_inav_param_handles); @@ -324,21 +322,21 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { struct parameter_update_s param_update; orb_copy(ORB_ID(parameter_update), parameter_update_sub, ¶m_update); /* read from param to clear updated flag */ /* FIRST PARAMETER UPDATE */ - parameters_update(&pos_inav_param_handles, &pos_inav_params); + parameters_update(&pos_inav_param_handles, ¶ms); /* END FIRST PARAMETER UPDATE */ /* wait until gps signal turns valid, only then can we initialize the projection */ - if (use_gps) { - struct pollfd fds_init[1] = { { .fd = vehicle_gps_position_sub, - .events = POLLIN } }; + if (params.use_gps) { + struct pollfd fds_init[1] = { + { .fd = vehicle_gps_position_sub, .events = POLLIN } + }; while (gps.fix_type < 3) { if (poll(fds_init, 1, 5000)) { /* poll only two first subscriptions */ if (fds_init[0].revents & POLLIN) { /* Wait for the GPS update to propagate (we have some time) */ usleep(5000); - orb_copy(ORB_ID(vehicle_gps_position), - vehicle_gps_position_sub, &gps); + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); } } static int printcounter = 0; @@ -354,6 +352,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { lat_current = ((double) (gps.lat)) * 1e-7; lon_current = ((double) (gps.lon)) * 1e-7; alt_current = gps.alt * 1e-3; + + pos.home_lat = lat_current * 1e7; + pos.home_lon = lon_current * 1e7; + pos.home_timestamp = hrt_absolute_time(); + /* initialize coordinates */ map_projection_init(lat_current, lon_current); /* publish global position messages only after first GPS message */ @@ -376,16 +379,18 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { uint32_t pub_interval = 4000; // limit publish rate to 250 Hz /* main loop */ - struct pollfd fds[5] = { { .fd = parameter_update_sub, .events = POLLIN }, { - .fd = vehicle_status_sub, .events = POLLIN }, { .fd = - vehicle_attitude_sub, .events = POLLIN }, { .fd = - sensor_combined_sub, .events = POLLIN }, { .fd = - vehicle_gps_position_sub, .events = POLLIN } }; + struct pollfd fds[5] = { + { .fd = parameter_update_sub, .events = POLLIN }, + { .fd = vehicle_status_sub, .events = POLLIN }, + { .fd = vehicle_attitude_sub, .events = POLLIN }, + { .fd = sensor_combined_sub, .events = POLLIN }, + { .fd = vehicle_gps_position_sub, .events = POLLIN } + }; printf("[position_estimator_inav] main loop started\n"); while (!thread_should_exit) { bool accelerometer_updated = false; bool baro_updated = false; - int ret = poll(fds, 5, 10); // wait maximal this 10 ms = 100 Hz minimum rate + int ret = poll(fds, params.use_gps ? 5 : 4, 10); // wait maximal this 10 ms = 100 Hz minimum rate if (ret < 0) { /* poll error */ printf("[position_estimator_inav] subscriptions poll error\n"); @@ -399,7 +404,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); /* update parameters */ - parameters_update(&pos_inav_param_handles, &pos_inav_params); + parameters_update(&pos_inav_param_handles, ¶ms); } /* vehicle status */ if (fds[1].revents & POLLIN) { @@ -442,7 +447,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } } } - if (use_gps) { + if (params.use_gps) { /* vehicle GPS position */ if (fds[4].revents & POLLIN) { /* new GPS value */ @@ -454,8 +459,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { ((double) (gps.lon)) * 1e-7, &(local_pos_gps[0]), &(local_pos_gps[1])); local_pos_gps[2] = (float) (gps.alt * 1e-3); + pos.valid = gps.fix_type >= 3; gps_updates++; } + } else { + pos.valid = true; } } /* end of poll return value check */ @@ -466,7 +474,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { /* calculate corrected acceleration vector in UAV frame */ float accel_corr[3]; acceleration_correct(accel_corr, sensor.accelerometer_raw, - pos_inav_params.acc_T, pos_inav_params.acc_offs); + params.acc_T, params.acc_offs); /* transform acceleration vector from UAV frame to NED frame */ float accel_NED[3]; for (int i = 0; i < 3; i++) { @@ -491,7 +499,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } if (use_z[0] || use_z[1]) { /* correction */ - kalman_filter_inertial_update(z_est, z_meas, pos_inav_params.k, + kalman_filter_inertial_update(z_est, z_meas, params.k, use_z); } if (verbose_mode) { @@ -513,21 +521,21 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } if (t - pub_last > pub_interval) { pub_last = t; - local_pos_est.x = 0.0f; - local_pos_est.vx = 0.0f; - local_pos_est.y = 0.0f; - local_pos_est.vy = 0.0f; - local_pos_est.z = z_est[0]; - local_pos_est.vz = z_est[1]; - local_pos_est.timestamp = hrt_absolute_time(); - if ((isfinite(local_pos_est.x)) && (isfinite(local_pos_est.vx)) - && (isfinite(local_pos_est.y)) - && (isfinite(local_pos_est.vy)) - && (isfinite(local_pos_est.z)) - && (isfinite(local_pos_est.vz))) { + pos.x = 0.0f; + pos.vx = 0.0f; + pos.y = 0.0f; + pos.vy = 0.0f; + pos.z = z_est[0]; + pos.vz = z_est[1]; + pos.timestamp = hrt_absolute_time(); + if ((isfinite(pos.x)) && (isfinite(pos.vx)) + && (isfinite(pos.y)) + && (isfinite(pos.vy)) + && (isfinite(pos.z)) + && (isfinite(pos.vz))) { orb_publish(ORB_ID( - vehicle_local_position), local_pos_est_pub, - &local_pos_est); + vehicle_local_position), vehicle_local_position_pub, + &pos); } } } diff --git a/apps/position_estimator_inav/position_estimator_inav_params.c b/apps/position_estimator_inav/position_estimator_inav_params.c index fb082fbcb2..d3a165947c 100644 --- a/apps/position_estimator_inav/position_estimator_inav_params.c +++ b/apps/position_estimator_inav/position_estimator_inav_params.c @@ -42,6 +42,8 @@ /* Kalman Filter covariances */ /* gps measurement noise standard deviation */ +PARAM_DEFINE_INT32(INAV_USE_GPS, 0); + PARAM_DEFINE_FLOAT(INAV_K_ALT_00, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_ALT_01, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_ALT_10, 0.0f); @@ -63,8 +65,9 @@ PARAM_DEFINE_FLOAT(INAV_ACC_T_20, 0.0f); PARAM_DEFINE_FLOAT(INAV_ACC_T_21, 0.0f); PARAM_DEFINE_FLOAT(INAV_ACC_T_22, 0.0021f); -int parameters_init(struct position_estimator_inav_param_handles *h) -{ +int parameters_init(struct position_estimator_inav_param_handles *h) { + h->use_gps = param_find("INAV_USE_GPS"); + h->k_alt_00 = param_find("INAV_K_ALT_00"); h->k_alt_01 = param_find("INAV_K_ALT_01"); h->k_alt_10 = param_find("INAV_K_ALT_10"); @@ -88,8 +91,9 @@ int parameters_init(struct position_estimator_inav_param_handles *h) return OK; } -int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) -{ +int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) { + param_get(h->use_gps, &(p->use_gps)); + param_get(h->k_alt_00, &(p->k[0][0])); param_get(h->k_alt_01, &(p->k[0][1])); param_get(h->k_alt_10, &(p->k[1][0])); @@ -97,9 +101,14 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->k_alt_20, &(p->k[2][0])); param_get(h->k_alt_21, &(p->k[2][1])); - param_get(h->acc_offs_0, &(p->acc_offs[0])); - param_get(h->acc_offs_1, &(p->acc_offs[1])); - param_get(h->acc_offs_2, &(p->acc_offs[2])); + /* read int32 and cast to int16 */ + int32_t t; + param_get(h->acc_offs_0, &t); + p->acc_offs[0] = t; + param_get(h->acc_offs_1, &t); + p->acc_offs[1] = t; + param_get(h->acc_offs_2, &t); + p->acc_offs[2] = t; param_get(h->acc_t_00, &(p->acc_T[0][0])); param_get(h->acc_t_01, &(p->acc_T[0][1])); diff --git a/apps/position_estimator_inav/position_estimator_inav_params.h b/apps/position_estimator_inav/position_estimator_inav_params.h index 694bf015bd..51e57a9146 100644 --- a/apps/position_estimator_inav/position_estimator_inav_params.h +++ b/apps/position_estimator_inav/position_estimator_inav_params.h @@ -41,12 +41,15 @@ #include struct position_estimator_inav_params { + int use_gps; float k[3][2]; int16_t acc_offs[3]; float acc_T[3][3]; }; struct position_estimator_inav_param_handles { + param_t use_gps; + param_t k_alt_00; param_t k_alt_01; param_t k_alt_10; From 825957cde0e441ce89be2c44fccacd90c4514b59 Mon Sep 17 00:00:00 2001 From: Anton Date: Mon, 8 Apr 2013 17:08:16 +0400 Subject: [PATCH 062/486] multirotor_pos_control: cleanup, altitude hold mode implemented --- .../multirotor_att_control_main.c | 190 +++++++------- .../multirotor_pos_control.c | 203 ++++++++++----- .../multirotor_pos_control_params.c | 29 ++- .../multirotor_pos_control_params.h | 18 +- .../multirotor_pos_control/position_control.c | 235 ------------------ .../multirotor_pos_control/position_control.h | 50 ---- 6 files changed, 262 insertions(+), 463 deletions(-) delete mode 100644 apps/multirotor_pos_control/position_control.c delete mode 100644 apps/multirotor_pos_control/position_control.h diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index d94c0a69c9..09710f0fcb 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -242,124 +242,124 @@ mc_thread_main(int argc, char *argv[]) } else if (state.flag_control_manual_enabled) { - if (state.flag_control_attitude_enabled) { + if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS) { + /* direct manual input */ + if (state.flag_control_attitude_enabled) { - /* initialize to current yaw if switching to manual or att control */ - if (state.flag_control_attitude_enabled != flag_control_attitude_enabled || - state.flag_control_manual_enabled != flag_control_manual_enabled || - state.flag_system_armed != flag_system_armed) { - att_sp.yaw_body = att.yaw; - } - - static bool rc_loss_first_time = true; - - /* if the RC signal is lost, try to stay level and go slowly back down to ground */ - if (state.rc_signal_lost) { - /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ - param_get(failsafe_throttle_handle, &failsafe_throttle); - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - - /* - * Only go to failsafe throttle if last known throttle was - * high enough to create some lift to make hovering state likely. - * - * This is to prevent that someone landing, but not disarming his - * multicopter (throttle = 0) does not make it jump up in the air - * if shutting down his remote. - */ - if (isfinite(manual.throttle) && manual.throttle > 0.2f) { - att_sp.thrust = failsafe_throttle; - - } else { - att_sp.thrust = 0.0f; - } - - /* keep current yaw, do not attempt to go to north orientation, - * since if the pilot regains RC control, he will be lost regarding - * the current orientation. - */ - if (rc_loss_first_time) - att_sp.yaw_body = att.yaw; - - rc_loss_first_time = false; - - } else { - rc_loss_first_time = true; - - att_sp.roll_body = manual.roll; - att_sp.pitch_body = manual.pitch; - - /* set attitude if arming */ - if (!flag_control_attitude_enabled && state.flag_system_armed) { + /* initialize to current yaw if switching to manual or att control */ + if (state.flag_control_attitude_enabled != flag_control_attitude_enabled || + state.flag_control_manual_enabled != flag_control_manual_enabled || + state.flag_system_armed != flag_system_armed) { att_sp.yaw_body = att.yaw; } - /* act if stabilization is active or if the (nonsense) direct pass through mode is set */ - if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS || - state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { + static bool rc_loss_first_time = true; - if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) { - rates_sp.yaw = manual.yaw; - control_yaw_position = false; + /* if the RC signal is lost, try to stay level and go slowly back down to ground */ + if (state.rc_signal_lost) { + /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ + param_get(failsafe_throttle_handle, &failsafe_throttle); + att_sp.roll_body = 0.0f; + att_sp.pitch_body = 0.0f; + + /* + * Only go to failsafe throttle if last known throttle was + * high enough to create some lift to make hovering state likely. + * + * This is to prevent that someone landing, but not disarming his + * multicopter (throttle = 0) does not make it jump up in the air + * if shutting down his remote. + */ + if (isfinite(manual.throttle) && manual.throttle > 0.2f) { + att_sp.thrust = failsafe_throttle; } else { - /* - * This mode SHOULD be the default mode, which is: - * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS - * - * However, we fall back to this setting for all other (nonsense) - * settings as well. - */ + att_sp.thrust = 0.0f; + } - /* only move setpoint if manual input is != 0 */ - if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { + /* keep current yaw, do not attempt to go to north orientation, + * since if the pilot regains RC control, he will be lost regarding + * the current orientation. + */ + if (rc_loss_first_time) + att_sp.yaw_body = att.yaw; + + rc_loss_first_time = false; + + } else { + rc_loss_first_time = true; + + att_sp.roll_body = manual.roll; + att_sp.pitch_body = manual.pitch; + + /* set attitude if arming */ + if (!flag_control_attitude_enabled && state.flag_system_armed) { + att_sp.yaw_body = att.yaw; + } + + /* act if stabilization is active or if the (nonsense) direct pass through mode is set */ + if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS || + state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { + + if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) { rates_sp.yaw = manual.yaw; control_yaw_position = false; - first_time_after_yaw_speed_control = true; } else { - if (first_time_after_yaw_speed_control) { - att_sp.yaw_body = att.yaw; - first_time_after_yaw_speed_control = false; - } + /* + * This mode SHOULD be the default mode, which is: + * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS + * + * However, we fall back to this setting for all other (nonsense) + * settings as well. + */ - control_yaw_position = true; + /* only move setpoint if manual input is != 0 */ + if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { + rates_sp.yaw = manual.yaw; + control_yaw_position = false; + first_time_after_yaw_speed_control = true; + + } else { + if (first_time_after_yaw_speed_control) { + att_sp.yaw_body = att.yaw; + first_time_after_yaw_speed_control = false; + } + + control_yaw_position = true; + } } } + + att_sp.thrust = manual.throttle; + att_sp.timestamp = hrt_absolute_time(); } - att_sp.thrust = manual.throttle; - att_sp.timestamp = hrt_absolute_time(); - } + if (motor_test_mode) { + printf("testmode"); + att_sp.roll_body = 0.0f; + att_sp.pitch_body = 0.0f; + att_sp.yaw_body = 0.0f; + att_sp.thrust = 0.1f; + att_sp.timestamp = hrt_absolute_time(); + } - /* STEP 2: publish the controller output */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - - if (motor_test_mode) { - printf("testmode"); - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - att_sp.yaw_body = 0.0f; - att_sp.thrust = 0.1f; - att_sp.timestamp = hrt_absolute_time(); - /* STEP 2: publish the result to the vehicle actuators */ + /* STEP 2: publish the controller output */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - } - } else { - /* manual rate inputs, from RC control or joystick */ - if (state.flag_control_rates_enabled && - state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) { - rates_sp.roll = manual.roll; + } else { + /* manual rate inputs, from RC control or joystick */ + if (state.flag_control_rates_enabled && + state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) { + rates_sp.roll = manual.roll; - rates_sp.pitch = manual.pitch; - rates_sp.yaw = manual.yaw; - rates_sp.thrust = manual.throttle; - rates_sp.timestamp = hrt_absolute_time(); + rates_sp.pitch = manual.pitch; + rates_sp.yaw = manual.yaw; + rates_sp.thrust = manual.throttle; + rates_sp.timestamp = hrt_absolute_time(); + } } } - } /** STEP 3: Identify the controller setup to run and set up the inputs correctly */ diff --git a/apps/multirotor_pos_control/multirotor_pos_control.c b/apps/multirotor_pos_control/multirotor_pos_control.c index 7b8d83aa8a..d2b6685aca 100644 --- a/apps/multirotor_pos_control/multirotor_pos_control.c +++ b/apps/multirotor_pos_control/multirotor_pos_control.c @@ -35,7 +35,7 @@ /** * @file multirotor_pos_control.c * - * Skeleton for multirotor position controller + * Multirotor position controller */ #include @@ -52,13 +52,16 @@ #include #include #include +#include #include #include #include #include +#include #include #include #include +#include #include "multirotor_pos_control_params.h" @@ -79,9 +82,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]); */ static void usage(const char *reason); -static void -usage(const char *reason) -{ +static void usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); fprintf(stderr, "usage: deamon {start|stop|status} [-p ]\n\n"); @@ -96,21 +97,20 @@ usage(const char *reason) * The actual stack size should be set in the call * to task_spawn(). */ -int multirotor_pos_control_main(int argc, char *argv[]) -{ +int multirotor_pos_control_main(int argc, char *argv[]) { if (argc < 1) usage("missing command"); if (!strcmp(argv[1], "start")) { if (thread_running) { - printf("multirotor pos control already running\n"); + printf("multirotor_pos_control already running\n"); /* this is not an error */ exit(0); } thread_should_exit = false; - deamon_task = task_spawn("multirotor pos control", + deamon_task = task_spawn("multirotor_pos_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 60, 4096, @@ -126,9 +126,9 @@ int multirotor_pos_control_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { - printf("\tmultirotor pos control app is running\n"); + printf("\tmultirotor_pos_control app is running\n"); } else { - printf("\tmultirotor pos control app not started\n"); + printf("\tmultirotor_pos_control app not started\n"); } exit(0); } @@ -137,98 +137,165 @@ int multirotor_pos_control_main(int argc, char *argv[]) exit(1); } -static int -multirotor_pos_control_thread_main(int argc, char *argv[]) -{ +static int multirotor_pos_control_thread_main(int argc, char *argv[]) { /* welcome user */ - printf("[multirotor pos control] Control started, taking over position control\n"); + printf("[multirotor_pos_control] started\n"); + static int mavlink_fd; + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + mavlink_log_info(mavlink_fd, "[multirotor_pos_control] started"); /* structures */ - struct vehicle_status_s state; + struct vehicle_status_s status; + memset(&status, 0, sizeof(status)); struct vehicle_attitude_s att; - //struct vehicle_global_position_setpoint_s global_pos_sp; - struct vehicle_local_position_setpoint_s local_pos_sp; - struct vehicle_vicon_position_s local_pos; - struct manual_control_setpoint_s manual; + memset(&att, 0, sizeof(att)); struct vehicle_attitude_setpoint_s att_sp; + memset(&att_sp, 0, sizeof(att_sp)); + struct manual_control_setpoint_s manual; + memset(&manual, 0, sizeof(manual)); + struct vehicle_local_position_s local_pos; + memset(&local_pos, 0, sizeof(local_pos)); + struct vehicle_local_position_setpoint_s local_pos_sp; + memset(&local_pos_sp, 0, sizeof(local_pos_sp)); /* subscribe to attitude, motor setpoints and system state */ - int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int param_sub = orb_subscribe(ORB_ID(parameter_update)); int state_sub = orb_subscribe(ORB_ID(vehicle_status)); + int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - int local_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); - //int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); + int local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - /* publish attitude setpoint */ + /* publish setpoint */ + orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &local_pos_sp); orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + bool reset_sp_alt = true; + bool reset_sp_pos = true; + hrt_abstime t_prev = 0; + float alt_integral = 0.0f; + float alt_ctl_dz = 0.2f; + thread_running = true; - int loopcounter = 0; + struct multirotor_position_control_params params; + struct multirotor_position_control_param_handles params_h; + parameters_init(¶ms_h); + parameters_update(¶ms_h, ¶ms); - struct multirotor_position_control_params p; - struct multirotor_position_control_param_handles h; - parameters_init(&h); - parameters_update(&h, &p); + int paramcheck_counter = 0; - - while (1) { + while (!thread_should_exit) { /* get a local copy of the vehicle state */ - orb_copy(ORB_ID(vehicle_status), state_sub, &state); + orb_copy(ORB_ID(vehicle_status), state_sub, &status); /* get a local copy of manual setpoint */ orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); /* get a local copy of attitude */ orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); /* get a local copy of local position */ - orb_copy(ORB_ID(vehicle_vicon_position), local_pos_sub, &local_pos); - /* get a local copy of local position setpoint */ - orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); - - if (loopcounter == 500) { - parameters_update(&h, &p); - loopcounter = 0; + orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); + if (status.state_machine == SYSTEM_STATE_AUTO) { + /* get a local copy of local position setpoint */ + orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); } - // if (state.state_machine == SYSTEM_STATE_AUTO) { - - // XXX IMPLEMENT POSITION CONTROL HERE + /* check parameters at 1 Hz*/ + paramcheck_counter++; + if (paramcheck_counter == 50) { + bool param_updated; + orb_check(param_sub, ¶m_updated); + if (param_updated) { + parameters_update(¶ms_h, ¶ms); + } + paramcheck_counter = 0; + } - float dT = 1.0f / 50.0f; + if (status.state_machine == SYSTEM_STATE_MANUAL) { + if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ALTITUDE || status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE) { + hrt_abstime t = hrt_absolute_time(); + if (reset_sp_alt) { + reset_sp_alt = false; + local_pos_sp.z = local_pos.z; + alt_integral = manual.throttle; + char str[80]; + sprintf(str, "reset alt setpoint: z = %.2f, throttle = %.2f", local_pos_sp.z, manual.throttle); + mavlink_log_info(mavlink_fd, str); + } - float x_setpoint = 0.0f; + float dt; + if (t_prev != 0) { + dt = (t - t_prev) * 0.000001f; + } else { + dt = 0.0f; + } + float err_linear_limit = params.alt_d / params.alt_p * params.alt_rate_max; + /* move altitude setpoint by manual controls */ + float alt_ctl = manual.throttle - 0.5f; + if (fabs(alt_ctl) < alt_ctl_dz) { + alt_ctl = 0.0f; + } else { + if (alt_ctl > 0.0f) + alt_ctl = (alt_ctl - alt_ctl_dz) / (0.5f - alt_ctl_dz); + else + alt_ctl = (alt_ctl + alt_ctl_dz) / (0.5f - alt_ctl_dz); + local_pos_sp.z -= alt_ctl * params.alt_rate_max * dt; + if (local_pos_sp.z > local_pos.z + err_linear_limit) + local_pos_sp.z = local_pos.z + err_linear_limit; + else if (local_pos_sp.z < local_pos.z - err_linear_limit) + local_pos_sp.z = local_pos.z - err_linear_limit; + } - // XXX enable switching between Vicon and local position estimate - /* local pos is the Vicon position */ - - // XXX just an example, lacks rotation around world-body transformation - att_sp.pitch_body = (local_pos.x - x_setpoint) * p.p; - att_sp.roll_body = 0.0f; - att_sp.yaw_body = 0.0f; - att_sp.thrust = 0.3f; - att_sp.timestamp = hrt_absolute_time(); - - /* publish new attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - // } else if (state.state_machine == SYSTEM_STATE_STABILIZED) { - /* set setpoint to current position */ - // XXX select pos reset channel on remote - /* reset setpoint to current position (position hold) */ - // if (1 == 2) { - // local_pos_sp.x = local_pos.x; - // local_pos_sp.y = local_pos.y; - // local_pos_sp.z = local_pos.z; - // local_pos_sp.yaw = att.yaw; - // } - // } + /* PID for altitude */ + float alt_err = local_pos.z - local_pos_sp.z; + /* don't accelerate more than ALT_RATE_MAX, limit error to corresponding value */ + if (alt_err > err_linear_limit) { + alt_err = err_linear_limit; + } else if (alt_err < -err_linear_limit) { + alt_err = -err_linear_limit; + } + /* PID for altitude rate */ + float thrust_ctl_pd = alt_err * params.alt_p + local_pos.vz * params.alt_d; + float thrust_ctl = thrust_ctl_pd + alt_integral; + if (thrust_ctl < params.thr_min) { + thrust_ctl = params.thr_min; + } else if (thrust_ctl > params.thr_max) { + thrust_ctl = params.thr_max; + } else { + /* integrate only in linear area (with 20% gap) and not on min/max throttle */ + if (fabs(thrust_ctl_pd) < err_linear_limit * params.alt_p * 0.8f) + alt_integral += thrust_ctl_pd / params.alt_p * params.alt_i * dt; + } + if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE) { + // TODO add position controller + att_sp.pitch_body = manual.pitch; + att_sp.roll_body = manual.roll; + att_sp.yaw_body = manual.yaw; + } else { + att_sp.pitch_body = manual.pitch; + att_sp.roll_body = manual.roll; + att_sp.yaw_body = manual.yaw; + } + att_sp.thrust = thrust_ctl; + att_sp.timestamp = hrt_absolute_time(); + /* publish local position setpoint */ + orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); + /* publish new attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + t_prev = t; + } else { + reset_sp_alt = true; + } + } else { + reset_sp_alt = true; + } /* run at approximately 50 Hz */ usleep(20000); - loopcounter++; } - printf("[multirotor pos control] ending now...\n"); + printf("[multirotor_pos_control] exiting\n"); + mavlink_log_info(mavlink_fd, "[multirotor_pos_control] exiting"); thread_running = false; diff --git a/apps/multirotor_pos_control/multirotor_pos_control_params.c b/apps/multirotor_pos_control/multirotor_pos_control_params.c index 6b73dc4052..90dd820f43 100644 --- a/apps/multirotor_pos_control/multirotor_pos_control_params.c +++ b/apps/multirotor_pos_control/multirotor_pos_control_params.c @@ -34,29 +34,40 @@ ****************************************************************************/ /* - * @file multirotor_position_control_params.c + * @file multirotor_pos_control_params.c * - * Parameters for EKF filter + * Parameters for multirotor_pos_control */ #include "multirotor_pos_control_params.h" -/* Extended Kalman Filter covariances */ - /* controller parameters */ -PARAM_DEFINE_FLOAT(MC_POS_P, 0.2f); +PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.3f); +PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.7f); +PARAM_DEFINE_FLOAT(MPC_ALT_P, 0.1f); +PARAM_DEFINE_FLOAT(MPC_ALT_I, 0.01f); +PARAM_DEFINE_FLOAT(MPC_ALT_D, 0.2f); +PARAM_DEFINE_FLOAT(MPC_ALT_RATE_MAX, 3.0f); int parameters_init(struct multirotor_position_control_param_handles *h) { + h->thr_min = param_find("MPC_THR_MIN"); + h->thr_max = param_find("MPC_THR_MAX"); /* PID parameters */ - h->p = param_find("MC_POS_P"); - + h->alt_p = param_find("MPC_ALT_P"); + h->alt_i = param_find("MPC_ALT_I"); + h->alt_d = param_find("MPC_ALT_D"); + h->alt_rate_max = param_find("MPC_ALT_RATE_MAX"); return OK; } int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p) { - param_get(h->p, &(p->p)); - + param_get(h->thr_min, &(p->thr_min)); + param_get(h->thr_max, &(p->thr_max)); + param_get(h->alt_p, &(p->alt_p)); + param_get(h->alt_i, &(p->alt_i)); + param_get(h->alt_d, &(p->alt_d)); + param_get(h->alt_rate_max, &(p->alt_rate_max)); return OK; } diff --git a/apps/multirotor_pos_control/multirotor_pos_control_params.h b/apps/multirotor_pos_control/multirotor_pos_control_params.h index 274f6c22a9..849055cd16 100644 --- a/apps/multirotor_pos_control/multirotor_pos_control_params.h +++ b/apps/multirotor_pos_control/multirotor_pos_control_params.h @@ -42,15 +42,21 @@ #include struct multirotor_position_control_params { - float p; - float i; - float d; + float thr_min; + float thr_max; + float alt_p; + float alt_i; + float alt_d; + float alt_rate_max; }; struct multirotor_position_control_param_handles { - param_t p; - param_t i; - param_t d; + param_t thr_min; + param_t thr_max; + param_t alt_p; + param_t alt_i; + param_t alt_d; + param_t alt_rate_max; }; /** diff --git a/apps/multirotor_pos_control/position_control.c b/apps/multirotor_pos_control/position_control.c deleted file mode 100644 index 9c53a5bf6c..0000000000 --- a/apps/multirotor_pos_control/position_control.c +++ /dev/null @@ -1,235 +0,0 @@ -// /**************************************************************************** -// * -// * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. -// * Author: @author Lorenz Meier -// * @author Laurens Mackay -// * @author Tobias Naegeli -// * @author Martin Rutschmann -// * -// * 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 multirotor_position_control.c -// * Implementation of the position control for a multirotor VTOL -// */ - -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include - -// #include "multirotor_position_control.h" - -// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual, -// const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos, -// const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp) -// { -// static PID_t distance_controller; - -// static int read_ret; -// static global_data_position_t position_estimated; - -// static uint16_t counter; - -// static bool initialized; -// static uint16_t pm_counter; - -// static float lat_next; -// static float lon_next; - -// static float pitch_current; - -// static float thrust_total; - - -// if (initialized == false) { - -// pid_init(&distance_controller, -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU], -// PID_MODE_DERIVATIV_CALC, 150);//150 - -// // pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM]; -// // pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM]; - -// thrust_total = 0.0f; - -// /* Position initialization */ -// /* Wait for new position estimate */ -// do { -// read_ret = read_lock_position(&position_estimated); -// } while (read_ret != 0); - -// lat_next = position_estimated.lat; -// lon_next = position_estimated.lon; - -// /* attitude initialization */ -// global_data_lock(&global_data_attitude->access_conf); -// pitch_current = global_data_attitude->pitch; -// global_data_unlock(&global_data_attitude->access_conf); - -// initialized = true; -// } - -// /* load new parameters with 10Hz */ -// if (counter % 50 == 0) { -// if (global_data_trylock(&global_data_parameter_storage->access_conf) == 0) { -// /* check whether new parameters are available */ -// if (global_data_parameter_storage->counter > pm_counter) { -// pid_set_parameters(&distance_controller, -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU]); - -// // -// // pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM]; -// // pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM]; - -// pm_counter = global_data_parameter_storage->counter; -// printf("Position controller changed pid parameters\n"); -// } -// } - -// global_data_unlock(&global_data_parameter_storage->access_conf); -// } - - -// /* Wait for new position estimate */ -// do { -// read_ret = read_lock_position(&position_estimated); -// } while (read_ret != 0); - -// /* Get next waypoint */ //TODO: add local copy - -// if (0 == global_data_trylock(&global_data_position_setpoint->access_conf)) { -// lat_next = global_data_position_setpoint->x; -// lon_next = global_data_position_setpoint->y; -// global_data_unlock(&global_data_position_setpoint->access_conf); -// } - -// /* Get distance to waypoint */ -// float distance_to_waypoint = get_distance_to_next_waypoint(position_estimated.lat , position_estimated.lon, lat_next, lon_next); -// // if(counter % 5 == 0) -// // printf("distance_to_waypoint: %.4f\n", distance_to_waypoint); - -// /* Get bearing to waypoint (direction on earth surface to next waypoint) */ -// float bearing = get_bearing_to_next_waypoint(position_estimated.lat, position_estimated.lon, lat_next, lon_next); - -// if (counter % 5 == 0) -// printf("bearing: %.4f\n", bearing); - -// /* Calculate speed in direction of bearing (needed for controller) */ -// float speed_norm = sqrtf(position_estimated.vx * position_estimated.vx + position_estimated.vy * position_estimated.vy); -// // if(counter % 5 == 0) -// // printf("speed_norm: %.4f\n", speed_norm); -// float speed_to_waypoint = 0; //(position_estimated.vx * cosf(bearing) + position_estimated.vy * sinf(bearing))/speed_norm; //FIXME, TODO: re-enable this once we have a full estimate of the speed, then we can do a PID for the distance controller - -// /* Control Thrust in bearing direction */ -// float horizontal_thrust = -pid_calculate(&distance_controller, 0, distance_to_waypoint, speed_to_waypoint, -// CONTROL_PID_POSITION_INTERVAL); //TODO: maybe this "-" sign is an error somewhere else - -// // if(counter % 5 == 0) -// // printf("horizontal thrust: %.4f\n", horizontal_thrust); - -// /* Get total thrust (from remote for now) */ -// if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) { -// thrust_total = (float)global_data_rc_channels->chan[THROTTLE].scale; //TODO: how should we use the RC_CHANNELS_FUNCTION enum? -// global_data_unlock(&global_data_rc_channels->access_conf); -// } - -// const float max_gas = 500.0f; -// thrust_total *= max_gas / 20000.0f; //TODO: check this -// thrust_total += max_gas / 2.0f; - - -// if (horizontal_thrust > thrust_total) { -// horizontal_thrust = thrust_total; - -// } else if (horizontal_thrust < -thrust_total) { -// horizontal_thrust = -thrust_total; -// } - - - -// //TODO: maybe we want to add a speed controller later... - -// /* Calclulate thrust in east and north direction */ -// float thrust_north = cosf(bearing) * horizontal_thrust; -// float thrust_east = sinf(bearing) * horizontal_thrust; - -// if (counter % 10 == 0) { -// printf("thrust north: %.4f\n", thrust_north); -// printf("thrust east: %.4f\n", thrust_east); -// fflush(stdout); -// } - -// /* Get current attitude */ -// if (0 == global_data_trylock(&global_data_attitude->access_conf)) { -// pitch_current = global_data_attitude->pitch; -// global_data_unlock(&global_data_attitude->access_conf); -// } - -// /* Get desired pitch & roll */ -// float pitch_desired = 0.0f; -// float roll_desired = 0.0f; - -// if (thrust_total != 0) { -// float pitch_fraction = -thrust_north / thrust_total; -// float roll_fraction = thrust_east / (cosf(pitch_current) * thrust_total); - -// if (roll_fraction < -1) { -// roll_fraction = -1; - -// } else if (roll_fraction > 1) { -// roll_fraction = 1; -// } - -// // if(counter % 5 == 0) -// // { -// // printf("pitch_fraction: %.4f, roll_fraction: %.4f\n",pitch_fraction, roll_fraction); -// // fflush(stdout); -// // } - -// pitch_desired = asinf(pitch_fraction); -// roll_desired = asinf(roll_fraction); -// } - -// att_sp.roll = roll_desired; -// att_sp.pitch = pitch_desired; - -// counter++; -// } diff --git a/apps/multirotor_pos_control/position_control.h b/apps/multirotor_pos_control/position_control.h deleted file mode 100644 index 2144ebc343..0000000000 --- a/apps/multirotor_pos_control/position_control.h +++ /dev/null @@ -1,50 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * @author Laurens Mackay - * @author Tobias Naegeli - * @author Martin Rutschmann - * - * 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 multirotor_position_control.h - * Definition of the position control for a multirotor VTOL - */ - -// #ifndef POSITION_CONTROL_H_ -// #define POSITION_CONTROL_H_ - -// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual, -// const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos, -// const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp); - -// #endif /* POSITION_CONTROL_H_ */ From eb3d6f228cd96ef2a2b8a33f667dd1cdc2d5fdc5 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 8 Apr 2013 21:53:23 -0700 Subject: [PATCH 063/486] Added some functions for changing rates etc (WIP) --- src/drivers/lsm303d/lsm303d.cpp | 377 ++++++++++++++++++++++---------- 1 file changed, 257 insertions(+), 120 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 32030a1f77..ff56bff17b 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -108,8 +108,10 @@ static const int ERROR = -1; #define ADDR_CTRL_REG3 0x22 #define ADDR_CTRL_REG4 0x23 #define ADDR_CTRL_REG5 0x24 +#define ADDR_CTRL_REG6 0x25 #define ADDR_CTRL_REG7 0x26 +#define REG1_RATE_BITS_A ((1<<7) | (1<<6) | (1<<5) | (1<<4)) #define REG1_POWERDOWN_A ((0<<7) | (0<<6) | (0<<5) | (0<<4)) #define REG1_RATE_3_125HZ_A ((0<<7) | (0<<6) | (0<<5) | (1<<4)) #define REG1_RATE_6_25HZ_A ((0<<7) | (0<<6) | (1<<5) | (0<<4)) @@ -127,11 +129,13 @@ static const int ERROR = -1; #define REG1_Y_ENABLE_A (1<<1) #define REG1_X_ENABLE_A (1<<0) +#define REG2_ANTIALIAS_FILTER_BW_BITS_A ((1<<7) | (1<<6)) #define REG2_AA_FILTER_BW_773HZ_A ((0<<7) | (0<<6)) #define REG2_AA_FILTER_BW_194HZ_A ((0<<7) | (1<<6)) #define REG2_AA_FILTER_BW_362HZ_A ((1<<7) | (0<<6)) #define REG2_AA_FILTER_BW_50HZ_A ((1<<7) | (1<<6)) +#define REG2_FULL_SCALE_BITS_A ((1<<5) | (1<<4) | (1<<3)) #define REG2_FULL_SCALE_2G_A ((0<<5) | (0<<4) | (0<<3)) #define REG2_FULL_SCALE_4G_A ((0<<5) | (0<<4) | (1<<3)) #define REG2_FULL_SCALE_6G_A ((0<<5) | (1<<4) | (0<<3)) @@ -143,6 +147,7 @@ static const int ERROR = -1; #define REG5_RES_HIGH_M ((1<<6) | (1<<5)) #define REG5_RES_LOW_M ((0<<6) | (0<<5)) +#define REG5_RATE_BITS_M ((1<<4) | (1<<3) | (1<<2)) #define REG5_RATE_3_125HZ_M ((0<<4) | (0<<3) | (0<<2)) #define REG5_RATE_6_25HZ_M ((0<<4) | (0<<3) | (1<<2)) #define REG5_RATE_12_5HZ_M ((0<<4) | (1<<3) | (0<<2)) @@ -151,6 +156,7 @@ static const int ERROR = -1; #define REG5_RATE_100HZ_M ((1<<4) | (0<<3) | (1<<2)) #define REG5_RATE_DO_NOT_USE_M ((1<<4) | (1<<3) | (0<<2)) +#define REG6_FULL_SCALE_BITS_M ((1<<7) | (1<<6)) #define REG6_FULL_SCALE_2GA_M ((0<<7) | (0<<6)) #define REG6_FULL_SCALE_4GA_M ((0<<7) | (1<<6)) #define REG6_FULL_SCALE_8GA_M ((1<<7) | (0<<6)) @@ -174,72 +180,66 @@ public: LSM303D(int bus, const char* path, spi_dev_e device); virtual ~LSM303D(); - virtual int init(); + virtual int init(); virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); /** * Diagnostics - print some basic information about the driver. */ - void print_info(); + void print_info(); protected: - virtual int probe(); + virtual int probe(); - friend class LSM303D_mag; + friend class LSM303D_mag; virtual ssize_t mag_read(struct file *filp, char *buffer, size_t buflen); - virtual int mag_ioctl(struct file *filp, int cmd, unsigned long arg); + virtual int mag_ioctl(struct file *filp, int cmd, unsigned long arg); private: - LSM303D_mag *_mag; + LSM303D_mag *_mag; struct hrt_call _accel_call; struct hrt_call _mag_call; - unsigned _call_accel_interval; - unsigned _call_mag_interval; + unsigned _call_accel_interval; + unsigned _call_mag_interval; - unsigned _num_accel_reports; + unsigned _num_accel_reports; volatile unsigned _next_accel_report; volatile unsigned _oldest_accel_report; struct accel_report *_accel_reports; struct accel_scale _accel_scale; - float _accel_range_scale; - float _accel_range_m_s2; + float _accel_range_scale; + float _accel_range_m_s2; orb_advert_t _accel_topic; - unsigned _num_mag_reports; + unsigned _num_mag_reports; volatile unsigned _next_mag_report; volatile unsigned _oldest_mag_report; struct mag_report *_mag_reports; struct mag_scale _mag_scale; - float _mag_range_scale; - float _mag_range_ga; + float _mag_range_scale; + float _mag_range_ga; orb_advert_t _mag_topic; - unsigned _current_accel_rate; - unsigned _current_accel_range; - - unsigned _current_mag_rate; - unsigned _current_mag_range; - perf_counter_t _accel_sample_perf; perf_counter_t _mag_sample_perf; /** * Start automatic measurement. */ - void start(); + void start(); /** * Stop automatic measurement. */ - void stop(); + void stop(); /** * Static trampoline from the hrt_call context; because we don't have a @@ -250,24 +250,24 @@ private: * * @param arg Instance pointer for the driver that is polling. */ - static void measure_trampoline(void *arg); + static void measure_trampoline(void *arg); /** * Static trampoline for the mag because it runs at a lower rate * * @param arg Instance pointer for the driver that is polling. */ - static void mag_measure_trampoline(void *arg); + static void mag_measure_trampoline(void *arg); /** * Fetch accel measurements from the sensor and update the report ring. */ - void measure(); + void measure(); /** * Fetch mag measurements from the sensor and update the report ring. */ - void mag_measure(); + void mag_measure(); /** * Read a register from the LSM303D @@ -275,7 +275,7 @@ private: * @param The register to read. * @return The value that was read. */ - uint8_t read_reg(unsigned reg); + uint8_t read_reg(unsigned reg); /** * Write a register in the LSM303D @@ -283,7 +283,7 @@ private: * @param reg The register to write. * @param value The new value to write. */ - void write_reg(unsigned reg, uint8_t value); + void write_reg(unsigned reg, uint8_t value); /** * Modify a register in the LSM303D @@ -294,27 +294,54 @@ private: * @param clearbits Bits in the register to clear. * @param setbits Bits in the register to set. */ - void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); + void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); /** - * Set the LSM303D measurement range. + * Set the LSM303D accel measurement range. * - * @param max_dps The measurement range is set to permit reading at least - * this rate in degrees per second. + * @param max_g The measurement range of the accel is in g (9.81m/s^2) * Zero selects the maximum supported range. * @return OK if the value can be supported, -ERANGE otherwise. */ - int set_range(unsigned max_dps); + int set_range(unsigned max_g); /** - * Set the LSM303D internal sampling frequency. + * Set the LSM303D mag measurement range. * - * @param frequency The internal sampling frequency is set to not less than + * @param max_ga The measurement range of the mag is in Ga + * Zero selects the maximum supported range. + * @return OK if the value can be supported, -ERANGE otherwise. + */ + int mag_set_range(unsigned max_g); + + /** + * Set the LSM303D accel anti-alias filter. + * + * @param bandwidth The anti-alias filter bandwidth in Hz + * Zero selects the highest bandwidth + * @return OK if the value can be supported, -ERANGE otherwise. + */ + int set_antialias_filter_bandwidth(unsigned max_g); + + /** + * Set the LSM303D internal accel sampling frequency. + * + * @param frequency The internal accel sampling frequency is set to not less than * this value. * Zero selects the maximum rate supported. * @return OK if the value can be supported. */ - int set_samplerate(unsigned frequency); + int set_samplerate(unsigned frequency); + + /** + * Set the LSM303D internal mag sampling frequency. + * + * @param frequency The internal mag sampling frequency is set to not less than + * this value. + * Zero selects the maximum rate supported. + * @return OK if the value can be supported. + */ + int mag_set_samplerate(unsigned frequency); }; /** @@ -327,18 +354,18 @@ public: ~LSM303D_mag(); virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); protected: friend class LSM303D; - void parent_poll_notify(); + void parent_poll_notify(); private: - LSM303D *_parent; + LSM303D *_parent; - void measure(); + void measure(); - void measure_trampoline(void *arg); + void measure_trampoline(void *arg); }; @@ -364,19 +391,13 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _mag_reports(nullptr), _mag_range_scale(0.0f), _mag_range_ga(0.0f), - _current_accel_rate(0), - _current_accel_range(0), - _current_mag_rate(0), - _current_mag_range(0), _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")) { // enable debug() calls _debug_enabled = true; - /* XXX fix this default values */ - _accel_range_scale = 1.0f; - _mag_range_scale = 1.0f; + _mag_range_scale = 12.0f/32767.0f; // default scale factors _accel_scale.x_offset = 0; @@ -446,18 +467,21 @@ LSM303D::init() memset(&_mag_reports[0], 0, sizeof(_mag_reports[0])); _mag_topic = orb_advertise(ORB_ID(sensor_mag), &_mag_reports[0]); - /* XXX do this with ioctls */ - /* set default configuration */ - write_reg(ADDR_CTRL_REG1, REG1_RATE_400HZ_A | REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A); + /* enable accel, XXX do this with an ioctl? */ + write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A); + + /* enable mag, XXX do this with an ioctl? */ write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); - write_reg(ADDR_CTRL_REG5, REG5_RATE_100HZ_M | REG5_RES_HIGH_M); + write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); - /* XXX should we enable FIFO */ + /* XXX should we enable FIFO? */ - set_range(500); /* default to 500dps */ - set_samplerate(0); /* max sample rate */ + set_range(8); /* XXX 16G mode seems wrong (shows 6 instead of 9.8m/s^2, therefore use 8G for now */ + set_antialias_filter_bandwidth(194); /* XXX: choose bandwidth: 50, 194, 362 or 773 Hz */ + set_samplerate(400); /* max sample rate */ -// _current_accel_rate = 100; + mag_set_range(12); /* XXX: take highest sensor range of 12GA? */ + mag_set_samplerate(100); /* XXX test this when another mag is used */ /* do CDev init for the mag device node, keep it optional */ @@ -590,9 +614,13 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: + + return ioctl(filp, SENSORIOCSPOLLRATE, 1600); + case SENSOR_POLLRATE_DEFAULT: /* With internal low pass filters enabled, 250 Hz is sufficient */ - return ioctl(filp, SENSORIOCSPOLLRATE, 250); + /* XXX for vibration tests with 800 Hz */ + return ioctl(filp, SENSORIOCSPOLLRATE, 800); /* adjust to a legal polling interval in Hz */ default: { @@ -606,6 +634,9 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) if (ticks < 1000) return -EINVAL; + /* adjust sample rate of sensor */ + set_samplerate(arg); + /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ _accel_call.period = _call_accel_interval = ticks; @@ -801,33 +832,111 @@ LSM303D::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) } int -LSM303D::set_range(unsigned max_dps) +LSM303D::set_range(unsigned max_g) { - /* XXX implement this */ -// uint8_t bits = REG4_BDU; -// -// if (max_dps == 0) -// max_dps = 2000; -// -// if (max_dps <= 250) { -// _current_range = 250; -// bits |= RANGE_250DPS; -// -// } else if (max_dps <= 500) { -// _current_range = 500; -// bits |= RANGE_500DPS; -// -// } else if (max_dps <= 2000) { -// _current_range = 2000; -// bits |= RANGE_2000DPS; -// -// } else { -// return -EINVAL; -// } -// -// _gyro_range_rad_s = _current_range / 180.0f * M_PI_F; -// _gyro_range_scale = _gyro_range_rad_s / 32768.0f; -// write_reg(ADDR_CTRL_REG4, bits); + uint8_t setbits = 0; + uint8_t clearbits = REG2_FULL_SCALE_BITS_A; + float new_range = 0.0f; + + if (max_g == 0) + max_g = 16; + + if (max_g <= 2) { + new_range = 2.0f; + setbits |= REG2_FULL_SCALE_2G_A; + + } else if (max_g <= 4) { + new_range = 4.0f; + setbits |= REG2_FULL_SCALE_4G_A; + + } else if (max_g <= 6) { + new_range = 6.0f; + setbits |= REG2_FULL_SCALE_6G_A; + + } else if (max_g <= 8) { + new_range = 8.0f; + setbits |= REG2_FULL_SCALE_8G_A; + + } else if (max_g <= 16) { + new_range = 16.0f; + setbits |= REG2_FULL_SCALE_16G_A; + + } else { + return -EINVAL; + } + + _accel_range_m_s2 = new_range * 9.80665f; + _accel_range_scale = _accel_range_m_s2 / 32768.0f; + + modify_reg(ADDR_CTRL_REG2, clearbits, setbits); + + return OK; +} + +int +LSM303D::mag_set_range(unsigned max_ga) +{ + uint8_t setbits = 0; + uint8_t clearbits = REG6_FULL_SCALE_BITS_M; + float new_range = 0.0f; + + if (max_ga == 0) + max_ga = 12; + + if (max_ga <= 2) { + new_range = 2.0f; + setbits |= REG6_FULL_SCALE_2GA_M; + + } else if (max_ga <= 4) { + new_range = 4.0f; + setbits |= REG6_FULL_SCALE_4GA_M; + + } else if (max_ga <= 8) { + new_range = 8.0f; + setbits |= REG6_FULL_SCALE_8GA_M; + + } else if (max_ga <= 12) { + new_range = 12.0f; + setbits |= REG6_FULL_SCALE_12GA_M; + + } else { + return -EINVAL; + } + + _mag_range_ga = new_range; + _mag_range_scale = _mag_range_ga / 32768.0f; + + modify_reg(ADDR_CTRL_REG6, clearbits, setbits); + + return OK; +} + +int +LSM303D::set_antialias_filter_bandwidth(unsigned bandwidth) +{ + uint8_t setbits = 0; + uint8_t clearbits = REG2_ANTIALIAS_FILTER_BW_BITS_A; + + if (bandwidth == 0) + bandwidth = 773; + + if (bandwidth <= 50) { + setbits |= REG2_AA_FILTER_BW_50HZ_A; + + } else if (bandwidth <= 194) { + setbits |= REG2_AA_FILTER_BW_194HZ_A; + + } else if (bandwidth <= 362) { + setbits |= REG2_AA_FILTER_BW_362HZ_A; + + } else if (bandwidth <= 773) { + setbits |= REG2_AA_FILTER_BW_773HZ_A; + + } else { + return -EINVAL; + } + + modify_reg(ADDR_CTRL_REG2, clearbits, setbits); return OK; } @@ -835,33 +944,60 @@ LSM303D::set_range(unsigned max_dps) int LSM303D::set_samplerate(unsigned frequency) { - /* XXX implement this */ -// uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE; -// -// if (frequency == 0) -// frequency = 760; -// -// if (frequency <= 95) { -// _current_rate = 95; -// bits |= RATE_95HZ_LP_25HZ; -// -// } else if (frequency <= 190) { -// _current_rate = 190; -// bits |= RATE_190HZ_LP_25HZ; -// -// } else if (frequency <= 380) { -// _current_rate = 380; -// bits |= RATE_380HZ_LP_30HZ; -// -// } else if (frequency <= 760) { -// _current_rate = 760; -// bits |= RATE_760HZ_LP_30HZ; -// -// } else { -// return -EINVAL; -// } -// -// write_reg(ADDR_CTRL_REG1, bits); + uint8_t setbits = 0; + uint8_t clearbits = REG1_RATE_BITS_A; + + if (frequency == 0) + frequency = 1600; + + if (frequency <= 100) { + setbits |= REG1_RATE_100HZ_A; + + } else if (frequency <= 200) { + setbits |= REG1_RATE_200HZ_A; + + } else if (frequency <= 400) { + setbits |= REG1_RATE_400HZ_A; + + } else if (frequency <= 800) { + setbits |= REG1_RATE_800HZ_A; + + } else if (frequency <= 1600) { + setbits |= REG1_RATE_1600HZ_A; + + } else { + return -EINVAL; + } + + modify_reg(ADDR_CTRL_REG1, clearbits, setbits); + + return OK; +} + +int +LSM303D::mag_set_samplerate(unsigned frequency) +{ + uint8_t setbits = 0; + uint8_t clearbits = REG5_RATE_BITS_M; + + if (frequency == 0) + frequency = 100; + + if (frequency <= 25) { + setbits |= REG5_RATE_25HZ_M; + + } else if (frequency <= 50) { + setbits |= REG5_RATE_50HZ_M; + + } else if (frequency <= 100) { + setbits |= REG5_RATE_100HZ_M; + + + } else { + return -EINVAL; + } + + modify_reg(ADDR_CTRL_REG5, clearbits, setbits); return OK; } @@ -956,8 +1092,8 @@ LSM303D::measure() accel_report->x = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; accel_report->y = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; accel_report->z = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; -// report->scaling = _gyro_range_scale; -// report->range_rad_s = _gyro_range_rad_s; + accel_report->scaling = _accel_range_scale; + accel_report->range_m_s2 = _accel_range_m_s2; /* post a report to the ring - note, not locked */ INCREMENT(_next_accel_report, _num_accel_reports); @@ -1183,15 +1319,13 @@ test() if (sz != sizeof(a_report)) err(1, "immediate read failed"); - /* XXX fix the test output */ -// warnx("accel x: \t% 9.5f\tm/s^2", (double)a_report.x); -// warnx("accel y: \t% 9.5f\tm/s^2", (double)a_report.y); -// warnx("accel z: \t% 9.5f\tm/s^2", (double)a_report.z); + warnx("accel x: \t% 9.5f\tm/s^2", (double)a_report.x); + warnx("accel y: \t% 9.5f\tm/s^2", (double)a_report.y); + warnx("accel z: \t% 9.5f\tm/s^2", (double)a_report.z); warnx("accel x: \t%d\traw", (int)a_report.x_raw); warnx("accel y: \t%d\traw", (int)a_report.y_raw); warnx("accel z: \t%d\traw", (int)a_report.z_raw); -// warnx("accel range: %8.4f m/s^2", (double)a_report.range_m_s2); - + warnx("accel range: %8.4f m/s^2", (double)a_report.range_m_s2); int fd_mag = -1; @@ -1209,10 +1343,13 @@ test() if (sz != sizeof(m_report)) err(1, "immediate read failed"); - /* XXX fix the test output */ + warnx("mag x: \t% 9.5f\tga", (double)m_report.x); + warnx("mag y: \t% 9.5f\tga", (double)m_report.y); + warnx("mag z: \t% 9.5f\tga", (double)m_report.z); warnx("mag x: \t%d\traw", (int)m_report.x_raw); warnx("mag y: \t%d\traw", (int)m_report.y_raw); warnx("mag z: \t%d\traw", (int)m_report.z_raw); + warnx("mag range: %8.4f ga", (double)m_report.range_ga); /* XXX add poll-rate tests here too */ From 1d327c42a6f34f8545940cd8630586726e4d3ae6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 8 Apr 2013 22:04:21 -0700 Subject: [PATCH 064/486] Mag sample rate was not actually changed by an ioctl --- src/drivers/lsm303d/lsm303d.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index ff56bff17b..b107b869fc 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -718,7 +718,7 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: /* 50 Hz is max for mag */ - return mag_ioctl(filp, SENSORIOCSPOLLRATE, 50); + return mag_ioctl(filp, SENSORIOCSPOLLRATE, 100); /* adjust to a legal polling interval in Hz */ default: { @@ -732,12 +732,13 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) if (ticks < 1000) return -EINVAL; + /* adjust sample rate of sensor */ + mag_set_samplerate(arg); + /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ _mag_call.period = _call_mag_interval = ticks; - - /* if we need to start the poll state machine, do it */ if (want_start) start(); From 3469fefe117f8fa3bfef3fbcb3751a32e81c324a Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 8 Apr 2013 23:47:19 -0700 Subject: [PATCH 065/486] Checked axes of LSM303D --- src/drivers/lsm303d/lsm303d.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index b107b869fc..8a64ee7024 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1067,7 +1067,6 @@ LSM303D::measure() raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT; transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report)); - /* XXX adapt the comment to specs */ /* * 1) Scale raw value to SI units using scaling from datasheet. * 2) Subtract static offset (in SI units) @@ -1085,7 +1084,7 @@ LSM303D::measure() accel_report->timestamp = hrt_absolute_time(); - /* XXX adjust for sensor alignment to board here */ + accel_report->x_raw = raw_accel_report.x; accel_report->y_raw = raw_accel_report.y; accel_report->z_raw = raw_accel_report.z; @@ -1136,7 +1135,6 @@ LSM303D::mag_measure() raw_mag_report.cmd = ADDR_STATUS_M | DIR_READ | ADDR_INCREMENT; transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report)); - /* XXX adapt the comment to specs */ /* * 1) Scale raw value to SI units using scaling from datasheet. * 2) Subtract static offset (in SI units) @@ -1154,15 +1152,15 @@ LSM303D::mag_measure() mag_report->timestamp = hrt_absolute_time(); - /* XXX adjust for sensor alignment to board here */ + mag_report->x_raw = raw_mag_report.x; mag_report->y_raw = raw_mag_report.y; mag_report->z_raw = raw_mag_report.z; mag_report->x = ((mag_report->x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; mag_report->y = ((mag_report->y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; mag_report->z = ((mag_report->z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; -// report->scaling = _gyro_range_scale; -// report->range_rad_s = _gyro_range_rad_s; + mag_report->scaling = _mag_range_scale; + mag_report->range_ga = _mag_range_ga; /* post a report to the ring - note, not locked */ INCREMENT(_next_mag_report, _num_mag_reports); From 1a8cca92e9a7b8fad153042a44c1754f31a2745b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 8 Apr 2013 23:29:24 -0700 Subject: [PATCH 066/486] Fixed axis in L3GD20 driver --- apps/drivers/l3gd20/l3gd20.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/apps/drivers/l3gd20/l3gd20.cpp b/apps/drivers/l3gd20/l3gd20.cpp index 6227df72aa..c7f433dd47 100644 --- a/apps/drivers/l3gd20/l3gd20.cpp +++ b/apps/drivers/l3gd20/l3gd20.cpp @@ -684,9 +684,10 @@ L3GD20::measure() * 74 from all measurements centers them around zero. */ report->timestamp = hrt_absolute_time(); - /* XXX adjust for sensor alignment to board here */ - report->x_raw = raw_report.x; - report->y_raw = raw_report.y; + + /* swap x and y and negate y */ + report->x_raw = raw_report.y; + report->y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x); report->z_raw = raw_report.z; report->x = ((report->x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; From 64ec950c58c693cf1a9c0c5feadcee96cc90c4cd Mon Sep 17 00:00:00 2001 From: Pat Hickey Date: Sun, 14 Apr 2013 20:53:42 -0700 Subject: [PATCH 067/486] px4iov2 nsh boots --- apps/drivers/boards/px4iov2/Makefile | 41 + apps/drivers/boards/px4iov2/module.mk | 5 + apps/drivers/boards/px4iov2/px4iov2_init.c | 172 ++++ .../drivers/boards/px4iov2/px4iov2_internal.h | 135 +++ nuttx/configs/px4iov2/README.txt | 806 ++++++++++++++++++ nuttx/configs/px4iov2/common/Make.defs | 175 ++++ nuttx/configs/px4iov2/common/ld.script | 129 +++ nuttx/configs/px4iov2/common/setenv.sh | 47 + nuttx/configs/px4iov2/include/README.txt | 1 + nuttx/configs/px4iov2/include/board.h | 173 ++++ .../configs/px4iov2/include/drv_i2c_device.h | 42 + nuttx/configs/px4iov2/io/Make.defs | 3 + nuttx/configs/px4iov2/io/appconfig | 40 + nuttx/configs/px4iov2/io/defconfig | 548 ++++++++++++ nuttx/configs/px4iov2/io/setenv.sh | 47 + nuttx/configs/px4iov2/nsh/Make.defs | 3 + nuttx/configs/px4iov2/nsh/appconfig | 44 + nuttx/configs/px4iov2/nsh/defconfig | 567 ++++++++++++ nuttx/configs/px4iov2/nsh/setenv.sh | 47 + nuttx/configs/px4iov2/src/Makefile | 84 ++ nuttx/configs/px4iov2/src/README.txt | 1 + nuttx/configs/px4iov2/src/drv_i2c_device.c | 618 ++++++++++++++ nuttx/configs/px4iov2/src/empty.c | 4 + 23 files changed, 3732 insertions(+) create mode 100644 apps/drivers/boards/px4iov2/Makefile create mode 100644 apps/drivers/boards/px4iov2/module.mk create mode 100644 apps/drivers/boards/px4iov2/px4iov2_init.c create mode 100644 apps/drivers/boards/px4iov2/px4iov2_internal.h create mode 100755 nuttx/configs/px4iov2/README.txt create mode 100644 nuttx/configs/px4iov2/common/Make.defs create mode 100755 nuttx/configs/px4iov2/common/ld.script create mode 100755 nuttx/configs/px4iov2/common/setenv.sh create mode 100755 nuttx/configs/px4iov2/include/README.txt create mode 100755 nuttx/configs/px4iov2/include/board.h create mode 100644 nuttx/configs/px4iov2/include/drv_i2c_device.h create mode 100644 nuttx/configs/px4iov2/io/Make.defs create mode 100644 nuttx/configs/px4iov2/io/appconfig create mode 100755 nuttx/configs/px4iov2/io/defconfig create mode 100755 nuttx/configs/px4iov2/io/setenv.sh create mode 100644 nuttx/configs/px4iov2/nsh/Make.defs create mode 100644 nuttx/configs/px4iov2/nsh/appconfig create mode 100755 nuttx/configs/px4iov2/nsh/defconfig create mode 100755 nuttx/configs/px4iov2/nsh/setenv.sh create mode 100644 nuttx/configs/px4iov2/src/Makefile create mode 100644 nuttx/configs/px4iov2/src/README.txt create mode 100644 nuttx/configs/px4iov2/src/drv_i2c_device.c create mode 100644 nuttx/configs/px4iov2/src/empty.c diff --git a/apps/drivers/boards/px4iov2/Makefile b/apps/drivers/boards/px4iov2/Makefile new file mode 100644 index 0000000000..85806fe6f5 --- /dev/null +++ b/apps/drivers/boards/px4iov2/Makefile @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (C) 2012 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. +# +############################################################################ + +# +# Board-specific startup code for the PX4IO +# + +INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common +LIBNAME = brd_px4io + +include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/boards/px4iov2/module.mk b/apps/drivers/boards/px4iov2/module.mk new file mode 100644 index 0000000000..d596ce4db9 --- /dev/null +++ b/apps/drivers/boards/px4iov2/module.mk @@ -0,0 +1,5 @@ +# +# Board-specific startup code for the PX4IOv2 +# + +SRCS = px4iov2_init.c diff --git a/apps/drivers/boards/px4iov2/px4iov2_init.c b/apps/drivers/boards/px4iov2/px4iov2_init.c new file mode 100644 index 0000000000..711bee4257 --- /dev/null +++ b/apps/drivers/boards/px4iov2/px4iov2_init.c @@ -0,0 +1,172 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 px4iov2_init.c + * + * PX4FMU-specific early startup code. This file implements the + * nsh_archinitialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialisation. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include + +#include + +#include "stm32_internal.h" +#include "px4iov2_internal.h" + +#include + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +/* Debug ********************************************************************/ + +#ifdef CONFIG_CPP_HAVE_VARARGS +# ifdef CONFIG_DEBUG +# define message(...) lowsyslog(__VA_ARGS__) +# else +# define message(...) printf(__VA_ARGS__) +# endif +#else +# ifdef CONFIG_DEBUG +# define message lowsyslog +# else +# define message printf +# endif +#endif + +/**************************************************************************** + * Protected Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void stm32_boardinitialize(void) +{ + + /* configure GPIOs */ + + /* turn off - all leds are active low */ + stm32_gpiowrite(BOARD_GPIO_LED1, true); + stm32_gpiowrite(BOARD_GPIO_LED2, true); + stm32_gpiowrite(BOARD_GPIO_LED3, true); + stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_LED1)); + stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_LED2)); + stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_LED3)); + + + stm32_configgpio(BOARD_GPIO_INPUT_FLOAT(BOARD_GPIO_BTN_SAFETY)); + + /* spektrum power enable is active high - disable it by default */ + stm32_gpiowrite(BOARD_GPIO_SPEKTRUM_PWR_EN, false); + stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_SPEKTRUM_PWR_EN)); + + /* servo power enable is active low, and has a pull down resistor + * to keep it low during boot (since it may power the whole board.) + */ + stm32_gpiowrite(BOARD_GPIO_SERVO_PWR_EN, false); + stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_SERVO_PWR_EN)); + + stm32_configgpio(BOARD_GPIO_INPUT_PUP(BOARD_GPIO_SERVO_FAULT_DETECT)); + + stm32_configgpio(BOARD_GPIO_INPUT_FLOAT(BOARD_GPIO_TIM_RSSI)); /* xxx alternate function */ + stm32_configgpio(BOARD_GPIO_INPUT_ANALOG(BOARD_GPIO_ADC_RSSI)); + stm32_configgpio(BOARD_GPIO_INPUT_ANALOG(BOARD_GPIO_ADC_VSERVO)); + + stm32_configgpio(BOARD_GPIO_INPUT_FLOAT(BOARD_GPIO_SBUS_INPUT)); /* xxx alternate function */ + + stm32_gpiowrite(BOARD_GPIO_SBUS_OUTPUT, false); + stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_SBUS_OUTPUT)); + /* sbus output enable is active low - disable it by default */ + stm32_gpiowrite(BOARD_GPIO_SBUS_OENABLE, true); + stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_SBUS_OENABLE)); + + + stm32_configgpio(BOARD_GPIO_INPUT_FLOAT(BOARD_GPIO_PPM)); /* xxx alternate function */ + + stm32_gpiowrite(BOARD_GPIO_PWM1, false); + stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_PWM1)); + + stm32_gpiowrite(BOARD_GPIO_PWM2, false); + stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_PWM2)); + + stm32_gpiowrite(BOARD_GPIO_PWM3, false); + stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_PWM3)); + + stm32_gpiowrite(BOARD_GPIO_PWM4, false); + stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_PWM4)); + + stm32_gpiowrite(BOARD_GPIO_PWM5, false); + stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_PWM5)); + + stm32_gpiowrite(BOARD_GPIO_PWM6, false); + stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_PWM6)); + + stm32_gpiowrite(BOARD_GPIO_PWM7, false); + stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_PWM7)); + + stm32_gpiowrite(BOARD_GPIO_PWM8, false); + stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_PWM8)); + +// message("[boot] Successfully initialized px4iov2 gpios\n"); + + return OK; +} diff --git a/apps/drivers/boards/px4iov2/px4iov2_internal.h b/apps/drivers/boards/px4iov2/px4iov2_internal.h new file mode 100644 index 0000000000..9675c6f366 --- /dev/null +++ b/apps/drivers/boards/px4iov2/px4iov2_internal.h @@ -0,0 +1,135 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 px4iov2_internal.h + * + * PX4IOV2 internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + +__BEGIN_DECLS + +/* these headers are not C++ safe */ +#include + + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ +/* Configuration ************************************************************************************/ + +/****************************************************************************** + * GPIOS + ******************************************************************************/ + +#define BOARD_GPIO_OUTPUT(pin) (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ + GPIO_OUTPUT_CLEAR|(pin)) +#define BOARD_GPIO_INPUT_FLOAT(pin) (GPIO_INPUT|GPIO_CNF_INFLOAT|\ + GPIO_MODE_INPUT|(pin)) +#define BOARD_GPIO_INPUT_PUP(pin) (GPIO_INPUT|GPIO_CNF_INPULLUP|\ + GPIO_MODE_INPUT|(pin)) +#define BOARD_GPIO_INPUT_ANALOG(pin) (GPIO_INPUT|GPIO_CNF_ANALOGIN|\ + GPIO_MODE_INPUT|(pin)) + +/* LEDS **********************************************************************/ + +#define BOARD_GPIO_LED1 (GPIO_PORTB|GPIO_PIN14) +#define BOARD_GPIO_LED2 (GPIO_PORTB|GPIO_PIN15) +#define BOARD_GPIO_LED3 (GPIO_PORTB|GPIO_PIN13) + +#define BOARD_GPIO_LED_BLUE BOARD_GPIO_LED1 +#define BOARD_GPIO_LED_AMBER BOARD_GPIO_LED2 +#define BOARD_GPIO_LED_SAFETY BOARD_GPIO_LED3 + +/* Safety switch button *******************************************************/ + +#define BOARD_GPIO_BTN_SAFETY (GPIO_PORTB|GPIO_PIN5) + +/* Power switch controls ******************************************************/ + +#define BOARD_GPIO_SPEKTRUM_PWR_EN (GPIO_PORTC|GPIO_PIN13) + +#define BOARD_GPIO_SERVO_PWR_EN (GPIO_PORTC|GPIO_PIN15) + +#define BOARD_GPIO_SERVO_FAULT_DETECT (GPIO_PORTB|GPIO_PIN13) + + +/* Analog inputs **************************************************************/ + +#define BOARD_GPIO_ADC_VSERVO (GPIO_PORTA|GPIO_PIN4) +/* the same rssi signal goes to both an adc and a timer input */ +#define BOARD_GPIO_ADC_RSSI (GPIO_PORTA|GPIO_PIN5) +#define BOARD_GPIO_TIM_RSSI (GPIO_PORTA|GPIO_PIN12) + +/* PWM pins **************************************************************/ + +#define BOARD_GPIO_PPM (GPIO_PORTA|GPIO_PIN8) + +#define BOARD_GPIO_PWM1 (GPIO_PORTA|GPIO_PIN0) +#define BOARD_GPIO_PWM2 (GPIO_PORTA|GPIO_PIN1) +#define BOARD_GPIO_PWM3 (GPIO_PORTB|GPIO_PIN8) +#define BOARD_GPIO_PWM4 (GPIO_PORTB|GPIO_PIN9) +#define BOARD_GPIO_PWM5 (GPIO_PORTA|GPIO_PIN6) +#define BOARD_GPIO_PWM6 (GPIO_PORTA|GPIO_PIN7) +#define BOARD_GPIO_PWM7 (GPIO_PORTB|GPIO_PIN0) +#define BOARD_GPIO_PWM8 (GPIO_PORTB|GPIO_PIN1) + +/* SBUS pins *************************************************************/ + +#define BOARD_GPIO_SBUS_INPUT (GPIO_PORTB|GPIO_PIN11) +#define BOARD_GPIO_SBUS_OUTPUT (GPIO_PORTB|GPIO_PIN10) +#define BOARD_GPIO_SBUS_OENABLE (GPIO_PORTB|GPIO_PIN4) + +/**************************************************************************************************** + * Public Types + ****************************************************************************************************/ + +/**************************************************************************************************** + * Public data + ****************************************************************************************************/ + +#ifndef __ASSEMBLY__ + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/nuttx/configs/px4iov2/README.txt b/nuttx/configs/px4iov2/README.txt new file mode 100755 index 0000000000..9b1615f42d --- /dev/null +++ b/nuttx/configs/px4iov2/README.txt @@ -0,0 +1,806 @@ +README +====== + +This README discusses issues unique to NuttX configurations for the +STMicro STM3210E-EVAL development board. + +Contents +======== + + - Development Environment + - GNU Toolchain Options + - IDEs + - NuttX buildroot Toolchain + - DFU and JTAG + - OpenOCD + - LEDs + - Temperature Sensor + - RTC + - STM3210E-EVAL-specific Configuration Options + - Configurations + +Development Environment +======================= + + Either Linux or Cygwin on Windows can be used for the development environment. + The source has been built only using the GNU toolchain (see below). Other + toolchains will likely cause problems. Testing was performed using the Cygwin + environment because the Raisonance R-Link emulatator and some RIDE7 development tools + were used and those tools works only under Windows. + +GNU Toolchain Options +===================== + + The NuttX make system has been modified to support the following different + toolchain options. + + 1. The CodeSourcery GNU toolchain, + 2. The devkitARM GNU toolchain, + 3. Raisonance GNU toolchain, or + 4. The NuttX buildroot Toolchain (see below). + + All testing has been conducted using the NuttX buildroot toolchain. However, + the make system is setup to default to use the devkitARM toolchain. To use + the CodeSourcery, devkitARM or Raisonance GNU toolchain, you simply need to + add one of the following configuration options to your .config (or defconfig) + file: + + CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows + CONFIG_STM32_CODESOURCERYL=y : CodeSourcery under Linux + CONFIG_STM32_DEVKITARM=y : devkitARM under Windows + CONFIG_STM32_RAISONANCE=y : Raisonance RIDE7 under Windows + CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin (default) + + If you are not using CONFIG_STM32_BUILDROOT, then you may also have to modify + the PATH in the setenv.h file if your make cannot find the tools. + + NOTE: the CodeSourcery (for Windows), devkitARM, and Raisonance toolchains are + Windows native toolchains. The CodeSourcey (for Linux) and NuttX buildroot + toolchains are Cygwin and/or Linux native toolchains. There are several limitations + to using a Windows based toolchain in a Cygwin environment. The three biggest are: + + 1. The Windows toolchain cannot follow Cygwin paths. Path conversions are + performed automatically in the Cygwin makefiles using the 'cygpath' utility + but you might easily find some new path problems. If so, check out 'cygpath -w' + + 2. Windows toolchains cannot follow Cygwin symbolic links. Many symbolic links + are used in Nuttx (e.g., include/arch). The make system works around these + problems for the Windows tools by copying directories instead of linking them. + But this can also cause some confusion for you: For example, you may edit + a file in a "linked" directory and find that your changes had no effect. + That is because you are building the copy of the file in the "fake" symbolic + directory. If you use a Windows toolchain, you should get in the habit of + making like this: + + make clean_context all + + An alias in your .bashrc file might make that less painful. + + 3. Dependencies are not made when using Windows versions of the GCC. This is + because the dependencies are generated using Windows pathes which do not + work with the Cygwin make. + + Support has been added for making dependencies with the windows-native toolchains. + That support can be enabled by modifying your Make.defs file as follows: + + - MKDEP = $(TOPDIR)/tools/mknulldeps.sh + + MKDEP = $(TOPDIR)/tools/mkdeps.sh --winpaths "$(TOPDIR)" + + If you have problems with the dependency build (for example, if you are not + building on C:), then you may need to modify tools/mkdeps.sh + + NOTE 1: The CodeSourcery toolchain (2009q1) does not work with default optimization + level of -Os (See Make.defs). It will work with -O0, -O1, or -O2, but not with + -Os. + + NOTE 2: The devkitARM toolchain includes a version of MSYS make. Make sure that + the paths to Cygwin's /bin and /usr/bin directories appear BEFORE the devkitARM + path or will get the wrong version of make. + +IDEs +==== + + NuttX is built using command-line make. It can be used with an IDE, but some + effort will be required to create the project (There is a simple RIDE project + in the RIDE subdirectory). + + Makefile Build + -------------- + Under Eclipse, it is pretty easy to set up an "empty makefile project" and + simply use the NuttX makefile to build the system. That is almost for free + under Linux. Under Windows, you will need to set up the "Cygwin GCC" empty + makefile project in order to work with Windows (Google for "Eclipse Cygwin" - + there is a lot of help on the internet). + + Native Build + ------------ + Here are a few tips before you start that effort: + + 1) Select the toolchain that you will be using in your .config file + 2) Start the NuttX build at least one time from the Cygwin command line + before trying to create your project. This is necessary to create + certain auto-generated files and directories that will be needed. + 3) Set up include pathes: You will need include/, arch/arm/src/stm32, + arch/arm/src/common, arch/arm/src/armv7-m, and sched/. + 4) All assembly files need to have the definition option -D __ASSEMBLY__ + on the command line. + + Startup files will probably cause you some headaches. The NuttX startup file + is arch/arm/src/stm32/stm32_vectors.S. With RIDE, I have to build NuttX + one time from the Cygwin command line in order to obtain the pre-built + startup object needed by RIDE. + +NuttX buildroot Toolchain +========================= + + A GNU GCC-based toolchain is assumed. The files */setenv.sh should + be modified to point to the correct path to the Cortex-M3 GCC toolchain (if + different from the default in your PATH variable). + + If you have no Cortex-M3 toolchain, one can be downloaded from the NuttX + SourceForge download site (https://sourceforge.net/project/showfiles.php?group_id=189573). + This GNU toolchain builds and executes in the Linux or Cygwin environment. + + 1. You must have already configured Nuttx in /nuttx. + + cd tools + ./configure.sh stm3210e-eval/ + + 2. Download the latest buildroot package into + + 3. unpack the buildroot tarball. The resulting directory may + have versioning information on it like buildroot-x.y.z. If so, + rename /buildroot-x.y.z to /buildroot. + + 4. cd /buildroot + + 5. cp configs/cortexm3-defconfig-4.3.3 .config + + 6. make oldconfig + + 7. make + + 8. Edit setenv.h, if necessary, so that the PATH variable includes + the path to the newly built binaries. + + See the file configs/README.txt in the buildroot source tree. That has more + detailed PLUS some special instructions that you will need to follow if you are + building a Cortex-M3 toolchain for Cygwin under Windows. + +DFU and JTAG +============ + + Enbling Support for the DFU Bootloader + -------------------------------------- + The linker files in these projects can be configured to indicate that you + will be loading code using STMicro built-in USB Device Firmware Upgrade (DFU) + loader or via some JTAG emulator. You can specify the DFU bootloader by + adding the following line: + + CONFIG_STM32_DFU=y + + to your .config file. Most of the configurations in this directory are set + up to use the DFU loader. + + If CONFIG_STM32_DFU is defined, the code will not be positioned at the beginning + of FLASH (0x08000000) but will be offset to 0x08003000. This offset is needed + to make space for the DFU loader and 0x08003000 is where the DFU loader expects + to find new applications at boot time. If you need to change that origin for some + other bootloader, you will need to edit the file(s) ld.script.dfu for each + configuration. + + The DFU SE PC-based software is available from the STMicro website, + http://www.st.com. General usage instructions: + + 1. Convert the NuttX Intel Hex file (nuttx.ihx) into a special DFU + file (nuttx.dfu)... see below for details. + 2. Connect the STM3210E-EVAL board to your computer using a USB + cable. + 3. Start the DFU loader on the STM3210E-EVAL board. You do this by + resetting the board while holding the "Key" button. Windows should + recognize that the DFU loader has been installed. + 3. Run the DFU SE program to load nuttx.dfu into FLASH. + + What if the DFU loader is not in FLASH? The loader code is available + inside of the Demo dirctory of the USBLib ZIP file that can be downloaded + from the STMicro Website. You can build it using RIDE (or other toolchains); + you will need a JTAG emulator to burn it into FLASH the first time. + + In order to use STMicro's built-in DFU loader, you will have to get + the NuttX binary into a special format with a .dfu extension. The + DFU SE PC_based software installation includes a file "DFU File Manager" + conversion program that a file in Intel Hex format to the special DFU + format. When you successfully build NuttX, you will find a file called + nutt.ihx in the top-level directory. That is the file that you should + provide to the DFU File Manager. You will need to rename it to nuttx.hex + in order to find it with the DFU File Manager. You will end up with + a file called nuttx.dfu that you can use with the STMicro DFU SE program. + + Enabling JTAG + ------------- + If you are not using the DFU, then you will probably also need to enable + JTAG support. By default, all JTAG support is disabled but there NuttX + configuration options to enable JTAG in various different ways. + + These configurations effect the setting of the SWJ_CFG[2:0] bits in the AFIO + MAPR register. These bits are used to configure the SWJ and trace alternate function I/Os. The SWJ (SerialWire JTAG) supports JTAG or SWD access to the + Cortex debug port. The default state in this port is for all JTAG support + to be disable. + + CONFIG_STM32_JTAG_FULL_ENABLE - sets SWJ_CFG[2:0] to 000 which enables full + SWJ (JTAG-DP + SW-DP) + + CONFIG_STM32_JTAG_NOJNTRST_ENABLE - sets SWJ_CFG[2:0] to 001 which enable + full SWJ (JTAG-DP + SW-DP) but without JNTRST. + + CONFIG_STM32_JTAG_SW_ENABLE - sets SWJ_CFG[2:0] to 010 which would set JTAG-DP + disabled and SW-DP enabled + + The default setting (none of the above defined) is SWJ_CFG[2:0] set to 100 + which disable JTAG-DP and SW-DP. + +OpenOCD +======= + +I have also used OpenOCD with the STM3210E-EVAL. In this case, I used +the Olimex USB ARM OCD. See the script in configs/stm3210e-eval/tools/oocd.sh +for more information. Using the script: + +1) Start the OpenOCD GDB server + + cd + configs/stm3210e-eval/tools/oocd.sh $PWD + +2) Load Nuttx + + cd + arm-none-eabi-gdb nuttx + gdb> target remote localhost:3333 + gdb> mon reset + gdb> mon halt + gdb> load nuttx + +3) Running NuttX + + gdb> mon reset + gdb> c + +LEDs +==== + +The STM3210E-EVAL board has four LEDs labeled LD1, LD2, LD3 and LD4 on the +board.. These LEDs are not used by the board port unless CONFIG_ARCH_LEDS is +defined. In that case, the usage by the board port is defined in +include/board.h and src/up_leds.c. The LEDs are used to encode OS-related +events as follows: + + SYMBOL Meaning LED1* LED2 LED3 LED4 + ---------------- ----------------------- ----- ----- ----- ----- + LED_STARTED NuttX has been started ON OFF OFF OFF + LED_HEAPALLOCATE Heap has been allocated OFF ON OFF OFF + LED_IRQSENABLED Interrupts enabled ON ON OFF OFF + LED_STACKCREATED Idle stack created OFF OFF ON OFF + LED_INIRQ In an interrupt** ON N/C N/C OFF + LED_SIGNAL In a signal handler*** N/C ON N/C OFF + LED_ASSERTION An assertion failed ON ON N/C OFF + LED_PANIC The system has crashed N/C N/C N/C ON + LED_IDLE STM32 is is sleep mode (Optional, not used) + + * If LED1, LED2, LED3 are statically on, then NuttX probably failed to boot + and these LEDs will give you some indication of where the failure was + ** The normal state is LED3 ON and LED1 faintly glowing. This faint glow + is because of timer interupts that result in the LED being illuminated + on a small proportion of the time. +*** LED2 may also flicker normally if signals are processed. + +Temperature Sensor +================== + +Support for the on-board LM-75 temperature sensor is available. This supported +has been verified, but has not been included in any of the available the +configurations. To set up the temperature sensor, add the following to the +NuttX configuration file + + CONFIG_I2C=y + CONFIG_I2C_LM75=y + +Then you can implement logic like the following to use the temperature sensor: + + #include + #include + + ret = stm32_lm75initialize("/dev/temp"); /* Register the temperature sensor */ + fd = open("/dev/temp", O_RDONLY); /* Open the temperature sensor device */ + ret = ioctl(fd, SNIOC_FAHRENHEIT, 0); /* Select Fahrenheit */ + bytesread = read(fd, buffer, 8*sizeof(b16_t)); /* Read temperature samples */ + +More complex temperature sensor operations are also available. See the IOCTAL +commands enumerated in include/nuttx/sensors/lm75.h. Also read the descriptions +of the stm32_lm75initialize() and stm32_lm75attach() interfaces in the +arch/board/board.h file (sames as configs/stm3210e-eval/include/board.h). + +RTC +=== + + The STM32 RTC may configured using the following settings. + + CONFIG_RTC - Enables general support for a hardware RTC. Specific + architectures may require other specific settings. + CONFIG_RTC_HIRES - The typical RTC keeps time to resolution of 1 + second, usually supporting a 32-bit time_t value. In this case, + the RTC is used to "seed" the normal NuttX timer and the + NuttX timer provides for higher resoution time. If CONFIG_RTC_HIRES + is enabled in the NuttX configuration, then the RTC provides higher + resolution time and completely replaces the system timer for purpose of + date and time. + CONFIG_RTC_FREQUENCY - If CONFIG_RTC_HIRES is defined, then the + frequency of the high resolution RTC must be provided. If CONFIG_RTC_HIRES + is not defined, CONFIG_RTC_FREQUENCY is assumed to be one. + CONFIG_RTC_ALARM - Enable if the RTC hardware supports setting of an alarm. + A callback function will be executed when the alarm goes off + + In hi-res mode, the STM32 RTC operates only at 16384Hz. Overflow interrupts + are handled when the 32-bit RTC counter overflows every 3 days and 43 minutes. + A BKP register is incremented on each overflow interrupt creating, effectively, + a 48-bit RTC counter. + + In the lo-res mode, the RTC operates at 1Hz. Overflow interrupts are not handled + (because the next overflow is not expected until the year 2106. + + WARNING: Overflow interrupts are lost whenever the STM32 is powered down. The + overflow interrupt may be lost even if the STM32 is powered down only momentarily. + Therefore hi-res solution is only useful in systems where the power is always on. + +STM3210E-EVAL-specific Configuration Options +============================================ + + CONFIG_ARCH - Identifies the arch/ subdirectory. This should + be set to: + + CONFIG_ARCH=arm + + CONFIG_ARCH_family - For use in C code: + + CONFIG_ARCH_ARM=y + + CONFIG_ARCH_architecture - For use in C code: + + CONFIG_ARCH_CORTEXM3=y + + CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory + + CONFIG_ARCH_CHIP=stm32 + + CONFIG_ARCH_CHIP_name - For use in C code to identify the exact + chip: + + CONFIG_ARCH_CHIP_STM32F103ZET6 + + CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG - Enables special STM32 clock + configuration features. + + CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG=n + + CONFIG_ARCH_BOARD - Identifies the configs subdirectory and + hence, the board that supports the particular chip or SoC. + + CONFIG_ARCH_BOARD=stm3210e_eval (for the STM3210E-EVAL development board) + + CONFIG_ARCH_BOARD_name - For use in C code + + CONFIG_ARCH_BOARD_STM3210E_EVAL=y + + CONFIG_ARCH_LOOPSPERMSEC - Must be calibrated for correct operation + of delay loops + + CONFIG_ENDIAN_BIG - define if big endian (default is little + endian) + + CONFIG_DRAM_SIZE - Describes the installed DRAM (SRAM in this case): + + CONFIG_DRAM_SIZE=0x00010000 (64Kb) + + CONFIG_DRAM_START - The start address of installed DRAM + + CONFIG_DRAM_START=0x20000000 + + CONFIG_DRAM_END - Last address+1 of installed RAM + + CONFIG_DRAM_END=(CONFIG_DRAM_START+CONFIG_DRAM_SIZE) + + CONFIG_ARCH_IRQPRIO - The STM32F103Z supports interrupt prioritization + + CONFIG_ARCH_IRQPRIO=y + + CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to boards that + have LEDs + + CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt + stack. If defined, this symbol is the size of the interrupt + stack in bytes. If not defined, the user task stacks will be + used during interrupt handling. + + CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions + + CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to board architecture. + + CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that + cause a 100 second delay during boot-up. This 100 second delay + serves no purpose other than it allows you to calibratre + CONFIG_ARCH_LOOPSPERMSEC. You simply use a stop watch to measure + the 100 second delay then adjust CONFIG_ARCH_LOOPSPERMSEC until + the delay actually is 100 seconds. + + Individual subsystems can be enabled: + AHB + --- + CONFIG_STM32_DMA1 + CONFIG_STM32_DMA2 + CONFIG_STM32_CRC + CONFIG_STM32_FSMC + CONFIG_STM32_SDIO + + APB1 + ---- + CONFIG_STM32_TIM2 + CONFIG_STM32_TIM3 + CONFIG_STM32_TIM4 + CONFIG_STM32_TIM5 + CONFIG_STM32_TIM6 + CONFIG_STM32_TIM7 + CONFIG_STM32_WWDG + CONFIG_STM32_SPI2 + CONFIG_STM32_SPI4 + CONFIG_STM32_USART2 + CONFIG_STM32_USART3 + CONFIG_STM32_UART4 + CONFIG_STM32_UART5 + CONFIG_STM32_I2C1 + CONFIG_STM32_I2C2 + CONFIG_STM32_USB + CONFIG_STM32_CAN + CONFIG_STM32_BKP + CONFIG_STM32_PWR + CONFIG_STM32_DAC1 + CONFIG_STM32_DAC2 + CONFIG_STM32_USB + + APB2 + ---- + CONFIG_STM32_ADC1 + CONFIG_STM32_ADC2 + CONFIG_STM32_TIM1 + CONFIG_STM32_SPI1 + CONFIG_STM32_TIM8 + CONFIG_STM32_USART1 + CONFIG_STM32_ADC3 + + Timer and I2C devices may need to the following to force power to be applied + unconditionally at power up. (Otherwise, the device is powered when it is + initialized). + + CONFIG_STM32_FORCEPOWER + + Timer devices may be used for different purposes. One special purpose is + to generate modulated outputs for such things as motor control. If CONFIG_STM32_TIMn + is defined (as above) then the following may also be defined to indicate that + the timer is intended to be used for pulsed output modulation, ADC conversion, + or DAC conversion. Note that ADC/DAC require two definition: Not only do you have + to assign the timer (n) for used by the ADC or DAC, but then you also have to + configure which ADC or DAC (m) it is assigned to. + + CONFIG_STM32_TIMn_PWM Reserve timer n for use by PWM, n=1,..,8 + CONFIG_STM32_TIMn_ADC Reserve timer n for use by ADC, n=1,..,8 + CONFIG_STM32_TIMn_ADCm Reserve timer n to trigger ADCm, n=1,..,8, m=1,..,3 + CONFIG_STM32_TIMn_DAC Reserve timer n for use by DAC, n=1,..,8 + CONFIG_STM32_TIMn_DACm Reserve timer n to trigger DACm, n=1,..,8, m=1,..,2 + + For each timer that is enabled for PWM usage, we need the following additional + configuration settings: + + CONFIG_STM32_TIMx_CHANNEL - Specifies the timer output channel {1,..,4} + + NOTE: The STM32 timers are each capable of generating different signals on + each of the four channels with different duty cycles. That capability is + not supported by this driver: Only one output channel per timer. + + Alternate pin mappings (should not be used with the STM3210E-EVAL board): + + CONFIG_STM32_TIM1_FULL_REMAP + CONFIG_STM32_TIM1_PARTIAL_REMAP + CONFIG_STM32_TIM2_FULL_REMAP + CONFIG_STM32_TIM2_PARTIAL_REMAP_1 + CONFIG_STM32_TIM2_PARTIAL_REMAP_2 + CONFIG_STM32_TIM3_FULL_REMAP + CONFIG_STM32_TIM3_PARTIAL_REMAP + CONFIG_STM32_TIM4_REMAP + CONFIG_STM32_USART1_REMAP + CONFIG_STM32_USART2_REMAP + CONFIG_STM32_USART3_FULL_REMAP + CONFIG_STM32_USART3_PARTIAL_REMAP + CONFIG_STM32_SPI1_REMAP + CONFIG_STM32_SPI3_REMAP + CONFIG_STM32_I2C1_REMAP + CONFIG_STM32_CAN1_FULL_REMAP + CONFIG_STM32_CAN1_PARTIAL_REMAP + CONFIG_STM32_CAN2_REMAP + + JTAG Enable settings (by default JTAG-DP and SW-DP are disabled): + CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) + CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) + but without JNTRST. + CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled + + STM32F103Z specific device driver settings + + CONFIG_U[S]ARTn_SERIAL_CONSOLE - selects the USARTn (n=1,2,3) or UART + m (m=4,5) for the console and ttys0 (default is the USART1). + CONFIG_U[S]ARTn_RXBUFSIZE - Characters are buffered as received. + This specific the size of the receive buffer + CONFIG_U[S]ARTn_TXBUFSIZE - Characters are buffered before + being sent. This specific the size of the transmit buffer + CONFIG_U[S]ARTn_BAUD - The configure BAUD of the UART. Must be + CONFIG_U[S]ARTn_BITS - The number of bits. Must be either 7 or 8. + CONFIG_U[S]ARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity + CONFIG_U[S]ARTn_2STOP - Two stop bits + + CONFIG_STM32_SPI_INTERRUPTS - Select to enable interrupt driven SPI + support. Non-interrupt-driven, poll-waiting is recommended if the + interrupt rate would be to high in the interrupt driven case. + CONFIG_STM32_SPI_DMA - Use DMA to improve SPI transfer performance. + Cannot be used with CONFIG_STM32_SPI_INTERRUPT. + + CONFIG_SDIO_DMA - Support DMA data transfers. Requires CONFIG_STM32_SDIO + and CONFIG_STM32_DMA2. + CONFIG_SDIO_PRI - Select SDIO interrupt prority. Default: 128 + CONFIG_SDIO_DMAPRIO - Select SDIO DMA interrupt priority. + Default: Medium + CONFIG_SDIO_WIDTH_D1_ONLY - Select 1-bit transfer mode. Default: + 4-bit transfer mode. + + STM3210E-EVAL CAN Configuration + + CONFIG_CAN - Enables CAN support (one or both of CONFIG_STM32_CAN1 or + CONFIG_STM32_CAN2 must also be defined) + CONFIG_CAN_EXTID - Enables support for the 29-bit extended ID. Default + Standard 11-bit IDs. + CONFIG_CAN_FIFOSIZE - The size of the circular buffer of CAN messages. + Default: 8 + CONFIG_CAN_NPENDINGRTR - The size of the list of pending RTR requests. + Default: 4 + CONFIG_CAN_LOOPBACK - A CAN driver may or may not support a loopback + mode for testing. The STM32 CAN driver does support loopback mode. + CONFIG_CAN1_BAUD - CAN1 BAUD rate. Required if CONFIG_STM32_CAN1 is defined. + CONFIG_CAN2_BAUD - CAN1 BAUD rate. Required if CONFIG_STM32_CAN2 is defined. + CONFIG_CAN_TSEG1 - The number of CAN time quanta in segment 1. Default: 6 + CONFIG_CAN_TSEG2 - the number of CAN time quanta in segment 2. Default: 7 + CONFIG_CAN_REGDEBUG - If CONFIG_DEBUG is set, this will generate an + dump of all CAN registers. + + STM3210E-EVAL LCD Hardware Configuration + + CONFIG_LCD_LANDSCAPE - Define for 320x240 display "landscape" + support. Default is this 320x240 "landscape" orientation + (this setting is informative only... not used). + CONFIG_LCD_PORTRAIT - Define for 240x320 display "portrait" + orientation support. In this orientation, the STM3210E-EVAL's + LCD ribbon cable is at the bottom of the display. Default is + 320x240 "landscape" orientation. + CONFIG_LCD_RPORTRAIT - Define for 240x320 display "reverse + portrait" orientation support. In this orientation, the + STM3210E-EVAL's LCD ribbon cable is at the top of the display. + Default is 320x240 "landscape" orientation. + CONFIG_LCD_BACKLIGHT - Define to support a backlight. + CONFIG_LCD_PWM - If CONFIG_STM32_TIM1 is also defined, then an + adjustable backlight will be provided using timer 1 to generate + various pulse widthes. The granularity of the settings is + determined by CONFIG_LCD_MAXPOWER. If CONFIG_LCD_PWM (or + CONFIG_STM32_TIM1) is not defined, then a simple on/off backlight + is provided. + CONFIG_LCD_RDSHIFT - When reading 16-bit gram data, there appears + to be a shift in the returned data. This value fixes the offset. + Default 5. + + The LCD driver dynamically selects the LCD based on the reported LCD + ID value. However, code size can be reduced by suppressing support for + individual LCDs using: + + CONFIG_STM32_AM240320_DISABLE + CONFIG_STM32_SPFD5408B_DISABLE + CONFIG_STM32_R61580_DISABLE + +Configurations +============== + +Each STM3210E-EVAL configuration is maintained in a sudirectory and +can be selected as follow: + + cd tools + ./configure.sh stm3210e-eval/ + cd - + . ./setenv.sh + +Where is one of the following: + + buttons: + -------- + + Uses apps/examples/buttons to exercise STM3210E-EVAL buttons and + button interrupts. + + CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows + + composite + --------- + + This configuration exercises a composite USB interface consisting + of a CDC/ACM device and a USB mass storage device. This configuration + uses apps/examples/composite. + + nsh and nsh2: + ------------ + Configure the NuttShell (nsh) located at examples/nsh. + + Differences between the two NSH configurations: + + =========== ======================= ================================ + nsh nsh2 + =========== ======================= ================================ + Toolchain: NuttX buildroot for Codesourcery for Windows (1) + Linux or Cygwin (1,2) + ----------- ----------------------- -------------------------------- + Loader: DfuSe DfuSe + ----------- ----------------------- -------------------------------- + Serial Debug output: USART1 Debug output: USART1 + Console: NSH output: USART1 NSH output: USART1 (3) + ----------- ----------------------- -------------------------------- + microSD Yes Yes + Support + ----------- ----------------------- -------------------------------- + FAT FS CONFIG_FAT_LCNAME=y CONFIG_FAT_LCNAME=y + Config CONFIG_FAT_LFN=n CONFIG_FAT_LFN=y (4) + ----------- ----------------------- -------------------------------- + Support for No Yes + Built-in + Apps + ----------- ----------------------- -------------------------------- + Built-in None apps/examples/nx + Apps apps/examples/nxhello + apps/examples/usbstorage (5) + =========== ======================= ================================ + + (1) You will probably need to modify nsh/setenv.sh or nsh2/setenv.sh + to set up the correct PATH variable for whichever toolchain you + may use. + (2) Since DfuSe is assumed, this configuration may only work under + Cygwin without modification. + (3) When any other device other than /dev/console is used for a user + interface, (1) linefeeds (\n) will not be expanded to carriage return + / linefeeds \r\n). You will need to configure your terminal program + to account for this. And (2) input is not automatically echoed so + you will have to turn local echo on. + (4) Microsoft holds several patents related to the design of + long file names in the FAT file system. Please refer to the + details in the top-level COPYING file. Please do not use FAT + long file name unless you are familiar with these patent issues. + (5) When built as an NSH add-on command (CONFIG_EXAMPLES_USBMSC_BUILTIN=y), + Caution should be used to assure that the SD drive is not in use when + the USB storage device is configured. Specifically, the SD driver + should be unmounted like: + + nsh> mount -t vfat /dev/mmcsd0 /mnt/sdcard # Card is mounted in NSH + ... + nsh> umount /mnd/sdcard # Unmount before connecting USB!!! + nsh> msconn # Connect the USB storage device + ... + nsh> msdis # Disconnect USB storate device + nsh> mount -t vfat /dev/mmcsd0 /mnt/sdcard # Restore the mount + + Failure to do this could result in corruption of the SD card format. + + nx: + --- + An example using the NuttX graphics system (NX). This example + focuses on general window controls, movement, mouse and keyboard + input. + + CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows + CONFIG_LCD_RPORTRAIT=y : 240x320 reverse portrait + + nxlines: + ------ + Another example using the NuttX graphics system (NX). This + example focuses on placing lines on the background in various + orientations. + + CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows + CONFIG_LCD_RPORTRAIT=y : 240x320 reverse portrait + + nxtext: + ------ + Another example using the NuttX graphics system (NX). This + example focuses on placing text on the background while pop-up + windows occur. Text should continue to update normally with + or without the popup windows present. + + CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin + CONFIG_LCD_RPORTRAIT=y : 240x320 reverse portrait + + NOTE: When I tried building this example with the CodeSourcery + tools, I got a hardfault inside of its libgcc. I haven't + retested since then, but beware if you choose to change the + toolchain. + + ostest: + ------ + This configuration directory, performs a simple OS test using + examples/ostest. By default, this project assumes that you are + using the DFU bootloader. + + CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin + + RIDE + ---- + This configuration builds a trivial bring-up binary. It is + useful only because it words with the RIDE7 IDE and R-Link debugger. + + CONFIG_STM32_RAISONANCE=y : Raisonance RIDE7 under Windows + + usbserial: + --------- + This configuration directory exercises the USB serial class + driver at examples/usbserial. See examples/README.txt for + more information. + + CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin + + USB debug output can be enabled as by changing the following + settings in the configuration file: + + -CONFIG_DEBUG=n + -CONFIG_DEBUG_VERBOSE=n + -CONFIG_DEBUG_USB=n + +CONFIG_DEBUG=y + +CONFIG_DEBUG_VERBOSE=y + +CONFIG_DEBUG_USB=y + + -CONFIG_EXAMPLES_USBSERIAL_TRACEINIT=n + -CONFIG_EXAMPLES_USBSERIAL_TRACECLASS=n + -CONFIG_EXAMPLES_USBSERIAL_TRACETRANSFERS=n + -CONFIG_EXAMPLES_USBSERIAL_TRACECONTROLLER=n + -CONFIG_EXAMPLES_USBSERIAL_TRACEINTERRUPTS=n + +CONFIG_EXAMPLES_USBSERIAL_TRACEINIT=y + +CONFIG_EXAMPLES_USBSERIAL_TRACECLASS=y + +CONFIG_EXAMPLES_USBSERIAL_TRACETRANSFERS=y + +CONFIG_EXAMPLES_USBSERIAL_TRACECONTROLLER=y + +CONFIG_EXAMPLES_USBSERIAL_TRACEINTERRUPTS=y + + By default, the usbserial example uses the Prolific PL2303 + serial/USB converter emulation. The example can be modified + to use the CDC/ACM serial class by making the following changes + to the configuration file: + + -CONFIG_PL2303=y + +CONFIG_PL2303=n + + -CONFIG_CDCACM=n + +CONFIG_CDCACM=y + + The example can also be converted to use the alternative + USB serial example at apps/examples/usbterm by changing the + following: + + -CONFIGURED_APPS += examples/usbserial + +CONFIGURED_APPS += examples/usbterm + + In either the original appconfig file (before configuring) + or in the final apps/.config file (after configuring). + + usbstorage: + ---------- + This configuration directory exercises the USB mass storage + class driver at examples/usbstorage. See examples/README.txt for + more information. + + CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin + diff --git a/nuttx/configs/px4iov2/common/Make.defs b/nuttx/configs/px4iov2/common/Make.defs new file mode 100644 index 0000000000..7f782b5b22 --- /dev/null +++ b/nuttx/configs/px4iov2/common/Make.defs @@ -0,0 +1,175 @@ +############################################################################ +# configs/px4fmu/common/Make.defs +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# +############################################################################ + +# +# Generic Make.defs for the PX4FMU +# Do not specify/use this file directly - it is included by config-specific +# Make.defs in the per-config directories. +# + +include ${TOPDIR}/tools/Config.mk + +# +# We only support building with the ARM bare-metal toolchain from +# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS. +# +CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI + +include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs + +CC = $(CROSSDEV)gcc +CXX = $(CROSSDEV)g++ +CPP = $(CROSSDEV)gcc -E +LD = $(CROSSDEV)ld +AR = $(CROSSDEV)ar rcs +NM = $(CROSSDEV)nm +OBJCOPY = $(CROSSDEV)objcopy +OBJDUMP = $(CROSSDEV)objdump + +MAXOPTIMIZATION = -O3 +ARCHCPUFLAGS = -mcpu=cortex-m3 \ + -mthumb \ + -march=armv7-m + +# enable precise stack overflow tracking +#INSTRUMENTATIONDEFINES = -finstrument-functions \ +# -ffixed-r10 + +# use our linker script +LDSCRIPT = ld.script + +ifeq ($(WINTOOL),y) + # Windows-native toolchains + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" + ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" + ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT)}" +else + ifeq ($(PX4_WINTOOL),y) + # Windows-native toolchains (MSYS) + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT) + else + # Linux/Cygwin-native toolchain + MKDEP = $(TOPDIR)/tools/mkdeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT) + endif +endif + +# tool versions +ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} +ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} + +# optimisation flags +ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ + -fno-strict-aliasing \ + -fno-strength-reduce \ + -fomit-frame-pointer \ + -funsafe-math-optimizations \ + -fno-builtin-printf \ + -ffunction-sections \ + -fdata-sections + +ifeq ("${CONFIG_DEBUG_SYMBOLS}","y") +ARCHOPTIMIZATION += -g +endif + +ARCHCFLAGS = -std=gnu99 +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x +ARCHWARNINGS = -Wall \ + -Wextra \ + -Wdouble-promotion \ + -Wshadow \ + -Wfloat-equal \ + -Wframe-larger-than=1024 \ + -Wpointer-arith \ + -Wlogical-op \ + -Wmissing-declarations \ + -Wpacked \ + -Wno-unused-parameter +# -Wcast-qual - generates spurious noreturn attribute warnings, try again later +# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code +# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives + +ARCHCWARNINGS = $(ARCHWARNINGS) \ + -Wbad-function-cast \ + -Wstrict-prototypes \ + -Wold-style-declaration \ + -Wmissing-parameter-type \ + -Wmissing-prototypes \ + -Wnested-externs \ + -Wunsuffixed-float-constants +ARCHWARNINGSXX = $(ARCHWARNINGS) +ARCHDEFINES = +ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 + +# this seems to be the only way to add linker flags +EXTRA_LIBS += --warn-common \ + --gc-sections + +CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common +CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) +CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe +CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) +CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) +AFLAGS = $(CFLAGS) -D__ASSEMBLY__ + +NXFLATLDFLAGS1 = -r -d -warn-common +NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections +LDNXFLATFLAGS = -e main -s 2048 + +OBJEXT = .o +LIBEXT = .a +EXEEXT = + +# produce partially-linked $1 from files in $2 +define PRELINK + @echo "PRELINK: $1" + $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 +endef + +HOSTCC = gcc +HOSTINCLUDES = -I. +HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe +HOSTLDFLAGS = + diff --git a/nuttx/configs/px4iov2/common/ld.script b/nuttx/configs/px4iov2/common/ld.script new file mode 100755 index 0000000000..69c2f9cb2e --- /dev/null +++ b/nuttx/configs/px4iov2/common/ld.script @@ -0,0 +1,129 @@ +/**************************************************************************** + * configs/stm3210e-eval/nsh/ld.script + * + * Copyright (C) 2009, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/* The STM32F100C8 has 64Kb of FLASH beginning at address 0x0800:0000 and + * 8Kb of SRAM beginning at address 0x2000:0000. When booting from FLASH, + * FLASH memory is aliased to address 0x0000:0000 where the code expects to + * begin execution by jumping to the entry point in the 0x0800:0000 address + * range. + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x08001000, LENGTH = 60K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 8K +} + +OUTPUT_ARCH(arm) +ENTRY(__start) /* treat __start as the anchor for dead code stripping */ +EXTERN(_vectors) /* force the vectors to be included in the output */ + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + /* The STM32F100CB has 8Kb of SRAM beginning at the following address */ + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/nuttx/configs/px4iov2/common/setenv.sh b/nuttx/configs/px4iov2/common/setenv.sh new file mode 100755 index 0000000000..ff9a4bf8ae --- /dev/null +++ b/nuttx/configs/px4iov2/common/setenv.sh @@ -0,0 +1,47 @@ +#!/bin/bash +# configs/stm3210e-eval/dfu/setenv.sh +# +# Copyright (C) 2009 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# + +if [ "$(basename $0)" = "setenv.sh" ] ; then + echo "You must source this script, not run it!" 1>&2 + exit 1 +fi + +if [ -z "${PATH_ORIG}" ]; then export PATH_ORIG="${PATH}"; fi + +WD=`pwd` +export RIDE_BIN="/cygdrive/c/Program Files/Raisonance/Ride/arm-gcc/bin" +export BUILDROOT_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin" +export PATH="${BUILDROOT_BIN}:${RIDE_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" + +echo "PATH : ${PATH}" diff --git a/nuttx/configs/px4iov2/include/README.txt b/nuttx/configs/px4iov2/include/README.txt new file mode 100755 index 0000000000..2264a80aa8 --- /dev/null +++ b/nuttx/configs/px4iov2/include/README.txt @@ -0,0 +1 @@ +This directory contains header files unique to the PX4IO board. diff --git a/nuttx/configs/px4iov2/include/board.h b/nuttx/configs/px4iov2/include/board.h new file mode 100755 index 0000000000..b8dc711442 --- /dev/null +++ b/nuttx/configs/px4iov2/include/board.h @@ -0,0 +1,173 @@ +/************************************************************************************ + * configs/px4io/include/board.h + * include/arch/board/board.h + * + * Copyright (C) 2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * Copyright (C) 2012 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 NuttX 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. + * + ************************************************************************************/ + +#ifndef __ARCH_BOARD_BOARD_H +#define __ARCH_BOARD_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include +#ifndef __ASSEMBLY__ +# include +# include +#endif +#include +#include +#include + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ + +/* On-board crystal frequency is 24MHz (HSE) */ + +#define STM32_BOARD_XTAL 24000000ul + +/* Use the HSE output as the system clock */ + +#define STM32_SYSCLK_SW RCC_CFGR_SW_HSE +#define STM32_SYSCLK_SWS RCC_CFGR_SWS_HSE +#define STM32_SYSCLK_FREQUENCY STM32_BOARD_XTAL + +/* AHB clock (HCLK) is SYSCLK (24MHz) */ + +#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK +#define STM32_HCLK_FREQUENCY STM32_SYSCLK_FREQUENCY +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ + +/* APB2 clock (PCLK2) is HCLK (24MHz) */ + +#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLK +#define STM32_PCLK2_FREQUENCY STM32_HCLK_FREQUENCY +#define STM32_APB2_CLKIN (STM32_PCLK2_FREQUENCY) /* Timers 2-4 */ + +/* APB2 timer 1 will receive PCLK2. */ + +#define STM32_APB2_TIM1_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (STM32_PCLK2_FREQUENCY) + +/* APB1 clock (PCLK1) is HCLK (24MHz) */ + +#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLK +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY) + +/* All timers run off PCLK */ + +#define STM32_APB1_TIM1_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB1_TIM2_CLKIN (STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (STM32_PCLK1_FREQUENCY) + +/* + * Some of the USART pins are not available; override the GPIO + * definitions with an invalid pin configuration. + */ +#define GPIO_USART2_CTS 0xffffffff +#define GPIO_USART2_RTS 0xffffffff +#define GPIO_USART2_CK 0xffffffff +#define GPIO_USART3_TX 0xffffffff +#define GPIO_USART3_CK 0xffffffff +#define GPIO_USART3_CTS 0xffffffff +#define GPIO_USART3_RTS 0xffffffff + +/* + * High-resolution timer + */ +#ifdef CONFIG_HRT_TIMER +# define HRT_TIMER 1 /* use timer1 for the HRT */ +# define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 2 */ +#endif + +/* + * PPM + * + * PPM input is handled by the HRT timer. + * + * Pin is PA8, timer 1, channel 1 + */ +#if defined(CONFIG_HRT_TIMER) && defined (CONFIG_HRT_PPM) +# define HRT_PPM_CHANNEL 1 /* use capture/compare channel 1 */ +# define GPIO_PPM_IN GPIO_TIM1_CH1IN +#endif + +/* LED definitions ******************************************************************/ +/* PX4 has two LEDs that we will encode as: */ + +#define LED_STARTED 0 /* LED? */ +#define LED_HEAPALLOCATE 1 /* LED? */ +#define LED_IRQSENABLED 2 /* LED? + LED? */ +#define LED_STACKCREATED 3 /* LED? */ +#define LED_INIRQ 4 /* LED? + LED? */ +#define LED_SIGNAL 5 /* LED? + LED? */ +#define LED_ASSERTION 6 /* LED? + LED? + LED? */ +#define LED_PANIC 7 /* N/C + N/C + N/C + LED? */ + +/************************************************************************************ + * Public Data + ************************************************************************************/ + +#ifndef __ASSEMBLY__ + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +/************************************************************************************ + * Public Function Prototypes + ************************************************************************************/ +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +EXTERN void stm32_boardinitialize(void); + +#endif /* __ASSEMBLY__ */ +#endif /* __ARCH_BOARD_BOARD_H */ diff --git a/nuttx/configs/px4iov2/include/drv_i2c_device.h b/nuttx/configs/px4iov2/include/drv_i2c_device.h new file mode 100644 index 0000000000..02582bc092 --- /dev/null +++ b/nuttx/configs/px4iov2/include/drv_i2c_device.h @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 A simple, polled I2C slave-mode driver. + * + * The master writes to and reads from a byte buffer, which the caller + * can update inbetween calls to the FSM. + */ + +extern void i2c_fsm_init(uint8_t *buffer, size_t buffer_size); +extern bool i2c_fsm(void); diff --git a/nuttx/configs/px4iov2/io/Make.defs b/nuttx/configs/px4iov2/io/Make.defs new file mode 100644 index 0000000000..369772d03f --- /dev/null +++ b/nuttx/configs/px4iov2/io/Make.defs @@ -0,0 +1,3 @@ +include ${TOPDIR}/.config + +include $(TOPDIR)/configs/px4io/common/Make.defs diff --git a/nuttx/configs/px4iov2/io/appconfig b/nuttx/configs/px4iov2/io/appconfig new file mode 100644 index 0000000000..628607a515 --- /dev/null +++ b/nuttx/configs/px4iov2/io/appconfig @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (C) 2012 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. +# +############################################################################ + +CONFIGURED_APPS += drivers/boards/px4io +CONFIGURED_APPS += drivers/stm32 + +CONFIGURED_APPS += px4io + +# Mixer library from systemlib +CONFIGURED_APPS += systemlib/mixer diff --git a/nuttx/configs/px4iov2/io/defconfig b/nuttx/configs/px4iov2/io/defconfig new file mode 100755 index 0000000000..bb937cf4ee --- /dev/null +++ b/nuttx/configs/px4iov2/io/defconfig @@ -0,0 +1,548 @@ +############################################################################ +# configs/px4io/nsh/defconfig +# +# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# +############################################################################ +# +# architecture selection +# +# CONFIG_ARCH - identifies the arch subdirectory and, hence, the +# processor architecture. +# CONFIG_ARCH_family - for use in C code. This identifies the +# particular chip family that the architecture is implemented +# in. +# CONFIG_ARCH_architecture - for use in C code. This identifies the +# specific architecture within the chip family. +# CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory +# CONFIG_ARCH_CHIP_name - For use in C code +# CONFIG_ARCH_BOARD - identifies the configs subdirectory and, hence, +# the board that supports the particular chip or SoC. +# CONFIG_ARCH_BOARD_name - for use in C code +# CONFIG_ENDIAN_BIG - define if big endian (default is little endian) +# CONFIG_BOARD_LOOPSPERMSEC - for delay loops +# CONFIG_DRAM_SIZE - Describes the installed DRAM. +# CONFIG_DRAM_START - The start address of DRAM (physical) +# CONFIG_ARCH_IRQPRIO - The ST32F100CB supports interrupt prioritization +# CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt +# stack. If defined, this symbol is the size of the interrupt +# stack in bytes. If not defined, the user task stacks will be +# used during interrupt handling. +# CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions +# CONFIG_ARCH_BOOTLOADER - Set if you are using a bootloader. +# CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to board architecture. +# CONFIG_ARCH_BUTTONS - Enable support for buttons. Unique to board architecture. +# CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that +# cause a 100 second delay during boot-up. This 100 second delay +# serves no purpose other than it allows you to calibrate +# CONFIG_BOARD_LOOPSPERMSEC. You simply use a stop watch to measure +# the 100 second delay then adjust CONFIG_BOARD_LOOPSPERMSEC until +# the delay actually is 100 seconds. +# CONFIG_ARCH_DMA - Support DMA initialization +# +CONFIG_ARCH="arm" +CONFIG_ARCH_ARM=y +CONFIG_ARCH_CORTEXM3=y +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARCH_CHIP_STM32F100C8=y +CONFIG_ARCH_BOARD="px4io" +CONFIG_ARCH_BOARD_PX4IO=y +CONFIG_BOARD_LOOPSPERMSEC=2000 +CONFIG_DRAM_SIZE=0x00002000 +CONFIG_DRAM_START=0x20000000 +CONFIG_ARCH_IRQPRIO=y +CONFIG_ARCH_INTERRUPTSTACK=n +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARCH_BOOTLOADER=n +CONFIG_ARCH_LEDS=n +CONFIG_ARCH_BUTTONS=n +CONFIG_ARCH_CALIBRATION=n +CONFIG_ARCH_DMA=y +CONFIG_ARCH_MATH_H=y + +CONFIG_ARMV7M_CMNVECTOR=y + +# +# JTAG Enable settings (by default JTAG-DP and SW-DP are disabled): +# +# CONFIG_STM32_DFU - Use the DFU bootloader, not JTAG +# +# JTAG Enable options: +# +# CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) +# CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) +# but without JNTRST. +# CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled +# +CONFIG_STM32_DFU=n +CONFIG_STM32_JTAG_FULL_ENABLE=y +CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n +CONFIG_STM32_JTAG_SW_ENABLE=n + +# +# Individual subsystems can be enabled: +# +# AHB: +CONFIG_STM32_DMA1=y +CONFIG_STM32_DMA2=n +CONFIG_STM32_CRC=n +# APB1: +# Timers 2,3 and 4 are owned by the PWM driver +CONFIG_STM32_TIM2=n +CONFIG_STM32_TIM3=n +CONFIG_STM32_TIM4=n +CONFIG_STM32_TIM5=n +CONFIG_STM32_TIM6=n +CONFIG_STM32_TIM7=n +CONFIG_STM32_WWDG=n +CONFIG_STM32_SPI2=n +CONFIG_STM32_USART2=y +CONFIG_STM32_USART3=y +CONFIG_STM32_I2C1=y +CONFIG_STM32_I2C2=n +CONFIG_STM32_BKP=n +CONFIG_STM32_PWR=n +CONFIG_STM32_DAC=n +# APB2: +# We use our own ADC driver, but leave this on for clocking purposes. +CONFIG_STM32_ADC1=y +CONFIG_STM32_ADC2=n +# TIM1 is owned by the HRT +CONFIG_STM32_TIM1=n +CONFIG_STM32_SPI1=n +CONFIG_STM32_TIM8=n +CONFIG_STM32_USART1=y +CONFIG_STM32_ADC3=n + + +# +# STM32F100 specific serial device driver settings +# +# CONFIG_USARTn_SERIAL_CONSOLE - selects the USARTn for the +# console and ttys0 (default is the USART1). +# CONFIG_USARTn_RXBUFSIZE - Characters are buffered as received. +# This specific the size of the receive buffer +# CONFIG_USARTn_TXBUFSIZE - Characters are buffered before +# being sent. This specific the size of the transmit buffer +# CONFIG_USARTn_BAUD - The configure BAUD of the UART. Must be +# CONFIG_USARTn_BITS - The number of bits. Must be either 7 or 8. +# CONFIG_USARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity +# CONFIG_USARTn_2STOP - Two stop bits +# +CONFIG_SERIAL_TERMIOS=y +CONFIG_STANDARD_SERIAL=y + +CONFIG_USART1_SERIAL_CONSOLE=y +CONFIG_USART2_SERIAL_CONSOLE=n +CONFIG_USART3_SERIAL_CONSOLE=n + +CONFIG_USART1_TXBUFSIZE=64 +CONFIG_USART2_TXBUFSIZE=64 +CONFIG_USART3_TXBUFSIZE=64 + +CONFIG_USART1_RXBUFSIZE=64 +CONFIG_USART2_RXBUFSIZE=64 +CONFIG_USART3_RXBUFSIZE=64 + +CONFIG_USART1_BAUD=115200 +CONFIG_USART2_BAUD=115200 +CONFIG_USART3_BAUD=115200 + +CONFIG_USART1_BITS=8 +CONFIG_USART2_BITS=8 +CONFIG_USART3_BITS=8 + +CONFIG_USART1_PARITY=0 +CONFIG_USART2_PARITY=0 +CONFIG_USART3_PARITY=0 + +CONFIG_USART1_2STOP=0 +CONFIG_USART2_2STOP=0 +CONFIG_USART3_2STOP=0 + +CONFIG_USART1_RXDMA=y +SERIAL_HAVE_CONSOLE_DMA=y +# Conflicts with I2C1 DMA +CONFIG_USART2_RXDMA=n +CONFIG_USART3_RXDMA=y + +# +# PX4IO specific driver settings +# +# CONFIG_HRT_TIMER +# Enables the high-resolution timer. The board definition must +# set HRT_TIMER and HRT_TIMER_CHANNEL to the timer and capture/ +# compare channels to be used. +# CONFIG_HRT_PPM +# Enables R/C PPM input using the HRT. The board definition must +# set HRT_PPM_CHANNEL to the timer capture/compare channel to be +# used, and define GPIO_PPM_IN to configure the appropriate timer +# GPIO. +# CONFIG_PWM_SERVO +# Enables the PWM servo driver. The driver configuration must be +# supplied by the board support at initialisation time. +# Note that USART2 must be disabled on the PX4 board for this to +# be available. +# +# +CONFIG_HRT_TIMER=y +CONFIG_HRT_PPM=y + +# +# General build options +# +# CONFIG_RRLOAD_BINARY - make the rrload binary format used with +# BSPs from www.ridgerun.com using the tools/mkimage.sh script +# CONFIG_INTELHEX_BINARY - make the Intel HEX binary format +# used with many different loaders using the GNU objcopy program +# Should not be selected if you are not using the GNU toolchain. +# CONFIG_MOTOROLA_SREC - make the Motorola S-Record binary format +# used with many different loaders using the GNU objcopy program +# Should not be selected if you are not using the GNU toolchain. +# CONFIG_RAW_BINARY - make a raw binary format file used with many +# different loaders using the GNU objcopy program. This option +# should not be selected if you are not using the GNU toolchain. +# CONFIG_HAVE_LIBM - toolchain supports libm.a +# +CONFIG_RRLOAD_BINARY=n +CONFIG_INTELHEX_BINARY=n +CONFIG_MOTOROLA_SREC=n +CONFIG_RAW_BINARY=y +CONFIG_HAVE_LIBM=n + +# +# General OS setup +# +# CONFIG_APPS_DIR - Identifies the relative path to the directory +# that builds the application to link with NuttX. Default: ../apps +# CONFIG_DEBUG - enables built-in debug options +# CONFIG_DEBUG_VERBOSE - enables verbose debug output +# CONFIG_DEBUG_SYMBOLS - build without optimization and with +# debug symbols (needed for use with a debugger). +# CONFIG_HAVE_CXX - Enable support for C++ +# CONFIG_HAVE_CXXINITIALIZE - The platform-specific logic includes support +# for initialization of static C++ instances for this architecture +# and for the selected toolchain (via up_cxxinitialize()). +# CONFIG_MM_REGIONS - If the architecture includes multiple +# regions of memory to allocate from, this specifies the +# number of memory regions that the memory manager must +# handle and enables the API mm_addregion(start, end); +# CONFIG_ARCH_LOWPUTC - architecture supports low-level, boot +# time console output +# CONFIG_MSEC_PER_TICK - The default system timer is 100Hz +# or MSEC_PER_TICK=10. This setting may be defined to +# inform NuttX that the processor hardware is providing +# system timer interrupts at some interrupt interval other +# than 10 msec. +# CONFIG_RR_INTERVAL - The round robin timeslice will be set +# this number of milliseconds; Round robin scheduling can +# be disabled by setting this value to zero. +# CONFIG_SCHED_INSTRUMENTATION - enables instrumentation in +# scheduler to monitor system performance +# CONFIG_TASK_NAME_SIZE - Spcifies that maximum size of a +# task name to save in the TCB. Useful if scheduler +# instrumentation is selected. Set to zero to disable. +# CONFIG_START_YEAR, CONFIG_START_MONTH, CONFIG_START_DAY - +# Used to initialize the internal time logic. +# CONFIG_GREGORIAN_TIME - Enables Gregorian time conversions. +# You would only need this if you are concerned about accurate +# time conversions in the past or in the distant future. +# CONFIG_JULIAN_TIME - Enables Julian time conversions. You +# would only need this if you are concerned about accurate +# time conversion in the distand past. You must also define +# CONFIG_GREGORIAN_TIME in order to use Julian time. +# CONFIG_DEV_CONSOLE - Set if architecture-specific logic +# provides /dev/console. Enables stdout, stderr, stdin. +# CONFIG_DEV_LOWCONSOLE - Use the simple, low-level serial console +# driver (minimul support) +# CONFIG_MUTEX_TYPES: Set to enable support for recursive and +# errorcheck mutexes. Enables pthread_mutexattr_settype(). +# CONFIG_PRIORITY_INHERITANCE : Set to enable support for priority +# inheritance on mutexes and semaphores. +# CONFIG_SEM_PREALLOCHOLDERS: This setting is only used if priority +# inheritance is enabled. It defines the maximum number of +# different threads (minus one) that can take counts on a +# semaphore with priority inheritance support. This may be +# set to zero if priority inheritance is disabled OR if you +# are only using semaphores as mutexes (only one holder) OR +# if no more than two threads participate using a counting +# semaphore. +# CONFIG_SEM_NNESTPRIO. If priority inheritance is enabled, +# then this setting is the maximum number of higher priority +# threads (minus 1) than can be waiting for another thread +# to release a count on a semaphore. This value may be set +# to zero if no more than one thread is expected to wait for +# a semaphore. +# CONFIG_FDCLONE_DISABLE. Disable cloning of all file descriptors +# by task_create() when a new task is started. If set, all +# files/drivers will appear to be closed in the new task. +# CONFIG_FDCLONE_STDIO. Disable cloning of all but the first +# three file descriptors (stdin, stdout, stderr) by task_create() +# when a new task is started. If set, all files/drivers will +# appear to be closed in the new task except for stdin, stdout, +# and stderr. +# CONFIG_SDCLONE_DISABLE. Disable cloning of all socket +# desciptors by task_create() when a new task is started. If +# set, all sockets will appear to be closed in the new task. +# CONFIG_SCHED_WORKQUEUE. Create a dedicated "worker" thread to +# handle delayed processing from interrupt handlers. This feature +# is required for some drivers but, if there are not complaints, +# can be safely disabled. The worker thread also performs +# garbage collection -- completing any delayed memory deallocations +# from interrupt handlers. If the worker thread is disabled, +# then that clean will be performed by the IDLE thread instead +# (which runs at the lowest of priority and may not be appropriate +# if memory reclamation is of high priority). If CONFIG_SCHED_WORKQUEUE +# is enabled, then the following options can also be used: +# CONFIG_SCHED_WORKPRIORITY - The execution priority of the worker +# thread. Default: 50 +# CONFIG_SCHED_WORKPERIOD - How often the worker thread checks for +# work in units of microseconds. Default: 50*1000 (50 MS). +# CONFIG_SCHED_WORKSTACKSIZE - The stack size allocated for the worker +# thread. Default: CONFIG_IDLETHREAD_STACKSIZE. +# CONFIG_SIG_SIGWORK - The signal number that will be used to wake-up +# the worker thread. Default: 4 +# CONFIG_SCHED_WAITPID - Enable the waitpid() API +# CONFIG_SCHED_ATEXIT - Enabled the atexit() API +# +CONFIG_USER_ENTRYPOINT="user_start" +#CONFIG_APPS_DIR= +CONFIG_DEBUG=n +CONFIG_DEBUG_VERBOSE=n +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_FS=n +CONFIG_DEBUG_GRAPHICS=n +CONFIG_DEBUG_LCD=n +CONFIG_DEBUG_USB=n +CONFIG_DEBUG_NET=n +CONFIG_DEBUG_RTC=n +CONFIG_DEBUG_ANALOG=n +CONFIG_DEBUG_PWM=n +CONFIG_DEBUG_CAN=n +CONFIG_DEBUG_I2C=n +CONFIG_DEBUG_INPUT=n + +CONFIG_MSEC_PER_TICK=1 +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_MM_REGIONS=1 +CONFIG_MM_SMALL=y +CONFIG_ARCH_LOWPUTC=y +CONFIG_RR_INTERVAL=0 +CONFIG_SCHED_INSTRUMENTATION=n +CONFIG_TASK_NAME_SIZE=8 +CONFIG_START_YEAR=1970 +CONFIG_START_MONTH=1 +CONFIG_START_DAY=1 +CONFIG_GREGORIAN_TIME=n +CONFIG_JULIAN_TIME=n +# this eats ~1KiB of RAM ... work out why +CONFIG_DEV_CONSOLE=y +CONFIG_DEV_LOWCONSOLE=n +CONFIG_MUTEX_TYPES=n +CONFIG_PRIORITY_INHERITANCE=n +CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_NNESTPRIO=0 +CONFIG_FDCLONE_DISABLE=y +CONFIG_FDCLONE_STDIO=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SCHED_WORKQUEUE=n +CONFIG_SCHED_WORKPRIORITY=50 +CONFIG_SCHED_WORKPERIOD=50000 +CONFIG_SCHED_WORKSTACKSIZE=1024 +CONFIG_SIG_SIGWORK=4 +CONFIG_SCHED_WAITPID=n +CONFIG_SCHED_ATEXIT=n + +# +# The following can be used to disable categories of +# APIs supported by the OS. If the compiler supports +# weak functions, then it should not be necessary to +# disable functions unless you want to restrict usage +# of those APIs. +# +# There are certain dependency relationships in these +# features. +# +# o mq_notify logic depends on signals to awaken tasks +# waiting for queues to become full or empty. +# o pthread_condtimedwait() depends on signals to wake +# up waiting tasks. +# +CONFIG_DISABLE_CLOCK=n +CONFIG_DISABLE_POSIX_TIMERS=y +CONFIG_DISABLE_PTHREAD=y +CONFIG_DISABLE_SIGNALS=y +CONFIG_DISABLE_MQUEUE=y +CONFIG_DISABLE_MOUNTPOINT=y +CONFIG_DISABLE_ENVIRON=y +CONFIG_DISABLE_POLL=y + +# +# Misc libc settings +# +# CONFIG_NOPRINTF_FIELDWIDTH - sprintf-related logic is a +# little smaller if we do not support fieldwidthes +# +CONFIG_NOPRINTF_FIELDWIDTH=n + +# +# Allow for architecture optimized implementations +# +# The architecture can provide optimized versions of the +# following to improve system performance +# +CONFIG_ARCH_MEMCPY=n +CONFIG_ARCH_MEMCMP=n +CONFIG_ARCH_MEMMOVE=n +CONFIG_ARCH_MEMSET=n +CONFIG_ARCH_STRCMP=n +CONFIG_ARCH_STRCPY=n +CONFIG_ARCH_STRNCPY=n +CONFIG_ARCH_STRLEN=n +CONFIG_ARCH_STRNLEN=n +CONFIG_ARCH_BZERO=n + +# +# Sizes of configurable things (0 disables) +# +# CONFIG_MAX_TASKS - The maximum number of simultaneously +# active tasks. This value must be a power of two. +# CONFIG_MAX_TASK_ARGS - This controls the maximum number of +# of parameters that a task may receive (i.e., maxmum value +# of 'argc') +# CONFIG_NPTHREAD_KEYS - The number of items of thread- +# specific data that can be retained +# CONFIG_NFILE_DESCRIPTORS - The maximum number of file +# descriptors (one for each open) +# CONFIG_NFILE_STREAMS - The maximum number of streams that +# can be fopen'ed +# CONFIG_NAME_MAX - The maximum size of a file name. +# CONFIG_STDIO_BUFFER_SIZE - Size of the buffer to allocate +# on fopen. (Only if CONFIG_NFILE_STREAMS > 0) +# CONFIG_STDIO_LINEBUFFER - If standard C buffered I/O is enabled +# (CONFIG_STDIO_BUFFER_SIZE > 0), then this option may be added +# to force automatic, line-oriented flushing the output buffer +# for putc(), fputc(), putchar(), puts(), fputs(), printf(), +# fprintf(), and vfprintf(). When a newline is encountered in +# the output string, the output buffer will be flushed. This +# (slightly) increases the NuttX footprint but supports the kind +# of behavior that people expect for printf(). +# CONFIG_NUNGET_CHARS - Number of characters that can be +# buffered by ungetc() (Only if CONFIG_NFILE_STREAMS > 0) +# CONFIG_PREALLOC_MQ_MSGS - The number of pre-allocated message +# structures. The system manages a pool of preallocated +# message structures to minimize dynamic allocations +# CONFIG_MQ_MAXMSGSIZE - Message structures are allocated with +# a fixed payload size given by this settin (does not include +# other message structure overhead. +# CONFIG_MAX_WDOGPARMS - Maximum number of parameters that +# can be passed to a watchdog handler +# CONFIG_PREALLOC_WDOGS - The number of pre-allocated watchdog +# structures. The system manages a pool of preallocated +# watchdog structures to minimize dynamic allocations +# CONFIG_PREALLOC_TIMERS - The number of pre-allocated POSIX +# timer structures. The system manages a pool of preallocated +# timer structures to minimize dynamic allocations. Set to +# zero for all dynamic allocations. +# +CONFIG_MAX_TASKS=4 +CONFIG_MAX_TASK_ARGS=4 +CONFIG_NPTHREAD_KEYS=2 +CONFIG_NFILE_DESCRIPTORS=8 +CONFIG_NFILE_STREAMS=0 +CONFIG_NAME_MAX=12 +CONFIG_STDIO_BUFFER_SIZE=32 +CONFIG_STDIO_LINEBUFFER=n +CONFIG_NUNGET_CHARS=2 +CONFIG_PREALLOC_MQ_MSGS=4 +CONFIG_MQ_MAXMSGSIZE=32 +CONFIG_MAX_WDOGPARMS=2 +CONFIG_PREALLOC_WDOGS=4 +CONFIG_PREALLOC_TIMERS=0 + + +# +# Settings for apps/nshlib +# +# CONFIG_NSH_BUILTIN_APPS - Support external registered, +# "named" applications that can be executed from the NSH +# command line (see apps/README.txt for more information). +# CONFIG_NSH_FILEIOSIZE - Size of a static I/O buffer +# CONFIG_NSH_STRERROR - Use strerror(errno) +# CONFIG_NSH_LINELEN - Maximum length of one command line +# CONFIG_NSH_NESTDEPTH - Max number of nested if-then[-else]-fi +# CONFIG_NSH_DISABLESCRIPT - Disable scripting support +# CONFIG_NSH_DISABLEBG - Disable background commands +# CONFIG_NSH_ROMFSETC - Use startup script in /etc +# CONFIG_NSH_CONSOLE - Use serial console front end +# CONFIG_NSH_TELNET - Use telnetd console front end +# CONFIG_NSH_ARCHINIT - Platform provides architecture +# specific initialization (nsh_archinitialize()). +# + +# Disable NSH completely +CONFIG_NSH_CONSOLE=n + +# +# Stack and heap information +# +# CONFIG_BOOT_RUNFROMFLASH - Some configurations support XIP +# operation from FLASH but must copy initialized .data sections to RAM. +# (should also be =n for the STM3210E-EVAL which always runs from flash) +# CONFIG_BOOT_COPYTORAM - Some configurations boot in FLASH +# but copy themselves entirely into RAM for better performance. +# CONFIG_CUSTOM_STACK - The up_ implementation will handle +# all stack operations outside of the nuttx model. +# CONFIG_STACK_POINTER - The initial stack pointer (arm7tdmi only) +# CONFIG_IDLETHREAD_STACKSIZE - The size of the initial stack. +# This is the thread that (1) performs the inital boot of the system up +# to the point where user_start() is spawned, and (2) there after is the +# IDLE thread that executes only when there is no other thread ready to +# run. +# CONFIG_USERMAIN_STACKSIZE - The size of the stack to allocate +# for the main user thread that begins at the user_start() entry point. +# CONFIG_PTHREAD_STACK_MIN - Minimum pthread stack size +# CONFIG_PTHREAD_STACK_DEFAULT - Default pthread stack size +# CONFIG_HEAP_BASE - The beginning of the heap +# CONFIG_HEAP_SIZE - The size of the heap +# +CONFIG_BOOT_RUNFROMFLASH=n +CONFIG_BOOT_COPYTORAM=n +CONFIG_CUSTOM_STACK=n +CONFIG_STACK_POINTER= +CONFIG_IDLETHREAD_STACKSIZE=1024 +CONFIG_USERMAIN_STACKSIZE=1200 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_PTHREAD_STACK_DEFAULT=1024 +CONFIG_HEAP_BASE= +CONFIG_HEAP_SIZE= diff --git a/nuttx/configs/px4iov2/io/setenv.sh b/nuttx/configs/px4iov2/io/setenv.sh new file mode 100755 index 0000000000..ff9a4bf8ae --- /dev/null +++ b/nuttx/configs/px4iov2/io/setenv.sh @@ -0,0 +1,47 @@ +#!/bin/bash +# configs/stm3210e-eval/dfu/setenv.sh +# +# Copyright (C) 2009 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# + +if [ "$(basename $0)" = "setenv.sh" ] ; then + echo "You must source this script, not run it!" 1>&2 + exit 1 +fi + +if [ -z "${PATH_ORIG}" ]; then export PATH_ORIG="${PATH}"; fi + +WD=`pwd` +export RIDE_BIN="/cygdrive/c/Program Files/Raisonance/Ride/arm-gcc/bin" +export BUILDROOT_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin" +export PATH="${BUILDROOT_BIN}:${RIDE_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" + +echo "PATH : ${PATH}" diff --git a/nuttx/configs/px4iov2/nsh/Make.defs b/nuttx/configs/px4iov2/nsh/Make.defs new file mode 100644 index 0000000000..87508e22ec --- /dev/null +++ b/nuttx/configs/px4iov2/nsh/Make.defs @@ -0,0 +1,3 @@ +include ${TOPDIR}/.config + +include $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/Make.defs diff --git a/nuttx/configs/px4iov2/nsh/appconfig b/nuttx/configs/px4iov2/nsh/appconfig new file mode 100644 index 0000000000..3cfc41b437 --- /dev/null +++ b/nuttx/configs/px4iov2/nsh/appconfig @@ -0,0 +1,44 @@ +############################################################################ +# configs/stm3210e-eval/nsh/appconfig +# +# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# +############################################################################ + +# Path to example in apps/examples containing the user_start entry point + +CONFIGURED_APPS += examples/nsh + +CONFIGURED_APPS += system/readline +CONFIGURED_APPS += nshlib +CONFIGURED_APPS += reboot + +CONFIGURED_APPS += drivers/boards/px4iov2 diff --git a/nuttx/configs/px4iov2/nsh/defconfig b/nuttx/configs/px4iov2/nsh/defconfig new file mode 100755 index 0000000000..14d7a64012 --- /dev/null +++ b/nuttx/configs/px4iov2/nsh/defconfig @@ -0,0 +1,567 @@ +############################################################################ +# configs/px4io/nsh/defconfig +# +# Copyright (C) 2009-2012 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# +############################################################################ +# +# architecture selection +# +# CONFIG_ARCH - identifies the arch subdirectory and, hence, the +# processor architecture. +# CONFIG_ARCH_family - for use in C code. This identifies the +# particular chip family that the architecture is implemented +# in. +# CONFIG_ARCH_architecture - for use in C code. This identifies the +# specific architecture within the chip familyl. +# CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory +# CONFIG_ARCH_CHIP_name - For use in C code +# CONFIG_ARCH_BOARD - identifies the configs subdirectory and, hence, +# the board that supports the particular chip or SoC. +# CONFIG_ARCH_BOARD_name - for use in C code +# CONFIG_ENDIAN_BIG - define if big endian (default is little endian) +# CONFIG_BOARD_LOOPSPERMSEC - for delay loops +# CONFIG_DRAM_SIZE - Describes the installed DRAM. +# CONFIG_DRAM_START - The start address of DRAM (physical) +# CONFIG_DRAM_END - Last address+1 of installed RAM +# CONFIG_ARCH_IRQPRIO - The ST32F100CB supports interrupt prioritization +# CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt +# stack. If defined, this symbol is the size of the interrupt +# stack in bytes. If not defined, the user task stacks will be +# used during interrupt handling. +# CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions +# CONFIG_ARCH_BOOTLOADER - Set if you are using a bootloader. +# CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to board architecture. +# CONFIG_ARCH_BUTTONS - Enable support for buttons. Unique to board architecture. +# CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that +# cause a 100 second delay during boot-up. This 100 second delay +# serves no purpose other than it allows you to calibrate +# CONFIG_BOARD_LOOPSPERMSEC. You simply use a stop watch to measure +# the 100 second delay then adjust CONFIG_BOARD_LOOPSPERMSEC until +# the delay actually is 100 seconds. +# CONFIG_ARCH_DMA - Support DMA initialization +# +CONFIG_ARCH=arm +CONFIG_ARCH_ARM=y +CONFIG_ARCH_CORTEXM3=y +CONFIG_ARCH_CHIP=stm32 +CONFIG_ARCH_CHIP_STM32F100C8=y +CONFIG_ARCH_BOARD=px4iov2 +CONFIG_ARCH_BOARD_PX4IOV2=y +CONFIG_BOARD_LOOPSPERMSEC=24000 +CONFIG_DRAM_SIZE=0x00002000 +CONFIG_DRAM_START=0x20000000 +CONFIG_DRAM_END=(CONFIG_DRAM_START+CONFIG_DRAM_SIZE) +CONFIG_ARCH_IRQPRIO=y +CONFIG_ARCH_INTERRUPTSTACK=n +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARCH_BOOTLOADER=n +CONFIG_ARCH_LEDS=n +CONFIG_ARCH_BUTTONS=y +CONFIG_ARCH_CALIBRATION=n +CONFIG_ARCH_DMA=n +CONFIG_ARMV7M_CMNVECTOR=y + +# +# JTAG Enable settings (by default JTAG-DP and SW-DP are disabled): +# +# CONFIG_STM32_DFU - Use the DFU bootloader, not JTAG +# +# JTAG Enable options: +# +# CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) +# CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) +# but without JNTRST. +# CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled +# +CONFIG_STM32_DFU=n +CONFIG_STM32_JTAG_FULL_ENABLE=y +CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n +CONFIG_STM32_JTAG_SW_ENABLE=n + +# +# Individual subsystems can be enabled: +# AHB: +CONFIG_STM32_DMA1=n +CONFIG_STM32_DMA2=n +CONFIG_STM32_CRC=n +# APB1: +# Timers 2,3 and 4 are owned by the PWM driver +CONFIG_STM32_TIM2=n +CONFIG_STM32_TIM3=n +CONFIG_STM32_TIM4=n +CONFIG_STM32_TIM5=n +CONFIG_STM32_TIM6=n +CONFIG_STM32_TIM7=n +CONFIG_STM32_WWDG=n +CONFIG_STM32_SPI2=n +CONFIG_STM32_USART2=y +CONFIG_STM32_USART3=y +CONFIG_STM32_I2C1=y +CONFIG_STM32_I2C2=n +CONFIG_STM32_BKP=n +CONFIG_STM32_PWR=n +CONFIG_STM32_DAC=n +# APB2: +CONFIG_STM32_ADC1=y +CONFIG_STM32_ADC2=n +# TIM1 is owned by the HRT +CONFIG_STM32_TIM1=n +CONFIG_STM32_SPI1=n +CONFIG_STM32_TIM8=n +CONFIG_STM32_USART1=y +CONFIG_STM32_ADC3=n + +# +# Timer and I2C devices may need to the following to force power to be applied: +# +#CONFIG_STM32_FORCEPOWER=y + +# +# STM32F100 specific serial device driver settings +# +# CONFIG_USARTn_SERIAL_CONSOLE - selects the USARTn for the +# console and ttys0 (default is the USART1). +# CONFIG_USARTn_RXBUFSIZE - Characters are buffered as received. +# This specific the size of the receive buffer +# CONFIG_USARTn_TXBUFSIZE - Characters are buffered before +# being sent. This specific the size of the transmit buffer +# CONFIG_USARTn_BAUD - The configure BAUD of the UART. Must be +# CONFIG_USARTn_BITS - The number of bits. Must be either 7 or 8. +# CONFIG_USARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity +# CONFIG_USARTn_2STOP - Two stop bits +# +CONFIG_USART1_SERIAL_CONSOLE=y +CONFIG_USART2_SERIAL_CONSOLE=n +CONFIG_USART3_SERIAL_CONSOLE=n + +CONFIG_USART1_TXBUFSIZE=64 +CONFIG_USART2_TXBUFSIZE=64 +CONFIG_USART3_TXBUFSIZE=64 + +CONFIG_USART1_RXBUFSIZE=64 +CONFIG_USART2_RXBUFSIZE=64 +CONFIG_USART3_RXBUFSIZE=64 + +CONFIG_USART1_BAUD=57600 +CONFIG_USART2_BAUD=115200 +CONFIG_USART3_BAUD=115200 + +CONFIG_USART1_BITS=8 +CONFIG_USART2_BITS=8 +CONFIG_USART3_BITS=8 + +CONFIG_USART1_PARITY=0 +CONFIG_USART2_PARITY=0 +CONFIG_USART3_PARITY=0 + +CONFIG_USART1_2STOP=0 +CONFIG_USART2_2STOP=0 +CONFIG_USART3_2STOP=0 + +# +# PX4IO specific driver settings +# +# CONFIG_HRT_TIMER +# Enables the high-resolution timer. The board definition must +# set HRT_TIMER and HRT_TIMER_CHANNEL to the timer and capture/ +# compare channels to be used. +# CONFIG_HRT_PPM +# Enables R/C PPM input using the HRT. The board definition must +# set HRT_PPM_CHANNEL to the timer capture/compare channel to be +# used, and define GPIO_PPM_IN to configure the appropriate timer +# GPIO. +# CONFIG_PWM_SERVO +# Enables the PWM servo driver. The driver configuration must be +# supplied by the board support at initialisation time. +# Note that USART2 must be disabled on the PX4 board for this to +# be available. +# +# +CONFIG_HRT_TIMER=y +CONFIG_HRT_PPM=y +CONFIG_PWM_SERVO=y + +# +# General build options +# +# CONFIG_RRLOAD_BINARY - make the rrload binary format used with +# BSPs from www.ridgerun.com using the tools/mkimage.sh script +# CONFIG_INTELHEX_BINARY - make the Intel HEX binary format +# used with many different loaders using the GNU objcopy program +# Should not be selected if you are not using the GNU toolchain. +# CONFIG_MOTOROLA_SREC - make the Motorola S-Record binary format +# used with many different loaders using the GNU objcopy program +# Should not be selected if you are not using the GNU toolchain. +# CONFIG_RAW_BINARY - make a raw binary format file used with many +# different loaders using the GNU objcopy program. This option +# should not be selected if you are not using the GNU toolchain. +# CONFIG_HAVE_LIBM - toolchain supports libm.a +# +CONFIG_RRLOAD_BINARY=n +CONFIG_INTELHEX_BINARY=n +CONFIG_MOTOROLA_SREC=n +CONFIG_RAW_BINARY=y +CONFIG_HAVE_LIBM=n + +# +# General OS setup +# +# CONFIG_APPS_DIR - Identifies the relative path to the directory +# that builds the application to link with NuttX. Default: ../apps +# CONFIG_DEBUG - enables built-in debug options +# CONFIG_DEBUG_VERBOSE - enables verbose debug output +# CONFIG_DEBUG_SYMBOLS - build without optimization and with +# debug symbols (needed for use with a debugger). +# CONFIG_HAVE_CXX - Enable support for C++ +# CONFIG_HAVE_CXXINITIALIZE - The platform-specific logic includes support +# for initialization of static C++ instances for this architecture +# and for the selected toolchain (via up_cxxinitialize()). +# CONFIG_MM_REGIONS - If the architecture includes multiple +# regions of memory to allocate from, this specifies the +# number of memory regions that the memory manager must +# handle and enables the API mm_addregion(start, end); +# CONFIG_ARCH_LOWPUTC - architecture supports low-level, boot +# time console output +# CONFIG_MSEC_PER_TICK - The default system timer is 100Hz +# or MSEC_PER_TICK=10. This setting may be defined to +# inform NuttX that the processor hardware is providing +# system timer interrupts at some interrupt interval other +# than 10 msec. +# CONFIG_RR_INTERVAL - The round robin timeslice will be set +# this number of milliseconds; Round robin scheduling can +# be disabled by setting this value to zero. +# CONFIG_SCHED_INSTRUMENTATION - enables instrumentation in +# scheduler to monitor system performance +# CONFIG_TASK_NAME_SIZE - Spcifies that maximum size of a +# task name to save in the TCB. Useful if scheduler +# instrumentation is selected. Set to zero to disable. +# CONFIG_START_YEAR, CONFIG_START_MONTH, CONFIG_START_DAY - +# Used to initialize the internal time logic. +# CONFIG_GREGORIAN_TIME - Enables Gregorian time conversions. +# You would only need this if you are concerned about accurate +# time conversions in the past or in the distant future. +# CONFIG_JULIAN_TIME - Enables Julian time conversions. You +# would only need this if you are concerned about accurate +# time conversion in the distand past. You must also define +# CONFIG_GREGORIAN_TIME in order to use Julian time. +# CONFIG_DEV_CONSOLE - Set if architecture-specific logic +# provides /dev/console. Enables stdout, stderr, stdin. +# CONFIG_DEV_LOWCONSOLE - Use the simple, low-level serial console +# driver (minimul support) +# CONFIG_MUTEX_TYPES: Set to enable support for recursive and +# errorcheck mutexes. Enables pthread_mutexattr_settype(). +# CONFIG_PRIORITY_INHERITANCE : Set to enable support for priority +# inheritance on mutexes and semaphores. +# CONFIG_SEM_PREALLOCHOLDERS: This setting is only used if priority +# inheritance is enabled. It defines the maximum number of +# different threads (minus one) that can take counts on a +# semaphore with priority inheritance support. This may be +# set to zero if priority inheritance is disabled OR if you +# are only using semaphores as mutexes (only one holder) OR +# if no more than two threads participate using a counting +# semaphore. +# CONFIG_SEM_NNESTPRIO. If priority inheritance is enabled, +# then this setting is the maximum number of higher priority +# threads (minus 1) than can be waiting for another thread +# to release a count on a semaphore. This value may be set +# to zero if no more than one thread is expected to wait for +# a semaphore. +# CONFIG_FDCLONE_DISABLE. Disable cloning of all file descriptors +# by task_create() when a new task is started. If set, all +# files/drivers will appear to be closed in the new task. +# CONFIG_FDCLONE_STDIO. Disable cloning of all but the first +# three file descriptors (stdin, stdout, stderr) by task_create() +# when a new task is started. If set, all files/drivers will +# appear to be closed in the new task except for stdin, stdout, +# and stderr. +# CONFIG_SDCLONE_DISABLE. Disable cloning of all socket +# desciptors by task_create() when a new task is started. If +# set, all sockets will appear to be closed in the new task. +# CONFIG_NXFLAT. Enable support for the NXFLAT binary format. +# This format will support execution of NuttX binaries located +# in a ROMFS filesystem (see examples/nxflat). +# CONFIG_SCHED_WORKQUEUE. Create a dedicated "worker" thread to +# handle delayed processing from interrupt handlers. This feature +# is required for some drivers but, if there are not complaints, +# can be safely disabled. The worker thread also performs +# garbage collection -- completing any delayed memory deallocations +# from interrupt handlers. If the worker thread is disabled, +# then that clean will be performed by the IDLE thread instead +# (which runs at the lowest of priority and may not be appropriate +# if memory reclamation is of high priority). If CONFIG_SCHED_WORKQUEUE +# is enabled, then the following options can also be used: +# CONFIG_SCHED_WORKPRIORITY - The execution priority of the worker +# thread. Default: 50 +# CONFIG_SCHED_WORKPERIOD - How often the worker thread checks for +# work in units of microseconds. Default: 50*1000 (50 MS). +# CONFIG_SCHED_WORKSTACKSIZE - The stack size allocated for the worker +# thread. Default: CONFIG_IDLETHREAD_STACKSIZE. +# CONFIG_SIG_SIGWORK - The signal number that will be used to wake-up +# the worker thread. Default: 4 +# +#CONFIG_APPS_DIR= +CONFIG_DEBUG=n +CONFIG_DEBUG_VERBOSE=n +CONFIG_DEBUG_SYMBOLS=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=n +CONFIG_MM_REGIONS=1 +CONFIG_MM_SMALL=y +CONFIG_ARCH_LOWPUTC=y +CONFIG_RR_INTERVAL=200 +CONFIG_SCHED_INSTRUMENTATION=n +CONFIG_TASK_NAME_SIZE=0 +CONFIG_START_YEAR=2009 +CONFIG_START_MONTH=9 +CONFIG_START_DAY=21 +CONFIG_GREGORIAN_TIME=n +CONFIG_JULIAN_TIME=n +CONFIG_DEV_CONSOLE=y +CONFIG_DEV_LOWCONSOLE=n +CONFIG_MUTEX_TYPES=n +CONFIG_PRIORITY_INHERITANCE=n +CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_NNESTPRIO=0 +CONFIG_FDCLONE_DISABLE=n +CONFIG_FDCLONE_STDIO=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_NXFLAT=n +CONFIG_SCHED_WORKQUEUE=n +CONFIG_SCHED_WORKPRIORITY=50 +CONFIG_SCHED_WORKPERIOD=(50*1000) +CONFIG_SCHED_WORKSTACKSIZE=512 +CONFIG_SIG_SIGWORK=4 + +CONFIG_USER_ENTRYPOINT="nsh_main" +# +# The following can be used to disable categories of +# APIs supported by the OS. If the compiler supports +# weak functions, then it should not be necessary to +# disable functions unless you want to restrict usage +# of those APIs. +# +# There are certain dependency relationships in these +# features. +# +# o mq_notify logic depends on signals to awaken tasks +# waiting for queues to become full or empty. +# o pthread_condtimedwait() depends on signals to wake +# up waiting tasks. +# +CONFIG_DISABLE_CLOCK=n +CONFIG_DISABLE_POSIX_TIMERS=y +CONFIG_DISABLE_PTHREAD=n +CONFIG_DISABLE_SIGNALS=n +CONFIG_DISABLE_MQUEUE=y +CONFIG_DISABLE_MOUNTPOINT=y +CONFIG_DISABLE_ENVIRON=y +CONFIG_DISABLE_POLL=y + +# +# Misc libc settings +# +# CONFIG_NOPRINTF_FIELDWIDTH - sprintf-related logic is a +# little smaller if we do not support fieldwidthes +# +CONFIG_NOPRINTF_FIELDWIDTH=n + +# +# Allow for architecture optimized implementations +# +# The architecture can provide optimized versions of the +# following to improve system performance +# +CONFIG_ARCH_MEMCPY=n +CONFIG_ARCH_MEMCMP=n +CONFIG_ARCH_MEMMOVE=n +CONFIG_ARCH_MEMSET=n +CONFIG_ARCH_STRCMP=n +CONFIG_ARCH_STRCPY=n +CONFIG_ARCH_STRNCPY=n +CONFIG_ARCH_STRLEN=n +CONFIG_ARCH_STRNLEN=n +CONFIG_ARCH_BZERO=n + +# +# Sizes of configurable things (0 disables) +# +# CONFIG_MAX_TASKS - The maximum number of simultaneously +# active tasks. This value must be a power of two. +# CONFIG_MAX_TASK_ARGS - This controls the maximum number of +# of parameters that a task may receive (i.e., maxmum value +# of 'argc') +# CONFIG_NPTHREAD_KEYS - The number of items of thread- +# specific data that can be retained +# CONFIG_NFILE_DESCRIPTORS - The maximum number of file +# descriptors (one for each open) +# CONFIG_NFILE_STREAMS - The maximum number of streams that +# can be fopen'ed +# CONFIG_NAME_MAX - The maximum size of a file name. +# CONFIG_STDIO_BUFFER_SIZE - Size of the buffer to allocate +# on fopen. (Only if CONFIG_NFILE_STREAMS > 0) +# CONFIG_NUNGET_CHARS - Number of characters that can be +# buffered by ungetc() (Only if CONFIG_NFILE_STREAMS > 0) +# CONFIG_PREALLOC_MQ_MSGS - The number of pre-allocated message +# structures. The system manages a pool of preallocated +# message structures to minimize dynamic allocations +# CONFIG_MQ_MAXMSGSIZE - Message structures are allocated with +# a fixed payload size given by this settin (does not include +# other message structure overhead. +# CONFIG_MAX_WDOGPARMS - Maximum number of parameters that +# can be passed to a watchdog handler +# CONFIG_PREALLOC_WDOGS - The number of pre-allocated watchdog +# structures. The system manages a pool of preallocated +# watchdog structures to minimize dynamic allocations +# CONFIG_PREALLOC_TIMERS - The number of pre-allocated POSIX +# timer structures. The system manages a pool of preallocated +# timer structures to minimize dynamic allocations. Set to +# zero for all dynamic allocations. +# +CONFIG_MAX_TASKS=4 +CONFIG_MAX_TASK_ARGS=4 +CONFIG_NPTHREAD_KEYS=2 +CONFIG_NFILE_DESCRIPTORS=6 +CONFIG_NFILE_STREAMS=4 +CONFIG_NAME_MAX=32 +CONFIG_STDIO_BUFFER_SIZE=64 +CONFIG_NUNGET_CHARS=2 +CONFIG_PREALLOC_MQ_MSGS=1 +CONFIG_MQ_MAXMSGSIZE=32 +CONFIG_MAX_WDOGPARMS=2 +CONFIG_PREALLOC_WDOGS=3 +CONFIG_PREALLOC_TIMERS=1 + + +# +# Settings for apps/nshlib +# +# CONFIG_NSH_BUILTIN_APPS - Support external registered, +# "named" applications that can be executed from the NSH +# command line (see apps/README.txt for more information). +# CONFIG_NSH_FILEIOSIZE - Size of a static I/O buffer +# CONFIG_NSH_STRERROR - Use strerror(errno) +# CONFIG_NSH_LINELEN - Maximum length of one command line +# CONFIG_NSH_NESTDEPTH - Max number of nested if-then[-else]-fi +# CONFIG_NSH_DISABLESCRIPT - Disable scripting support +# CONFIG_NSH_DISABLEBG - Disable background commands +# CONFIG_NSH_ROMFSETC - Use startup script in /etc +# CONFIG_NSH_CONSOLE - Use serial console front end +# CONFIG_NSH_TELNET - Use telnetd console front end +# CONFIG_NSH_ARCHINIT - Platform provides architecture +# specific initialization (nsh_archinitialize()). +# +# If CONFIG_NSH_TELNET is selected: +# CONFIG_NSH_IOBUFFER_SIZE -- Telnetd I/O buffer size +# CONFIG_NSH_DHCPC - Obtain address using DHCP +# CONFIG_NSH_IPADDR - Provides static IP address +# CONFIG_NSH_DRIPADDR - Provides static router IP address +# CONFIG_NSH_NETMASK - Provides static network mask +# CONFIG_NSH_NOMAC - Use a bogus MAC address +# +# If CONFIG_NSH_ROMFSETC is selected: +# CONFIG_NSH_ROMFSMOUNTPT - ROMFS mountpoint +# CONFIG_NSH_INITSCRIPT - Relative path to init script +# CONFIG_NSH_ROMFSDEVNO - ROMFS RAM device minor +# CONFIG_NSH_ROMFSSECTSIZE - ROMF sector size +# CONFIG_NSH_FATDEVNO - FAT FS RAM device minor +# CONFIG_NSH_FATSECTSIZE - FAT FS sector size +# CONFIG_NSH_FATNSECTORS - FAT FS number of sectors +# CONFIG_NSH_FATMOUNTPT - FAT FS mountpoint +# +CONFIG_BUILTIN=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_FILEIOSIZE=64 +CONFIG_NSH_STRERROR=n +CONFIG_NSH_LINELEN=64 +CONFIG_NSH_NESTDEPTH=1 +CONFIG_NSH_DISABLESCRIPT=y +CONFIG_NSH_DISABLEBG=n +CONFIG_NSH_ROMFSETC=n +CONFIG_NSH_CONSOLE=y +CONFIG_NSH_TELNET=n +CONFIG_NSH_ARCHINIT=n +CONFIG_NSH_IOBUFFER_SIZE=256 +CONFIG_NSH_STACKSIZE=1024 +CONFIG_NSH_DHCPC=n +CONFIG_NSH_NOMAC=n +CONFIG_NSH_IPADDR=(10<<24|0<<16|0<<8|2) +CONFIG_NSH_DRIPADDR=(10<<24|0<<16|0<<8|1) +CONFIG_NSH_NETMASK=(255<<24|255<<16|255<<8|0) +CONFIG_NSH_ROMFSMOUNTPT="/etc" +CONFIG_NSH_INITSCRIPT="init.d/rcS" +CONFIG_NSH_ROMFSDEVNO=0 +CONFIG_NSH_ROMFSSECTSIZE=64 +CONFIG_NSH_FATDEVNO=1 +CONFIG_NSH_FATSECTSIZE=512 +CONFIG_NSH_FATNSECTORS=1024 +CONFIG_NSH_FATMOUNTPT=/tmp + +# +# Architecture-specific NSH options +# +CONFIG_NSH_MMCSDSPIPORTNO=0 +CONFIG_NSH_MMCSDSLOTNO=0 +CONFIG_NSH_MMCSDMINOR=0 + +# +# Stack and heap information +# +# CONFIG_BOOT_RUNFROMFLASH - Some configurations support XIP +# operation from FLASH but must copy initialized .data sections to RAM. +# (should also be =n for the STM3210E-EVAL which always runs from flash) +# CONFIG_BOOT_COPYTORAM - Some configurations boot in FLASH +# but copy themselves entirely into RAM for better performance. +# CONFIG_CUSTOM_STACK - The up_ implementation will handle +# all stack operations outside of the nuttx model. +# CONFIG_STACK_POINTER - The initial stack pointer (arm7tdmi only) +# CONFIG_IDLETHREAD_STACKSIZE - The size of the initial stack. +# This is the thread that (1) performs the inital boot of the system up +# to the point where user_start() is spawned, and (2) there after is the +# IDLE thread that executes only when there is no other thread ready to +# run. +# CONFIG_USERMAIN_STACKSIZE - The size of the stack to allocate +# for the main user thread that begins at the user_start() entry point. +# CONFIG_PTHREAD_STACK_MIN - Minimum pthread stack size +# CONFIG_PTHREAD_STACK_DEFAULT - Default pthread stack size +# CONFIG_HEAP_BASE - The beginning of the heap +# CONFIG_HEAP_SIZE - The size of the heap +# +CONFIG_BOOT_RUNFROMFLASH=n +CONFIG_BOOT_COPYTORAM=n +CONFIG_CUSTOM_STACK=n +CONFIG_STACK_POINTER= +CONFIG_IDLETHREAD_STACKSIZE=800 +CONFIG_USERMAIN_STACKSIZE=1024 +CONFIG_PTHREAD_STACK_MIN=256 +CONFIG_PTHREAD_STACK_DEFAULT=512 +CONFIG_HEAP_BASE= +CONFIG_HEAP_SIZE= diff --git a/nuttx/configs/px4iov2/nsh/setenv.sh b/nuttx/configs/px4iov2/nsh/setenv.sh new file mode 100755 index 0000000000..d836851921 --- /dev/null +++ b/nuttx/configs/px4iov2/nsh/setenv.sh @@ -0,0 +1,47 @@ +#!/bin/bash +# configs/stm3210e-eval/dfu/setenv.sh +# +# Copyright (C) 2009 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# + +if [ "$(basename $0)" = "setenv.sh" ] ; then + echo "You must source this script, not run it!" 1>&2 + exit 1 +fi + +if [ -z "${PATH_ORIG}" ]; then export PATH_ORIG="${PATH}"; fi + +WD=`pwd` +export RIDE_BIN="/cygdrive/c/Program Files/Raisonance/Ride/arm-gcc/bin" +export BUILDROOT_BIN="${WD}/../buildroot/build_arm_nofpu/staging_dir/bin" +export PATH="${BUILDROOT_BIN}:${RIDE_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" + +echo "PATH : ${PATH}" diff --git a/nuttx/configs/px4iov2/src/Makefile b/nuttx/configs/px4iov2/src/Makefile new file mode 100644 index 0000000000..bb9539d16a --- /dev/null +++ b/nuttx/configs/px4iov2/src/Makefile @@ -0,0 +1,84 @@ +############################################################################ +# configs/stm3210e-eval/src/Makefile +# +# Copyright (C) 2009-2010 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# +############################################################################ + +-include $(TOPDIR)/Make.defs + +CFLAGS += -I$(TOPDIR)/sched + +ASRCS = +AOBJS = $(ASRCS:.S=$(OBJEXT)) + +CSRCS = empty.c + +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ARCH_SRCDIR = $(TOPDIR)/arch/$(CONFIG_ARCH)/src +ifeq ($(WINTOOL),y) + CFLAGS += -I "${shell cygpath -w $(ARCH_SRCDIR)/chip}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/common}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/armv7-m}" +else + CFLAGS += -I$(ARCH_SRCDIR)/chip -I$(ARCH_SRCDIR)/common -I$(ARCH_SRCDIR)/armv7-m +endif + +all: libboard$(LIBEXT) + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS) $(LINKOBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +libboard$(LIBEXT): $(OBJS) + $(call ARCHIVE, $@, $(OBJS)) + +.depend: Makefile $(SRCS) + @$(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @touch $@ + +depend: .depend + +clean: + $(call DELFILE, libboard$(LIBEXT)) + $(call CLEAN) + +distclean: clean + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) + +-include Make.dep diff --git a/nuttx/configs/px4iov2/src/README.txt b/nuttx/configs/px4iov2/src/README.txt new file mode 100644 index 0000000000..d4eda82fd7 --- /dev/null +++ b/nuttx/configs/px4iov2/src/README.txt @@ -0,0 +1 @@ +This directory contains drivers unique to the STMicro STM3210E-EVAL development board. diff --git a/nuttx/configs/px4iov2/src/drv_i2c_device.c b/nuttx/configs/px4iov2/src/drv_i2c_device.c new file mode 100644 index 0000000000..1f5931ae5e --- /dev/null +++ b/nuttx/configs/px4iov2/src/drv_i2c_device.c @@ -0,0 +1,618 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 A simple, polled I2C slave-mode driver. + * + * The master writes to and reads from a byte buffer, which the caller + * can update inbetween calls to the FSM. + */ + +#include + +#include "stm32_i2c.h" + +#include + +/* + * I2C register definitions. + */ +#define I2C_BASE STM32_I2C1_BASE + +#define REG(_reg) (*(volatile uint32_t *)(I2C_BASE + _reg)) + +#define rCR1 REG(STM32_I2C_CR1_OFFSET) +#define rCR2 REG(STM32_I2C_CR2_OFFSET) +#define rOAR1 REG(STM32_I2C_OAR1_OFFSET) +#define rOAR2 REG(STM32_I2C_OAR2_OFFSET) +#define rDR REG(STM32_I2C_DR_OFFSET) +#define rSR1 REG(STM32_I2C_SR1_OFFSET) +#define rSR2 REG(STM32_I2C_SR2_OFFSET) +#define rCCR REG(STM32_I2C_CCR_OFFSET) +#define rTRISE REG(STM32_I2C_TRISE_OFFSET) + +/* + * "event" values (cr2 << 16 | cr1) as described in the ST DriverLib + */ +#define I2C_EVENT_SLAVE_RECEIVER_ADDRESS_MATCHED ((uint32_t)0x00020002) /* BUSY and ADDR flags */ +#define I2C_EVENT_SLAVE_TRANSMITTER_ADDRESS_MATCHED ((uint32_t)0x00060082) /* TRA, BUSY, TXE and ADDR flags */ +#define I2C_EVENT_SLAVE_BYTE_RECEIVED ((uint32_t)0x00020040) /* BUSY and RXNE flags */ +#define I2C_EVENT_SLAVE_STOP_DETECTED ((uint32_t)0x00000010) /* STOPF flag */ +#define I2C_EVENT_SLAVE_BYTE_TRANSMITTED ((uint32_t)0x00060084) /* TRA, BUSY, TXE and BTF flags */ +#define I2C_EVENT_SLAVE_BYTE_TRANSMITTING ((uint32_t)0x00060080) /* TRA, BUSY and TXE flags */ +#define I2C_EVENT_SLAVE_ACK_FAILURE ((uint32_t)0x00000400) /* AF flag */ + +/** + * States implemented by the I2C FSM. + */ +enum fsm_state { + BAD_PHASE, // must be zero, default exit on a bad state transition + + WAIT_FOR_MASTER, + + /* write from master */ + WAIT_FOR_COMMAND, + RECEIVE_COMMAND, + RECEIVE_DATA, + HANDLE_COMMAND, + + /* read from master */ + WAIT_TO_SEND, + SEND_STATUS, + SEND_DATA, + + NUM_STATES +}; + +/** + * Events recognised by the I2C FSM. + */ +enum fsm_event { + /* automatic transition */ + AUTO, + + /* write from master */ + ADDRESSED_WRITE, + BYTE_RECEIVED, + STOP_RECEIVED, + + /* read from master */ + ADDRESSED_READ, + BYTE_SENDABLE, + ACK_FAILED, + + NUM_EVENTS +}; + +/** + * Context for the I2C FSM + */ +static struct fsm_context { + enum fsm_state state; + + /* XXX want to eliminate these */ + uint8_t command; + uint8_t status; + + uint8_t *data_ptr; + uint32_t data_count; + + size_t buffer_size; + uint8_t *buffer; +} context; + +/** + * Structure defining one FSM state and its outgoing transitions. + */ +struct fsm_transition { + void (*handler)(void); + enum fsm_state next_state[NUM_EVENTS]; +}; + +static bool i2c_command_received; + +static void fsm_event(enum fsm_event event); + +static void go_bad(void); +static void go_wait_master(void); + +static void go_wait_command(void); +static void go_receive_command(void); +static void go_receive_data(void); +static void go_handle_command(void); + +static void go_wait_send(void); +static void go_send_status(void); +static void go_send_buffer(void); + +/** + * The FSM state graph. + */ +static const struct fsm_transition fsm[NUM_STATES] = { + [BAD_PHASE] = { + .handler = go_bad, + .next_state = { + [AUTO] = WAIT_FOR_MASTER, + }, + }, + + [WAIT_FOR_MASTER] = { + .handler = go_wait_master, + .next_state = { + [ADDRESSED_WRITE] = WAIT_FOR_COMMAND, + [ADDRESSED_READ] = WAIT_TO_SEND, + }, + }, + + /* write from master*/ + [WAIT_FOR_COMMAND] = { + .handler = go_wait_command, + .next_state = { + [BYTE_RECEIVED] = RECEIVE_COMMAND, + [STOP_RECEIVED] = WAIT_FOR_MASTER, + }, + }, + [RECEIVE_COMMAND] = { + .handler = go_receive_command, + .next_state = { + [BYTE_RECEIVED] = RECEIVE_DATA, + [STOP_RECEIVED] = HANDLE_COMMAND, + }, + }, + [RECEIVE_DATA] = { + .handler = go_receive_data, + .next_state = { + [BYTE_RECEIVED] = RECEIVE_DATA, + [STOP_RECEIVED] = HANDLE_COMMAND, + }, + }, + [HANDLE_COMMAND] = { + .handler = go_handle_command, + .next_state = { + [AUTO] = WAIT_FOR_MASTER, + }, + }, + + /* buffer send */ + [WAIT_TO_SEND] = { + .handler = go_wait_send, + .next_state = { + [BYTE_SENDABLE] = SEND_STATUS, + }, + }, + [SEND_STATUS] = { + .handler = go_send_status, + .next_state = { + [BYTE_SENDABLE] = SEND_DATA, + [ACK_FAILED] = WAIT_FOR_MASTER, + }, + }, + [SEND_DATA] = { + .handler = go_send_buffer, + .next_state = { + [BYTE_SENDABLE] = SEND_DATA, + [ACK_FAILED] = WAIT_FOR_MASTER, + }, + }, +}; + + +/* debug support */ +#if 1 +struct fsm_logentry { + char kind; + uint32_t code; +}; + +#define LOG_ENTRIES 32 +static struct fsm_logentry fsm_log[LOG_ENTRIES]; +int fsm_logptr; +#define LOG_NEXT(_x) (((_x) + 1) % LOG_ENTRIES) +#define LOGx(_kind, _code) \ + do { \ + fsm_log[fsm_logptr].kind = _kind; \ + fsm_log[fsm_logptr].code = _code; \ + fsm_logptr = LOG_NEXT(fsm_logptr); \ + fsm_log[fsm_logptr].kind = 0; \ + } while(0) + +#define LOG(_kind, _code) \ + do {\ + if (fsm_logptr < LOG_ENTRIES) { \ + fsm_log[fsm_logptr].kind = _kind; \ + fsm_log[fsm_logptr].code = _code; \ + fsm_logptr++;\ + }\ + }while(0) + +#else +#define LOG(_kind, _code) +#endif + + +static void i2c_setclock(uint32_t frequency); + +/** + * Initialise I2C + * + */ +void +i2c_fsm_init(uint8_t *buffer, size_t buffer_size) +{ + /* save the buffer */ + context.buffer = buffer; + context.buffer_size = buffer_size; + + // initialise the FSM + context.status = 0; + context.command = 0; + context.state = BAD_PHASE; + fsm_event(AUTO); + +#if 0 + // enable the i2c block clock and reset it + modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_I2C1EN); + modifyreg32(STM32_RCC_APB1RSTR, 0, RCC_APB1RSTR_I2C1RST); + modifyreg32(STM32_RCC_APB1RSTR, RCC_APB1RSTR_I2C1RST, 0); + + // configure the i2c GPIOs + stm32_configgpio(GPIO_I2C1_SCL); + stm32_configgpio(GPIO_I2C1_SDA); + + // set the peripheral clock to match the APB clock + rCR2 = STM32_PCLK1_FREQUENCY / 1000000; + + // configure for 100kHz operation + i2c_setclock(100000); + + // enable i2c + rCR1 = I2C_CR1_PE; +#endif +} + +/** + * Run the I2C FSM for some period. + * + * @return True if the buffer has been updated by a command. + */ +bool +i2c_fsm(void) +{ + uint32_t event; + int idle_iterations = 0; + + for (;;) { + // handle bus error states by discarding the current operation + if (rSR1 & I2C_SR1_BERR) { + context.state = WAIT_FOR_MASTER; + rSR1 = ~I2C_SR1_BERR; + } + + // we do not anticipate over/underrun errors as clock-stretching is enabled + + // fetch the most recent event + event = ((rSR2 << 16) | rSR1) & 0x00ffffff; + + // generate FSM events based on I2C events + switch (event) { + case I2C_EVENT_SLAVE_RECEIVER_ADDRESS_MATCHED: + LOG('w', 0); + fsm_event(ADDRESSED_WRITE); + break; + + case I2C_EVENT_SLAVE_TRANSMITTER_ADDRESS_MATCHED: + LOG('r', 0); + fsm_event(ADDRESSED_READ); + break; + + case I2C_EVENT_SLAVE_BYTE_RECEIVED: + LOG('R', 0); + fsm_event(BYTE_RECEIVED); + break; + + case I2C_EVENT_SLAVE_STOP_DETECTED: + LOG('s', 0); + fsm_event(STOP_RECEIVED); + break; + + case I2C_EVENT_SLAVE_BYTE_TRANSMITTING: + //case I2C_EVENT_SLAVE_BYTE_TRANSMITTED: + LOG('T', 0); + fsm_event(BYTE_SENDABLE); + break; + + case I2C_EVENT_SLAVE_ACK_FAILURE: + LOG('a', 0); + fsm_event(ACK_FAILED); + break; + + default: + idle_iterations++; +// if ((event) && (event != 0x00020000)) +// LOG('e', event); + break; + } + + /* if we have just received something, drop out and let the caller handle it */ + if (i2c_command_received) { + i2c_command_received = false; + return true; + } + + /* if we have done nothing recently, drop out and let the caller have a slice */ + if (idle_iterations > 1000) + return false; + } +} + +/** + * Update the FSM with an event + * + * @param event New event. + */ +static void +fsm_event(enum fsm_event event) +{ + // move to the next state + context.state = fsm[context.state].next_state[event]; + + LOG('f', context.state); + + // call the state entry handler + if (fsm[context.state].handler) { + fsm[context.state].handler(); + } +} + +static void +go_bad() +{ + LOG('B', 0); + fsm_event(AUTO); +} + +/** + * Wait for the master to address us. + * + */ +static void +go_wait_master() +{ + // We currently don't have a command byte. + // + context.command = '\0'; + + // The data pointer starts pointing to the start of the data buffer. + // + context.data_ptr = context.buffer; + + // The data count is either: + // - the size of the data buffer + // - some value less than or equal the size of the data buffer during a write or a read + // + context.data_count = context.buffer_size; + + // (re)enable the peripheral, clear the stop event flag in + // case we just finished receiving data + rCR1 |= I2C_CR1_PE; + + // clear the ACK failed flag in case we just finished sending data + rSR1 = ~I2C_SR1_AF; +} + +/** + * Prepare to receive a command byte. + * + */ +static void +go_wait_command() +{ + // NOP +} + +/** + * Command byte has been received, save it and prepare to handle the data. + * + */ +static void +go_receive_command() +{ + + // fetch the command byte + context.command = (uint8_t)rDR; + LOG('c', context.command); + +} + +/** + * Receive a data byte. + * + */ +static void +go_receive_data() +{ + uint8_t d; + + // fetch the byte + d = (uint8_t)rDR; + LOG('d', d); + + // if we have somewhere to put it, do so + if (context.data_count) { + *context.data_ptr++ = d; + context.data_count--; + } +} + +/** + * Handle a command once the host is done sending it to us. + * + */ +static void +go_handle_command() +{ + // presume we are happy with the command + context.status = 0; + + // make a note that the buffer contains a fresh command + i2c_command_received = true; + + // kick along to the next state + fsm_event(AUTO); +} + +/** + * Wait to be able to send the status byte. + * + */ +static void +go_wait_send() +{ + // NOP +} + +/** + * Send the status byte. + * + */ +static void +go_send_status() +{ + rDR = context.status; + LOG('?', context.status); +} + +/** + * Send a data or pad byte. + * + */ +static void +go_send_buffer() +{ + if (context.data_count) { + LOG('D', *context.data_ptr); + rDR = *(context.data_ptr++); + context.data_count--; + } else { + LOG('-', 0); + rDR = 0xff; + } +} + +/* cribbed directly from the NuttX master driver */ +static void +i2c_setclock(uint32_t frequency) +{ + uint16_t cr1; + uint16_t ccr; + uint16_t trise; + uint16_t freqmhz; + uint16_t speed; + + /* Disable the selected I2C peripheral to configure TRISE */ + + cr1 = rCR1; + rCR1 &= ~I2C_CR1_PE; + + /* Update timing and control registers */ + + freqmhz = (uint16_t)(STM32_PCLK1_FREQUENCY / 1000000); + ccr = 0; + + /* Configure speed in standard mode */ + + if (frequency <= 100000) { + /* Standard mode speed calculation */ + + speed = (uint16_t)(STM32_PCLK1_FREQUENCY / (frequency << 1)); + + /* The CCR fault must be >= 4 */ + + if (speed < 4) { + /* Set the minimum allowed value */ + + speed = 4; + } + ccr |= speed; + + /* Set Maximum Rise Time for standard mode */ + + trise = freqmhz + 1; + + /* Configure speed in fast mode */ + } else { /* (frequency <= 400000) */ + /* Fast mode speed calculation with Tlow/Thigh = 16/9 */ + +#ifdef CONFIG_I2C_DUTY16_9 + speed = (uint16_t)(STM32_PCLK1_FREQUENCY / (frequency * 25)); + + /* Set DUTY and fast speed bits */ + + ccr |= (I2C_CCR_DUTY|I2C_CCR_FS); +#else + /* Fast mode speed calculation with Tlow/Thigh = 2 */ + + speed = (uint16_t)(STM32_PCLK1_FREQUENCY / (frequency * 3)); + + /* Set fast speed bit */ + + ccr |= I2C_CCR_FS; +#endif + + /* Verify that the CCR speed value is nonzero */ + + if (speed < 1) { + /* Set the minimum allowed value */ + + speed = 1; + } + ccr |= speed; + + /* Set Maximum Rise Time for fast mode */ + + trise = (uint16_t)(((freqmhz * 300) / 1000) + 1); + } + + /* Write the new values of the CCR and TRISE registers */ + + rCCR = ccr; + rTRISE = trise; + + /* Bit 14 of OAR1 must be configured and kept at 1 */ + + rOAR1 = I2C_OAR1_ONE); + + /* Re-enable the peripheral (or not) */ + + rCR1 = cr1; +} diff --git a/nuttx/configs/px4iov2/src/empty.c b/nuttx/configs/px4iov2/src/empty.c new file mode 100644 index 0000000000..ace900866c --- /dev/null +++ b/nuttx/configs/px4iov2/src/empty.c @@ -0,0 +1,4 @@ +/* + * There are no source files here, but libboard.a can't be empty, so + * we have this empty source file to keep it company. + */ From 31c6440b6491cd6310c9db72be200a4ffba689c9 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 17 Apr 2013 11:54:24 -0700 Subject: [PATCH 068/486] Un-ignore the *.d directories in the ROMFS directory to avoid ignoring the init.d directory and friends. This is rinky, but the alternatives are all a mess. --- .gitignore | 3 + ROMFS/px4fmu_common/init.d/rc.FMU_quad_x | 67 ++++++++++++++++ ROMFS/px4fmu_common/init.d/rc.PX4IO | 80 ++++++++++++++++++ ROMFS/px4fmu_common/init.d/rc.PX4IOAR | 98 +++++++++++++++++++++++ ROMFS/px4fmu_common/init.d/rc.boarddetect | 66 +++++++++++++++ ROMFS/px4fmu_common/init.d/rc.jig | 10 +++ ROMFS/px4fmu_common/init.d/rc.logging | 9 +++ ROMFS/px4fmu_common/init.d/rc.sensors | 34 ++++++++ ROMFS/px4fmu_common/init.d/rc.standalone | 13 +++ ROMFS/px4fmu_common/init.d/rcS | 79 ++++++++++++++++++ 10 files changed, 459 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/rc.FMU_quad_x create mode 100644 ROMFS/px4fmu_common/init.d/rc.PX4IO create mode 100644 ROMFS/px4fmu_common/init.d/rc.PX4IOAR create mode 100644 ROMFS/px4fmu_common/init.d/rc.boarddetect create mode 100644 ROMFS/px4fmu_common/init.d/rc.jig create mode 100644 ROMFS/px4fmu_common/init.d/rc.logging create mode 100644 ROMFS/px4fmu_common/init.d/rc.sensors create mode 100644 ROMFS/px4fmu_common/init.d/rc.standalone create mode 100755 ROMFS/px4fmu_common/init.d/rcS diff --git a/.gitignore b/.gitignore index 46b347f72e..91358e1e18 100644 --- a/.gitignore +++ b/.gitignore @@ -56,3 +56,6 @@ core mkdeps Archives Build +!ROMFS/*/*.d +!ROMFS/*/*/*.d +!ROMFS/*/*/*/*.d diff --git a/ROMFS/px4fmu_common/init.d/rc.FMU_quad_x b/ROMFS/px4fmu_common/init.d/rc.FMU_quad_x new file mode 100644 index 0000000000..8787443ea2 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.FMU_quad_x @@ -0,0 +1,67 @@ +#!nsh +# +# Flight startup script for PX4FMU with PWM outputs. +# + +# Disable the USB interface +set USB no + +# Disable autostarting other apps +set MODE custom + +echo "[init] doing PX4FMU Quad startup..." + +# +# Start the ORB +# +uorb start + +# +# Load microSD params +# +echo "[init] loading microSD params" +param select /fs/microsd/parameters +if [ -f /fs/microsd/parameters ] +then + param load /fs/microsd/parameters +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor +# see https://pixhawk.ethz.ch/mavlink/ +# +param set MAV_TYPE 2 + +# +# Start MAVLink +# +mavlink start -d /dev/ttyS0 -b 57600 +usleep 5000 + +# +# Start the sensors and test them. +# +sh /etc/init.d/rc.sensors + +# +# Start the commander. +# +commander start + +# +# Start the attitude estimator +# +attitude_estimator_ekf start + +echo "[init] starting PWM output" +fmu mode_pwm +mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix + +# +# Start attitude control +# +multirotor_att_control start + +echo "[init] startup done, exiting" +exit \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rc.PX4IO b/ROMFS/px4fmu_common/init.d/rc.PX4IO new file mode 100644 index 0000000000..1e3963b9ad --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.PX4IO @@ -0,0 +1,80 @@ +#!nsh + +# Disable USB and autostart +set USB no +set MODE camflyer + +# +# Start the ORB +# +uorb start + +# +# Load microSD params +# +echo "[init] loading microSD params" +param select /fs/microsd/parameters +if [ -f /fs/microsd/parameters ] +then + param load /fs/microsd/parameters +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor +# see https://pixhawk.ethz.ch/mavlink/ +# +param set MAV_TYPE 1 + +# +# Start the sensors. +# +sh /etc/init.d/rc.sensors + +# +# Start MAVLink +# +mavlink start -d /dev/ttyS1 -b 57600 +usleep 5000 + +# +# Start the commander. +# +commander start + +# +# Start GPS interface +# +gps start + +# +# Start the attitude estimator +# +kalman_demo start + +# +# Start PX4IO interface +# +px4io start + +# +# Load mixer and start controllers +# +mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix +control_demo start + +# +# Start logging +# +sdlog start -s 10 + +# +# Start system state +# +if blinkm start +then + echo "using BlinkM for state indication" + blinkm systemstate +else + echo "no BlinkM found, OK." +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.PX4IOAR b/ROMFS/px4fmu_common/init.d/rc.PX4IOAR new file mode 100644 index 0000000000..72df68e350 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.PX4IOAR @@ -0,0 +1,98 @@ +#!nsh +# +# Flight startup script for PX4FMU on PX4IOAR carrier board. +# + +# Disable the USB interface +set USB no + +# Disable autostarting other apps +set MODE ardrone + +echo "[init] doing PX4IOAR startup..." + +# +# Start the ORB +# +uorb start + +# +# Init the parameter storage +# +echo "[init] loading microSD params" +param select /fs/microsd/parameters +if [ -f /fs/microsd/parameters ] +then + param load /fs/microsd/parameters +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor +# see https://pixhawk.ethz.ch/mavlink/ +# +param set MAV_TYPE 2 + +# +# Start the sensors. +# +sh /etc/init.d/rc.sensors + +# +# Start MAVLink +# +mavlink start -d /dev/ttyS0 -b 57600 +usleep 5000 + +# +# Start the commander. +# +commander start + +# +# Start the attitude estimator +# +attitude_estimator_ekf start + +# +# Configure PX4FMU for operation with PX4IOAR +# +fmu mode_gpio_serial + +# +# Fire up the multi rotor attitude controller +# +multirotor_att_control start + +# +# Fire up the AR.Drone interface. +# +ardrone_interface start -d /dev/ttyS1 + +# +# Start GPS capture +# +gps start + +# +# Start logging +# +sdlog start -s 10 + +# +# Start system state +# +if blinkm start +then + echo "using BlinkM for state indication" + blinkm systemstate +else + echo "no BlinkM found, OK." +fi + +# +# startup is done; we don't want the shell because we +# use the same UART for telemetry +# +echo "[init] startup done" +exit \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rc.boarddetect b/ROMFS/px4fmu_common/init.d/rc.boarddetect new file mode 100644 index 0000000000..f233e51df4 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.boarddetect @@ -0,0 +1,66 @@ +#!nsh +# +# If we are still in flight mode, work out what airframe +# configuration we have and start up accordingly. +# +if [ $MODE != autostart ] +then + echo "[init] automatic startup cancelled by user script" +else + echo "[init] detecting attached hardware..." + + # + # Assume that we are PX4FMU in standalone mode + # + set BOARD PX4FMU + + # + # Are we attached to a PX4IOAR (AR.Drone carrier board)? + # + if boardinfo test name PX4IOAR + then + set BOARD PX4IOAR + if [ -f /etc/init.d/rc.PX4IOAR ] + then + echo "[init] reading /etc/init.d/rc.PX4IOAR" + usleep 500 + sh /etc/init.d/rc.PX4IOAR + fi + else + echo "[init] PX4IOAR not detected" + fi + + # + # Are we attached to a PX4IO? + # + if boardinfo test name PX4IO + then + set BOARD PX4IO + if [ -f /etc/init.d/rc.PX4IO ] + then + echo "[init] reading /etc/init.d/rc.PX4IO" + usleep 500 + sh /etc/init.d/rc.PX4IO + fi + else + echo "[init] PX4IO not detected" + fi + + # + # Looks like we are stand-alone + # + if [ $BOARD == PX4FMU ] + then + echo "[init] no expansion board detected" + if [ -f /etc/init.d/rc.standalone ] + then + echo "[init] reading /etc/init.d/rc.standalone" + sh /etc/init.d/rc.standalone + fi + fi + + # + # We may not reach here if the airframe-specific script exits the shell. + # + echo "[init] startup done." +fi \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rc.jig b/ROMFS/px4fmu_common/init.d/rc.jig new file mode 100644 index 0000000000..e2b5d8f30d --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.jig @@ -0,0 +1,10 @@ +#!nsh +# +# Test jig startup script +# + +echo "[testing] doing production test.." + +tests jig + +echo "[testing] testing done" diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging new file mode 100644 index 0000000000..09c2d00d19 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -0,0 +1,9 @@ +#!nsh +# +# Initialise logging services. +# + +if [ -d /fs/microsd ] +then + sdlog start +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors new file mode 100644 index 0000000000..42c2f52e94 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -0,0 +1,34 @@ +#!nsh +# +# Standard startup script for PX4FMU onboard sensor drivers. +# + +# +# Start sensor drivers here. +# + +ms5611 start +adc start + +if mpu6000 start +then + echo "using MPU6000 and HMC5883L" + hmc5883 start +else + echo "using L3GD20 and LSM303D" + l3gd20 start + lsm303 start +fi + +# +# Start the sensor collection task. +# IMPORTANT: this also loads param offsets +# ALWAYS start this task before the +# preflight_check. +# +sensors start + +# +# Check sensors - run AFTER 'sensors start' +# +preflight_check \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rc.standalone b/ROMFS/px4fmu_common/init.d/rc.standalone new file mode 100644 index 0000000000..67e95215b9 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.standalone @@ -0,0 +1,13 @@ +#!nsh +# +# Flight startup script for PX4FMU standalone configuration. +# + +echo "[init] doing standalone PX4FMU startup..." + +# +# Start the ORB +# +uorb start + +echo "[init] startup done" diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS new file mode 100755 index 0000000000..89a7678797 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -0,0 +1,79 @@ +#!nsh +# +# PX4FMU startup script. +# +# This script is responsible for: +# +# - mounting the microSD card (if present) +# - running the user startup script from the microSD card (if present) +# - detecting the configuration of the system and picking a suitable +# startup script to continue with +# +# Note: DO NOT add configuration-specific commands to this script; +# add them to the per-configuration scripts instead. +# + +# +# Default to auto-start mode. An init script on the microSD card +# can change this to prevent automatic startup of the flight script. +# +set MODE autostart +set USB autoconnect + +# +# Start playing the startup tune +# +tone_alarm start + +# +# Try to mount the microSD card. +# +echo "[init] looking for microSD..." +if mount -t vfat /dev/mmcsd0 /fs/microsd +then + echo "[init] card mounted at /fs/microsd" +else + echo "[init] no microSD card found" +fi + +# +# Look for an init script on the microSD card. +# +# To prevent automatic startup in the current flight mode, +# the script should set MODE to some other value. +# +if [ -f /fs/microsd/etc/rc ] +then + echo "[init] reading /fs/microsd/etc/rc" + sh /fs/microsd/etc/rc +fi +# Also consider rc.txt files +if [ -f /fs/microsd/etc/rc.txt ] +then + echo "[init] reading /fs/microsd/etc/rc.txt" + sh /fs/microsd/etc/rc.txt +fi + +# +# Check for USB host +# +if [ $USB != autoconnect ] +then + echo "[init] not connecting USB" +else + if sercon + then + echo "[init] USB interface connected" + else + echo "[init] No USB connected" + fi +fi + +# if this is an APM build then there will be a rc.APM script +# from an EXTERNAL_SCRIPTS build option +if [ -f /etc/init.d/rc.APM ] +then + echo Running rc.APM + # if APM startup is successful then nsh will exit + sh /etc/init.d/rc.APM +fi From 0eddcb335707284269cf9c89ac6b820efa1a6b13 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 17 Apr 2013 10:56:58 -0700 Subject: [PATCH 069/486] Tried to collect some changes that I needed to build for FMUv2 into a commit --- makefiles/config_px4fmu_default.mk | 5 +- nuttx/configs/px4fmu/include/board.h | 6 + nuttx/configs/px4fmu/nsh/appconfig | 5 +- nuttx/configs/px4fmu/nsh/defconfig | 2 +- src/drivers/boards/px4fmu/module.mk | 9 + src/drivers/boards/px4fmu/px4fmu_can.c | 144 ++++++++++ src/drivers/boards/px4fmu/px4fmu_init.c | 266 +++++++++++++++++++ src/drivers/boards/px4fmu/px4fmu_internal.h | 162 +++++++++++ src/drivers/boards/px4fmu/px4fmu_pwm_servo.c | 87 ++++++ src/drivers/boards/px4fmu/px4fmu_spi.c | 154 +++++++++++ src/drivers/boards/px4fmu/px4fmu_usb.c | 108 ++++++++ 11 files changed, 942 insertions(+), 6 deletions(-) create mode 100644 src/drivers/boards/px4fmu/module.mk create mode 100644 src/drivers/boards/px4fmu/px4fmu_can.c create mode 100644 src/drivers/boards/px4fmu/px4fmu_init.c create mode 100644 src/drivers/boards/px4fmu/px4fmu_internal.h create mode 100644 src/drivers/boards/px4fmu/px4fmu_pwm_servo.c create mode 100644 src/drivers/boards/px4fmu/px4fmu_spi.c create mode 100644 src/drivers/boards/px4fmu/px4fmu_usb.c diff --git a/makefiles/config_px4fmu_default.mk b/makefiles/config_px4fmu_default.mk index 1a6c91b269..81dd202e79 100644 --- a/makefiles/config_px4fmu_default.mk +++ b/makefiles/config_px4fmu_default.mk @@ -10,7 +10,9 @@ ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common # # Board support modules # -MODULES += drivers/px4fmu +MODULES += drivers/boards/px4fmu +MODULES += drivers/lsm303d +MODULES += drivers/l3gd20 # # Transitional support - add commands from the NuttX export archive. @@ -44,7 +46,6 @@ BUILTIN_COMMANDS := \ $(call _B, hmc5883, , 4096, hmc5883_main ) \ $(call _B, hott_telemetry, , 2048, hott_telemetry_main ) \ $(call _B, kalman_demo, SCHED_PRIORITY_MAX-30, 2048, kalman_demo_main ) \ - $(call _B, l3gd20, , 2048, l3gd20_main ) \ $(call _B, math_demo, , 8192, math_demo_main ) \ $(call _B, mavlink, , 2048, mavlink_main ) \ $(call _B, mavlink_onboard, , 2048, mavlink_onboard_main ) \ diff --git a/nuttx/configs/px4fmu/include/board.h b/nuttx/configs/px4fmu/include/board.h index 0bc0b30216..294b6c3984 100755 --- a/nuttx/configs/px4fmu/include/board.h +++ b/nuttx/configs/px4fmu/include/board.h @@ -308,6 +308,10 @@ #define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 #define GPIO_SPI1_SCK GPIO_SPI1_SCK_1 +#define GPIO_SPI2_MISO GPIO_SPI2_MISO_2 +#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_2 +#define GPIO_SPI2_SCK GPIO_SPI2_SCK_2 + #define GPIO_SPI3_MISO GPIO_SPI3_MISO_2 #define GPIO_SPI3_MOSI GPIO_SPI3_MOSI_1 #define GPIO_SPI3_SCK GPIO_SPI3_SCK_2 @@ -321,6 +325,8 @@ #define PX4_SPIDEV_ACCEL 2 #define PX4_SPIDEV_MPU 3 +#define PX4_SPIDEV_ACCEL_MAG 2 // external for anti vibration test + /* * Tone alarm output */ diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index d642b46923..b60bbfdd92 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -110,16 +110,15 @@ endif CONFIGURED_APPS += systemcmds/i2c # Communication and Drivers -CONFIGURED_APPS += drivers/boards/px4fmu +#CONFIGURED_APPS += drivers/boards/px4fmu CONFIGURED_APPS += drivers/device CONFIGURED_APPS += drivers/ms5611 CONFIGURED_APPS += drivers/hmc5883 CONFIGURED_APPS += drivers/mpu6000 CONFIGURED_APPS += drivers/bma180 -CONFIGURED_APPS += drivers/l3gd20 CONFIGURED_APPS += drivers/px4io CONFIGURED_APPS += drivers/stm32 -CONFIGURED_APPS += drivers/led +#CONFIGURED_APPS += drivers/led CONFIGURED_APPS += drivers/blinkm CONFIGURED_APPS += drivers/stm32/tone_alarm CONFIGURED_APPS += drivers/stm32/adc diff --git a/nuttx/configs/px4fmu/nsh/defconfig b/nuttx/configs/px4fmu/nsh/defconfig index 130886aac2..cf30b835fc 100755 --- a/nuttx/configs/px4fmu/nsh/defconfig +++ b/nuttx/configs/px4fmu/nsh/defconfig @@ -85,7 +85,7 @@ CONFIG_ARCH_FPU=y CONFIG_ARCH_INTERRUPTSTACK=n CONFIG_ARCH_STACKDUMP=y CONFIG_ARCH_BOOTLOADER=n -CONFIG_ARCH_LEDS=y +CONFIG_ARCH_LEDS=n CONFIG_ARCH_BUTTONS=n CONFIG_ARCH_CALIBRATION=n CONFIG_ARCH_DMA=y diff --git a/src/drivers/boards/px4fmu/module.mk b/src/drivers/boards/px4fmu/module.mk new file mode 100644 index 0000000000..2cb261d305 --- /dev/null +++ b/src/drivers/boards/px4fmu/module.mk @@ -0,0 +1,9 @@ +# +# Board-specific startup code for the PX4FMU +# + +SRCS = px4fmu_can.c \ + px4fmu_init.c \ + px4fmu_pwm_servo.c \ + px4fmu_spi.c \ + px4fmu_usb.c diff --git a/src/drivers/boards/px4fmu/px4fmu_can.c b/src/drivers/boards/px4fmu/px4fmu_can.c new file mode 100644 index 0000000000..86ba183b83 --- /dev/null +++ b/src/drivers/boards/px4fmu/px4fmu_can.c @@ -0,0 +1,144 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 px4fmu_can.c + * + * Board-specific CAN functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include + +#include +#include + +#include "chip.h" +#include "up_arch.h" + +#include "stm32.h" +#include "stm32_can.h" +#include "px4fmu_internal.h" + +#ifdef CONFIG_CAN + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ +/* Configuration ********************************************************************/ + +#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2) +# warning "Both CAN1 and CAN2 are enabled. Assuming only CAN1." +# undef CONFIG_STM32_CAN2 +#endif + +#ifdef CONFIG_STM32_CAN1 +# define CAN_PORT 1 +#else +# define CAN_PORT 2 +#endif + +/* Debug ***************************************************************************/ +/* Non-standard debug that may be enabled just for testing CAN */ + +#ifdef CONFIG_DEBUG_CAN +# define candbg dbg +# define canvdbg vdbg +# define canlldbg lldbg +# define canllvdbg llvdbg +#else +# define candbg(x...) +# define canvdbg(x...) +# define canlldbg(x...) +# define canllvdbg(x...) +#endif + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: can_devinit + * + * Description: + * All STM32 architectures must provide the following interface to work with + * examples/can. + * + ************************************************************************************/ + +int can_devinit(void) +{ + static bool initialized = false; + struct can_dev_s *can; + int ret; + + /* Check if we have already initialized */ + + if (!initialized) { + /* Call stm32_caninitialize() to get an instance of the CAN interface */ + + can = stm32_caninitialize(CAN_PORT); + + if (can == NULL) { + candbg("ERROR: Failed to get CAN interface\n"); + return -ENODEV; + } + + /* Register the CAN driver at "/dev/can0" */ + + ret = can_register("/dev/can0", can); + + if (ret < 0) { + candbg("ERROR: can_register failed: %d\n", ret); + return ret; + } + + /* Now we are initialized */ + + initialized = true; + } + + return OK; +} + +#endif \ No newline at end of file diff --git a/src/drivers/boards/px4fmu/px4fmu_init.c b/src/drivers/boards/px4fmu/px4fmu_init.c new file mode 100644 index 0000000000..96c7fa2df3 --- /dev/null +++ b/src/drivers/boards/px4fmu/px4fmu_init.c @@ -0,0 +1,266 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 px4fmu_init.c + * + * PX4FMU-specific early startup code. This file implements the + * nsh_archinitialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialisation. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "stm32_internal.h" +#include "px4fmu_internal.h" +#include "stm32_uart.h" + +#include + +#include +#include + +#include + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +/* Debug ********************************************************************/ + +#ifdef CONFIG_CPP_HAVE_VARARGS +# ifdef CONFIG_DEBUG +# define message(...) lowsyslog(__VA_ARGS__) +# else +# define message(...) printf(__VA_ARGS__) +# endif +#else +# ifdef CONFIG_DEBUG +# define message lowsyslog +# else +# define message printf +# endif +#endif + +/**************************************************************************** + * Protected Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void stm32_boardinitialize(void) +{ + /* configure SPI interfaces */ + stm32_spiinitialize(); + + /* configure LEDs */ + up_ledinit(); +} + +/**************************************************************************** + * Name: nsh_archinitialize + * + * Description: + * Perform architecture specific initialization + * + ****************************************************************************/ + +static struct spi_dev_s *spi1; +static struct spi_dev_s *spi2; +static struct spi_dev_s *spi3; + +#include + +#ifdef __cplusplus +__EXPORT int matherr(struct __exception *e) +{ + return 1; +} +#else +__EXPORT int matherr(struct exception *e) +{ + return 1; +} +#endif + +__EXPORT int nsh_archinitialize(void) +{ + int result; + +// /* configure ADC pins */ +// stm32_configgpio(GPIO_ADC1_IN10); +// stm32_configgpio(GPIO_ADC1_IN11); +// stm32_configgpio(GPIO_ADC1_IN12); + +// /* configure power supply control pins */ +// stm32_configgpio(GPIO_VDD_5V_PERIPH_EN); +// stm32_configgpio(GPIO_VDD_2V8_SENSORS_EN); +// stm32_configgpio(GPIO_VDD_5V_HIPOWER_OC); +// stm32_configgpio(GPIO_VDD_5V_PERIPH_OC); + + /* configure the high-resolution time/callout interface */ + hrt_init(); + + /* configure CPU load estimation */ +#ifdef CONFIG_SCHED_INSTRUMENTATION + cpuload_initialize_once(); +#endif + + /* set up the serial DMA polling */ + static struct hrt_call serial_dma_call; + struct timespec ts; + + /* + * Poll at 1ms intervals for received bytes that have not triggered + * a DMA event. + */ + ts.tv_sec = 0; + ts.tv_nsec = 1000000; + + hrt_call_every(&serial_dma_call, + ts_to_abstime(&ts), + ts_to_abstime(&ts), + (hrt_callout)stm32_serial_dma_poll, + NULL); + + // initial LED state +// drv_led_start(); + up_ledoff(LED_BLUE); + up_ledoff(LED_AMBER); + up_ledon(LED_BLUE); + + /* Configure SPI-based devices */ + + spi1 = up_spiinitialize(1); + + if (!spi1) { + message("[boot] FAILED to initialize SPI port 1\r\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + + // Default SPI1 to 1MHz and de-assert the known chip selects. + SPI_SETFREQUENCY(spi1, 10000000); + SPI_SETBITS(spi1, 8); + SPI_SETMODE(spi1, SPIDEV_MODE3); + SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false); + SPI_SELECT(spi1, PX4_SPIDEV_ACCEL_MAG, false); +// SPI_SELECT(spi1, PX4_SPIDEV_ACCEL, false); +// SPI_SELECT(spi1, PX4_SPIDEV_MPU, false); + up_udelay(20); + + message("[boot] Successfully initialized SPI port 1\r\n"); + + + spi2 = up_spiinitialize(2); + + if (!spi2) { + message("[boot] FAILED to initialize SPI port 2\r\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + + // Default SPI1 to 1MHz and de-assert the known chip selects. + SPI_SETFREQUENCY(spi2, 10000000); + SPI_SETBITS(spi2, 8); + SPI_SETMODE(spi2, SPIDEV_MODE3); + SPI_SELECT(spi2, PX4_SPIDEV_GYRO, false); + SPI_SELECT(spi2, PX4_SPIDEV_ACCEL_MAG, false); +// SPI_SELECT(spi2, PX4_SPIDEV_ACCEL, false); +// SPI_SELECT(spi2, PX4_SPIDEV_MPU, false); + up_udelay(20); + + message("[boot] Successfully initialized SPI port2\r\n"); + + /* Get the SPI port for the microSD slot */ + + message("[boot] Initializing SPI port 3\n"); + spi3 = up_spiinitialize(3); + + if (!spi3) { + message("[boot] FAILED to initialize SPI port 3\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + + message("[boot] Successfully initialized SPI port 3\n"); + + /* Now bind the SPI interface to the MMCSD driver */ + result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi3); + + if (result != OK) { + message("[boot] FAILED to bind SPI port 3 to the MMCSD driver\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + + message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n"); + + stm32_configgpio(GPIO_ADC1_IN10); + stm32_configgpio(GPIO_ADC1_IN11); +// stm32_configgpio(GPIO_ADC1_IN12); +// stm32_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards + + return OK; +} diff --git a/src/drivers/boards/px4fmu/px4fmu_internal.h b/src/drivers/boards/px4fmu/px4fmu_internal.h new file mode 100644 index 0000000000..671a58f8f6 --- /dev/null +++ b/src/drivers/boards/px4fmu/px4fmu_internal.h @@ -0,0 +1,162 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 px4fmu_internal.h + * + * PX4FMU internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + +__BEGIN_DECLS + +/* these headers are not C++ safe */ +#include + + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ +/* Configuration ************************************************************************************/ + +//#ifdef CONFIG_STM32_SPI2 +//# error "SPI2 is not supported on this board" +//#endif + +#if defined(CONFIG_STM32_CAN1) +# warning "CAN1 is not supported on this board" +#endif + +/* PX4FMU GPIOs ***********************************************************************************/ +/* LEDs */ + +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15) +#define GPIO_LED2 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14) + +/* External interrupts */ +#define GPIO_EXTI_COMPASS (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN1) +// XXX MPU6000 DRDY? + +/* SPI chip selects */ + +#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN14) +#define GPIO_SPI_CS_ACCEL (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) +#define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0) +#define GPIO_SPI_CS_SDCARD (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4) + +/* User GPIOs + * + * GPIO0-1 are the buffered high-power GPIOs. + * GPIO2-5 are the USART2 pins. + * GPIO6-7 are the CAN2 pins. + */ +#define GPIO_GPIO0_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4) +#define GPIO_GPIO1_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN5) +#define GPIO_GPIO2_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN0) +#define GPIO_GPIO3_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN1) +#define GPIO_GPIO4_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN2) +#define GPIO_GPIO5_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN3) +#define GPIO_GPIO6_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN13) +#define GPIO_GPIO7_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN2) +#define GPIO_GPIO0_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN4) +#define GPIO_GPIO1_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN5) +#define GPIO_GPIO2_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0) +#define GPIO_GPIO3_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1) +#define GPIO_GPIO4_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN2) +#define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN3) +#define GPIO_GPIO6_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN13) +#define GPIO_GPIO7_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN12) +#define GPIO_GPIO_DIR (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) + +/* USB OTG FS + * + * PA9 OTG_FS_VBUS VBUS sensing (also connected to the green LED) + */ +#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9) + +/* PWM + * + * The PX4FMU has five PWM outputs, of which only the output on + * pin PC8 is fixed assigned to this function. The other four possible + * pwm sources are the TX, RX, RTS and CTS pins of USART2 + * + * Alternate function mapping: + * PC8 - BUZZER - TIM8_CH3/SDIO_D0 /TIM3_CH3/ USART6_CK / DCMI_D2 + */ + +#ifdef CONFIG_PWM +# if defined(CONFIG_STM32_TIM3_PWM) +# define BUZZER_PWMCHANNEL 3 +# define BUZZER_PWMTIMER 3 +# elif defined(CONFIG_STM32_TIM8_PWM) +# define BUZZER_PWMCHANNEL 3 +# define BUZZER_PWMTIMER 8 +# endif +#endif + +/**************************************************************************************************** + * Public Types + ****************************************************************************************************/ + +/**************************************************************************************************** + * Public data + ****************************************************************************************************/ + +#ifndef __ASSEMBLY__ + +/**************************************************************************************************** + * Public Functions + ****************************************************************************************************/ + +/**************************************************************************************************** + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ****************************************************************************************************/ + +extern void stm32_spiinitialize(void); + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c new file mode 100644 index 0000000000..cb8918306b --- /dev/null +++ b/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 px4fmu_pwm_servo.c + * + * Configuration data for the stm32 pwm_servo driver. + * + * Note that these arrays must always be fully-sized. + */ + +#include + +#include + +#include +#include + +#include +#include +#include + +__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = { + { + .base = STM32_TIM2_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM2EN, + .clock_freq = STM32_APB1_TIM2_CLKIN + } +}; + +__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = { + { + .gpio = GPIO_TIM2_CH1OUT, + .timer_index = 0, + .timer_channel = 1, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM2_CH2OUT, + .timer_index = 0, + .timer_channel = 2, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM2_CH3OUT, + .timer_index = 0, + .timer_channel = 3, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM2_CH4OUT, + .timer_index = 0, + .timer_channel = 4, + .default_value = 1000, + } +}; diff --git a/src/drivers/boards/px4fmu/px4fmu_spi.c b/src/drivers/boards/px4fmu/px4fmu_spi.c new file mode 100644 index 0000000000..b5d00eac0d --- /dev/null +++ b/src/drivers/boards/px4fmu/px4fmu_spi.c @@ -0,0 +1,154 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 px4fmu_spi.c + * + * Board-specific SPI functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include + +#include +#include + +#include "up_arch.h" +#include "chip.h" +#include "stm32_internal.h" +#include "px4fmu_internal.h" + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ************************************************************************************/ + +__EXPORT void weak_function stm32_spiinitialize(void) +{ + stm32_configgpio(GPIO_SPI_CS_GYRO); + stm32_configgpio(GPIO_SPI_CS_ACCEL); + stm32_configgpio(GPIO_SPI_CS_MPU); + stm32_configgpio(GPIO_SPI_CS_SDCARD); + + /* De-activate all peripherals, + * required for some peripheral + * state machines + */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + stm32_gpiowrite(GPIO_SPI_CS_SDCARD, 1); +} + +__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* SPI select is active low, so write !selected to select the device */ + + switch (devid) { + case PX4_SPIDEV_GYRO: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1); + break; + + case PX4_SPIDEV_ACCEL: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_ACCEL, !selected); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + break; + + case PX4_SPIDEV_MPU: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1); + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_MPU, !selected); + break; + + default: + break; + + } +} + +__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + return SPI_STATUS_PRESENT; +} + +__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* SPI select is active low, so write !selected to select the device */ + + switch (devid) { + break; + + default: + break; + + } +} + +__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + return SPI_STATUS_PRESENT; +} + + +__EXPORT void stm32_spi3select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* there can only be one device on this bus, so always select it */ + stm32_gpiowrite(GPIO_SPI_CS_SDCARD, !selected); +} + +__EXPORT uint8_t stm32_spi3status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + /* this is actually bogus, but PX4 has no way to sense the presence of an SD card */ + return SPI_STATUS_PRESENT; +} + diff --git a/src/drivers/boards/px4fmu/px4fmu_usb.c b/src/drivers/boards/px4fmu/px4fmu_usb.c new file mode 100644 index 0000000000..b0b669fbed --- /dev/null +++ b/src/drivers/boards/px4fmu/px4fmu_usb.c @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 px4fmu_usb.c + * + * Board-specific USB functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include + +#include "up_arch.h" +#include "stm32_internal.h" +#include "px4fmu_internal.h" + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: stm32_usbinitialize + * + * Description: + * Called to setup USB-related GPIO pins for the PX4FMU board. + * + ************************************************************************************/ + +__EXPORT void stm32_usbinitialize(void) +{ + /* The OTG FS has an internal soft pull-up */ + + /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ + +#ifdef CONFIG_STM32_OTGFS + stm32_configgpio(GPIO_OTGFS_VBUS); + /* XXX We only support device mode + stm32_configgpio(GPIO_OTGFS_PWRON); + stm32_configgpio(GPIO_OTGFS_OVER); + */ +#endif +} + +/************************************************************************************ + * Name: stm32_usbsuspend + * + * Description: + * Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is + * used. This function is called whenever the USB enters or leaves suspend mode. + * This is an opportunity for the board logic to shutdown clocks, power, etc. + * while the USB is suspended. + * + ************************************************************************************/ + +__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume) +{ + ulldbg("resume: %d\n", resume); +} + From 76497502cb696a1c6b9b01ed8ef5c3a5a740cb52 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 17 Apr 2013 11:20:31 -0700 Subject: [PATCH 070/486] Moved the L3GD20 driver to the new driver, working on FMU v1 and v2 --- apps/drivers/l3gd20/Makefile | 42 ------------------------- makefiles/config_px4fmuv2_default.mk | 2 +- nuttx/configs/px4fmuv2/nsh/appconfig | 2 -- {apps => src}/drivers/l3gd20/l3gd20.cpp | 0 src/drivers/l3gd20/module.mk | 6 ++++ 5 files changed, 7 insertions(+), 45 deletions(-) delete mode 100644 apps/drivers/l3gd20/Makefile rename {apps => src}/drivers/l3gd20/l3gd20.cpp (100%) create mode 100644 src/drivers/l3gd20/module.mk diff --git a/apps/drivers/l3gd20/Makefile b/apps/drivers/l3gd20/Makefile deleted file mode 100644 index 98e2d57c55..0000000000 --- a/apps/drivers/l3gd20/Makefile +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (C) 2012 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Makefile to build the L3GD20 driver. -# - -APPNAME = l3gd20 -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 - -include $(APPDIR)/mk/app.mk diff --git a/makefiles/config_px4fmuv2_default.mk b/makefiles/config_px4fmuv2_default.mk index d296c5379c..ee0c17bcb4 100644 --- a/makefiles/config_px4fmuv2_default.mk +++ b/makefiles/config_px4fmuv2_default.mk @@ -12,6 +12,7 @@ ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common # MODULES += drivers/boards/px4fmuv2 MODULES += drivers/lsm303d +MODULES += drivers/l3gd20 MODULES += drivers/px4fmu MODULES += drivers/rgbled @@ -43,7 +44,6 @@ BUILTIN_COMMANDS := \ $(call _B, hil, , 2048, hil_main ) \ $(call _B, hott_telemetry, , 2048, hott_telemetry_main ) \ $(call _B, kalman_demo, SCHED_PRIORITY_MAX-30, 2048, kalman_demo_main ) \ - $(call _B, l3gd20, , 2048, l3gd20_main ) \ $(call _B, math_demo, , 8192, math_demo_main ) \ $(call _B, mavlink, , 2048, mavlink_main ) \ $(call _B, mavlink_onboard, , 2048, mavlink_onboard_main ) \ diff --git a/nuttx/configs/px4fmuv2/nsh/appconfig b/nuttx/configs/px4fmuv2/nsh/appconfig index fff1406733..205b4448c7 100644 --- a/nuttx/configs/px4fmuv2/nsh/appconfig +++ b/nuttx/configs/px4fmuv2/nsh/appconfig @@ -115,8 +115,6 @@ CONFIGURED_APPS += drivers/boards/px4fmuv2 CONFIGURED_APPS += drivers/device # XXX needs conversion to SPI #CONFIGURED_APPS += drivers/ms5611 -CONFIGURED_APPS += drivers/l3gd20 -CONFIGURED_APPS += drivers/lsm303d # XXX needs conversion to serial #CONFIGURED_APPS += drivers/px4io CONFIGURED_APPS += drivers/stm32 diff --git a/apps/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp similarity index 100% rename from apps/drivers/l3gd20/l3gd20.cpp rename to src/drivers/l3gd20/l3gd20.cpp diff --git a/src/drivers/l3gd20/module.mk b/src/drivers/l3gd20/module.mk new file mode 100644 index 0000000000..23e77e8712 --- /dev/null +++ b/src/drivers/l3gd20/module.mk @@ -0,0 +1,6 @@ +# +# LSM303D accel/mag driver +# + +MODULE_COMMAND = l3gd20 +SRCS = l3gd20.cpp From b7e947cb3d53cbb0f9b194980a6a63588ba56bf2 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 17 Apr 2013 11:22:08 -0700 Subject: [PATCH 071/486] Anti-Aliasing frequency of the LSM303D can now be read too, not just written --- src/drivers/lsm303d/lsm303d.cpp | 52 ++++++++++++++++++++++++++++++--- 1 file changed, 48 insertions(+), 4 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 8a64ee7024..ba7316e557 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -321,7 +321,15 @@ private: * Zero selects the highest bandwidth * @return OK if the value can be supported, -ERANGE otherwise. */ - int set_antialias_filter_bandwidth(unsigned max_g); + int set_antialias_filter_bandwidth(unsigned bandwith); + + /** + * Get the LSM303D accel anti-alias filter. + * + * @param bandwidth The anti-alias filter bandwidth in Hz + * @return OK if the value was read and supported, ERROR otherwise. + */ + int get_antialias_filter_bandwidth(unsigned &bandwidth); /** * Set the LSM303D internal accel sampling frequency. @@ -477,10 +485,10 @@ LSM303D::init() /* XXX should we enable FIFO? */ set_range(8); /* XXX 16G mode seems wrong (shows 6 instead of 9.8m/s^2, therefore use 8G for now */ - set_antialias_filter_bandwidth(194); /* XXX: choose bandwidth: 50, 194, 362 or 773 Hz */ + set_antialias_filter_bandwidth(50); /* available bandwidths: 50, 194, 362 or 773 Hz */ set_samplerate(400); /* max sample rate */ - mag_set_range(12); /* XXX: take highest sensor range of 12GA? */ + mag_set_range(4); /* XXX: take highest sensor range of 12GA? */ mag_set_samplerate(100); /* XXX test this when another mag is used */ @@ -687,6 +695,17 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) /* XXX implement */ return -EINVAL; +// case ACCELIOCSLOWPASS: + case ACCELIOCGLOWPASS: + + unsigned bandwidth; + + if (OK == get_antialias_filter_bandwidth(bandwidth)) + return bandwidth; + else + return -EINVAL; + + default: /* give it to the superclass */ return SPI::ioctl(filp, cmd, arg); @@ -942,6 +961,25 @@ LSM303D::set_antialias_filter_bandwidth(unsigned bandwidth) return OK; } +int +LSM303D::get_antialias_filter_bandwidth(unsigned &bandwidth) +{ + uint8_t readbits = read_reg(ADDR_CTRL_REG2); + + if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_50HZ_A) + bandwidth = 50; + else if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_194HZ_A) + bandwidth = 194; + else if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_362HZ_A) + bandwidth = 362; + else if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_773HZ_A) + bandwidth = 773; + else + return ERROR; + + return OK; +} + int LSM303D::set_samplerate(unsigned frequency) { @@ -1305,6 +1343,7 @@ test() int fd_accel = -1; struct accel_report a_report; ssize_t sz; + int filter_bandwidth; /* get the driver */ fd_accel = open(ACCEL_DEVICE_PATH, O_RDONLY); @@ -1318,14 +1357,19 @@ test() if (sz != sizeof(a_report)) err(1, "immediate read failed"); + warnx("accel x: \t% 9.5f\tm/s^2", (double)a_report.x); warnx("accel y: \t% 9.5f\tm/s^2", (double)a_report.y); warnx("accel z: \t% 9.5f\tm/s^2", (double)a_report.z); warnx("accel x: \t%d\traw", (int)a_report.x_raw); warnx("accel y: \t%d\traw", (int)a_report.y_raw); warnx("accel z: \t%d\traw", (int)a_report.z_raw); - warnx("accel range: %8.4f m/s^2", (double)a_report.range_m_s2); + warnx("accel range: %8.4f m/s^2", (double)a_report.range_m_s2); + if (ERROR == (filter_bandwidth = ioctl(fd_accel, ACCELIOCGLOWPASS, 0))) + warnx("accel antialias filter bandwidth: fail"); + else + warnx("accel antialias filter bandwidth: %d Hz", filter_bandwidth); int fd_mag = -1; struct mag_report m_report; From 5f2601836524055c3eb046535d53a38b0749ca52 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 17 Apr 2013 22:25:50 -0700 Subject: [PATCH 072/486] Improve the implementation of CONFIG_FILE handling in firmware.mk --- makefiles/firmware.mk | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index a2227b5c45..f6057d2fc8 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -66,6 +66,12 @@ # CONFIG: # Used to set the output filename; defaults to 'firmware'. # +# CONFIG_FILE: +# If set, overrides the configuration file search logic. Sets +# CONFIG to the name of the configuration file, strips any +# leading config_ prefix and any suffix. e.g. config_board_foo.mk +# results in CONFIG being set to 'board_foo'. +# # WORK_DIR: # Sets the directory in which the firmware will be built. Defaults # to the directory 'build' under the directory containing the @@ -115,13 +121,14 @@ include $(MK_DIR)/setup.mk # # Locate the configuration file # +ifneq ($(CONFIG_FILE),) +CONFIG := $(subst config_,,$(basename $(notdir $(CONFIG_FILE)))) +else +CONFIG_FILE := $(wildcard $(PX4_MK_DIR)/config_$(CONFIG).mk) +endif ifeq ($(CONFIG),) $(error Missing configuration name or file (specify with CONFIG=)) endif -CONFIG_FILE := $(firstword $(wildcard $(CONFIG)) $(wildcard $(PX4_MK_DIR)/config_$(CONFIG).mk)) -ifeq ($(CONFIG_FILE),) -$(error Can't find a config file called $(CONFIG) or $(PX4_MK_DIR)/config_$(CONFIG).mk) -endif export CONFIG include $(CONFIG_FILE) $(info % CONFIG = $(CONFIG)) From 94084ec22abd3c08cdd06783483e827ed8b7fd66 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 18 Apr 2013 22:27:55 +0200 Subject: [PATCH 073/486] Enable support for RAMTRON, enable support for EEPROM on FMU 1.x --- makefiles/config_px4fmu_default.mk | 3 +- makefiles/config_px4fmuv2_default.mk | 2 + nuttx/configs/px4fmu/nsh/appconfig | 1 - nuttx/configs/px4fmuv2/nsh/appconfig | 6 +- nuttx/configs/px4fmuv2/nsh/defconfig | 4 + nuttx/drivers/mtd/ramtron.c | 8 + src/drivers/boards/px4fmuv2/px4fmu_init.c | 17 +- src/systemcmds/eeprom/24xxxx_mtd.c | 571 ++++++++++++++++++++++ src/systemcmds/eeprom/eeprom.c | 265 ++++++++++ src/systemcmds/eeprom/module.mk | 6 + src/systemcmds/ramtron/module.mk | 6 + src/systemcmds/ramtron/ramtron.c | 268 ++++++++++ 12 files changed, 1142 insertions(+), 15 deletions(-) create mode 100644 src/systemcmds/eeprom/24xxxx_mtd.c create mode 100644 src/systemcmds/eeprom/eeprom.c create mode 100644 src/systemcmds/eeprom/module.mk create mode 100644 src/systemcmds/ramtron/module.mk create mode 100644 src/systemcmds/ramtron/ramtron.c diff --git a/makefiles/config_px4fmu_default.mk b/makefiles/config_px4fmu_default.mk index 1a6c91b269..291711820b 100644 --- a/makefiles/config_px4fmu_default.mk +++ b/makefiles/config_px4fmu_default.mk @@ -11,6 +11,7 @@ ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common # Board support modules # MODULES += drivers/px4fmu +MODULES += systemcmds/eeprom # # Transitional support - add commands from the NuttX export archive. @@ -36,7 +37,6 @@ BUILTIN_COMMANDS := \ $(call _B, commander, SCHED_PRIORITY_MAX-30, 2048, commander_main ) \ $(call _B, control_demo, , 2048, control_demo_main ) \ $(call _B, delay_test, , 2048, delay_test_main ) \ - $(call _B, eeprom, , 4096, eeprom_main ) \ $(call _B, fixedwing_att_control, SCHED_PRIORITY_MAX-30, 2048, fixedwing_att_control_main ) \ $(call _B, fixedwing_pos_control, SCHED_PRIORITY_MAX-30, 2048, fixedwing_pos_control_main ) \ $(call _B, gps, , 2048, gps_main ) \ @@ -66,4 +66,5 @@ BUILTIN_COMMANDS := \ $(call _B, tests, , 12000, tests_main ) \ $(call _B, tone_alarm, , 2048, tone_alarm_main ) \ $(call _B, top, SCHED_PRIORITY_DEFAULT-10, 3000, top_main ) \ + $(call _B, param, SCHED_PRIORITY_DEFAULT-10, 2048, param_main ) \ $(call _B, uorb, , 4096, uorb_main ) diff --git a/makefiles/config_px4fmuv2_default.mk b/makefiles/config_px4fmuv2_default.mk index d296c5379c..6d583bd5f1 100644 --- a/makefiles/config_px4fmuv2_default.mk +++ b/makefiles/config_px4fmuv2_default.mk @@ -14,6 +14,7 @@ MODULES += drivers/boards/px4fmuv2 MODULES += drivers/lsm303d MODULES += drivers/px4fmu MODULES += drivers/rgbled +MODULES += systemcmds/ramtron # # Transitional support - add commands from the NuttX export archive. @@ -61,4 +62,5 @@ BUILTIN_COMMANDS := \ $(call _B, tests, , 12000, tests_main ) \ $(call _B, tone_alarm, , 2048, tone_alarm_main ) \ $(call _B, top, SCHED_PRIORITY_DEFAULT-10, 3000, top_main ) \ + $(call _B, param, SCHED_PRIORITY_DEFAULT-10, 2048, param_main ) \ $(call _B, uorb, , 4096, uorb_main ) diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index d642b46923..9092e9541f 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -65,7 +65,6 @@ CONFIGURED_APPS += systemcmds/perf CONFIGURED_APPS += systemcmds/top CONFIGURED_APPS += systemcmds/boardinfo CONFIGURED_APPS += systemcmds/mixer -CONFIGURED_APPS += systemcmds/eeprom CONFIGURED_APPS += systemcmds/param CONFIGURED_APPS += systemcmds/pwm CONFIGURED_APPS += systemcmds/bl_update diff --git a/nuttx/configs/px4fmuv2/nsh/appconfig b/nuttx/configs/px4fmuv2/nsh/appconfig index fff1406733..9e3f50a13d 100644 --- a/nuttx/configs/px4fmuv2/nsh/appconfig +++ b/nuttx/configs/px4fmuv2/nsh/appconfig @@ -65,9 +65,7 @@ CONFIGURED_APPS += systemcmds/perf CONFIGURED_APPS += systemcmds/top CONFIGURED_APPS += systemcmds/boardinfo CONFIGURED_APPS += systemcmds/mixer -# No I2C EEPROM - need new param interface -#CONFIGURED_APPS += systemcmds/eeprom -#CONFIGURED_APPS += systemcmds/param +CONFIGURED_APPS += systemcmds/param CONFIGURED_APPS += systemcmds/pwm CONFIGURED_APPS += systemcmds/bl_update CONFIGURED_APPS += systemcmds/preflight_check @@ -95,10 +93,8 @@ CONFIGURED_APPS += sdlog CONFIGURED_APPS += sensors ifneq ($(CONFIG_APM),y) -#CONFIGURED_APPS += ardrone_interface CONFIGURED_APPS += multirotor_att_control CONFIGURED_APPS += multirotor_pos_control -#CONFIGURED_APPS += fixedwing_control CONFIGURED_APPS += fixedwing_att_control CONFIGURED_APPS += fixedwing_pos_control CONFIGURED_APPS += position_estimator diff --git a/nuttx/configs/px4fmuv2/nsh/defconfig b/nuttx/configs/px4fmuv2/nsh/defconfig index beeb475517..d103095800 100755 --- a/nuttx/configs/px4fmuv2/nsh/defconfig +++ b/nuttx/configs/px4fmuv2/nsh/defconfig @@ -365,6 +365,10 @@ CONFIG_I2C_RESET=y # XXX fixed per-transaction timeout CONFIG_STM32_I2CTIMEOMS=10 +# +# MTD support +# +CONFIG_MTD=y # XXX re-enable after integration testing diff --git a/nuttx/drivers/mtd/ramtron.c b/nuttx/drivers/mtd/ramtron.c index 34273bccfc..45aff59cc5 100644 --- a/nuttx/drivers/mtd/ramtron.c +++ b/nuttx/drivers/mtd/ramtron.c @@ -161,6 +161,14 @@ struct ramtron_dev_s static struct ramtron_parts_s ramtron_parts[] = { + { + "FM25V01", /* name */ + 0x21, /* id1 */ + 0x00, /* id2 */ + 16L*1024L, /* size */ + 2, /* addr_len */ + 40000000 /* speed */ + }, { "FM25V02", /* name */ 0x22, /* id1 */ diff --git a/src/drivers/boards/px4fmuv2/px4fmu_init.c b/src/drivers/boards/px4fmuv2/px4fmu_init.c index 1d99f15bfc..2fd3a2c1b8 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu_init.c +++ b/src/drivers/boards/px4fmuv2/px4fmu_init.c @@ -192,12 +192,12 @@ __EXPORT int nsh_archinitialize(void) spi1 = up_spiinitialize(1); if (!spi1) { - message("[boot] FAILED to initialize SPI port 1\r\n"); + message("[boot] FAILED to initialize SPI port 1\n"); up_ledon(LED_AMBER); return -ENODEV; } - // Default SPI1 to 1MHz and de-assert the known chip selects. + /* Default SPI1 to 1MHz and de-assert the known chip selects. */ SPI_SETFREQUENCY(spi1, 10000000); SPI_SETBITS(spi1, 8); SPI_SETMODE(spi1, SPIDEV_MODE3); @@ -206,11 +206,10 @@ __EXPORT int nsh_archinitialize(void) SPI_SELECT(spi1, PX4_SPIDEV_BARO, false); up_udelay(20); - message("[boot] Successfully initialized SPI port 1\r\n"); + message("[boot] Successfully initialized SPI port 1\n"); /* Get the SPI port for the FRAM */ - message("[boot] Initializing SPI port 2\n"); spi2 = up_spiinitialize(2); if (!spi2) { @@ -219,11 +218,13 @@ __EXPORT int nsh_archinitialize(void) return -ENODEV; } + /* Default SPI2 to 37.5 MHz (F4 max) and de-assert the known chip selects. */ + SPI_SETFREQUENCY(spi2, 375000000); + SPI_SETBITS(spi2, 8); + SPI_SETMODE(spi2, SPIDEV_MODE3); + SPI_SELECT(spi2, SPIDEV_FLASH, false); + message("[boot] Successfully initialized SPI port 2\n"); - /* XXX need a driver to bind the FRAM to */ - - //message("[boot] Successfully bound SPI port 2 to the FRAM driver\n"); - return OK; } diff --git a/src/systemcmds/eeprom/24xxxx_mtd.c b/src/systemcmds/eeprom/24xxxx_mtd.c new file mode 100644 index 0000000000..e34be44e31 --- /dev/null +++ b/src/systemcmds/eeprom/24xxxx_mtd.c @@ -0,0 +1,571 @@ +/************************************************************************************ + * Driver for 24xxxx-style I2C EEPROMs. + * + * Adapted from: + * + * drivers/mtd/at24xx.c + * Driver for I2C-based at24cxx EEPROM(at24c32,at24c64,at24c128,at24c256) + * + * Copyright (C) 2011 Li Zhuoyi. All rights reserved. + * Author: Li Zhuoyi + * History: 0.1 2011-08-20 initial version + * + * 2011-11-1 Added support for larger MTD block sizes: Hal Glenn + * + * Derived from drivers/mtd/m25px.c + * + * Copyright (C) 2009-2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ************************************************************************************/ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "systemlib/perf_counter.h" + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ + +/* + * Configuration is as for the AT24 driver, but the CONFIG_MTD_AT24XX define should be + * omitted in order to avoid building the AT24XX driver as well. + */ + +/* As a minimum, the size of the AT24 part and its 7-bit I2C address are required. */ + +#ifndef CONFIG_AT24XX_SIZE +# warning "Assuming AT24 size 64" +# define CONFIG_AT24XX_SIZE 64 +#endif +#ifndef CONFIG_AT24XX_ADDR +# warning "Assuming AT24 address of 0x50" +# define CONFIG_AT24XX_ADDR 0x50 +#endif + +/* Get the part configuration based on the size configuration */ + +#if CONFIG_AT24XX_SIZE == 32 +# define AT24XX_NPAGES 128 +# define AT24XX_PAGESIZE 32 +#elif CONFIG_AT24XX_SIZE == 48 +# define AT24XX_NPAGES 192 +# define AT24XX_PAGESIZE 32 +#elif CONFIG_AT24XX_SIZE == 64 +# define AT24XX_NPAGES 256 +# define AT24XX_PAGESIZE 32 +#elif CONFIG_AT24XX_SIZE == 128 +# define AT24XX_NPAGES 256 +# define AT24XX_PAGESIZE 64 +#elif CONFIG_AT24XX_SIZE == 256 +# define AT24XX_NPAGES 512 +# define AT24XX_PAGESIZE 64 +#endif + +/* For applications where a file system is used on the AT24, the tiny page sizes + * will result in very inefficient FLASH usage. In such cases, it is better if + * blocks are comprised of "clusters" of pages so that the file system block + * size is, say, 256 or 512 bytes. In any event, the block size *must* be an + * even multiple of the pages. + */ + +#ifndef CONFIG_AT24XX_MTD_BLOCKSIZE +# warning "Assuming driver block size is the same as the FLASH page size" +# define CONFIG_AT24XX_MTD_BLOCKSIZE AT24XX_PAGESIZE +#endif + +/* The AT24 does not respond on the bus during write cycles, so we depend on a long + * timeout to detect problems. The max program time is typically ~5ms. + */ +#ifndef CONFIG_AT24XX_WRITE_TIMEOUT_MS +# define CONFIG_AT24XX_WRITE_TIMEOUT_MS 20 +#endif + +/************************************************************************************ + * Private Types + ************************************************************************************/ + +/* This type represents the state of the MTD device. The struct mtd_dev_s + * must appear at the beginning of the definition so that you can freely + * cast between pointers to struct mtd_dev_s and struct at24c_dev_s. + */ + +struct at24c_dev_s { + struct mtd_dev_s mtd; /* MTD interface */ + FAR struct i2c_dev_s *dev; /* Saved I2C interface instance */ + uint8_t addr; /* I2C address */ + uint16_t pagesize; /* 32, 63 */ + uint16_t npages; /* 128, 256, 512, 1024 */ + + perf_counter_t perf_reads; + perf_counter_t perf_writes; + perf_counter_t perf_resets; + perf_counter_t perf_read_retries; + perf_counter_t perf_read_errors; + perf_counter_t perf_write_errors; +}; + +/************************************************************************************ + * Private Function Prototypes + ************************************************************************************/ + +/* MTD driver methods */ + +static int at24c_erase(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks); +static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock, + size_t nblocks, FAR uint8_t *buf); +static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, + size_t nblocks, FAR const uint8_t *buf); +static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg); + +void at24c_test(void); + +/************************************************************************************ + * Private Data + ************************************************************************************/ + +/* At present, only a single AT24 part is supported. In this case, a statically + * allocated state structure may be used. + */ + +static struct at24c_dev_s g_at24c; + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +static int at24c_eraseall(FAR struct at24c_dev_s *priv) +{ + int startblock = 0; + uint8_t buf[AT24XX_PAGESIZE + 2]; + + struct i2c_msg_s msgv[1] = { + { + .addr = priv->addr, + .flags = 0, + .buffer = &buf[0], + .length = sizeof(buf), + } + }; + + memset(&buf[2], 0xff, priv->pagesize); + + for (startblock = 0; startblock < priv->npages; startblock++) { + uint16_t offset = startblock * priv->pagesize; + buf[1] = offset & 0xff; + buf[0] = (offset >> 8) & 0xff; + + while (I2C_TRANSFER(priv->dev, &msgv[0], 1) < 0) { + fvdbg("erase stall\n"); + usleep(10000); + } + } + + return OK; +} + +/************************************************************************************ + * Name: at24c_erase + ************************************************************************************/ + +static int at24c_erase(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks) +{ + /* EEprom need not erase */ + + return (int)nblocks; +} + +/************************************************************************************ + * Name: at24c_test + ************************************************************************************/ + +void at24c_test(void) +{ + uint8_t buf[CONFIG_AT24XX_MTD_BLOCKSIZE]; + unsigned count = 0; + unsigned errors = 0; + + for (count = 0; count < 10000; count++) { + ssize_t result = at24c_bread(&g_at24c.mtd, 0, 1, buf); + if (result == ERROR) { + if (errors++ > 2) { + vdbg("too many errors\n"); + return; + } + } else if (result != 1) { + vdbg("unexpected %u\n", result); + } + if ((count % 100) == 0) + vdbg("test %u errors %u\n", count, errors); + } +} + +/************************************************************************************ + * Name: at24c_bread + ************************************************************************************/ + +static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock, + size_t nblocks, FAR uint8_t *buffer) +{ + FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; + size_t blocksleft; + uint8_t addr[2]; + int ret; + + struct i2c_msg_s msgv[2] = { + { + .addr = priv->addr, + .flags = 0, + .buffer = &addr[0], + .length = sizeof(addr), + }, + { + .addr = priv->addr, + .flags = I2C_M_READ, + .buffer = 0, + .length = priv->pagesize, + } + }; + +#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE + startblock *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); + nblocks *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); +#endif + blocksleft = nblocks; + + fvdbg("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks); + + if (startblock >= priv->npages) { + return 0; + } + + if (startblock + nblocks > priv->npages) { + nblocks = priv->npages - startblock; + } + + while (blocksleft-- > 0) { + uint16_t offset = startblock * priv->pagesize; + unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS; + + addr[1] = offset & 0xff; + addr[0] = (offset >> 8) & 0xff; + msgv[1].buffer = buffer; + + for (;;) { + + perf_begin(priv->perf_reads); + ret = I2C_TRANSFER(priv->dev, &msgv[0], 2); + perf_end(priv->perf_reads); + + if (ret >= 0) + break; + + fvdbg("read stall"); + usleep(1000); + + /* We should normally only be here on the first read after + * a write. + * + * XXX maybe do special first-read handling with optional + * bus reset as well? + */ + perf_count(priv->perf_read_retries); + + if (--tries == 0) { + perf_count(priv->perf_read_errors); + return ERROR; + } + } + + startblock++; + buffer += priv->pagesize; + } + +#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE + return nblocks / (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); +#else + return nblocks; +#endif +} + +/************************************************************************************ + * Name: at24c_bwrite + * + * Operates on MTD block's and translates to FLASH pages + * + ************************************************************************************/ + +static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks, + FAR const uint8_t *buffer) +{ + FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; + size_t blocksleft; + uint8_t buf[AT24XX_PAGESIZE + 2]; + int ret; + + struct i2c_msg_s msgv[1] = { + { + .addr = priv->addr, + .flags = 0, + .buffer = &buf[0], + .length = sizeof(buf), + } + }; + +#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE + startblock *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); + nblocks *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); +#endif + blocksleft = nblocks; + + if (startblock >= priv->npages) { + return 0; + } + + if (startblock + nblocks > priv->npages) { + nblocks = priv->npages - startblock; + } + + fvdbg("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks); + + while (blocksleft-- > 0) { + uint16_t offset = startblock * priv->pagesize; + unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS; + + buf[1] = offset & 0xff; + buf[0] = (offset >> 8) & 0xff; + memcpy(&buf[2], buffer, priv->pagesize); + + for (;;) { + + perf_begin(priv->perf_writes); + ret = I2C_TRANSFER(priv->dev, &msgv[0], 1); + perf_end(priv->perf_writes); + + if (ret >= 0) + break; + + fvdbg("write stall"); + usleep(1000); + + /* We expect to see a number of retries per write cycle as we + * poll for write completion. + */ + if (--tries == 0) { + perf_count(priv->perf_write_errors); + return ERROR; + } + } + + startblock++; + buffer += priv->pagesize; + } + +#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE + return nblocks / (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); +#else + return nblocks; +#endif +} + +/************************************************************************************ + * Name: at24c_ioctl + ************************************************************************************/ + +static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg) +{ + FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; + int ret = -EINVAL; /* Assume good command with bad parameters */ + + fvdbg("cmd: %d \n", cmd); + + switch (cmd) { + case MTDIOC_GEOMETRY: { + FAR struct mtd_geometry_s *geo = (FAR struct mtd_geometry_s *)((uintptr_t)arg); + + if (geo) { + /* Populate the geometry structure with information need to know + * the capacity and how to access the device. + * + * NOTE: that the device is treated as though it where just an array + * of fixed size blocks. That is most likely not true, but the client + * will expect the device logic to do whatever is necessary to make it + * appear so. + * + * blocksize: + * May be user defined. The block size for the at24XX devices may be + * larger than the page size in order to better support file systems. + * The read and write functions translate BLOCKS to pages for the + * small flash devices + * erasesize: + * It has to be at least as big as the blocksize, bigger serves no + * purpose. + * neraseblocks + * Note that the device size is in kilobits and must be scaled by + * 1024 / 8 + */ + +#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE + geo->blocksize = CONFIG_AT24XX_MTD_BLOCKSIZE; + geo->erasesize = CONFIG_AT24XX_MTD_BLOCKSIZE; + geo->neraseblocks = (CONFIG_AT24XX_SIZE * 1024 / 8) / CONFIG_AT24XX_MTD_BLOCKSIZE; +#else + geo->blocksize = priv->pagesize; + geo->erasesize = priv->pagesize; + geo->neraseblocks = priv->npages; +#endif + ret = OK; + + fvdbg("blocksize: %d erasesize: %d neraseblocks: %d\n", + geo->blocksize, geo->erasesize, geo->neraseblocks); + } + } + break; + + case MTDIOC_BULKERASE: + ret = at24c_eraseall(priv); + break; + + case MTDIOC_XIPBASE: + default: + ret = -ENOTTY; /* Bad command */ + break; + } + + return ret; +} + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: at24c_initialize + * + * Description: + * Create an initialize MTD device instance. MTD devices are not registered + * in the file system, but are created as instances that can be bound to + * other functions (such as a block or character driver front end). + * + ************************************************************************************/ + +FAR struct mtd_dev_s *at24c_initialize(FAR struct i2c_dev_s *dev) { + FAR struct at24c_dev_s *priv; + + fvdbg("dev: %p\n", dev); + + /* Allocate a state structure (we allocate the structure instead of using + * a fixed, static allocation so that we can handle multiple FLASH devices. + * The current implementation would handle only one FLASH part per I2C + * device (only because of the SPIDEV_FLASH definition) and so would have + * to be extended to handle multiple FLASH parts on the same I2C bus. + */ + + priv = &g_at24c; + + if (priv) { + /* Initialize the allocated structure */ + + priv->addr = CONFIG_AT24XX_ADDR; + priv->pagesize = AT24XX_PAGESIZE; + priv->npages = AT24XX_NPAGES; + + priv->mtd.erase = at24c_erase; + priv->mtd.bread = at24c_bread; + priv->mtd.bwrite = at24c_bwrite; + priv->mtd.ioctl = at24c_ioctl; + priv->dev = dev; + + priv->perf_reads = perf_alloc(PC_ELAPSED, "EEPROM read"); + priv->perf_writes = perf_alloc(PC_ELAPSED, "EEPROM write"); + priv->perf_resets = perf_alloc(PC_COUNT, "EEPROM reset"); + priv->perf_read_retries = perf_alloc(PC_COUNT, "EEPROM read retries"); + priv->perf_read_errors = perf_alloc(PC_COUNT, "EEPROM read errors"); + priv->perf_write_errors = perf_alloc(PC_COUNT, "EEPROM write errors"); + } + + /* attempt to read to validate device is present */ + unsigned char buf[5]; + uint8_t addrbuf[2] = {0, 0}; + + struct i2c_msg_s msgv[2] = { + { + .addr = priv->addr, + .flags = 0, + .buffer = &addrbuf[0], + .length = sizeof(addrbuf), + }, + { + .addr = priv->addr, + .flags = I2C_M_READ, + .buffer = &buf[0], + .length = sizeof(buf), + } + }; + + perf_begin(priv->perf_reads); + int ret = I2C_TRANSFER(priv->dev, &msgv[0], 2); + perf_end(priv->perf_reads); + + if (ret < 0) { + return NULL; + } + + /* Return the implementation-specific state structure as the MTD device */ + + fvdbg("Return %p\n", priv); + return (FAR struct mtd_dev_s *)priv; +} + +/* + * XXX: debug hackery + */ +int at24c_nuke(void) +{ + return at24c_eraseall(&g_at24c); +} diff --git a/src/systemcmds/eeprom/eeprom.c b/src/systemcmds/eeprom/eeprom.c new file mode 100644 index 0000000000..49da513580 --- /dev/null +++ b/src/systemcmds/eeprom/eeprom.c @@ -0,0 +1,265 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * + * 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 eeprom.c + * + * EEPROM service and utility app. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include "systemlib/systemlib.h" +#include "systemlib/param/param.h" +#include "systemlib/err.h" + +#ifndef PX4_I2C_BUS_ONBOARD +# error PX4_I2C_BUS_ONBOARD not defined, cannot locate onboard EEPROM +#endif + +__EXPORT int eeprom_main(int argc, char *argv[]); + +static void eeprom_attach(void); +static void eeprom_start(void); +static void eeprom_erase(void); +static void eeprom_ioctl(unsigned operation); +static void eeprom_save(const char *name); +static void eeprom_load(const char *name); +static void eeprom_test(void); + +static bool attached = false; +static bool started = false; +static struct mtd_dev_s *eeprom_mtd; + +int eeprom_main(int argc, char *argv[]) +{ + if (argc >= 2) { + if (!strcmp(argv[1], "start")) + eeprom_start(); + + if (!strcmp(argv[1], "save_param")) + eeprom_save(argv[2]); + + if (!strcmp(argv[1], "load_param")) + eeprom_load(argv[2]); + + if (!strcmp(argv[1], "erase")) + eeprom_erase(); + + if (!strcmp(argv[1], "test")) + eeprom_test(); + + if (0) { /* these actually require a file on the filesystem... */ + + if (!strcmp(argv[1], "reformat")) + eeprom_ioctl(FIOC_REFORMAT); + + if (!strcmp(argv[1], "repack")) + eeprom_ioctl(FIOC_OPTIMIZE); + } + } + + errx(1, "expected a command, try 'start'\n\t'save_param /eeprom/parameters'\n\t'load_param /eeprom/parameters'\n\t'erase'\n"); +} + + +static void +eeprom_attach(void) +{ + /* find the right I2C */ + struct i2c_dev_s *i2c = up_i2cinitialize(PX4_I2C_BUS_ONBOARD); + /* this resets the I2C bus, set correct bus speed again */ + I2C_SETFREQUENCY(i2c, 400000); + + if (i2c == NULL) + errx(1, "failed to locate I2C bus"); + + /* start the MTD driver, attempt 5 times */ + for (int i = 0; i < 5; i++) { + eeprom_mtd = at24c_initialize(i2c); + if (eeprom_mtd) { + /* abort on first valid result */ + if (i > 0) { + warnx("warning: EEPROM needed %d attempts to attach", i+1); + } + break; + } + } + + /* if last attempt is still unsuccessful, abort */ + if (eeprom_mtd == NULL) + errx(1, "failed to initialize EEPROM driver"); + + attached = true; +} + +static void +eeprom_start(void) +{ + int ret; + + if (started) + errx(1, "EEPROM already mounted"); + + if (!attached) + eeprom_attach(); + + /* start NXFFS */ + ret = nxffs_initialize(eeprom_mtd); + + if (ret < 0) + errx(1, "failed to initialize NXFFS - erase EEPROM to reformat"); + + /* mount the EEPROM */ + ret = mount(NULL, "/eeprom", "nxffs", 0, NULL); + + if (ret < 0) + errx(1, "failed to mount /eeprom - erase EEPROM to reformat"); + + started = true; + warnx("mounted EEPROM at /eeprom"); + exit(0); +} + +extern int at24c_nuke(void); + +static void +eeprom_erase(void) +{ + if (!attached) + eeprom_attach(); + + if (at24c_nuke()) + errx(1, "erase failed"); + + errx(0, "erase done, reboot now"); +} + +static void +eeprom_ioctl(unsigned operation) +{ + int fd; + + fd = open("/eeprom/.", 0); + + if (fd < 0) + err(1, "open /eeprom"); + + if (ioctl(fd, operation, 0) < 0) + err(1, "ioctl"); + + exit(0); +} + +static void +eeprom_save(const char *name) +{ + if (!started) + errx(1, "must be started first"); + + if (!name) + err(1, "missing argument for device name, try '/eeprom/parameters'"); + + warnx("WARNING: 'eeprom save_param' deprecated - use 'param save' instead"); + + /* delete the file in case it exists */ + unlink(name); + + /* create the file */ + int fd = open(name, O_WRONLY | O_CREAT | O_EXCL); + + if (fd < 0) + err(1, "opening '%s' failed", name); + + int result = param_export(fd, false); + close(fd); + + if (result < 0) { + unlink(name); + errx(1, "error exporting to '%s'", name); + } + + exit(0); +} + +static void +eeprom_load(const char *name) +{ + if (!started) + errx(1, "must be started first"); + + if (!name) + err(1, "missing argument for device name, try '/eeprom/parameters'"); + + warnx("WARNING: 'eeprom load_param' deprecated - use 'param load' instead"); + + int fd = open(name, O_RDONLY); + + if (fd < 0) + err(1, "open '%s'", name); + + int result = param_load(fd); + close(fd); + + if (result < 0) + errx(1, "error importing from '%s'", name); + + exit(0); +} + +extern void at24c_test(void); + +static void +eeprom_test(void) +{ + at24c_test(); + exit(0); +} diff --git a/src/systemcmds/eeprom/module.mk b/src/systemcmds/eeprom/module.mk new file mode 100644 index 0000000000..3b4fc04791 --- /dev/null +++ b/src/systemcmds/eeprom/module.mk @@ -0,0 +1,6 @@ +# +# EEPROM file system driver +# + +MODULE_COMMAND = eeprom +SRCS = 24xxxx_mtd.c eeprom.c diff --git a/src/systemcmds/ramtron/module.mk b/src/systemcmds/ramtron/module.mk new file mode 100644 index 0000000000..e4eb1d143c --- /dev/null +++ b/src/systemcmds/ramtron/module.mk @@ -0,0 +1,6 @@ +# +# RAMTRON file system driver +# + +MODULE_COMMAND = ramtron +SRCS = ramtron.c diff --git a/src/systemcmds/ramtron/ramtron.c b/src/systemcmds/ramtron/ramtron.c new file mode 100644 index 0000000000..5e9499c553 --- /dev/null +++ b/src/systemcmds/ramtron/ramtron.c @@ -0,0 +1,268 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * + * 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 ramtron.c + * + * ramtron service and utility app. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include "systemlib/systemlib.h" +#include "systemlib/param/param.h" +#include "systemlib/err.h" + +__EXPORT int ramtron_main(int argc, char *argv[]); + +static void ramtron_attach(void); +static void ramtron_start(void); +static void ramtron_erase(void); +static void ramtron_ioctl(unsigned operation); +static void ramtron_save(const char *name); +static void ramtron_load(const char *name); +static void ramtron_test(void); + +static bool attached = false; +static bool started = false; +static struct mtd_dev_s *ramtron_mtd; + +int ramtron_main(int argc, char *argv[]) +{ + if (argc >= 2) { + if (!strcmp(argv[1], "start")) + ramtron_start(); + + if (!strcmp(argv[1], "save_param")) + ramtron_save(argv[2]); + + if (!strcmp(argv[1], "load_param")) + ramtron_load(argv[2]); + + if (!strcmp(argv[1], "erase")) + ramtron_erase(); + + if (!strcmp(argv[1], "test")) + ramtron_test(); + + if (0) { /* these actually require a file on the filesystem... */ + + if (!strcmp(argv[1], "reformat")) + ramtron_ioctl(FIOC_REFORMAT); + + if (!strcmp(argv[1], "repack")) + ramtron_ioctl(FIOC_OPTIMIZE); + } + } + + errx(1, "expected a command, try 'start'\n\t'save_param /ramtron/parameters'\n\t'load_param /ramtron/parameters'\n\t'erase'\n"); +} + +struct mtd_dev_s *ramtron_initialize(FAR struct spi_dev_s *dev); + + +static void +ramtron_attach(void) +{ + /* find the right spi */ + struct spi_dev_s *spi = up_spiinitialize(2); + /* this resets the spi bus, set correct bus speed again */ + // xxx set in ramtron driver, leave this out +// SPI_SETFREQUENCY(spi, 4000000); + SPI_SETFREQUENCY(spi, 375000000); + SPI_SETBITS(spi, 8); + SPI_SETMODE(spi, SPIDEV_MODE3); + SPI_SELECT(spi, SPIDEV_FLASH, false); + + if (spi == NULL) + errx(1, "failed to locate spi bus"); + + /* start the MTD driver, attempt 5 times */ + for (int i = 0; i < 5; i++) { + ramtron_mtd = ramtron_initialize(spi); + if (ramtron_mtd) { + /* abort on first valid result */ + if (i > 0) { + warnx("warning: ramtron needed %d attempts to attach", i+1); + } + break; + } + } + + /* if last attempt is still unsuccessful, abort */ + if (ramtron_mtd == NULL) + errx(1, "failed to initialize ramtron driver"); + + attached = true; +} + +static void +ramtron_start(void) +{ + int ret; + + if (started) + errx(1, "ramtron already mounted"); + + if (!attached) + ramtron_attach(); + + /* start NXFFS */ + ret = nxffs_initialize(ramtron_mtd); + + if (ret < 0) + errx(1, "failed to initialize NXFFS - erase ramtron to reformat"); + + /* mount the ramtron */ + ret = mount(NULL, "/ramtron", "nxffs", 0, NULL); + + if (ret < 0) + errx(1, "failed to mount /ramtron - erase ramtron to reformat"); + + started = true; + warnx("mounted ramtron at /ramtron"); + exit(0); +} + +//extern int at24c_nuke(void); + +static void +ramtron_erase(void) +{ + if (!attached) + ramtron_attach(); + +// if (at24c_nuke()) + errx(1, "erase failed"); + + errx(0, "erase done, reboot now"); +} + +static void +ramtron_ioctl(unsigned operation) +{ + int fd; + + fd = open("/ramtron/.", 0); + + if (fd < 0) + err(1, "open /ramtron"); + + if (ioctl(fd, operation, 0) < 0) + err(1, "ioctl"); + + exit(0); +} + +static void +ramtron_save(const char *name) +{ + if (!started) + errx(1, "must be started first"); + + if (!name) + err(1, "missing argument for device name, try '/ramtron/parameters'"); + + warnx("WARNING: 'ramtron save_param' deprecated - use 'param save' instead"); + + /* delete the file in case it exists */ + unlink(name); + + /* create the file */ + int fd = open(name, O_WRONLY | O_CREAT | O_EXCL); + + if (fd < 0) + err(1, "opening '%s' failed", name); + + int result = param_export(fd, false); + close(fd); + + if (result < 0) { + unlink(name); + errx(1, "error exporting to '%s'", name); + } + + exit(0); +} + +static void +ramtron_load(const char *name) +{ + if (!started) + errx(1, "must be started first"); + + if (!name) + err(1, "missing argument for device name, try '/ramtron/parameters'"); + + warnx("WARNING: 'ramtron load_param' deprecated - use 'param load' instead"); + + int fd = open(name, O_RDONLY); + + if (fd < 0) + err(1, "open '%s'", name); + + int result = param_load(fd); + close(fd); + + if (result < 0) + errx(1, "error importing from '%s'", name); + + exit(0); +} + +//extern void at24c_test(void); + +static void +ramtron_test(void) +{ +// at24c_test(); + exit(0); +} From b149b834c835190fbb3f7e1914346d5e0620036d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 18 Apr 2013 22:56:25 +0200 Subject: [PATCH 074/486] Initial attempt at getting SDIO to work --- nuttx/configs/px4fmuv2/include/board.h | 11 +++++++ nuttx/configs/px4fmuv2/nsh/defconfig | 35 ++++++++++++++++++++--- src/drivers/boards/px4fmuv2/px4fmu_init.c | 25 ++++++++++++++++ 3 files changed, 67 insertions(+), 4 deletions(-) diff --git a/nuttx/configs/px4fmuv2/include/board.h b/nuttx/configs/px4fmuv2/include/board.h index fd8f78b80b..be4cdcdfdc 100755 --- a/nuttx/configs/px4fmuv2/include/board.h +++ b/nuttx/configs/px4fmuv2/include/board.h @@ -181,6 +181,17 @@ # define SDIO_SDXFR_CLKDIV (3 << SDIO_CLKCR_CLKDIV_SHIFT) #endif +/* DMA Channl/Stream Selections *****************************************************/ +/* Stream selections are arbitrary for now but might become important in the future + * is we set aside more DMA channels/streams. + * + * SDIO DMA + *   DMAMAP_SDIO_1 = Channel 4, Stream 3 <- may later be used by SPI DMA + *   DMAMAP_SDIO_2 = Channel 4, Stream 6 + */ + +#define DMAMAP_SDIO DMAMAP_SDIO_1 + /* High-resolution timer */ #ifdef CONFIG_HRT_TIMER diff --git a/nuttx/configs/px4fmuv2/nsh/defconfig b/nuttx/configs/px4fmuv2/nsh/defconfig index beeb475517..d2f711b2dd 100755 --- a/nuttx/configs/px4fmuv2/nsh/defconfig +++ b/nuttx/configs/px4fmuv2/nsh/defconfig @@ -192,7 +192,7 @@ CONFIG_STM32_USART6=y CONFIG_STM32_ADC1=y CONFIG_STM32_ADC2=n CONFIG_STM32_ADC3=n -CONFIG_STM32_SDIO=n +CONFIG_STM32_SDIO=y CONFIG_STM32_SPI1=y CONFIG_STM32_SYSCFG=y CONFIG_STM32_TIM9=y @@ -780,13 +780,40 @@ CONFIG_FS_BINFS=y # CONFIG_MMCSD_SPICLOCK - Maximum SPI clock to drive MMC/SD card. # Default is 20MHz, current setting 24 MHz # -CONFIG_MMCSD=n +#CONFIG_MMCSD=n # XXX need to rejig this for SDIO #CONFIG_MMCSD_SPI=y #CONFIG_MMCSD_NSLOTS=1 #CONFIG_MMCSD_READONLY=n #CONFIG_MMCSD_SPICLOCK=24000000 +# +# Maintain legacy build behavior (revisit) +# + +CONFIG_MMCSD=y +CONFIG_MMCSD_SPI=y +CONFIG_MMCSD_SDIO=y +CONFIG_MTD=y + +# +# SPI-based MMC/SD driver +# +CONFIG_MMCSD_NSLOTS=1 +CONFIG_MMCSD_READONLY=n +CONFIG_MMCSD_SPICLOCK=12500000 + +# +# STM32 SDIO-based MMC/SD driver +# +CONFIG_SDIO_DMA=y +#CONFIG_SDIO_PRI=128 +#CONFIG_SDIO_DMAPRIO +#CONFIG_SDIO_WIDTH_D1_ONLY +CONFIG_MMCSD_MULTIBLOCK_DISABLE=y +CONFIG_MMCSD_MMCSUPPORT=n +CONFIG_MMCSD_HAVECARDDETECT=n + # # Block driver buffering # @@ -1004,8 +1031,8 @@ CONFIG_NSH_FATMOUNTPT=/tmp # Architecture-specific NSH options # #CONFIG_NSH_MMCSDSPIPORTNO=3 -#CONFIG_NSH_MMCSDSLOTNO=0 -#CONFIG_NSH_MMCSDMINOR=0 +CONFIG_NSH_MMCSDSLOTNO=0 +CONFIG_NSH_MMCSDMINOR=0 # diff --git a/src/drivers/boards/px4fmuv2/px4fmu_init.c b/src/drivers/boards/px4fmuv2/px4fmu_init.c index 1d99f15bfc..2222e59a8e 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu_init.c +++ b/src/drivers/boards/px4fmuv2/px4fmu_init.c @@ -55,6 +55,7 @@ #include #include #include +#include #include #include @@ -128,6 +129,7 @@ __EXPORT void stm32_boardinitialize(void) static struct spi_dev_s *spi1; static struct spi_dev_s *spi2; +static struct sdio_dev_s *sdio; #include @@ -225,5 +227,28 @@ __EXPORT int nsh_archinitialize(void) //message("[boot] Successfully bound SPI port 2 to the FRAM driver\n"); + #ifdef CONFIG_MMCSD + /* First, get an instance of the SDIO interface */ + + sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO); + if (!sdio) { + message("nsh_archinitialize: Failed to initialize SDIO slot %d\n", + CONFIG_NSH_MMCSDSLOTNO); + return -ENODEV; + } + + /* Now bind the SDIO interface to the MMC/SD driver */ + int ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdio); + if (ret != OK) { + message("nsh_archinitialize: Failed to bind SDIO to the MMC/SD driver: %d\n", ret); + return ret; + } + + /* Then let's guess and say that there is a card in the slot. There is no card detect GPIO. */ + sdio_mediachange(sdio, true); + + message("[boot] Initialized SDIO\n"); + #endif + return OK; } From 5abae2c78d0f7f41e1340fe5e396936da2c7a580 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 20 Apr 2013 09:14:26 +0400 Subject: [PATCH 075/486] Publish manual_sas_mode immediately, SAS modes switch order changed to more safe --- apps/commander/commander.c | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 7c0a25f862..3c74c15efa 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1510,6 +1510,7 @@ int commander_thread_main(int argc, char *argv[]) uint64_t start_time = hrt_absolute_time(); uint64_t failsave_ll_start_time = 0; + enum VEHICLE_MANUAL_SAS_MODE manual_sas_mode; bool state_changed = true; bool param_init_forced = true; @@ -1931,8 +1932,9 @@ int commander_thread_main(int argc, char *argv[]) } else if (sp_man.manual_sas_switch < -STICK_ON_OFF_LIMIT) { - /* bottom stick position, set altitude hold */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ALTITUDE; + /* bottom stick position, set default */ + /* this MUST be mapped to extremal position to switch easy in case of emergency */ + current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS; } else if (sp_man.manual_sas_switch > STICK_ON_OFF_LIMIT) { @@ -1940,8 +1942,14 @@ int commander_thread_main(int argc, char *argv[]) current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_SIMPLE; } else { - /* center stick position, set default */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS; + /* center stick position, set altitude hold */ + current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ALTITUDE; + } + + if (current_status.manual_sas_mode != manual_sas_mode) { + /* publish SAS mode changes immediately */ + manual_sas_mode = current_status.manual_sas_mode; + state_changed = true; } /* From 57cca9dbb41a1803c3cdb61151947e99625c59cb Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 20 Apr 2013 12:33:02 +0400 Subject: [PATCH 076/486] Abs yaw and other bugs fixed --- .../multirotor_att_control_main.c | 207 ++++++++++-------- .../multirotor_pos_control.c | 154 +++++++------ 2 files changed, 199 insertions(+), 162 deletions(-) diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 09710f0fcb..2582998285 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -241,123 +241,140 @@ mc_thread_main(int argc, char *argv[]) } else if (state.flag_control_manual_enabled) { + /* direct manual input */ + if (state.flag_control_attitude_enabled) { + /* Control attitude, update attitude setpoint depending on SAS mode: + * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS: roll, pitch, yaw, thrust + * VEHICLE_MANUAL_SAS_MODE_ALTITUDE: roll, pitch, yaw + * VEHICLE_MANUAL_SAS_MODE_SIMPLE: yaw + * */ - if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS) { - /* direct manual input */ - if (state.flag_control_attitude_enabled) { + /* initialize to current yaw if switching to manual or att control */ + if (state.flag_control_attitude_enabled != flag_control_attitude_enabled || + state.flag_control_manual_enabled != flag_control_manual_enabled || + state.flag_system_armed != flag_system_armed) { + att_sp.yaw_body = att.yaw; + } - /* initialize to current yaw if switching to manual or att control */ - if (state.flag_control_attitude_enabled != flag_control_attitude_enabled || - state.flag_control_manual_enabled != flag_control_manual_enabled || - state.flag_system_armed != flag_system_armed) { + static bool rc_loss_first_time = true; + + /* if the RC signal is lost, try to stay level and go slowly back down to ground */ + if (state.rc_signal_lost) { + if (state.manual_sas_mode != VEHICLE_MANUAL_SAS_MODE_SIMPLE) { + /* Don't reset attitude setpoint in SIMPLE SAS mode, it's handled by position controller. */ + att_sp.roll_body = 0.0f; + att_sp.pitch_body = 0.0f; + + if (state.manual_sas_mode != VEHICLE_MANUAL_SAS_MODE_ALTITUDE) { + /* Don't touch throttle in modes with altitude hold, it's handled by position controller. + * + * Only go to failsafe throttle if last known throttle was + * high enough to create some lift to make hovering state likely. + * + * This is to prevent that someone landing, but not disarming his + * multicopter (throttle = 0) does not make it jump up in the air + * if shutting down his remote. + */ + if (isfinite(manual.throttle) && manual.throttle > 0.2f) { + /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ + param_get(failsafe_throttle_handle, &failsafe_throttle); + att_sp.thrust = failsafe_throttle; + + } else { + att_sp.thrust = 0.0f; + } + } + } + + /* keep current yaw, do not attempt to go to north orientation, + * since if the pilot regains RC control, he will be lost regarding + * the current orientation. + */ + if (rc_loss_first_time) + att_sp.yaw_body = att.yaw; + + rc_loss_first_time = false; + + } else { + rc_loss_first_time = true; + + /* control yaw in all SAS modes */ + /* set yaw if arming */ + if (!flag_control_attitude_enabled && state.flag_system_armed) { att_sp.yaw_body = att.yaw; } - static bool rc_loss_first_time = true; + /* act if stabilization is active or if the (nonsense) direct pass through mode is set */ + if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS || + state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { - /* if the RC signal is lost, try to stay level and go slowly back down to ground */ - if (state.rc_signal_lost) { - /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ - param_get(failsafe_throttle_handle, &failsafe_throttle); - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - - /* - * Only go to failsafe throttle if last known throttle was - * high enough to create some lift to make hovering state likely. - * - * This is to prevent that someone landing, but not disarming his - * multicopter (throttle = 0) does not make it jump up in the air - * if shutting down his remote. - */ - if (isfinite(manual.throttle) && manual.throttle > 0.2f) { - att_sp.thrust = failsafe_throttle; + if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) { + rates_sp.yaw = manual.yaw; + control_yaw_position = false; } else { - att_sp.thrust = 0.0f; - } + /* + * This mode SHOULD be the default mode, which is: + * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS + * + * However, we fall back to this setting for all other (nonsense) + * settings as well. + */ - /* keep current yaw, do not attempt to go to north orientation, - * since if the pilot regains RC control, he will be lost regarding - * the current orientation. - */ - if (rc_loss_first_time) - att_sp.yaw_body = att.yaw; - - rc_loss_first_time = false; - - } else { - rc_loss_first_time = true; - - att_sp.roll_body = manual.roll; - att_sp.pitch_body = manual.pitch; - - /* set attitude if arming */ - if (!flag_control_attitude_enabled && state.flag_system_armed) { - att_sp.yaw_body = att.yaw; - } - - /* act if stabilization is active or if the (nonsense) direct pass through mode is set */ - if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS || - state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { - - if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) { + /* only move setpoint if manual input is != 0 */ + if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && ( + state.manual_sas_mode != VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS || + manual.throttle > 0.3f)) { rates_sp.yaw = manual.yaw; control_yaw_position = false; + first_time_after_yaw_speed_control = true; } else { - /* - * This mode SHOULD be the default mode, which is: - * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS - * - * However, we fall back to this setting for all other (nonsense) - * settings as well. - */ - - /* only move setpoint if manual input is != 0 */ - if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { - rates_sp.yaw = manual.yaw; - control_yaw_position = false; - first_time_after_yaw_speed_control = true; - - } else { - if (first_time_after_yaw_speed_control) { - att_sp.yaw_body = att.yaw; - first_time_after_yaw_speed_control = false; - } - - control_yaw_position = true; + if (first_time_after_yaw_speed_control) { + att_sp.yaw_body = att.yaw; + first_time_after_yaw_speed_control = false; } + + control_yaw_position = true; } } - - att_sp.thrust = manual.throttle; - att_sp.timestamp = hrt_absolute_time(); } - if (motor_test_mode) { - printf("testmode"); - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - att_sp.yaw_body = 0.0f; - att_sp.thrust = 0.1f; - att_sp.timestamp = hrt_absolute_time(); + if (state.manual_sas_mode != VEHICLE_MANUAL_SAS_MODE_SIMPLE) { + /* don't update attitude setpoint in SIMPLE SAS mode */ + att_sp.roll_body = manual.roll; + att_sp.pitch_body = manual.pitch; + if (state.manual_sas_mode != VEHICLE_MANUAL_SAS_MODE_ALTITUDE) { + /* don't set throttle in alt hold modes */ + att_sp.thrust = manual.throttle; + } } - /* STEP 2: publish the controller output */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + att_sp.timestamp = hrt_absolute_time(); + } - } else { - /* manual rate inputs, from RC control or joystick */ - if (state.flag_control_rates_enabled && - state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) { - rates_sp.roll = manual.roll; + if (motor_test_mode) { + printf("testmode"); + att_sp.roll_body = 0.0f; + att_sp.pitch_body = 0.0f; + att_sp.yaw_body = 0.0f; + att_sp.thrust = 0.1f; + att_sp.timestamp = hrt_absolute_time(); + } - rates_sp.pitch = manual.pitch; - rates_sp.yaw = manual.yaw; - rates_sp.thrust = manual.throttle; - rates_sp.timestamp = hrt_absolute_time(); - } + /* STEP 2: publish the controller output */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + + } else { + /* manual rate inputs, from RC control or joystick */ + if (state.flag_control_rates_enabled && + state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) { + rates_sp.roll = manual.roll; + + rates_sp.pitch = manual.pitch; + rates_sp.yaw = manual.yaw; + rates_sp.thrust = manual.throttle; + rates_sp.timestamp = hrt_absolute_time(); } } } diff --git a/apps/multirotor_pos_control/multirotor_pos_control.c b/apps/multirotor_pos_control/multirotor_pos_control.c index d2b6685aca..7757a78c11 100644 --- a/apps/multirotor_pos_control/multirotor_pos_control.c +++ b/apps/multirotor_pos_control/multirotor_pos_control.c @@ -162,6 +162,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { int param_sub = orb_subscribe(ORB_ID(parameter_update)); int state_sub = orb_subscribe(ORB_ID(vehicle_status)); int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); int local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); @@ -186,18 +187,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { int paramcheck_counter = 0; while (!thread_should_exit) { - /* get a local copy of the vehicle state */ orb_copy(ORB_ID(vehicle_status), state_sub, &status); - /* get a local copy of manual setpoint */ - orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); - /* get a local copy of attitude */ - orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - /* get a local copy of local position */ - orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); - if (status.state_machine == SYSTEM_STATE_AUTO) { - /* get a local copy of local position setpoint */ - orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); - } /* check parameters at 1 Hz*/ paramcheck_counter++; @@ -210,26 +200,58 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { paramcheck_counter = 0; } - if (status.state_machine == SYSTEM_STATE_MANUAL) { - if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ALTITUDE || status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE) { - hrt_abstime t = hrt_absolute_time(); - if (reset_sp_alt) { - reset_sp_alt = false; - local_pos_sp.z = local_pos.z; - alt_integral = manual.throttle; - char str[80]; - sprintf(str, "reset alt setpoint: z = %.2f, throttle = %.2f", local_pos_sp.z, manual.throttle); - mavlink_log_info(mavlink_fd, str); - } + /* Check if controller should act */ + bool act = status.flag_system_armed && ( + /* SAS modes */ + ( + status.flag_control_manual_enabled && + status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS && ( + status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ALTITUDE || + status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE + ) + ) || + /* AUTO mode */ + status.state_machine == SYSTEM_STATE_AUTO + ); - float dt; - if (t_prev != 0) { - dt = (t - t_prev) * 0.000001f; - } else { - dt = 0.0f; - } - float err_linear_limit = params.alt_d / params.alt_p * params.alt_rate_max; - /* move altitude setpoint by manual controls */ + if (act) { + orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); + orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); + orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); + orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); + if (status.state_machine == SYSTEM_STATE_AUTO) { + orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); + } + + hrt_abstime t = hrt_absolute_time(); + if (reset_sp_alt) { + reset_sp_alt = false; + local_pos_sp.z = local_pos.z; + alt_integral = manual.throttle; + char str[80]; + sprintf(str, "reset alt setpoint: z = %.2f, throttle = %.2f", local_pos_sp.z, manual.throttle); + mavlink_log_info(mavlink_fd, str); + } + + if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE && reset_sp_pos) { + reset_sp_pos = false; + local_pos_sp.x = local_pos.x; + local_pos_sp.y = local_pos.y; + char str[80]; + sprintf(str, "reset pos setpoint: x = %.2f, y = %.2f", local_pos_sp.x, local_pos_sp.y); + mavlink_log_info(mavlink_fd, str); + } + + float dt; + if (t_prev != 0) { + dt = (t - t_prev) * 0.000001f; + } else { + dt = 0.0f; + } + float err_linear_limit = params.alt_d / params.alt_p * params.alt_rate_max; + + if (status.flag_control_manual_enabled) { + /* move altitude setpoint with manual controls */ float alt_ctl = manual.throttle - 0.5f; if (fabs(alt_ctl) < alt_ctl_dz) { alt_ctl = 0.0f; @@ -245,48 +267,46 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { local_pos_sp.z = local_pos.z - err_linear_limit; } - /* PID for altitude */ - float alt_err = local_pos.z - local_pos_sp.z; - /* don't accelerate more than ALT_RATE_MAX, limit error to corresponding value */ - if (alt_err > err_linear_limit) { - alt_err = err_linear_limit; - } else if (alt_err < -err_linear_limit) { - alt_err = -err_linear_limit; - } - /* PID for altitude rate */ - float thrust_ctl_pd = alt_err * params.alt_p + local_pos.vz * params.alt_d; - float thrust_ctl = thrust_ctl_pd + alt_integral; - if (thrust_ctl < params.thr_min) { - thrust_ctl = params.thr_min; - } else if (thrust_ctl > params.thr_max) { - thrust_ctl = params.thr_max; - } else { - /* integrate only in linear area (with 20% gap) and not on min/max throttle */ - if (fabs(thrust_ctl_pd) < err_linear_limit * params.alt_p * 0.8f) - alt_integral += thrust_ctl_pd / params.alt_p * params.alt_i * dt; - } if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE) { - // TODO add position controller - att_sp.pitch_body = manual.pitch; - att_sp.roll_body = manual.roll; - att_sp.yaw_body = manual.yaw; - } else { - att_sp.pitch_body = manual.pitch; - att_sp.roll_body = manual.roll; - att_sp.yaw_body = manual.yaw; + // TODO move position setpoint with manual controls } - att_sp.thrust = thrust_ctl; - att_sp.timestamp = hrt_absolute_time(); - /* publish local position setpoint */ - orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); - /* publish new attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - t_prev = t; - } else { - reset_sp_alt = true; } + + /* PID for altitude */ + float alt_err = local_pos.z - local_pos_sp.z; + /* don't accelerate more than ALT_RATE_MAX, limit error to corresponding value */ + if (alt_err > err_linear_limit) { + alt_err = err_linear_limit; + } else if (alt_err < -err_linear_limit) { + alt_err = -err_linear_limit; + } + /* P and D components */ + float thrust_ctl_pd = alt_err * params.alt_p + local_pos.vz * params.alt_d; + /* add I component */ + float thrust_ctl = thrust_ctl_pd + alt_integral; + alt_integral += thrust_ctl_pd / params.alt_p * params.alt_i * dt; + if (thrust_ctl < params.thr_min) { + thrust_ctl = params.thr_min; + } else if (thrust_ctl > params.thr_max) { + thrust_ctl = params.thr_max; + } + if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) { + // TODO add position controller + } else { + reset_sp_pos = true; + } + att_sp.thrust = thrust_ctl; + att_sp.timestamp = hrt_absolute_time(); + if (status.flag_control_manual_enabled) { + /* publish local position setpoint in manual mode */ + orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); + } + /* publish new attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + t_prev = t; } else { reset_sp_alt = true; + reset_sp_pos = true; } /* run at approximately 50 Hz */ From 10dfd2ba393089e5008978eb0469e8c1289d5917 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 20 Apr 2013 22:38:38 +0400 Subject: [PATCH 077/486] dt calculation bug fixed --- .../multirotor_pos_control.c | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/apps/multirotor_pos_control/multirotor_pos_control.c b/apps/multirotor_pos_control/multirotor_pos_control.c index 7757a78c11..ac7645764c 100644 --- a/apps/multirotor_pos_control/multirotor_pos_control.c +++ b/apps/multirotor_pos_control/multirotor_pos_control.c @@ -214,6 +214,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { status.state_machine == SYSTEM_STATE_AUTO ); + hrt_abstime t = hrt_absolute_time(); + float dt; + if (t_prev != 0) { + dt = (t - t_prev) * 0.000001f; + } else { + dt = 0.0f; + } + t_prev = t; if (act) { orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); @@ -223,7 +231,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); } - hrt_abstime t = hrt_absolute_time(); if (reset_sp_alt) { reset_sp_alt = false; local_pos_sp.z = local_pos.z; @@ -242,12 +249,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { mavlink_log_info(mavlink_fd, str); } - float dt; - if (t_prev != 0) { - dt = (t - t_prev) * 0.000001f; - } else { - dt = 0.0f; - } float err_linear_limit = params.alt_d / params.alt_p * params.alt_rate_max; if (status.flag_control_manual_enabled) { @@ -303,7 +304,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { } /* publish new attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - t_prev = t; } else { reset_sp_alt = true; reset_sp_pos = true; From 7e8d8f9e7226bcc04a5f8dd4b01c9a6a4f1f9910 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 20 Apr 2013 12:49:56 -0700 Subject: [PATCH 078/486] Call sub-makes with -r to make them start faster (mostly on Windows, where this inhibits an enormous amount of silly scanning for things). Force non-parallel builds for the NuttX archives. --- Makefile | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/Makefile b/Makefile index 0b8a65d94a..2347b09bf3 100644 --- a/Makefile +++ b/Makefile @@ -96,7 +96,7 @@ $(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4: @echo %%%% Building $(config) in $(work_dir) @echo %%%% $(Q) mkdir -p $(work_dir) - $(Q) make -C $(work_dir) \ + $(Q) make -r -C $(work_dir) \ -f $(PX4_MK_DIR)firmware.mk \ CONFIG=$(config) \ WORK_DIR=$(work_dir) \ @@ -119,15 +119,21 @@ NUTTX_ARCHIVES = $(foreach board,$(BOARDS),$(ARCHIVE_DIR)$(board).export) .PHONY: archives archives: $(NUTTX_ARCHIVES) +# We cannot build these parallel; note that we also force -j1 for the +# sub-make invocations. +ifneq ($(filter archives,$(MAKECMDGOALS)),) +.NOTPARALLEL: +endif + $(ARCHIVE_DIR)%.export: board = $(notdir $(basename $@)) $(ARCHIVE_DIR)%.export: configuration = $(if $(filter $(board),px4io),io,nsh) $(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) $(NUTTX_APPS) @echo %% Configuring NuttX for $(board) $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) - $(Q) make -C $(NUTTX_SRC) -r $(MQUIET) distclean + $(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean $(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(board)/$(configuration)) @echo %% Exporting NuttX for $(board) - $(Q) make -C $(NUTTX_SRC) -r $(MQUIET) export + $(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) export $(Q) mkdir -p $(dir $@) $(Q) $(COPY) $(NUTTX_SRC)nuttx-export.zip $@ From 754a11f4fcad7c9d6c2d771cf2f5ff58f90e2224 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 20 Apr 2013 12:51:07 -0700 Subject: [PATCH 079/486] Use a .elf suffix for the ELF object file (seems more sensible that way) Detect the case where PX4_BASE contains spaces and stop before we cause any more damage. Call sub-makes with -r to improve startup time. --- makefiles/firmware.mk | 25 ++++++++++++++----------- 1 file changed, 14 insertions(+), 11 deletions(-) diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index f6057d2fc8..83bba3a9b5 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -64,7 +64,10 @@ # path to this file. # # CONFIG: -# Used to set the output filename; defaults to 'firmware'. +# Used when searching for the configuration file, and available +# to module Makefiles to select optional features. +# If not set, CONFIG_FILE must be set and CONFIG will be derived +# automatically from it. # # CONFIG_FILE: # If set, overrides the configuration file search logic. Sets @@ -98,11 +101,14 @@ # If PX4_BASE wasn't set previously, work out what it should be # and set it here now. # -MK_DIR ?= $(dir $(lastword $(MAKEFILE_LIST))) +MK_DIR ?= $(dir $(lastword $(MAKEFILE_LIST))) ifeq ($(PX4_BASE),) export PX4_BASE := $(abspath $(MK_DIR)/..) endif $(info % PX4_BASE = $(PX4_BASE)) +ifneq ($(words $(PX4_BASE)),1) +$(error Cannot build when the PX4_BASE path contains one or more space characters.) +endif # # Set a default target so that included makefiles or errors here don't @@ -218,10 +224,7 @@ MODULE_OBJS := $(foreach path,$(dir $(MODULE_MKFILES)),$(WORK_DIR)$(path)module $(MODULE_OBJS): relpath = $(patsubst $(WORK_DIR)%,%,$@) $(MODULE_OBJS): mkfile = $(patsubst %module.pre.o,%module.mk,$(relpath)) $(MODULE_OBJS): $(GLOBAL_DEPS) $(NUTTX_CONFIG_HEADER) - @$(ECHO) %% - @$(ECHO) %% Building module using $(mkfile) - @$(ECHO) %% - $(Q) $(MAKE) -f $(PX4_MK_DIR)module.mk \ + $(Q) $(MAKE) -r -f $(PX4_MK_DIR)module.mk \ MODULE_WORK_DIR=$(dir $@) \ MODULE_OBJ=$@ \ MODULE_MK=$(mkfile) \ @@ -237,7 +240,7 @@ $(MODULE_CLEANS): relpath = $(patsubst $(WORK_DIR)%,%,$@) $(MODULE_CLEANS): mkfile = $(patsubst %clean,%module.mk,$(relpath)) $(MODULE_CLEANS): @$(ECHO) %% cleaning using $(mkfile) - $(Q) $(MAKE) -f $(PX4_MK_DIR)module.mk \ + $(Q) $(MAKE) -r -f $(PX4_MK_DIR)module.mk \ MODULE_WORK_DIR=$(dir $@) \ MODULE_MK=$(mkfile) \ clean @@ -372,7 +375,7 @@ endif # PRODUCT_BUNDLE = $(WORK_DIR)firmware.px4 PRODUCT_BIN = $(WORK_DIR)firmware.bin -PRODUCT_SYM = $(WORK_DIR)firmware.sym +PRODUCT_ELF = $(WORK_DIR)firmware.elf .PHONY: firmware firmware: $(PRODUCT_BUNDLE) @@ -407,10 +410,10 @@ $(PRODUCT_BUNDLE): $(PRODUCT_BIN) --git_identity $(PX4_BASE) \ --image $< > $@ -$(PRODUCT_BIN): $(PRODUCT_SYM) +$(PRODUCT_BIN): $(PRODUCT_ELF) $(call SYM_TO_BIN,$<,$@) -$(PRODUCT_SYM): $(OBJS) $(MODULE_OBJS) $(GLOBAL_DEPS) $(LINK_DEPS) $(MODULE_MKFILES) +$(PRODUCT_ELF): $(OBJS) $(MODULE_OBJS) $(GLOBAL_DEPS) $(LINK_DEPS) $(MODULE_MKFILES) $(call LINK,$@,$(OBJS) $(MODULE_OBJS)) # @@ -428,7 +431,7 @@ upload: $(PRODUCT_BUNDLE) $(PRODUCT_BIN) .PHONY: clean clean: $(MODULE_CLEANS) @$(ECHO) %% cleaning - $(Q) $(REMOVE) $(PRODUCT_BUNDLE) $(PRODUCT_BIN) $(PRODUCT_SYM) + $(Q) $(REMOVE) $(PRODUCT_BUNDLE) $(PRODUCT_BIN) $(PRODUCT_ELF) $(Q) $(REMOVE) $(OBJS) $(DEP_INCLUDES) $(EXTRA_CLEANS) $(Q) $(RMDIR) $(NUTTX_EXPORT_DIR) From a7cf9e2a366b3ac4d92e8e12da23308e285d6ead Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 20 Apr 2013 14:57:15 -0700 Subject: [PATCH 080/486] Make 'make upload' work --- Makefile | 17 ++++++++++++++++- Tools/px_uploader.py | 2 +- makefiles/firmware.mk | 4 +++- makefiles/module.mk | 3 ++- makefiles/upload.mk | 9 ++++++--- 5 files changed, 28 insertions(+), 7 deletions(-) diff --git a/Makefile b/Makefile index 2347b09bf3..c3ef7ad0df 100644 --- a/Makefile +++ b/Makefile @@ -63,11 +63,26 @@ MQUIET = --no-print-directory # # If the user has listed a config as a target, strip it out and override CONFIGS. # +FIRMWARE_GOAL = firmware EXPLICIT_CONFIGS := $(filter $(CONFIGS),$(MAKECMDGOALS)) ifneq ($(EXPLICIT_CONFIGS),) CONFIGS := $(EXPLICIT_CONFIGS) .PHONY: $(EXPLICIT_CONFIGS) $(EXPLICIT_CONFIGS): all + +# +# If the user has asked to upload, they must have also specified exactly one +# config. +# +ifneq ($(filter upload,$(MAKECMDGOALS)),) +ifneq ($(words $(EXPLICIT_CONFIGS)),1) +$(error In order to upload, exactly one board config must be specified) +endif +FIRMWARE_GOAL = upload +.PHONY: upload +upload: + @: +endif endif # @@ -100,7 +115,7 @@ $(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4: -f $(PX4_MK_DIR)firmware.mk \ CONFIG=$(config) \ WORK_DIR=$(work_dir) \ - firmware + $(FIRMWARE_GOAL) # # Build the NuttX export archives. diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index cce388d710..423bdb42a2 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -167,7 +167,7 @@ class uploader(object): def __init__(self, portname, baudrate): # open the port, keep the default timeout short so we can poll quickly - self.port = serial.Serial(portname, baudrate, timeout=0.25) + self.port = serial.Serial(portname, baudrate, timeout=0.5) def close(self): if self.port is not None: diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index 83bba3a9b5..fff0e1c65c 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -424,7 +424,8 @@ $(PRODUCT_ELF): $(OBJS) $(MODULE_OBJS) $(GLOBAL_DEPS) $(LINK_DEPS) $(MODULE_MKF upload: $(PRODUCT_BUNDLE) $(PRODUCT_BIN) $(Q) $(MAKE) -f $(PX4_MK_DIR)/upload.mk \ METHOD=serial \ - PRODUCT=$(PRODUCT) \ + CONFIG=$(CONFIG) \ + BOARD=$(BOARD) \ BUNDLE=$(PRODUCT_BUNDLE) \ BIN=$(PRODUCT_BIN) @@ -435,6 +436,7 @@ clean: $(MODULE_CLEANS) $(Q) $(REMOVE) $(OBJS) $(DEP_INCLUDES) $(EXTRA_CLEANS) $(Q) $(RMDIR) $(NUTTX_EXPORT_DIR) + # # DEP_INCLUDES is defined by the toolchain include in terms of $(OBJS) # diff --git a/makefiles/module.mk b/makefiles/module.mk index 1db0f6fee6..e2a1041e06 100644 --- a/makefiles/module.mk +++ b/makefiles/module.mk @@ -102,7 +102,7 @@ ifeq ($(MODULE_MK),) $(error No module makefile specified) endif -$(info % MODULE_MK = $(MODULE_MK)) +$(info %% MODULE_MK = $(MODULE_MK)) # # Get the board/toolchain config @@ -147,6 +147,7 @@ $(MODULE_COMMAND_FILES): exclude = $(dir $@)COMMAND.$(command).* $(MODULE_COMMAND_FILES): $(GLOBAL_DEPS) @$(REMOVE) -f $(exclude) @$(MKDIR) -p $(dir $@) + @echo "CMD: $(command)" $(Q) $(TOUCH) $@ endif diff --git a/makefiles/upload.mk b/makefiles/upload.mk index 5308aaa3ee..a1159d157a 100644 --- a/makefiles/upload.mk +++ b/makefiles/upload.mk @@ -21,11 +21,14 @@ ifeq ($(SERIAL_PORTS),) SERIAL_PORTS = "\\\\.\\COM32,\\\\.\\COM31,\\\\.\\COM30,\\\\.\\COM29,\\\\.\\COM28,\\\\.\\COM27,\\\\.\\COM26,\\\\.\\COM25,\\\\.\\COM24,\\\\.\\COM23,\\\\.\\COM22,\\\\.\\COM21,\\\\.\\COM20,\\\\.\\COM19,\\\\.\\COM18,\\\\.\\COM17,\\\\.\\COM16,\\\\.\\COM15,\\\\.\\COM14,\\\\.\\COM13,\\\\.\\COM12,\\\\.\\COM11,\\\\.\\COM10,\\\\.\\COM9,\\\\.\\COM8,\\\\.\\COM7,\\\\.\\COM6,\\\\.\\COM5,\\\\.\\COM4,\\\\.\\COM3,\\\\.\\COM2,\\\\.\\COM1,\\\\.\\COM0" endif -.PHONY: all upload-$(METHOD)-$(PRODUCT) -all: upload-$(METHOD)-$(PRODUCT) +.PHONY: all upload-$(METHOD)-$(BOARD) +all: upload-$(METHOD)-$(BOARD) upload-serial-px4fmu: $(BUNDLE) $(UPLOADER) - @python -u $(UPLOADER) --port $(SERIAL_PORTS) $(PRODUCT_BUNDLE) + @python -u $(UPLOADER) --port $(SERIAL_PORTS) $(BUNDLE) + +upload-serial-px4fmuv2: $(BUNDLE) $(UPLOADER) + @python -u $(UPLOADER) --port $(SERIAL_PORTS) $(BUNDLE) # # JTAG firmware uploading with OpenOCD From 7e0f8b3edaf584a48cd3bc3351e3205fd0106cdc Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 20 Apr 2013 15:18:10 -0700 Subject: [PATCH 081/486] Formatting changes to make the Python style checker happy (copied from the bootloader project). Increase the erase timeout to avoid issues with large/slow flash. --- Tools/px_uploader.py | 587 ++++++++++++++++++++++--------------------- 1 file changed, 294 insertions(+), 293 deletions(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index 423bdb42a2..d2ebf16987 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -41,20 +41,19 @@ # The uploader uses the following fields from the firmware file: # # image -# The firmware that will be uploaded. +# The firmware that will be uploaded. # image_size -# The size of the firmware in bytes. +# The size of the firmware in bytes. # board_id -# The board for which the firmware is intended. +# The board for which the firmware is intended. # board_revision -# Currently only used for informational purposes. +# Currently only used for informational purposes. # import sys import argparse import binascii import serial -import os import struct import json import zlib @@ -64,292 +63,294 @@ import array from sys import platform as _platform + class firmware(object): - '''Loads a firmware file''' + '''Loads a firmware file''' - desc = {} - image = bytes() - crctab = array.array('I', [ - 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, - 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, - 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, - 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, - 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, - 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, - 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, - 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, - 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, - 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, - 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, - 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, - 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, - 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, - 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, - 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, - 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, - 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, - 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, - 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, - 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, - 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, - 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, - 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, - 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, - 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, - 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, - 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, - 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, - 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, - 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, - 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d ]) - crcpad = bytearray('\xff\xff\xff\xff') + desc = {} + image = bytes() + crctab = array.array('I', [ + 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, + 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, + 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, + 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, + 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, + 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, + 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, + 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, + 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, + 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, + 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, + 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, + 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, + 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, + 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, + 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, + 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, + 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, + 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, + 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, + 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, + 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, + 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, + 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, + 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, + 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, + 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, + 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, + 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, + 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, + 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, + 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d]) + crcpad = bytearray('\xff\xff\xff\xff') - def __init__(self, path): + def __init__(self, path): - # read the file - f = open(path, "r") - self.desc = json.load(f) - f.close() + # read the file + f = open(path, "r") + self.desc = json.load(f) + f.close() - self.image = bytearray(zlib.decompress(base64.b64decode(self.desc['image']))) + self.image = bytearray(zlib.decompress(base64.b64decode(self.desc['image']))) - # pad image to 4-byte length - while ((len(self.image) % 4) != 0): - self.image.append('\xff') + # pad image to 4-byte length + while ((len(self.image) % 4) != 0): + self.image.append('\xff') - def property(self, propname): - return self.desc[propname] + def property(self, propname): + return self.desc[propname] - def __crc32(self, bytes, state): - for byte in bytes: - index = (state ^ byte) & 0xff - state = self.crctab[index] ^ (state >> 8) - return state + def __crc32(self, bytes, state): + for byte in bytes: + index = (state ^ byte) & 0xff + state = self.crctab[index] ^ (state >> 8) + return state + + def crc(self, padlen): + state = self.__crc32(self.image, int(0)) + for i in range(len(self.image), (padlen - 1), 4): + state = self.__crc32(self.crcpad, state) + return state - def crc(self, padlen): - state = self.__crc32(self.image, int(0)) - for i in range(len(self.image), (padlen - 1), 4): - state = self.__crc32(self.crcpad, state) - return state class uploader(object): - '''Uploads a firmware file to the PX FMU bootloader''' + '''Uploads a firmware file to the PX FMU bootloader''' - # protocol bytes - INSYNC = chr(0x12) - EOC = chr(0x20) + # protocol bytes + INSYNC = chr(0x12) + EOC = chr(0x20) - # reply bytes - OK = chr(0x10) - FAILED = chr(0x11) - INVALID = chr(0x13) # rev3+ + # reply bytes + OK = chr(0x10) + FAILED = chr(0x11) + INVALID = chr(0x13) # rev3+ - # command bytes - NOP = chr(0x00) # guaranteed to be discarded by the bootloader - GET_SYNC = chr(0x21) - GET_DEVICE = chr(0x22) - CHIP_ERASE = chr(0x23) - CHIP_VERIFY = chr(0x24) # rev2 only - PROG_MULTI = chr(0x27) - READ_MULTI = chr(0x28) # rev2 only - GET_CRC = chr(0x29) # rev3+ - REBOOT = chr(0x30) - - INFO_BL_REV = chr(1) # bootloader protocol revision - BL_REV_MIN = 2 # minimum supported bootloader protocol - BL_REV_MAX = 3 # maximum supported bootloader protocol - INFO_BOARD_ID = chr(2) # board type - INFO_BOARD_REV = chr(3) # board revision - INFO_FLASH_SIZE = chr(4) # max firmware size in bytes + # command bytes + NOP = chr(0x00) # guaranteed to be discarded by the bootloader + GET_SYNC = chr(0x21) + GET_DEVICE = chr(0x22) + CHIP_ERASE = chr(0x23) + CHIP_VERIFY = chr(0x24) # rev2 only + PROG_MULTI = chr(0x27) + READ_MULTI = chr(0x28) # rev2 only + GET_CRC = chr(0x29) # rev3+ + REBOOT = chr(0x30) + + INFO_BL_REV = chr(1) # bootloader protocol revision + BL_REV_MIN = 2 # minimum supported bootloader protocol + BL_REV_MAX = 3 # maximum supported bootloader protocol + INFO_BOARD_ID = chr(2) # board type + INFO_BOARD_REV = chr(3) # board revision + INFO_FLASH_SIZE = chr(4) # max firmware size in bytes - PROG_MULTI_MAX = 60 # protocol max is 255, must be multiple of 4 - READ_MULTI_MAX = 60 # protocol max is 255, something overflows with >= 64 + PROG_MULTI_MAX = 60 # protocol max is 255, must be multiple of 4 + READ_MULTI_MAX = 60 # protocol max is 255, something overflows with >= 64 - def __init__(self, portname, baudrate): - # open the port, keep the default timeout short so we can poll quickly - self.port = serial.Serial(portname, baudrate, timeout=0.5) + def __init__(self, portname, baudrate): + # open the port, keep the default timeout short so we can poll quickly + self.port = serial.Serial(portname, baudrate, timeout=0.5) - def close(self): - if self.port is not None: - self.port.close() + def close(self): + if self.port is not None: + self.port.close() - def __send(self, c): -# print("send " + binascii.hexlify(c)) - self.port.write(str(c)) + def __send(self, c): +# print("send " + binascii.hexlify(c)) + self.port.write(str(c)) - def __recv(self, count = 1): - c = self.port.read(count) - if len(c) < 1: - raise RuntimeError("timeout waiting for data") -# print("recv " + binascii.hexlify(c)) - return c + def __recv(self, count=1): + c = self.port.read(count) + if len(c) < 1: + raise RuntimeError("timeout waiting for data") +# print("recv " + binascii.hexlify(c)) + return c - def __recv_int(self): - raw = self.__recv(4) - val = struct.unpack("= 3: - self.__getSync() + # send a PROG_MULTI command to write a collection of bytes + def __program_multi(self, data): + self.__send(uploader.PROG_MULTI + + chr(len(data))) + self.__send(data) + self.__send(uploader.EOC) + self.__getSync() - # split a sequence into a list of size-constrained pieces - def __split_len(self, seq, length): - return [seq[i:i+length] for i in range(0, len(seq), length)] + # verify multiple bytes in flash + def __verify_multi(self, data): + self.__send(uploader.READ_MULTI + + chr(len(data)) + + uploader.EOC) + self.port.flush() + programmed = self.__recv(len(data)) + if programmed != data: + print("got " + binascii.hexlify(programmed)) + print("expect " + binascii.hexlify(data)) + return False + self.__getSync() + return True - # upload code - def __program(self, fw): - code = fw.image - groups = self.__split_len(code, uploader.PROG_MULTI_MAX) - for bytes in groups: - self.__program_multi(bytes) + # send the reboot command + def __reboot(self): + self.__send(uploader.REBOOT + + uploader.EOC) + self.port.flush() - # verify code - def __verify_v2(self, fw): - self.__send(uploader.CHIP_VERIFY - + uploader.EOC) - self.__getSync() - code = fw.image - groups = self.__split_len(code, uploader.READ_MULTI_MAX) - for bytes in groups: - if (not self.__verify_multi(bytes)): - raise RuntimeError("Verification failed") + # v3+ can report failure if the first word flash fails + if self.bl_rev >= 3: + self.__getSync() - def __verify_v3(self, fw): - expect_crc = fw.crc(self.fw_maxsize) - self.__send(uploader.GET_CRC - + uploader.EOC) - report_crc = self.__recv_int() - self.__getSync() - if report_crc != expect_crc: - print(("Expected 0x%x" % expect_crc)) - print(("Got 0x%x" % report_crc)) - raise RuntimeError("Program CRC failed") + # split a sequence into a list of size-constrained pieces + def __split_len(self, seq, length): + return [seq[i:i+length] for i in range(0, len(seq), length)] - # get basic data about the board - def identify(self): - # make sure we are in sync before starting - self.__sync() + # upload code + def __program(self, fw): + code = fw.image + groups = self.__split_len(code, uploader.PROG_MULTI_MAX) + for bytes in groups: + self.__program_multi(bytes) - # get the bootloader protocol ID first - self.bl_rev = self.__getInfo(uploader.INFO_BL_REV) - if (self.bl_rev < uploader.BL_REV_MIN) or (self.bl_rev > uploader.BL_REV_MAX): - print(("Unsupported bootloader protocol %d" % uploader.INFO_BL_REV)) - raise RuntimeError("Bootloader protocol mismatch") + # verify code + def __verify_v2(self, fw): + self.__send(uploader.CHIP_VERIFY + + uploader.EOC) + self.__getSync() + code = fw.image + groups = self.__split_len(code, uploader.READ_MULTI_MAX) + for bytes in groups: + if (not self.__verify_multi(bytes)): + raise RuntimeError("Verification failed") - self.board_type = self.__getInfo(uploader.INFO_BOARD_ID) - self.board_rev = self.__getInfo(uploader.INFO_BOARD_REV) - self.fw_maxsize = self.__getInfo(uploader.INFO_FLASH_SIZE) + def __verify_v3(self, fw): + expect_crc = fw.crc(self.fw_maxsize) + self.__send(uploader.GET_CRC + + uploader.EOC) + report_crc = self.__recv_int() + self.__getSync() + if report_crc != expect_crc: + print("Expected 0x%x" % expect_crc) + print("Got 0x%x" % report_crc) + raise RuntimeError("Program CRC failed") - # upload the firmware - def upload(self, fw): - # Make sure we are doing the right thing - if self.board_type != fw.property('board_id'): - raise RuntimeError("Firmware not suitable for this board (run 'make configure_px4fmu && make clean' or 'make configure_px4io && make clean' to reconfigure).") - if self.fw_maxsize < fw.property('image_size'): - raise RuntimeError("Firmware image is too large for this board") + # get basic data about the board + def identify(self): + # make sure we are in sync before starting + self.__sync() - print("erase...") - self.__erase() + # get the bootloader protocol ID first + self.bl_rev = self.__getInfo(uploader.INFO_BL_REV) + if (self.bl_rev < uploader.BL_REV_MIN) or (self.bl_rev > uploader.BL_REV_MAX): + print("Unsupported bootloader protocol %d" % uploader.INFO_BL_REV) + raise RuntimeError("Bootloader protocol mismatch") - print("program...") - self.__program(fw) + self.board_type = self.__getInfo(uploader.INFO_BOARD_ID) + self.board_rev = self.__getInfo(uploader.INFO_BOARD_REV) + self.fw_maxsize = self.__getInfo(uploader.INFO_FLASH_SIZE) - print("verify...") - if self.bl_rev == 2: - self.__verify_v2(fw) - else: - self.__verify_v3(fw) + # upload the firmware + def upload(self, fw): + # Make sure we are doing the right thing + if self.board_type != fw.property('board_id'): + raise RuntimeError("Firmware not suitable for this board") + if self.fw_maxsize < fw.property('image_size'): + raise RuntimeError("Firmware image is too large for this board") + + print("erase...") + self.__erase() + + print("program...") + self.__program(fw) + + print("verify...") + if self.bl_rev == 2: + self.__verify_v2(fw) + else: + self.__verify_v3(fw) + + print("done, rebooting.") + self.__reboot() + self.port.close() - print("done, rebooting.") - self.__reboot() - self.port.close() - # Parse commandline arguments parser = argparse.ArgumentParser(description="Firmware uploader for the PX autopilot system.") @@ -360,57 +361,57 @@ args = parser.parse_args() # Load the firmware file fw = firmware(args.firmware) -print(("Loaded firmware for %x,%x, waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision')))) +print("Loaded firmware for %x,%x, waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision'))) # Spin waiting for a device to show up while True: - for port in args.port.split(","): + for port in args.port.split(","): - #print("Trying %s" % port) + #print("Trying %s" % port) - # create an uploader attached to the port - try: - if "linux" in _platform: - # Linux, don't open Mac OS and Win ports - if not "COM" in port and not "tty.usb" in port: - up = uploader(port, args.baud) - elif "darwin" in _platform: - # OS X, don't open Windows and Linux ports - if not "COM" in port and not "ACM" in port: - up = uploader(port, args.baud) - elif "win" in _platform: - # Windows, don't open POSIX ports - if not "/" in port: - up = uploader(port, args.baud) - except: - # open failed, rate-limit our attempts - time.sleep(0.05) + # create an uploader attached to the port + try: + if "linux" in _platform: + # Linux, don't open Mac OS and Win ports + if not "COM" in port and not "tty.usb" in port: + up = uploader(port, args.baud) + elif "darwin" in _platform: + # OS X, don't open Windows and Linux ports + if not "COM" in port and not "ACM" in port: + up = uploader(port, args.baud) + elif "win" in _platform: + # Windows, don't open POSIX ports + if not "/" in port: + up = uploader(port, args.baud) + except: + # open failed, rate-limit our attempts + time.sleep(0.05) - # and loop to the next port - continue + # and loop to the next port + continue - # port is open, try talking to it - try: - # identify the bootloader - up.identify() - print(("Found board %x,%x bootloader rev %x on %s" % (up.board_type, up.board_rev, up.bl_rev, port))) + # port is open, try talking to it + try: + # identify the bootloader + up.identify() + print("Found board %x,%x bootloader rev %x on %s" % (up.board_type, up.board_rev, up.bl_rev, port)) - except: - # most probably a timeout talking to the port, no bootloader - continue + except: + # most probably a timeout talking to the port, no bootloader + continue - try: - # ok, we have a bootloader, try flashing it - up.upload(fw) + try: + # ok, we have a bootloader, try flashing it + up.upload(fw) - except RuntimeError as ex: + except RuntimeError as ex: - # print the error - print(("ERROR: %s" % ex.args)) + # print the error + print("ERROR: %s" % ex.args) - finally: - # always close the port - up.close() + finally: + # always close the port + up.close() - # we could loop here if we wanted to wait for more boards... - sys.exit(0) + # we could loop here if we wanted to wait for more boards... + sys.exit(0) From 8fcbb4f669d8c9003f778f35a94278383e0360ac Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 22 Apr 2013 22:56:18 -0700 Subject: [PATCH 082/486] Merge SDIO changes and hack config to make it work. We need to resolve the DMA-safe memory allocation story, but until then let's disable the CCM. We still have as much RAM as the v1.x boards in this mode. --- nuttx/arch/arm/src/stm32/stm32f40xxx_dma.c | 2 +- nuttx/configs/px4fmuv2/nsh/defconfig | 9 +++++---- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/nuttx/arch/arm/src/stm32/stm32f40xxx_dma.c b/nuttx/arch/arm/src/stm32/stm32f40xxx_dma.c index 40fce8cb54..5140ee4f92 100644 --- a/nuttx/arch/arm/src/stm32/stm32f40xxx_dma.c +++ b/nuttx/arch/arm/src/stm32/stm32f40xxx_dma.c @@ -130,7 +130,7 @@ static struct stm32_dma_s g_dma[DMA_NSTREAMS] = .stream = 3, .irq = STM32_IRQ_DMA1S3, .shift = DMA_INT_STREAM3_SHIFT, - .base = STM32_DMA1_BASE + STM32_DMA_OFFSET(4), + .base = STM32_DMA1_BASE + STM32_DMA_OFFSET(3), }, { .stream = 4, diff --git a/nuttx/configs/px4fmuv2/nsh/defconfig b/nuttx/configs/px4fmuv2/nsh/defconfig index 8359980727..307c5079cd 100755 --- a/nuttx/configs/px4fmuv2/nsh/defconfig +++ b/nuttx/configs/px4fmuv2/nsh/defconfig @@ -118,6 +118,7 @@ CONFIG_STM32_JTAG_SW_ENABLE=y # to do this if DMA is enabled to prevent non-DMA-able CCM memory from # being a part of the stack. # +CONFIG_STM32_CCMEXCLUDE=y # # On-board FSMC SRAM configuration @@ -796,16 +797,16 @@ CONFIG_FS_BINFS=y # CONFIG_MMCSD=y -CONFIG_MMCSD_SPI=y +#CONFIG_MMCSD_SPI=y CONFIG_MMCSD_SDIO=y CONFIG_MTD=y # # SPI-based MMC/SD driver # -CONFIG_MMCSD_NSLOTS=1 -CONFIG_MMCSD_READONLY=n -CONFIG_MMCSD_SPICLOCK=12500000 +#CONFIG_MMCSD_NSLOTS=1 +#CONFIG_MMCSD_READONLY=n +#CONFIG_MMCSD_SPICLOCK=12500000 # # STM32 SDIO-based MMC/SD driver From c52278f67942733ac3d462e8a91a01b22b913d40 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 23 Apr 2013 12:35:19 +0200 Subject: [PATCH 083/486] Allowed board to init properly as intended with or without SPI2 --- src/drivers/boards/px4fmu/px4fmu_init.c | 59 ++++++++++--------------- 1 file changed, 24 insertions(+), 35 deletions(-) diff --git a/src/drivers/boards/px4fmu/px4fmu_init.c b/src/drivers/boards/px4fmu/px4fmu_init.c index 96c7fa2df3..5dd70c3f95 100644 --- a/src/drivers/boards/px4fmu/px4fmu_init.c +++ b/src/drivers/boards/px4fmu/px4fmu_init.c @@ -148,16 +148,10 @@ __EXPORT int nsh_archinitialize(void) { int result; -// /* configure ADC pins */ -// stm32_configgpio(GPIO_ADC1_IN10); -// stm32_configgpio(GPIO_ADC1_IN11); -// stm32_configgpio(GPIO_ADC1_IN12); - -// /* configure power supply control pins */ -// stm32_configgpio(GPIO_VDD_5V_PERIPH_EN); -// stm32_configgpio(GPIO_VDD_2V8_SENSORS_EN); -// stm32_configgpio(GPIO_VDD_5V_HIPOWER_OC); -// stm32_configgpio(GPIO_VDD_5V_PERIPH_OC); + /* configure always-on ADC pins */ + stm32_configgpio(GPIO_ADC1_IN10); + stm32_configgpio(GPIO_ADC1_IN11); + /* IN12 and IN13 further below */ /* configure the high-resolution time/callout interface */ hrt_init(); @@ -200,39 +194,39 @@ __EXPORT int nsh_archinitialize(void) return -ENODEV; } - // Default SPI1 to 1MHz and de-assert the known chip selects. + /* Default SPI1 to 1MHz and de-assert the known chip selects. */ SPI_SETFREQUENCY(spi1, 10000000); SPI_SETBITS(spi1, 8); SPI_SETMODE(spi1, SPIDEV_MODE3); SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false); - SPI_SELECT(spi1, PX4_SPIDEV_ACCEL_MAG, false); -// SPI_SELECT(spi1, PX4_SPIDEV_ACCEL, false); -// SPI_SELECT(spi1, PX4_SPIDEV_MPU, false); + SPI_SELECT(spi1, PX4_SPIDEV_ACCEL, false); + SPI_SELECT(spi1, PX4_SPIDEV_MPU, false); up_udelay(20); message("[boot] Successfully initialized SPI port 1\r\n"); - + /* + * If SPI2 is enabled in the defconfig, we loose some ADC pins as chip selects. + * Keep the SPI2 init optional and conditionally initialize the ADC pins + */ spi2 = up_spiinitialize(2); if (!spi2) { - message("[boot] FAILED to initialize SPI port 2\r\n"); - up_ledon(LED_AMBER); - return -ENODEV; + message("[boot] Enabling IN12/13 instead of SPI2\n"); + /* no SPI2, use pins for ADC */ + stm32_configgpio(GPIO_ADC1_IN12); + stm32_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards + } else { + /* Default SPI2 to 1MHz and de-assert the known chip selects. */ + SPI_SETFREQUENCY(spi2, 10000000); + SPI_SETBITS(spi2, 8); + SPI_SETMODE(spi2, SPIDEV_MODE3); + SPI_SELECT(spi2, PX4_SPIDEV_GYRO, false); + SPI_SELECT(spi2, PX4_SPIDEV_ACCEL_MAG, false); + + message("[boot] Initialized SPI port2 (ADC IN12/13 blocked)\n"); } - // Default SPI1 to 1MHz and de-assert the known chip selects. - SPI_SETFREQUENCY(spi2, 10000000); - SPI_SETBITS(spi2, 8); - SPI_SETMODE(spi2, SPIDEV_MODE3); - SPI_SELECT(spi2, PX4_SPIDEV_GYRO, false); - SPI_SELECT(spi2, PX4_SPIDEV_ACCEL_MAG, false); -// SPI_SELECT(spi2, PX4_SPIDEV_ACCEL, false); -// SPI_SELECT(spi2, PX4_SPIDEV_MPU, false); - up_udelay(20); - - message("[boot] Successfully initialized SPI port2\r\n"); - /* Get the SPI port for the microSD slot */ message("[boot] Initializing SPI port 3\n"); @@ -257,10 +251,5 @@ __EXPORT int nsh_archinitialize(void) message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n"); - stm32_configgpio(GPIO_ADC1_IN10); - stm32_configgpio(GPIO_ADC1_IN11); -// stm32_configgpio(GPIO_ADC1_IN12); -// stm32_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards - return OK; } From 3ecdca41e504cbf49b03fb543239813886590bd1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 23 Apr 2013 12:36:26 +0200 Subject: [PATCH 084/486] Cut over attitude estimator to new-style config for all boards --- apps/attitude_estimator_ekf/Makefile | 56 ------------------- makefiles/config_px4fmu_default.mk | 6 +- makefiles/config_px4fmuv2_default.mk | 6 +- .../attitude_estimator_ekf_main.c | 1 - .../attitude_estimator_ekf_params.c | 0 .../attitude_estimator_ekf_params.h | 0 .../codegen/attitudeKalmanfilter.c | 0 .../codegen/attitudeKalmanfilter.h | 0 .../codegen/attitudeKalmanfilter_initialize.c | 0 .../codegen/attitudeKalmanfilter_initialize.h | 0 .../codegen/attitudeKalmanfilter_terminate.c | 0 .../codegen/attitudeKalmanfilter_terminate.h | 0 .../codegen/attitudeKalmanfilter_types.h | 0 .../attitude_estimator_ekf/codegen/cross.c | 0 .../attitude_estimator_ekf/codegen/cross.h | 0 .../attitude_estimator_ekf/codegen/eye.c | 0 .../attitude_estimator_ekf/codegen/eye.h | 0 .../attitude_estimator_ekf/codegen/mrdivide.c | 0 .../attitude_estimator_ekf/codegen/mrdivide.h | 0 .../attitude_estimator_ekf/codegen/norm.c | 0 .../attitude_estimator_ekf/codegen/norm.h | 0 .../attitude_estimator_ekf/codegen/rdivide.c | 0 .../attitude_estimator_ekf/codegen/rdivide.h | 0 .../attitude_estimator_ekf/codegen/rtGetInf.c | 0 .../attitude_estimator_ekf/codegen/rtGetInf.h | 0 .../attitude_estimator_ekf/codegen/rtGetNaN.c | 0 .../attitude_estimator_ekf/codegen/rtGetNaN.h | 0 .../codegen/rt_defines.h | 0 .../codegen/rt_nonfinite.c | 0 .../codegen/rt_nonfinite.h | 0 .../attitude_estimator_ekf/codegen/rtwtypes.h | 0 src/modules/attitude_estimator_ekf/module.mk | 16 ++++++ 32 files changed, 26 insertions(+), 59 deletions(-) delete mode 100755 apps/attitude_estimator_ekf/Makefile rename {apps => src/modules}/attitude_estimator_ekf/attitude_estimator_ekf_main.c (99%) rename {apps => src/modules}/attitude_estimator_ekf/attitude_estimator_ekf_params.c (100%) rename {apps => src/modules}/attitude_estimator_ekf/attitude_estimator_ekf_params.h (100%) rename {apps => src/modules}/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c (100%) rename {apps => src/modules}/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h (100%) rename {apps => src/modules}/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c (100%) rename {apps => src/modules}/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h (100%) rename {apps => src/modules}/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c (100%) rename {apps => src/modules}/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h (100%) rename {apps => src/modules}/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h (100%) rename {apps => src/modules}/attitude_estimator_ekf/codegen/cross.c (100%) rename {apps => src/modules}/attitude_estimator_ekf/codegen/cross.h (100%) rename {apps => src/modules}/attitude_estimator_ekf/codegen/eye.c (100%) rename {apps => src/modules}/attitude_estimator_ekf/codegen/eye.h (100%) rename {apps => src/modules}/attitude_estimator_ekf/codegen/mrdivide.c (100%) rename {apps => src/modules}/attitude_estimator_ekf/codegen/mrdivide.h (100%) rename {apps => src/modules}/attitude_estimator_ekf/codegen/norm.c (100%) rename {apps => src/modules}/attitude_estimator_ekf/codegen/norm.h (100%) rename {apps => src/modules}/attitude_estimator_ekf/codegen/rdivide.c (100%) rename {apps => src/modules}/attitude_estimator_ekf/codegen/rdivide.h (100%) rename {apps => src/modules}/attitude_estimator_ekf/codegen/rtGetInf.c (100%) rename {apps => src/modules}/attitude_estimator_ekf/codegen/rtGetInf.h (100%) rename {apps => src/modules}/attitude_estimator_ekf/codegen/rtGetNaN.c (100%) rename {apps => src/modules}/attitude_estimator_ekf/codegen/rtGetNaN.h (100%) rename {apps => src/modules}/attitude_estimator_ekf/codegen/rt_defines.h (100%) rename {apps => src/modules}/attitude_estimator_ekf/codegen/rt_nonfinite.c (100%) rename {apps => src/modules}/attitude_estimator_ekf/codegen/rt_nonfinite.h (100%) rename {apps => src/modules}/attitude_estimator_ekf/codegen/rtwtypes.h (100%) create mode 100644 src/modules/attitude_estimator_ekf/module.mk diff --git a/apps/attitude_estimator_ekf/Makefile b/apps/attitude_estimator_ekf/Makefile deleted file mode 100755 index 734af7cc1c..0000000000 --- a/apps/attitude_estimator_ekf/Makefile +++ /dev/null @@ -1,56 +0,0 @@ -############################################################################ -# -# Copyright (C) 2012 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. -# -############################################################################ - -APPNAME = attitude_estimator_ekf -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 - -CSRCS = attitude_estimator_ekf_main.c \ - attitude_estimator_ekf_params.c \ - codegen/eye.c \ - codegen/attitudeKalmanfilter.c \ - codegen/mrdivide.c \ - codegen/rdivide.c \ - codegen/attitudeKalmanfilter_initialize.c \ - codegen/attitudeKalmanfilter_terminate.c \ - codegen/rt_nonfinite.c \ - codegen/rtGetInf.c \ - codegen/rtGetNaN.c \ - codegen/norm.c \ - codegen/cross.c - - -# XXX this is *horribly* broken -INCLUDES += $(TOPDIR)/../mavlink/include/mavlink - -include $(APPDIR)/mk/app.mk diff --git a/makefiles/config_px4fmu_default.mk b/makefiles/config_px4fmu_default.mk index 4dd2a2257d..c7109f213c 100644 --- a/makefiles/config_px4fmu_default.mk +++ b/makefiles/config_px4fmu_default.mk @@ -16,6 +16,11 @@ MODULES += drivers/lsm303d MODULES += drivers/l3gd20 MODULES += systemcmds/eeprom +# +# Estimation modules (EKF / other filters) +# +MODULES += modules/attitude_estimator_ekf + # # Transitional support - add commands from the NuttX export archive. # @@ -32,7 +37,6 @@ endef BUILTIN_COMMANDS := \ $(call _B, adc, , 2048, adc_main ) \ $(call _B, ardrone_interface, SCHED_PRIORITY_MAX-15, 2048, ardrone_interface_main ) \ - $(call _B, attitude_estimator_ekf, , 2048, attitude_estimator_ekf_main) \ $(call _B, bl_update, , 4096, bl_update_main ) \ $(call _B, blinkm, , 2048, blinkm_main ) \ $(call _B, bma180, , 2048, bma180_main ) \ diff --git a/makefiles/config_px4fmuv2_default.mk b/makefiles/config_px4fmuv2_default.mk index f86604a735..659b9c95be 100644 --- a/makefiles/config_px4fmuv2_default.mk +++ b/makefiles/config_px4fmuv2_default.mk @@ -17,6 +17,11 @@ MODULES += drivers/px4fmu MODULES += drivers/rgbled MODULES += systemcmds/ramtron +# +# Estimation modules (EKF / other filters) +# +MODULES += modules/attitude_estimator_ekf + # # Transitional support - add commands from the NuttX export archive. # @@ -32,7 +37,6 @@ endef # command priority stack entrypoint BUILTIN_COMMANDS := \ $(call _B, adc, , 2048, adc_main ) \ - $(call _B, attitude_estimator_ekf, , 2048, attitude_estimator_ekf_main) \ $(call _B, bl_update, , 4096, bl_update_main ) \ $(call _B, blinkm, , 2048, blinkm_main ) \ $(call _B, boardinfo, , 2048, boardinfo_main ) \ diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.c similarity index 99% rename from apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c rename to src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.c index bd972f03f3..9fc4dfc838 100755 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -46,7 +46,6 @@ #include #include #include -#include #include #include #include diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c similarity index 100% rename from apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c rename to src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h similarity index 100% rename from apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h rename to src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c similarity index 100% rename from apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c rename to src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h similarity index 100% rename from apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h rename to src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c similarity index 100% rename from apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c rename to src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h similarity index 100% rename from apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h rename to src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c similarity index 100% rename from apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c rename to src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h similarity index 100% rename from apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h rename to src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h similarity index 100% rename from apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h rename to src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h diff --git a/apps/attitude_estimator_ekf/codegen/cross.c b/src/modules/attitude_estimator_ekf/codegen/cross.c similarity index 100% rename from apps/attitude_estimator_ekf/codegen/cross.c rename to src/modules/attitude_estimator_ekf/codegen/cross.c diff --git a/apps/attitude_estimator_ekf/codegen/cross.h b/src/modules/attitude_estimator_ekf/codegen/cross.h similarity index 100% rename from apps/attitude_estimator_ekf/codegen/cross.h rename to src/modules/attitude_estimator_ekf/codegen/cross.h diff --git a/apps/attitude_estimator_ekf/codegen/eye.c b/src/modules/attitude_estimator_ekf/codegen/eye.c similarity index 100% rename from apps/attitude_estimator_ekf/codegen/eye.c rename to src/modules/attitude_estimator_ekf/codegen/eye.c diff --git a/apps/attitude_estimator_ekf/codegen/eye.h b/src/modules/attitude_estimator_ekf/codegen/eye.h similarity index 100% rename from apps/attitude_estimator_ekf/codegen/eye.h rename to src/modules/attitude_estimator_ekf/codegen/eye.h diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.c b/src/modules/attitude_estimator_ekf/codegen/mrdivide.c similarity index 100% rename from apps/attitude_estimator_ekf/codegen/mrdivide.c rename to src/modules/attitude_estimator_ekf/codegen/mrdivide.c diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.h b/src/modules/attitude_estimator_ekf/codegen/mrdivide.h similarity index 100% rename from apps/attitude_estimator_ekf/codegen/mrdivide.h rename to src/modules/attitude_estimator_ekf/codegen/mrdivide.h diff --git a/apps/attitude_estimator_ekf/codegen/norm.c b/src/modules/attitude_estimator_ekf/codegen/norm.c similarity index 100% rename from apps/attitude_estimator_ekf/codegen/norm.c rename to src/modules/attitude_estimator_ekf/codegen/norm.c diff --git a/apps/attitude_estimator_ekf/codegen/norm.h b/src/modules/attitude_estimator_ekf/codegen/norm.h similarity index 100% rename from apps/attitude_estimator_ekf/codegen/norm.h rename to src/modules/attitude_estimator_ekf/codegen/norm.h diff --git a/apps/attitude_estimator_ekf/codegen/rdivide.c b/src/modules/attitude_estimator_ekf/codegen/rdivide.c similarity index 100% rename from apps/attitude_estimator_ekf/codegen/rdivide.c rename to src/modules/attitude_estimator_ekf/codegen/rdivide.c diff --git a/apps/attitude_estimator_ekf/codegen/rdivide.h b/src/modules/attitude_estimator_ekf/codegen/rdivide.h similarity index 100% rename from apps/attitude_estimator_ekf/codegen/rdivide.h rename to src/modules/attitude_estimator_ekf/codegen/rdivide.h diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.c b/src/modules/attitude_estimator_ekf/codegen/rtGetInf.c similarity index 100% rename from apps/attitude_estimator_ekf/codegen/rtGetInf.c rename to src/modules/attitude_estimator_ekf/codegen/rtGetInf.c diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.h b/src/modules/attitude_estimator_ekf/codegen/rtGetInf.h similarity index 100% rename from apps/attitude_estimator_ekf/codegen/rtGetInf.h rename to src/modules/attitude_estimator_ekf/codegen/rtGetInf.h diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.c b/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c similarity index 100% rename from apps/attitude_estimator_ekf/codegen/rtGetNaN.c rename to src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.h b/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h similarity index 100% rename from apps/attitude_estimator_ekf/codegen/rtGetNaN.h rename to src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h diff --git a/apps/attitude_estimator_ekf/codegen/rt_defines.h b/src/modules/attitude_estimator_ekf/codegen/rt_defines.h similarity index 100% rename from apps/attitude_estimator_ekf/codegen/rt_defines.h rename to src/modules/attitude_estimator_ekf/codegen/rt_defines.h diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c b/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c similarity index 100% rename from apps/attitude_estimator_ekf/codegen/rt_nonfinite.c rename to src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h b/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h similarity index 100% rename from apps/attitude_estimator_ekf/codegen/rt_nonfinite.h rename to src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h diff --git a/apps/attitude_estimator_ekf/codegen/rtwtypes.h b/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h similarity index 100% rename from apps/attitude_estimator_ekf/codegen/rtwtypes.h rename to src/modules/attitude_estimator_ekf/codegen/rtwtypes.h diff --git a/src/modules/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk new file mode 100644 index 0000000000..4033617c4b --- /dev/null +++ b/src/modules/attitude_estimator_ekf/module.mk @@ -0,0 +1,16 @@ + +MODULE_NAME = attitude_estimator_ekf +SRCS = attitude_estimator_ekf_main.c \ + attitude_estimator_ekf_params.c \ + codegen/attitudeKalmanfilter_initialize.c \ + codegen/attitudeKalmanfilter_terminate.c \ + codegen/attitudeKalmanfilter.c \ + codegen/cross.c \ + codegen/eye.c \ + codegen/mrdivide.c \ + codegen/norm.c \ + codegen/rdivide.c \ + codegen/rt_nonfinite.c \ + codegen/rtGetInf.c \ + codegen/rtGetNaN.c + From 0c6eaae8632f3a6520afdaf60adf13a8b6e55566 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 24 Apr 2013 18:42:34 +0400 Subject: [PATCH 085/486] Estimate accelerometers offset in position_estimator_inav --- .../position_estimator_inav_main.c | 74 +++++++++++-------- .../position_estimator_inav_params.c | 8 ++ .../position_estimator_inav_params.h | 3 + 3 files changed, 53 insertions(+), 32 deletions(-) diff --git a/apps/position_estimator_inav/position_estimator_inav_main.c b/apps/position_estimator_inav/position_estimator_inav_main.c index 97d669612b..6e8c0ab5fe 100644 --- a/apps/position_estimator_inav/position_estimator_inav_main.c +++ b/apps/position_estimator_inav/position_estimator_inav_main.c @@ -280,6 +280,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { static float x_est[3] = { 0.0f, 0.0f, 0.0f }; static float y_est[3] = { 0.0f, 0.0f, 0.0f }; static float z_est[3] = { 0.0f, 0.0f, 0.0f }; + float accel_offs_est[3] = { 0.0f, 0.0f, 0.0f }; int baro_loop_cnt = 0; int baro_loop_end = 70; /* measurement for 1 second */ @@ -471,36 +472,45 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { hrt_abstime t = hrt_absolute_time(); float dt = (t - last_time) / 1000000.0; last_time = t; - /* calculate corrected acceleration vector in UAV frame */ - float accel_corr[3]; - acceleration_correct(accel_corr, sensor.accelerometer_raw, - params.acc_T, params.acc_offs); - /* transform acceleration vector from UAV frame to NED frame */ - float accel_NED[3]; - for (int i = 0; i < 3; i++) { - accel_NED[i] = 0.0f; - for (int j = 0; j < 3; j++) { - accel_NED[i] += att.R[i][j] * accel_corr[j]; + if (att.R_valid) { + /* calculate corrected acceleration vector in UAV frame */ + float accel_corr[3]; + acceleration_correct(accel_corr, sensor.accelerometer_raw, + params.acc_T, params.acc_offs); + /* transform acceleration vector from UAV frame to NED frame */ + float accel_NED[3]; + for (int i = 0; i < 3; i++) { + accel_NED[i] = 0.0f; + for (int j = 0; j < 3; j++) { + accel_NED[i] += att.R[i][j] * (accel_corr[j] - accel_offs_est[j]); + } + } + accel_NED[2] += const_earth_gravity; + /* accelerometers offset drift correction: rotate acceleration error back to UAV frame and integrate */ + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + /* the inverse of a rotation matrix is its transpose, just swap i and j */ + accel_offs_est[i] += att.R[j][i] * accel_NED[j] * params.acc_offs_w * dt; + } + } + /* kalman filter prediction */ + kalman_filter_inertial_predict(dt, z_est); + /* prepare vectors for kalman filter correction */ + float z_meas[2]; // position, acceleration + bool use_z[2] = { false, false }; + if (local_flag_baroINITdone && baro_updated) { + z_meas[0] = baro_alt0 - sensor.baro_alt_meter; // Z = -alt + use_z[0] = true; + } + if (accelerometer_updated) { + z_meas[1] = accel_NED[2]; + use_z[1] = true; + } + if (use_z[0] || use_z[1]) { + /* correction */ + kalman_filter_inertial_update(z_est, z_meas, params.k, + use_z); } - } - accel_NED[2] += const_earth_gravity; - /* kalman filter prediction */ - kalman_filter_inertial_predict(dt, z_est); - /* prepare vectors for kalman filter correction */ - float z_meas[2]; // position, acceleration - bool use_z[2] = { false, false }; - if (local_flag_baroINITdone && baro_updated) { - z_meas[0] = baro_alt0 - sensor.baro_alt_meter; // Z = -alt - use_z[0] = true; - } - if (accelerometer_updated) { - z_meas[1] = accel_NED[2]; - use_z[1] = true; - } - if (use_z[0] || use_z[1]) { - /* correction */ - kalman_filter_inertial_update(z_est, z_meas, params.k, - use_z); } if (verbose_mode) { /* print updates rate */ @@ -521,9 +531,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } if (t - pub_last > pub_interval) { pub_last = t; - pos.x = 0.0f; - pos.vx = 0.0f; - pos.y = 0.0f; + pos.x = accel_offs_est[0]; + pos.vx = accel_offs_est[1]; + pos.y = accel_offs_est[2]; pos.vy = 0.0f; pos.z = z_est[0]; pos.vz = z_est[1]; diff --git a/apps/position_estimator_inav/position_estimator_inav_params.c b/apps/position_estimator_inav/position_estimator_inav_params.c index d3a165947c..8cd9844c7d 100644 --- a/apps/position_estimator_inav/position_estimator_inav_params.c +++ b/apps/position_estimator_inav/position_estimator_inav_params.c @@ -39,6 +39,7 @@ */ #include "position_estimator_inav_params.h" +#include "acceleration_transform.h" /* Kalman Filter covariances */ /* gps measurement noise standard deviation */ @@ -51,6 +52,8 @@ PARAM_DEFINE_FLOAT(INAV_K_ALT_11, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_ALT_20, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_ALT_21, 0.0f); +PARAM_DEFINE_FLOAT(INAV_ACC_OFFS_W, 0.0f); + PARAM_DEFINE_INT32(INAV_ACC_OFFS_0, 0); PARAM_DEFINE_INT32(INAV_ACC_OFFS_1, 0); PARAM_DEFINE_INT32(INAV_ACC_OFFS_2, 0); @@ -75,6 +78,8 @@ int parameters_init(struct position_estimator_inav_param_handles *h) { h->k_alt_20 = param_find("INAV_K_ALT_20"); h->k_alt_21 = param_find("INAV_K_ALT_21"); + h->acc_offs_w = param_find("INAV_ACC_OFFS_W"); + h->acc_offs_0 = param_find("INAV_ACC_OFFS_0"); h->acc_offs_1 = param_find("INAV_ACC_OFFS_1"); h->acc_offs_2 = param_find("INAV_ACC_OFFS_2"); @@ -101,6 +106,8 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->k_alt_20, &(p->k[2][0])); param_get(h->k_alt_21, &(p->k[2][1])); + param_get(h->acc_offs_w, &(p->acc_offs_w)); + /* read int32 and cast to int16 */ int32_t t; param_get(h->acc_offs_0, &t); @@ -119,5 +126,6 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->acc_t_20, &(p->acc_T[2][0])); param_get(h->acc_t_21, &(p->acc_T[2][1])); param_get(h->acc_t_22, &(p->acc_T[2][2])); + return OK; } diff --git a/apps/position_estimator_inav/position_estimator_inav_params.h b/apps/position_estimator_inav/position_estimator_inav_params.h index 51e57a9146..2c6fb3df2e 100644 --- a/apps/position_estimator_inav/position_estimator_inav_params.h +++ b/apps/position_estimator_inav/position_estimator_inav_params.h @@ -43,6 +43,7 @@ struct position_estimator_inav_params { int use_gps; float k[3][2]; + float acc_offs_w; int16_t acc_offs[3]; float acc_T[3][3]; }; @@ -57,6 +58,8 @@ struct position_estimator_inav_param_handles { param_t k_alt_20; param_t k_alt_21; + param_t acc_offs_w; + param_t acc_offs_0; param_t acc_offs_1; param_t acc_offs_2; From 98a2445065383015dfe2134a384ebdf3fb7dfc26 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 24 Apr 2013 17:45:08 -0700 Subject: [PATCH 086/486] Include the correct Make.defs file --- nuttx/configs/px4fmuv2/nsh/Make.defs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx/configs/px4fmuv2/nsh/Make.defs b/nuttx/configs/px4fmuv2/nsh/Make.defs index 3e6f88bd3a..3306e4bf10 100644 --- a/nuttx/configs/px4fmuv2/nsh/Make.defs +++ b/nuttx/configs/px4fmuv2/nsh/Make.defs @@ -1,3 +1,3 @@ include ${TOPDIR}/.config -include $(TOPDIR)/configs/px4fmu/common/Make.defs +include $(TOPDIR)/configs/px4fmuv2/common/Make.defs From aa85fba9796a7eda6874a2719e0bab7cfa2897ff Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 26 Apr 2013 11:01:47 +0200 Subject: [PATCH 087/486] Ported AR.Drone interface to new style config --- makefiles/config_px4fmu_default.mk | 2 +- .../drivers}/ardrone_interface/ardrone_interface.c | 0 .../ardrone_interface/ardrone_motor_control.c | 0 .../ardrone_interface/ardrone_motor_control.h | 0 .../drivers/ardrone_interface/module.mk | 12 +++++------- 5 files changed, 6 insertions(+), 8 deletions(-) rename {apps => src/drivers}/ardrone_interface/ardrone_interface.c (100%) rename {apps => src/drivers}/ardrone_interface/ardrone_motor_control.c (100%) rename {apps => src/drivers}/ardrone_interface/ardrone_motor_control.h (100%) rename apps/ardrone_interface/Makefile => src/drivers/ardrone_interface/module.mk (88%) diff --git a/makefiles/config_px4fmu_default.mk b/makefiles/config_px4fmu_default.mk index c7109f213c..1302d15821 100644 --- a/makefiles/config_px4fmu_default.mk +++ b/makefiles/config_px4fmu_default.mk @@ -14,6 +14,7 @@ MODULES += drivers/px4fmu MODULES += drivers/boards/px4fmu MODULES += drivers/lsm303d MODULES += drivers/l3gd20 +MODULES += drivers/ardrone_interface MODULES += systemcmds/eeprom # @@ -36,7 +37,6 @@ endef # command priority stack entrypoint BUILTIN_COMMANDS := \ $(call _B, adc, , 2048, adc_main ) \ - $(call _B, ardrone_interface, SCHED_PRIORITY_MAX-15, 2048, ardrone_interface_main ) \ $(call _B, bl_update, , 4096, bl_update_main ) \ $(call _B, blinkm, , 2048, blinkm_main ) \ $(call _B, bma180, , 2048, bma180_main ) \ diff --git a/apps/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c similarity index 100% rename from apps/ardrone_interface/ardrone_interface.c rename to src/drivers/ardrone_interface/ardrone_interface.c diff --git a/apps/ardrone_interface/ardrone_motor_control.c b/src/drivers/ardrone_interface/ardrone_motor_control.c similarity index 100% rename from apps/ardrone_interface/ardrone_motor_control.c rename to src/drivers/ardrone_interface/ardrone_motor_control.c diff --git a/apps/ardrone_interface/ardrone_motor_control.h b/src/drivers/ardrone_interface/ardrone_motor_control.h similarity index 100% rename from apps/ardrone_interface/ardrone_motor_control.h rename to src/drivers/ardrone_interface/ardrone_motor_control.h diff --git a/apps/ardrone_interface/Makefile b/src/drivers/ardrone_interface/module.mk similarity index 88% rename from apps/ardrone_interface/Makefile rename to src/drivers/ardrone_interface/module.mk index fea96082f6..058bd1397d 100644 --- a/apps/ardrone_interface/Makefile +++ b/src/drivers/ardrone_interface/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -32,11 +32,9 @@ ############################################################################ # -# Makefile to build ardrone interface +# AR.Drone motor driver # -APPNAME = ardrone_interface -PRIORITY = SCHED_PRIORITY_MAX - 15 -STACKSIZE = 2048 - -include $(APPDIR)/mk/app.mk +MODULE_COMMAND = ardrone_interface +SRCS = ardrone_interface.c \ + ardrone_motor_control.c From 9169ceb3f4884a863d527c6b8e7ea237b41a48ce Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 26 Apr 2013 11:10:39 +0200 Subject: [PATCH 088/486] Cut over commander app to new build system --- apps/commander/.context | 0 makefiles/config_px4fmu_default.mk | 6 +++++- makefiles/config_px4fmuv2_default.mk | 5 +++++ .../modules}/commander/calibration_routines.c | 0 .../modules}/commander/calibration_routines.h | 0 {apps => src/modules}/commander/commander.c | 0 {apps => src/modules}/commander/commander.h | 0 .../Makefile => src/modules/commander/module.mk | 16 ++++++---------- .../modules}/commander/state_machine_helper.c | 0 .../modules}/commander/state_machine_helper.h | 0 10 files changed, 16 insertions(+), 11 deletions(-) delete mode 100644 apps/commander/.context rename {apps => src/modules}/commander/calibration_routines.c (100%) rename {apps => src/modules}/commander/calibration_routines.h (100%) rename {apps => src/modules}/commander/commander.c (100%) rename {apps => src/modules}/commander/commander.h (100%) rename apps/commander/Makefile => src/modules/commander/module.mk (86%) rename {apps => src/modules}/commander/state_machine_helper.c (100%) rename {apps => src/modules}/commander/state_machine_helper.h (100%) diff --git a/apps/commander/.context b/apps/commander/.context deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/makefiles/config_px4fmu_default.mk b/makefiles/config_px4fmu_default.mk index 1302d15821..6c43377e39 100644 --- a/makefiles/config_px4fmu_default.mk +++ b/makefiles/config_px4fmu_default.mk @@ -17,6 +17,11 @@ MODULES += drivers/l3gd20 MODULES += drivers/ardrone_interface MODULES += systemcmds/eeprom +# +# General system control +# +MODULES += modules/commander + # # Estimation modules (EKF / other filters) # @@ -41,7 +46,6 @@ BUILTIN_COMMANDS := \ $(call _B, blinkm, , 2048, blinkm_main ) \ $(call _B, bma180, , 2048, bma180_main ) \ $(call _B, boardinfo, , 2048, boardinfo_main ) \ - $(call _B, commander, SCHED_PRIORITY_MAX-30, 2048, commander_main ) \ $(call _B, control_demo, , 2048, control_demo_main ) \ $(call _B, delay_test, , 2048, delay_test_main ) \ $(call _B, fixedwing_att_control, SCHED_PRIORITY_MAX-30, 2048, fixedwing_att_control_main ) \ diff --git a/makefiles/config_px4fmuv2_default.mk b/makefiles/config_px4fmuv2_default.mk index 659b9c95be..fd69baa29a 100644 --- a/makefiles/config_px4fmuv2_default.mk +++ b/makefiles/config_px4fmuv2_default.mk @@ -17,6 +17,11 @@ MODULES += drivers/px4fmu MODULES += drivers/rgbled MODULES += systemcmds/ramtron +# +# General system control +# +MODULES += modules/commander + # # Estimation modules (EKF / other filters) # diff --git a/apps/commander/calibration_routines.c b/src/modules/commander/calibration_routines.c similarity index 100% rename from apps/commander/calibration_routines.c rename to src/modules/commander/calibration_routines.c diff --git a/apps/commander/calibration_routines.h b/src/modules/commander/calibration_routines.h similarity index 100% rename from apps/commander/calibration_routines.h rename to src/modules/commander/calibration_routines.h diff --git a/apps/commander/commander.c b/src/modules/commander/commander.c similarity index 100% rename from apps/commander/commander.c rename to src/modules/commander/commander.c diff --git a/apps/commander/commander.h b/src/modules/commander/commander.h similarity index 100% rename from apps/commander/commander.h rename to src/modules/commander/commander.h diff --git a/apps/commander/Makefile b/src/modules/commander/module.mk similarity index 86% rename from apps/commander/Makefile rename to src/modules/commander/module.mk index d12697274b..b90da8289f 100644 --- a/apps/commander/Makefile +++ b/src/modules/commander/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -32,14 +32,10 @@ ############################################################################ # -# Commander application +# Main system state machine # -APPNAME = commander -PRIORITY = SCHED_PRIORITY_MAX - 30 -STACKSIZE = 2048 - -INCLUDES = $(TOPDIR)/../mavlink/include/mavlink - -include $(APPDIR)/mk/app.mk - +MODULE_COMMAND = commander +SRCS = commander.c \ + state_machine_helper.c \ + calibration_routines.c diff --git a/apps/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c similarity index 100% rename from apps/commander/state_machine_helper.c rename to src/modules/commander/state_machine_helper.c diff --git a/apps/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h similarity index 100% rename from apps/commander/state_machine_helper.h rename to src/modules/commander/state_machine_helper.h From a39d5230be4ab841cbdf1210a6ca62901bce12b4 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 26 Apr 2013 15:03:46 -0700 Subject: [PATCH 089/486] Cull .context files. --- .gitignore | 1 + apps/mavlink/.context | 0 apps/position_estimator/.context | 0 apps/px4/tests/.context | 0 apps/sensors/.context | 0 apps/systemcmds/boardinfo/.context | 0 apps/systemcmds/perf/.context | 0 apps/systemcmds/reboot/.context | 0 apps/systemcmds/top/.context | 0 apps/uORB/.context | 0 10 files changed, 1 insertion(+) delete mode 100644 apps/mavlink/.context delete mode 100644 apps/position_estimator/.context delete mode 100644 apps/px4/tests/.context delete mode 100644 apps/sensors/.context delete mode 100644 apps/systemcmds/boardinfo/.context delete mode 100644 apps/systemcmds/perf/.context delete mode 100644 apps/systemcmds/reboot/.context delete mode 100644 apps/systemcmds/top/.context delete mode 100644 apps/uORB/.context diff --git a/.gitignore b/.gitignore index 91358e1e18..de03b0a607 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,5 @@ .built +.context *.context *.bdat *.pdat diff --git a/apps/mavlink/.context b/apps/mavlink/.context deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/apps/position_estimator/.context b/apps/position_estimator/.context deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/apps/px4/tests/.context b/apps/px4/tests/.context deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/apps/sensors/.context b/apps/sensors/.context deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/apps/systemcmds/boardinfo/.context b/apps/systemcmds/boardinfo/.context deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/apps/systemcmds/perf/.context b/apps/systemcmds/perf/.context deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/apps/systemcmds/reboot/.context b/apps/systemcmds/reboot/.context deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/apps/systemcmds/top/.context b/apps/systemcmds/top/.context deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/apps/uORB/.context b/apps/uORB/.context deleted file mode 100644 index e69de29bb2..0000000000 From 68c8de4cf13dd4620d0a9a5a0069bc894fcd92e1 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 26 Apr 2013 15:04:04 -0700 Subject: [PATCH 090/486] Document INCLUDE_DIRS --- makefiles/module.mk | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/makefiles/module.mk b/makefiles/module.mk index e2a1041e06..538f6d318e 100644 --- a/makefiles/module.mk +++ b/makefiles/module.mk @@ -75,6 +75,12 @@ # the list should be formatted as: # ... # +# INCLUDE_DIRS (optional, must be appended) +# +# The list of directories searched for include files. If non-standard +# includes (e.g. those from another module) are required, paths to search +# can be added here. +# # DEFAULT_VISIBILITY (optional) # # If not set, global symbols defined in a module will not be visible From 4fe9e1c4472e3747df62d1808d727768ef3b3751 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 26 Apr 2013 15:05:12 -0700 Subject: [PATCH 091/486] Avoid spurious += in INCLUDE_DIRS --- makefiles/nuttx.mk | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/makefiles/nuttx.mk b/makefiles/nuttx.mk index 5186dc3efe..346735a02a 100644 --- a/makefiles/nuttx.mk +++ b/makefiles/nuttx.mk @@ -64,8 +64,8 @@ LDSCRIPT += $(NUTTX_EXPORT_DIR)build/ld.script # Add directories from the NuttX export to the relevant search paths # INCLUDE_DIRS += $(NUTTX_EXPORT_DIR)include \ - += $(NUTTX_EXPORT_DIR)arch/chip \ - += $(NUTTX_EXPORT_DIR)arch/common + $(NUTTX_EXPORT_DIR)arch/chip \ + $(NUTTX_EXPORT_DIR)arch/common LIB_DIRS += $(NUTTX_EXPORT_DIR)libs LIBS += -lapps -lnuttx From c71f4cf8690a707cd7385a90d826d9e0a5a351e2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 27 Apr 2013 00:10:20 +0200 Subject: [PATCH 092/486] Cut over MAVLink to new style build system --- makefiles/config_px4fmu_default.mk | 4 ++-- makefiles/config_px4fmuv2_default.mk | 4 ++-- {apps => src/include}/mavlink/mavlink_log.h | 0 {apps => src/modules}/mavlink/.context | 0 {apps => src/modules}/mavlink/.gitignore | 0 {apps => src/modules}/mavlink/mavlink.c | 2 +- .../modules}/mavlink/mavlink_bridge_header.h | 2 +- {apps => src/modules}/mavlink/mavlink_hil.h | 0 {apps => src/modules}/mavlink/mavlink_log.c | 2 +- .../modules}/mavlink/mavlink_parameters.c | 0 .../modules}/mavlink/mavlink_parameters.h | 0 .../modules}/mavlink/mavlink_receiver.c | 4 ++-- {apps => src/modules}/mavlink/missionlib.c | 2 +- {apps => src/modules}/mavlink/missionlib.h | 0 .../Makefile => src/modules/mavlink/module.mk | 19 +++++++++++-------- {apps => src/modules}/mavlink/orb_listener.c | 3 ++- {apps => src/modules}/mavlink/orb_topics.h | 0 {apps => src/modules}/mavlink/util.h | 0 {apps => src/modules}/mavlink/waypoints.c | 0 {apps => src/modules}/mavlink/waypoints.h | 0 .../modules}/mavlink_onboard/mavlink.c | 0 .../mavlink_onboard/mavlink_bridge_header.h | 2 +- .../mavlink_onboard/mavlink_receiver.c | 0 .../modules/mavlink_onboard/module.mk | 14 ++++++-------- .../modules}/mavlink_onboard/orb_topics.h | 0 {apps => src/modules}/mavlink_onboard/util.h | 0 26 files changed, 30 insertions(+), 28 deletions(-) rename {apps => src/include}/mavlink/mavlink_log.h (100%) rename {apps => src/modules}/mavlink/.context (100%) rename {apps => src/modules}/mavlink/.gitignore (100%) rename {apps => src/modules}/mavlink/mavlink.c (99%) rename {apps => src/modules}/mavlink/mavlink_bridge_header.h (98%) rename {apps => src/modules}/mavlink/mavlink_hil.h (100%) rename {apps => src/modules}/mavlink/mavlink_log.c (98%) rename {apps => src/modules}/mavlink/mavlink_parameters.c (100%) rename {apps => src/modules}/mavlink/mavlink_parameters.h (100%) rename {apps => src/modules}/mavlink/mavlink_receiver.c (99%) rename {apps => src/modules}/mavlink/missionlib.c (99%) rename {apps => src/modules}/mavlink/missionlib.h (100%) rename apps/mavlink_onboard/Makefile => src/modules/mavlink/module.mk (82%) rename {apps => src/modules}/mavlink/orb_listener.c (99%) rename {apps => src/modules}/mavlink/orb_topics.h (100%) rename {apps => src/modules}/mavlink/util.h (100%) rename {apps => src/modules}/mavlink/waypoints.c (100%) rename {apps => src/modules}/mavlink/waypoints.h (100%) rename {apps => src/modules}/mavlink_onboard/mavlink.c (100%) rename {apps => src/modules}/mavlink_onboard/mavlink_bridge_header.h (98%) rename {apps => src/modules}/mavlink_onboard/mavlink_receiver.c (100%) rename apps/mavlink/Makefile => src/modules/mavlink_onboard/module.mk (85%) rename {apps => src/modules}/mavlink_onboard/orb_topics.h (100%) rename {apps => src/modules}/mavlink_onboard/util.h (100%) diff --git a/makefiles/config_px4fmu_default.mk b/makefiles/config_px4fmu_default.mk index 6c43377e39..ada6b7ab7d 100644 --- a/makefiles/config_px4fmu_default.mk +++ b/makefiles/config_px4fmu_default.mk @@ -21,6 +21,8 @@ MODULES += systemcmds/eeprom # General system control # MODULES += modules/commander +MODULES += modules/mavlink +MODULES += modules/mavlink_onboard # # Estimation modules (EKF / other filters) @@ -56,8 +58,6 @@ BUILTIN_COMMANDS := \ $(call _B, hott_telemetry, , 2048, hott_telemetry_main ) \ $(call _B, kalman_demo, SCHED_PRIORITY_MAX-30, 2048, kalman_demo_main ) \ $(call _B, math_demo, , 8192, math_demo_main ) \ - $(call _B, mavlink, , 2048, mavlink_main ) \ - $(call _B, mavlink_onboard, , 2048, mavlink_onboard_main ) \ $(call _B, mixer, , 4096, mixer_main ) \ $(call _B, mpu6000, , 4096, mpu6000_main ) \ $(call _B, ms5611, , 2048, ms5611_main ) \ diff --git a/makefiles/config_px4fmuv2_default.mk b/makefiles/config_px4fmuv2_default.mk index fd69baa29a..9aa4ec3daf 100644 --- a/makefiles/config_px4fmuv2_default.mk +++ b/makefiles/config_px4fmuv2_default.mk @@ -21,6 +21,8 @@ MODULES += systemcmds/ramtron # General system control # MODULES += modules/commander +MODULES += modules/mavlink +MODULES += modules/mavlink_onboard # # Estimation modules (EKF / other filters) @@ -55,8 +57,6 @@ BUILTIN_COMMANDS := \ $(call _B, hott_telemetry, , 2048, hott_telemetry_main ) \ $(call _B, kalman_demo, SCHED_PRIORITY_MAX-30, 2048, kalman_demo_main ) \ $(call _B, math_demo, , 8192, math_demo_main ) \ - $(call _B, mavlink, , 2048, mavlink_main ) \ - $(call _B, mavlink_onboard, , 2048, mavlink_onboard_main ) \ $(call _B, mixer, , 4096, mixer_main ) \ $(call _B, multirotor_att_control, SCHED_PRIORITY_MAX-15, 2048, multirotor_att_control_main) \ $(call _B, multirotor_pos_control, SCHED_PRIORITY_MAX-25, 2048, multirotor_pos_control_main) \ diff --git a/apps/mavlink/mavlink_log.h b/src/include/mavlink/mavlink_log.h similarity index 100% rename from apps/mavlink/mavlink_log.h rename to src/include/mavlink/mavlink_log.h diff --git a/apps/mavlink/.context b/src/modules/mavlink/.context similarity index 100% rename from apps/mavlink/.context rename to src/modules/mavlink/.context diff --git a/apps/mavlink/.gitignore b/src/modules/mavlink/.gitignore similarity index 100% rename from apps/mavlink/.gitignore rename to src/modules/mavlink/.gitignore diff --git a/apps/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c similarity index 99% rename from apps/mavlink/mavlink.c rename to src/modules/mavlink/mavlink.c index 644b779af2..de78cd1399 100644 --- a/apps/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -64,9 +64,9 @@ #include #include #include +#include #include "waypoints.h" -#include "mavlink_log.h" #include "orb_topics.h" #include "missionlib.h" #include "mavlink_hil.h" diff --git a/apps/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h similarity index 98% rename from apps/mavlink/mavlink_bridge_header.h rename to src/modules/mavlink/mavlink_bridge_header.h index 270ec17274..0010bb341e 100644 --- a/apps/mavlink/mavlink_bridge_header.h +++ b/src/modules/mavlink/mavlink_bridge_header.h @@ -51,7 +51,7 @@ #define MAVLINK_GET_CHANNEL_BUFFER mavlink_get_channel_buffer #define MAVLINK_GET_CHANNEL_STATUS mavlink_get_channel_status -#include "v1.0/mavlink_types.h" +#include #include diff --git a/apps/mavlink/mavlink_hil.h b/src/modules/mavlink/mavlink_hil.h similarity index 100% rename from apps/mavlink/mavlink_hil.h rename to src/modules/mavlink/mavlink_hil.h diff --git a/apps/mavlink/mavlink_log.c b/src/modules/mavlink/mavlink_log.c similarity index 98% rename from apps/mavlink/mavlink_log.c rename to src/modules/mavlink/mavlink_log.c index ed65fb883c..fa974dc0b6 100644 --- a/apps/mavlink/mavlink_log.c +++ b/src/modules/mavlink/mavlink_log.c @@ -42,7 +42,7 @@ #include #include -#include "mavlink_log.h" +#include void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size) { diff --git a/apps/mavlink/mavlink_parameters.c b/src/modules/mavlink/mavlink_parameters.c similarity index 100% rename from apps/mavlink/mavlink_parameters.c rename to src/modules/mavlink/mavlink_parameters.c diff --git a/apps/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h similarity index 100% rename from apps/mavlink/mavlink_parameters.h rename to src/modules/mavlink/mavlink_parameters.h diff --git a/apps/mavlink/mavlink_receiver.c b/src/modules/mavlink/mavlink_receiver.c similarity index 99% rename from apps/mavlink/mavlink_receiver.c rename to src/modules/mavlink/mavlink_receiver.c index 22c2fcdade..e62e4dcc4c 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/src/modules/mavlink/mavlink_receiver.c @@ -64,9 +64,9 @@ #include #include +#include #include "waypoints.h" -#include "mavlink_log.h" #include "orb_topics.h" #include "missionlib.h" #include "mavlink_hil.h" @@ -312,7 +312,7 @@ handle_message(mavlink_message_t *msg) static const float ground_press = 1013.25f; // mbar static const float ground_tempC = 21.0f; static const float ground_alt = 0.0f; - static const float T0 = 273.15; + static const float T0 = 273.15f; static const float R = 287.05f; static const float g = 9.806f; diff --git a/apps/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c similarity index 99% rename from apps/mavlink/missionlib.c rename to src/modules/mavlink/missionlib.c index 8064febc47..d369e05ffa 100644 --- a/apps/mavlink/missionlib.c +++ b/src/modules/mavlink/missionlib.c @@ -62,9 +62,9 @@ #include #include +#include #include "waypoints.h" -#include "mavlink_log.h" #include "orb_topics.h" #include "missionlib.h" #include "mavlink_hil.h" diff --git a/apps/mavlink/missionlib.h b/src/modules/mavlink/missionlib.h similarity index 100% rename from apps/mavlink/missionlib.h rename to src/modules/mavlink/missionlib.h diff --git a/apps/mavlink_onboard/Makefile b/src/modules/mavlink/module.mk similarity index 82% rename from apps/mavlink_onboard/Makefile rename to src/modules/mavlink/module.mk index 8b1cb9b2c7..cbf08aeb20 100644 --- a/apps/mavlink_onboard/Makefile +++ b/src/modules/mavlink/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -32,13 +32,16 @@ ############################################################################ # -# Mavlink Application +# MAVLink protocol to uORB interface process # -APPNAME = mavlink_onboard -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 +MODULE_COMMAND = mavlink +SRCS += mavlink.c \ + missionlib.c \ + mavlink_parameters.c \ + mavlink_log.c \ + mavlink_receiver.c \ + orb_listener.c \ + waypoints.c -INCLUDES = $(TOPDIR)/../mavlink/include/mavlink - -include $(APPDIR)/mk/app.mk +INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/apps/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c similarity index 99% rename from apps/mavlink/orb_listener.c rename to src/modules/mavlink/orb_listener.c index 5f15facf87..295cd5e28c 100644 --- a/apps/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -56,8 +56,9 @@ #include #include +#include + #include "waypoints.h" -#include "mavlink_log.h" #include "orb_topics.h" #include "missionlib.h" #include "mavlink_hil.h" diff --git a/apps/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h similarity index 100% rename from apps/mavlink/orb_topics.h rename to src/modules/mavlink/orb_topics.h diff --git a/apps/mavlink/util.h b/src/modules/mavlink/util.h similarity index 100% rename from apps/mavlink/util.h rename to src/modules/mavlink/util.h diff --git a/apps/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c similarity index 100% rename from apps/mavlink/waypoints.c rename to src/modules/mavlink/waypoints.c diff --git a/apps/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h similarity index 100% rename from apps/mavlink/waypoints.h rename to src/modules/mavlink/waypoints.h diff --git a/apps/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c similarity index 100% rename from apps/mavlink_onboard/mavlink.c rename to src/modules/mavlink_onboard/mavlink.c diff --git a/apps/mavlink_onboard/mavlink_bridge_header.h b/src/modules/mavlink_onboard/mavlink_bridge_header.h similarity index 98% rename from apps/mavlink_onboard/mavlink_bridge_header.h rename to src/modules/mavlink_onboard/mavlink_bridge_header.h index bf7c5354cc..3ad3bb6175 100644 --- a/apps/mavlink_onboard/mavlink_bridge_header.h +++ b/src/modules/mavlink_onboard/mavlink_bridge_header.h @@ -51,7 +51,7 @@ #define MAVLINK_GET_CHANNEL_BUFFER mavlink_get_channel_buffer #define MAVLINK_GET_CHANNEL_STATUS mavlink_get_channel_status -#include "v1.0/mavlink_types.h" +#include #include diff --git a/apps/mavlink_onboard/mavlink_receiver.c b/src/modules/mavlink_onboard/mavlink_receiver.c similarity index 100% rename from apps/mavlink_onboard/mavlink_receiver.c rename to src/modules/mavlink_onboard/mavlink_receiver.c diff --git a/apps/mavlink/Makefile b/src/modules/mavlink_onboard/module.mk similarity index 85% rename from apps/mavlink/Makefile rename to src/modules/mavlink_onboard/module.mk index 8ddb69b71d..c40fa042e8 100644 --- a/apps/mavlink/Makefile +++ b/src/modules/mavlink_onboard/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -32,13 +32,11 @@ ############################################################################ # -# Mavlink Application +# MAVLink protocol to uORB interface process (XXX hack for onboard use) # -APPNAME = mavlink -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 +MODULE_COMMAND = mavlink_onboard +SRCS = mavlink.c \ + mavlink_receiver.c -INCLUDES = $(TOPDIR)/../mavlink/include/mavlink - -include $(APPDIR)/mk/app.mk +INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/apps/mavlink_onboard/orb_topics.h b/src/modules/mavlink_onboard/orb_topics.h similarity index 100% rename from apps/mavlink_onboard/orb_topics.h rename to src/modules/mavlink_onboard/orb_topics.h diff --git a/apps/mavlink_onboard/util.h b/src/modules/mavlink_onboard/util.h similarity index 100% rename from apps/mavlink_onboard/util.h rename to src/modules/mavlink_onboard/util.h From 63136e354330bcf8efe2757187515eeaa8bc3bcb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 27 Apr 2013 00:11:16 +0200 Subject: [PATCH 093/486] Resurrected C++ change commit, now back up to same state as master --- src/modules/attitude_estimator_ekf/Makefile | 57 +++++++++++++++++ ...main.c => attitude_estimator_ekf_main.cpp} | 18 ++++-- src/modules/attitude_estimator_ekf/module.mk | 63 +++++++++++++++---- 3 files changed, 120 insertions(+), 18 deletions(-) create mode 100755 src/modules/attitude_estimator_ekf/Makefile rename src/modules/attitude_estimator_ekf/{attitude_estimator_ekf_main.c => attitude_estimator_ekf_main.cpp} (97%) diff --git a/src/modules/attitude_estimator_ekf/Makefile b/src/modules/attitude_estimator_ekf/Makefile new file mode 100755 index 0000000000..46a54c6607 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/Makefile @@ -0,0 +1,57 @@ +############################################################################ +# +# Copyright (C) 2012 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. +# +############################################################################ + +APPNAME = attitude_estimator_ekf +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +CXXSRCS = attitude_estimator_ekf_main.cpp + +CSRCS = attitude_estimator_ekf_params.c \ + codegen/eye.c \ + codegen/attitudeKalmanfilter.c \ + codegen/mrdivide.c \ + codegen/rdivide.c \ + codegen/attitudeKalmanfilter_initialize.c \ + codegen/attitudeKalmanfilter_terminate.c \ + codegen/rt_nonfinite.c \ + codegen/rtGetInf.c \ + codegen/rtGetNaN.c \ + codegen/norm.c \ + codegen/cross.c + + +# XXX this is *horribly* broken +INCLUDES += $(TOPDIR)/../mavlink/include/mavlink + +include $(APPDIR)/mk/app.mk diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp similarity index 97% rename from src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.c rename to src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 9fc4dfc838..1a50dde0f4 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -46,6 +46,7 @@ #include #include #include +#include #include #include #include @@ -65,11 +66,17 @@ #include #include +#ifdef __cplusplus +extern "C" { +#endif #include "codegen/attitudeKalmanfilter_initialize.h" #include "codegen/attitudeKalmanfilter.h" #include "attitude_estimator_ekf_params.h" +#ifdef __cplusplus +} +#endif -__EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]); +extern "C" __EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]); static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ @@ -264,10 +271,11 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* Main loop*/ while (!thread_should_exit) { - struct pollfd fds[2] = { - { .fd = sub_raw, .events = POLLIN }, - { .fd = sub_params, .events = POLLIN } - }; + struct pollfd fds[2]; + fds[0].fd = sub_raw; + fds[0].events = POLLIN; + fds[1].fd = sub_params; + fds[1].events = POLLIN; int ret = poll(fds, 2, 1000); if (ret < 0) { diff --git a/src/modules/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk index 4033617c4b..3034018aa2 100644 --- a/src/modules/attitude_estimator_ekf/module.mk +++ b/src/modules/attitude_estimator_ekf/module.mk @@ -1,16 +1,53 @@ +############################################################################ +# +# Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Attitude estimator (Extended Kalman Filter) +# MODULE_NAME = attitude_estimator_ekf -SRCS = attitude_estimator_ekf_main.c \ - attitude_estimator_ekf_params.c \ - codegen/attitudeKalmanfilter_initialize.c \ - codegen/attitudeKalmanfilter_terminate.c \ - codegen/attitudeKalmanfilter.c \ - codegen/cross.c \ - codegen/eye.c \ - codegen/mrdivide.c \ - codegen/norm.c \ - codegen/rdivide.c \ - codegen/rt_nonfinite.c \ - codegen/rtGetInf.c \ - codegen/rtGetNaN.c +CXXSRCS = attitude_estimator_ekf_main.cpp + +SRCS = attitude_estimator_ekf_params.c \ + codegen/eye.c \ + codegen/attitudeKalmanfilter.c \ + codegen/mrdivide.c \ + codegen/rdivide.c \ + codegen/attitudeKalmanfilter_initialize.c \ + codegen/attitudeKalmanfilter_terminate.c \ + codegen/rt_nonfinite.c \ + codegen/rtGetInf.c \ + codegen/rtGetNaN.c \ + codegen/norm.c \ + codegen/cross.c From 0c4b8a54c7779d198e016470889456b3458e2303 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 27 Apr 2013 00:13:03 +0200 Subject: [PATCH 094/486] Deleted old-style EEPROM driver, new one is already functional --- apps/systemcmds/eeprom/24xxxx_mtd.c | 571 ---------------------------- apps/systemcmds/eeprom/Makefile | 45 --- apps/systemcmds/eeprom/eeprom.c | 265 ------------- 3 files changed, 881 deletions(-) delete mode 100644 apps/systemcmds/eeprom/24xxxx_mtd.c delete mode 100644 apps/systemcmds/eeprom/Makefile delete mode 100644 apps/systemcmds/eeprom/eeprom.c diff --git a/apps/systemcmds/eeprom/24xxxx_mtd.c b/apps/systemcmds/eeprom/24xxxx_mtd.c deleted file mode 100644 index e34be44e31..0000000000 --- a/apps/systemcmds/eeprom/24xxxx_mtd.c +++ /dev/null @@ -1,571 +0,0 @@ -/************************************************************************************ - * Driver for 24xxxx-style I2C EEPROMs. - * - * Adapted from: - * - * drivers/mtd/at24xx.c - * Driver for I2C-based at24cxx EEPROM(at24c32,at24c64,at24c128,at24c256) - * - * Copyright (C) 2011 Li Zhuoyi. All rights reserved. - * Author: Li Zhuoyi - * History: 0.1 2011-08-20 initial version - * - * 2011-11-1 Added support for larger MTD block sizes: Hal Glenn - * - * Derived from drivers/mtd/m25px.c - * - * Copyright (C) 2009-2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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 NuttX 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. - * - ************************************************************************************/ - -/************************************************************************************ - * Included Files - ************************************************************************************/ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include "systemlib/perf_counter.h" - -/************************************************************************************ - * Pre-processor Definitions - ************************************************************************************/ - -/* - * Configuration is as for the AT24 driver, but the CONFIG_MTD_AT24XX define should be - * omitted in order to avoid building the AT24XX driver as well. - */ - -/* As a minimum, the size of the AT24 part and its 7-bit I2C address are required. */ - -#ifndef CONFIG_AT24XX_SIZE -# warning "Assuming AT24 size 64" -# define CONFIG_AT24XX_SIZE 64 -#endif -#ifndef CONFIG_AT24XX_ADDR -# warning "Assuming AT24 address of 0x50" -# define CONFIG_AT24XX_ADDR 0x50 -#endif - -/* Get the part configuration based on the size configuration */ - -#if CONFIG_AT24XX_SIZE == 32 -# define AT24XX_NPAGES 128 -# define AT24XX_PAGESIZE 32 -#elif CONFIG_AT24XX_SIZE == 48 -# define AT24XX_NPAGES 192 -# define AT24XX_PAGESIZE 32 -#elif CONFIG_AT24XX_SIZE == 64 -# define AT24XX_NPAGES 256 -# define AT24XX_PAGESIZE 32 -#elif CONFIG_AT24XX_SIZE == 128 -# define AT24XX_NPAGES 256 -# define AT24XX_PAGESIZE 64 -#elif CONFIG_AT24XX_SIZE == 256 -# define AT24XX_NPAGES 512 -# define AT24XX_PAGESIZE 64 -#endif - -/* For applications where a file system is used on the AT24, the tiny page sizes - * will result in very inefficient FLASH usage. In such cases, it is better if - * blocks are comprised of "clusters" of pages so that the file system block - * size is, say, 256 or 512 bytes. In any event, the block size *must* be an - * even multiple of the pages. - */ - -#ifndef CONFIG_AT24XX_MTD_BLOCKSIZE -# warning "Assuming driver block size is the same as the FLASH page size" -# define CONFIG_AT24XX_MTD_BLOCKSIZE AT24XX_PAGESIZE -#endif - -/* The AT24 does not respond on the bus during write cycles, so we depend on a long - * timeout to detect problems. The max program time is typically ~5ms. - */ -#ifndef CONFIG_AT24XX_WRITE_TIMEOUT_MS -# define CONFIG_AT24XX_WRITE_TIMEOUT_MS 20 -#endif - -/************************************************************************************ - * Private Types - ************************************************************************************/ - -/* This type represents the state of the MTD device. The struct mtd_dev_s - * must appear at the beginning of the definition so that you can freely - * cast between pointers to struct mtd_dev_s and struct at24c_dev_s. - */ - -struct at24c_dev_s { - struct mtd_dev_s mtd; /* MTD interface */ - FAR struct i2c_dev_s *dev; /* Saved I2C interface instance */ - uint8_t addr; /* I2C address */ - uint16_t pagesize; /* 32, 63 */ - uint16_t npages; /* 128, 256, 512, 1024 */ - - perf_counter_t perf_reads; - perf_counter_t perf_writes; - perf_counter_t perf_resets; - perf_counter_t perf_read_retries; - perf_counter_t perf_read_errors; - perf_counter_t perf_write_errors; -}; - -/************************************************************************************ - * Private Function Prototypes - ************************************************************************************/ - -/* MTD driver methods */ - -static int at24c_erase(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks); -static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock, - size_t nblocks, FAR uint8_t *buf); -static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, - size_t nblocks, FAR const uint8_t *buf); -static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg); - -void at24c_test(void); - -/************************************************************************************ - * Private Data - ************************************************************************************/ - -/* At present, only a single AT24 part is supported. In this case, a statically - * allocated state structure may be used. - */ - -static struct at24c_dev_s g_at24c; - -/************************************************************************************ - * Private Functions - ************************************************************************************/ - -static int at24c_eraseall(FAR struct at24c_dev_s *priv) -{ - int startblock = 0; - uint8_t buf[AT24XX_PAGESIZE + 2]; - - struct i2c_msg_s msgv[1] = { - { - .addr = priv->addr, - .flags = 0, - .buffer = &buf[0], - .length = sizeof(buf), - } - }; - - memset(&buf[2], 0xff, priv->pagesize); - - for (startblock = 0; startblock < priv->npages; startblock++) { - uint16_t offset = startblock * priv->pagesize; - buf[1] = offset & 0xff; - buf[0] = (offset >> 8) & 0xff; - - while (I2C_TRANSFER(priv->dev, &msgv[0], 1) < 0) { - fvdbg("erase stall\n"); - usleep(10000); - } - } - - return OK; -} - -/************************************************************************************ - * Name: at24c_erase - ************************************************************************************/ - -static int at24c_erase(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks) -{ - /* EEprom need not erase */ - - return (int)nblocks; -} - -/************************************************************************************ - * Name: at24c_test - ************************************************************************************/ - -void at24c_test(void) -{ - uint8_t buf[CONFIG_AT24XX_MTD_BLOCKSIZE]; - unsigned count = 0; - unsigned errors = 0; - - for (count = 0; count < 10000; count++) { - ssize_t result = at24c_bread(&g_at24c.mtd, 0, 1, buf); - if (result == ERROR) { - if (errors++ > 2) { - vdbg("too many errors\n"); - return; - } - } else if (result != 1) { - vdbg("unexpected %u\n", result); - } - if ((count % 100) == 0) - vdbg("test %u errors %u\n", count, errors); - } -} - -/************************************************************************************ - * Name: at24c_bread - ************************************************************************************/ - -static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock, - size_t nblocks, FAR uint8_t *buffer) -{ - FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; - size_t blocksleft; - uint8_t addr[2]; - int ret; - - struct i2c_msg_s msgv[2] = { - { - .addr = priv->addr, - .flags = 0, - .buffer = &addr[0], - .length = sizeof(addr), - }, - { - .addr = priv->addr, - .flags = I2C_M_READ, - .buffer = 0, - .length = priv->pagesize, - } - }; - -#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE - startblock *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); - nblocks *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); -#endif - blocksleft = nblocks; - - fvdbg("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks); - - if (startblock >= priv->npages) { - return 0; - } - - if (startblock + nblocks > priv->npages) { - nblocks = priv->npages - startblock; - } - - while (blocksleft-- > 0) { - uint16_t offset = startblock * priv->pagesize; - unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS; - - addr[1] = offset & 0xff; - addr[0] = (offset >> 8) & 0xff; - msgv[1].buffer = buffer; - - for (;;) { - - perf_begin(priv->perf_reads); - ret = I2C_TRANSFER(priv->dev, &msgv[0], 2); - perf_end(priv->perf_reads); - - if (ret >= 0) - break; - - fvdbg("read stall"); - usleep(1000); - - /* We should normally only be here on the first read after - * a write. - * - * XXX maybe do special first-read handling with optional - * bus reset as well? - */ - perf_count(priv->perf_read_retries); - - if (--tries == 0) { - perf_count(priv->perf_read_errors); - return ERROR; - } - } - - startblock++; - buffer += priv->pagesize; - } - -#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE - return nblocks / (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); -#else - return nblocks; -#endif -} - -/************************************************************************************ - * Name: at24c_bwrite - * - * Operates on MTD block's and translates to FLASH pages - * - ************************************************************************************/ - -static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks, - FAR const uint8_t *buffer) -{ - FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; - size_t blocksleft; - uint8_t buf[AT24XX_PAGESIZE + 2]; - int ret; - - struct i2c_msg_s msgv[1] = { - { - .addr = priv->addr, - .flags = 0, - .buffer = &buf[0], - .length = sizeof(buf), - } - }; - -#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE - startblock *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); - nblocks *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); -#endif - blocksleft = nblocks; - - if (startblock >= priv->npages) { - return 0; - } - - if (startblock + nblocks > priv->npages) { - nblocks = priv->npages - startblock; - } - - fvdbg("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks); - - while (blocksleft-- > 0) { - uint16_t offset = startblock * priv->pagesize; - unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS; - - buf[1] = offset & 0xff; - buf[0] = (offset >> 8) & 0xff; - memcpy(&buf[2], buffer, priv->pagesize); - - for (;;) { - - perf_begin(priv->perf_writes); - ret = I2C_TRANSFER(priv->dev, &msgv[0], 1); - perf_end(priv->perf_writes); - - if (ret >= 0) - break; - - fvdbg("write stall"); - usleep(1000); - - /* We expect to see a number of retries per write cycle as we - * poll for write completion. - */ - if (--tries == 0) { - perf_count(priv->perf_write_errors); - return ERROR; - } - } - - startblock++; - buffer += priv->pagesize; - } - -#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE - return nblocks / (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); -#else - return nblocks; -#endif -} - -/************************************************************************************ - * Name: at24c_ioctl - ************************************************************************************/ - -static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg) -{ - FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; - int ret = -EINVAL; /* Assume good command with bad parameters */ - - fvdbg("cmd: %d \n", cmd); - - switch (cmd) { - case MTDIOC_GEOMETRY: { - FAR struct mtd_geometry_s *geo = (FAR struct mtd_geometry_s *)((uintptr_t)arg); - - if (geo) { - /* Populate the geometry structure with information need to know - * the capacity and how to access the device. - * - * NOTE: that the device is treated as though it where just an array - * of fixed size blocks. That is most likely not true, but the client - * will expect the device logic to do whatever is necessary to make it - * appear so. - * - * blocksize: - * May be user defined. The block size for the at24XX devices may be - * larger than the page size in order to better support file systems. - * The read and write functions translate BLOCKS to pages for the - * small flash devices - * erasesize: - * It has to be at least as big as the blocksize, bigger serves no - * purpose. - * neraseblocks - * Note that the device size is in kilobits and must be scaled by - * 1024 / 8 - */ - -#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE - geo->blocksize = CONFIG_AT24XX_MTD_BLOCKSIZE; - geo->erasesize = CONFIG_AT24XX_MTD_BLOCKSIZE; - geo->neraseblocks = (CONFIG_AT24XX_SIZE * 1024 / 8) / CONFIG_AT24XX_MTD_BLOCKSIZE; -#else - geo->blocksize = priv->pagesize; - geo->erasesize = priv->pagesize; - geo->neraseblocks = priv->npages; -#endif - ret = OK; - - fvdbg("blocksize: %d erasesize: %d neraseblocks: %d\n", - geo->blocksize, geo->erasesize, geo->neraseblocks); - } - } - break; - - case MTDIOC_BULKERASE: - ret = at24c_eraseall(priv); - break; - - case MTDIOC_XIPBASE: - default: - ret = -ENOTTY; /* Bad command */ - break; - } - - return ret; -} - -/************************************************************************************ - * Public Functions - ************************************************************************************/ - -/************************************************************************************ - * Name: at24c_initialize - * - * Description: - * Create an initialize MTD device instance. MTD devices are not registered - * in the file system, but are created as instances that can be bound to - * other functions (such as a block or character driver front end). - * - ************************************************************************************/ - -FAR struct mtd_dev_s *at24c_initialize(FAR struct i2c_dev_s *dev) { - FAR struct at24c_dev_s *priv; - - fvdbg("dev: %p\n", dev); - - /* Allocate a state structure (we allocate the structure instead of using - * a fixed, static allocation so that we can handle multiple FLASH devices. - * The current implementation would handle only one FLASH part per I2C - * device (only because of the SPIDEV_FLASH definition) and so would have - * to be extended to handle multiple FLASH parts on the same I2C bus. - */ - - priv = &g_at24c; - - if (priv) { - /* Initialize the allocated structure */ - - priv->addr = CONFIG_AT24XX_ADDR; - priv->pagesize = AT24XX_PAGESIZE; - priv->npages = AT24XX_NPAGES; - - priv->mtd.erase = at24c_erase; - priv->mtd.bread = at24c_bread; - priv->mtd.bwrite = at24c_bwrite; - priv->mtd.ioctl = at24c_ioctl; - priv->dev = dev; - - priv->perf_reads = perf_alloc(PC_ELAPSED, "EEPROM read"); - priv->perf_writes = perf_alloc(PC_ELAPSED, "EEPROM write"); - priv->perf_resets = perf_alloc(PC_COUNT, "EEPROM reset"); - priv->perf_read_retries = perf_alloc(PC_COUNT, "EEPROM read retries"); - priv->perf_read_errors = perf_alloc(PC_COUNT, "EEPROM read errors"); - priv->perf_write_errors = perf_alloc(PC_COUNT, "EEPROM write errors"); - } - - /* attempt to read to validate device is present */ - unsigned char buf[5]; - uint8_t addrbuf[2] = {0, 0}; - - struct i2c_msg_s msgv[2] = { - { - .addr = priv->addr, - .flags = 0, - .buffer = &addrbuf[0], - .length = sizeof(addrbuf), - }, - { - .addr = priv->addr, - .flags = I2C_M_READ, - .buffer = &buf[0], - .length = sizeof(buf), - } - }; - - perf_begin(priv->perf_reads); - int ret = I2C_TRANSFER(priv->dev, &msgv[0], 2); - perf_end(priv->perf_reads); - - if (ret < 0) { - return NULL; - } - - /* Return the implementation-specific state structure as the MTD device */ - - fvdbg("Return %p\n", priv); - return (FAR struct mtd_dev_s *)priv; -} - -/* - * XXX: debug hackery - */ -int at24c_nuke(void) -{ - return at24c_eraseall(&g_at24c); -} diff --git a/apps/systemcmds/eeprom/Makefile b/apps/systemcmds/eeprom/Makefile deleted file mode 100644 index 58c8cb5ec7..0000000000 --- a/apps/systemcmds/eeprom/Makefile +++ /dev/null @@ -1,45 +0,0 @@ -############################################################################ -# -# Copyright (C) 2012 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. -# -############################################################################ - -# -# Build the eeprom tool. -# - -APPNAME = eeprom -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 4096 -MAXOPTIMIZATION = -Os - -include $(APPDIR)/mk/app.mk - -MAXOPTIMIZATION = -Os diff --git a/apps/systemcmds/eeprom/eeprom.c b/apps/systemcmds/eeprom/eeprom.c deleted file mode 100644 index 49da513580..0000000000 --- a/apps/systemcmds/eeprom/eeprom.c +++ /dev/null @@ -1,265 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier - * - * 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 eeprom.c - * - * EEPROM service and utility app. - */ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include - -#include "systemlib/systemlib.h" -#include "systemlib/param/param.h" -#include "systemlib/err.h" - -#ifndef PX4_I2C_BUS_ONBOARD -# error PX4_I2C_BUS_ONBOARD not defined, cannot locate onboard EEPROM -#endif - -__EXPORT int eeprom_main(int argc, char *argv[]); - -static void eeprom_attach(void); -static void eeprom_start(void); -static void eeprom_erase(void); -static void eeprom_ioctl(unsigned operation); -static void eeprom_save(const char *name); -static void eeprom_load(const char *name); -static void eeprom_test(void); - -static bool attached = false; -static bool started = false; -static struct mtd_dev_s *eeprom_mtd; - -int eeprom_main(int argc, char *argv[]) -{ - if (argc >= 2) { - if (!strcmp(argv[1], "start")) - eeprom_start(); - - if (!strcmp(argv[1], "save_param")) - eeprom_save(argv[2]); - - if (!strcmp(argv[1], "load_param")) - eeprom_load(argv[2]); - - if (!strcmp(argv[1], "erase")) - eeprom_erase(); - - if (!strcmp(argv[1], "test")) - eeprom_test(); - - if (0) { /* these actually require a file on the filesystem... */ - - if (!strcmp(argv[1], "reformat")) - eeprom_ioctl(FIOC_REFORMAT); - - if (!strcmp(argv[1], "repack")) - eeprom_ioctl(FIOC_OPTIMIZE); - } - } - - errx(1, "expected a command, try 'start'\n\t'save_param /eeprom/parameters'\n\t'load_param /eeprom/parameters'\n\t'erase'\n"); -} - - -static void -eeprom_attach(void) -{ - /* find the right I2C */ - struct i2c_dev_s *i2c = up_i2cinitialize(PX4_I2C_BUS_ONBOARD); - /* this resets the I2C bus, set correct bus speed again */ - I2C_SETFREQUENCY(i2c, 400000); - - if (i2c == NULL) - errx(1, "failed to locate I2C bus"); - - /* start the MTD driver, attempt 5 times */ - for (int i = 0; i < 5; i++) { - eeprom_mtd = at24c_initialize(i2c); - if (eeprom_mtd) { - /* abort on first valid result */ - if (i > 0) { - warnx("warning: EEPROM needed %d attempts to attach", i+1); - } - break; - } - } - - /* if last attempt is still unsuccessful, abort */ - if (eeprom_mtd == NULL) - errx(1, "failed to initialize EEPROM driver"); - - attached = true; -} - -static void -eeprom_start(void) -{ - int ret; - - if (started) - errx(1, "EEPROM already mounted"); - - if (!attached) - eeprom_attach(); - - /* start NXFFS */ - ret = nxffs_initialize(eeprom_mtd); - - if (ret < 0) - errx(1, "failed to initialize NXFFS - erase EEPROM to reformat"); - - /* mount the EEPROM */ - ret = mount(NULL, "/eeprom", "nxffs", 0, NULL); - - if (ret < 0) - errx(1, "failed to mount /eeprom - erase EEPROM to reformat"); - - started = true; - warnx("mounted EEPROM at /eeprom"); - exit(0); -} - -extern int at24c_nuke(void); - -static void -eeprom_erase(void) -{ - if (!attached) - eeprom_attach(); - - if (at24c_nuke()) - errx(1, "erase failed"); - - errx(0, "erase done, reboot now"); -} - -static void -eeprom_ioctl(unsigned operation) -{ - int fd; - - fd = open("/eeprom/.", 0); - - if (fd < 0) - err(1, "open /eeprom"); - - if (ioctl(fd, operation, 0) < 0) - err(1, "ioctl"); - - exit(0); -} - -static void -eeprom_save(const char *name) -{ - if (!started) - errx(1, "must be started first"); - - if (!name) - err(1, "missing argument for device name, try '/eeprom/parameters'"); - - warnx("WARNING: 'eeprom save_param' deprecated - use 'param save' instead"); - - /* delete the file in case it exists */ - unlink(name); - - /* create the file */ - int fd = open(name, O_WRONLY | O_CREAT | O_EXCL); - - if (fd < 0) - err(1, "opening '%s' failed", name); - - int result = param_export(fd, false); - close(fd); - - if (result < 0) { - unlink(name); - errx(1, "error exporting to '%s'", name); - } - - exit(0); -} - -static void -eeprom_load(const char *name) -{ - if (!started) - errx(1, "must be started first"); - - if (!name) - err(1, "missing argument for device name, try '/eeprom/parameters'"); - - warnx("WARNING: 'eeprom load_param' deprecated - use 'param load' instead"); - - int fd = open(name, O_RDONLY); - - if (fd < 0) - err(1, "open '%s'", name); - - int result = param_load(fd); - close(fd); - - if (result < 0) - errx(1, "error importing from '%s'", name); - - exit(0); -} - -extern void at24c_test(void); - -static void -eeprom_test(void) -{ - at24c_test(); - exit(0); -} From 680b48e048973887a65e908d475f2c0f7afe5fd3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 27 Apr 2013 00:13:35 +0200 Subject: [PATCH 095/486] Deleted old cruft --- apps/systemcmds/delay_test/Makefile | 44 ------- apps/systemcmds/delay_test/delay_test.c | 160 ------------------------ 2 files changed, 204 deletions(-) delete mode 100644 apps/systemcmds/delay_test/Makefile delete mode 100644 apps/systemcmds/delay_test/delay_test.c diff --git a/apps/systemcmds/delay_test/Makefile b/apps/systemcmds/delay_test/Makefile deleted file mode 100644 index e54cf2f4e4..0000000000 --- a/apps/systemcmds/delay_test/Makefile +++ /dev/null @@ -1,44 +0,0 @@ -############################################################################ -# -# Copyright (C) 2012 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. -# -############################################################################ - -# -# Delay test -# - -APPNAME = delay_test -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 - -include $(APPDIR)/mk/app.mk - -MAXOPTIMIZATION = -Os diff --git a/apps/systemcmds/delay_test/delay_test.c b/apps/systemcmds/delay_test/delay_test.c deleted file mode 100644 index 51cce38fca..0000000000 --- a/apps/systemcmds/delay_test/delay_test.c +++ /dev/null @@ -1,160 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier - * - * 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 delay_test.c - * - * Simple but effective delay test using leds and a scope to measure - * communication delays end-to-end accurately. - * - * @author Lorenz Meier - */ - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include - -#include -#include - -__EXPORT int delay_test_main(int argc, char *argv[]); -static int led_on(int leds, int led); -static int led_off(int leds, int led); - -int delay_test_main(int argc, char *argv[]) -{ - bool vicon_msg = false; - bool command_msg = false; - - if (argc > 1 && !strcmp(argv[1], "--help")) { - warnx("usage: delay_test [vicon] [command]\n"); - exit(1); - } - - if (argc > 1 && !strcmp(argv[1], "vicon")) { - vicon_msg = true; - } - - if (argc > 1 && !strcmp(argv[1], "command")) { - command_msg = true; - } - - int buzzer = open("/dev/tone_alarm", O_WRONLY); - int leds = open(LED_DEVICE_PATH, 0); - - /* prepare use of amber led */ - led_off(leds, LED_AMBER); - - int topic; - - /* subscribe to topic */ - if (vicon_msg) { - topic = orb_subscribe(ORB_ID(vehicle_vicon_position)); - } else if (command_msg) { - topic = orb_subscribe(ORB_ID(vehicle_command)); - } else { - errx(1, "No topic selected for delay test, use --help for a list of topics."); - } - - const int testcount = 20; - - warnx("running %d iterations..\n", testcount); - - struct pollfd fds[1]; - fds[0].fd = topic; - fds[0].events = POLLIN; - - /* display and sound error */ - for (int i = 0; i < testcount; i++) - { - /* wait for topic */ - int ret = poll(&fds[0], 1, 2000); - - /* this would be bad... */ - if (ret < 0) { - warnx("poll error %d", errno); - usleep(1000000); - continue; - } - - /* do we have a topic update? */ - if (fds[0].revents & POLLIN) { - - /* copy object to disable poll ready state */ - if (vicon_msg) { - struct vehicle_vicon_position_s vicon_position; - orb_copy(ORB_ID(vehicle_vicon_position), topic, &vicon_position); - } else if (command_msg) { - struct vehicle_command_s vehicle_command; - orb_copy(ORB_ID(vehicle_command), topic, &vehicle_command); - } - - led_on(leds, LED_AMBER); - ioctl(buzzer, TONE_SET_ALARM, 4); - /* keep led on for 50 ms to make it barely visible */ - usleep(50000); - led_off(leds, LED_AMBER); - } - } - - /* stop alarm */ - ioctl(buzzer, TONE_SET_ALARM, 0); - - /* switch on leds */ - led_on(leds, LED_BLUE); - led_on(leds, LED_AMBER); - - exit(0); -} - -static int led_off(int leds, int led) -{ - return ioctl(leds, LED_OFF, led); -} - -static int led_on(int leds, int led) -{ - return ioctl(leds, LED_ON, led); -} \ No newline at end of file From 686139c7d8229365fd3b8f4bd6cdaab0d2f06b8b Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 26 Apr 2013 23:09:11 -0700 Subject: [PATCH 096/486] HACK: don't call the card-changed callback with interrupts disabled, as it means that timeouts don't work. --- nuttx/arch/arm/src/stm32/stm32_sdio.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/nuttx/arch/arm/src/stm32/stm32_sdio.c b/nuttx/arch/arm/src/stm32/stm32_sdio.c index a8bcae3070..dad94e8a19 100644 --- a/nuttx/arch/arm/src/stm32/stm32_sdio.c +++ b/nuttx/arch/arm/src/stm32/stm32_sdio.c @@ -2797,13 +2797,14 @@ void sdio_mediachange(FAR struct sdio_dev_s *dev, bool cardinslot) } fvdbg("cdstatus OLD: %02x NEW: %02x\n", cdstatus, priv->cdstatus); + irqrestore(flags); + /* Perform any requested callback if the status has changed */ if (cdstatus != priv->cdstatus) { stm32_callback(priv); } - irqrestore(flags); } /**************************************************************************** From 9d4d1ace437f94bfc517b7ff9036657fd5b2c354 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 26 Apr 2013 23:09:38 -0700 Subject: [PATCH 097/486] Pick up the MAVlink headers from the right place --- src/modules/attitude_estimator_ekf/module.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk index 5ce5451122..15d17e30db 100644 --- a/src/modules/attitude_estimator_ekf/module.mk +++ b/src/modules/attitude_estimator_ekf/module.mk @@ -14,3 +14,4 @@ SRCS = attitude_estimator_ekf_main.cpp \ codegen/rtGetInf.c \ codegen/rtGetNaN.c +INCLUDE_DIRS += $(PX4_BASE)/mavlink/include/mavlink From 2289c0bb216027419a9ba5e52103a1310ff03292 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 27 Apr 2013 11:30:41 +0200 Subject: [PATCH 098/486] Moved all system commands to new build system --- apps/systemcmds/calibration/Makefile | 44 --- apps/systemcmds/calibration/calibration.c | 147 ---------- apps/systemcmds/calibration/calibration.h | 60 ---- apps/systemcmds/calibration/channels_cal.c | 196 ------------- apps/systemcmds/calibration/range_cal.c | 224 --------------- apps/systemcmds/calibration/servo_cal.c | 264 ------------------ apps/systemcmds/top/Makefile | 44 --- makefiles/config_px4fmu_default.mk | 24 +- makefiles/config_px4fmuv2_default.mk | 23 +- src/modules/mavlink_onboard/module.mk | 2 +- .../systemcmds/bl_update/bl_update.c | 2 +- .../systemcmds/bl_update/module.mk | 11 +- {apps => src}/systemcmds/boardinfo/.context | 0 .../systemcmds/boardinfo/boardinfo.c | 2 +- .../systemcmds/boardinfo/module.mk | 11 +- src/systemcmds/eeprom/module.mk | 33 +++ {apps => src}/systemcmds/i2c/i2c.c | 2 +- .../Makefile => src/systemcmds/i2c/module.mk | 9 +- {apps => src}/systemcmds/mixer/mixer.c | 0 .../systemcmds/mixer/module.mk | 9 +- .../systemcmds/param/module.mk | 10 +- {apps => src}/systemcmds/param/param.c | 0 {apps => src}/systemcmds/perf/.context | 0 .../Makefile => src/systemcmds/perf/module.mk | 9 +- {apps => src}/systemcmds/perf/perf.c | 2 +- .../systemcmds/preflight_check/module.mk | 12 +- .../preflight_check/preflight_check.c | 2 +- .../Makefile => src/systemcmds/pwm/module.mk | 9 +- {apps => src}/systemcmds/pwm/pwm.c | 0 {apps => src}/systemcmds/reboot/.context | 0 src/systemcmds/reboot/module.mk | 41 +++ {apps => src}/systemcmds/reboot/reboot.c | 0 {apps => src}/systemcmds/top/.context | 0 .../Makefile => src/systemcmds/top/module.mk | 12 +- {apps => src}/systemcmds/top/top.c | 0 35 files changed, 147 insertions(+), 1057 deletions(-) delete mode 100644 apps/systemcmds/calibration/Makefile delete mode 100755 apps/systemcmds/calibration/calibration.c delete mode 100644 apps/systemcmds/calibration/calibration.h delete mode 100755 apps/systemcmds/calibration/channels_cal.c delete mode 100755 apps/systemcmds/calibration/range_cal.c delete mode 100644 apps/systemcmds/calibration/servo_cal.c delete mode 100644 apps/systemcmds/top/Makefile rename {apps => src}/systemcmds/bl_update/bl_update.c (98%) rename apps/systemcmds/bl_update/Makefile => src/systemcmds/bl_update/module.mk (88%) rename {apps => src}/systemcmds/boardinfo/.context (100%) rename {apps => src}/systemcmds/boardinfo/boardinfo.c (99%) rename apps/systemcmds/preflight_check/Makefile => src/systemcmds/boardinfo/module.mk (89%) rename {apps => src}/systemcmds/i2c/i2c.c (98%) rename apps/systemcmds/i2c/Makefile => src/systemcmds/i2c/module.mk (91%) rename {apps => src}/systemcmds/mixer/mixer.c (100%) rename apps/systemcmds/mixer/Makefile => src/systemcmds/mixer/module.mk (91%) rename apps/systemcmds/param/Makefile => src/systemcmds/param/module.mk (91%) rename {apps => src}/systemcmds/param/param.c (100%) rename {apps => src}/systemcmds/perf/.context (100%) rename apps/systemcmds/perf/Makefile => src/systemcmds/perf/module.mk (91%) rename {apps => src}/systemcmds/perf/perf.c (97%) rename apps/systemcmds/boardinfo/Makefile => src/systemcmds/preflight_check/module.mk (86%) rename {apps => src}/systemcmds/preflight_check/preflight_check.c (98%) rename apps/systemcmds/pwm/Makefile => src/systemcmds/pwm/module.mk (91%) rename {apps => src}/systemcmds/pwm/pwm.c (100%) rename {apps => src}/systemcmds/reboot/.context (100%) create mode 100644 src/systemcmds/reboot/module.mk rename {apps => src}/systemcmds/reboot/reboot.c (100%) rename {apps => src}/systemcmds/top/.context (100%) rename apps/systemcmds/reboot/Makefile => src/systemcmds/top/module.mk (90%) rename {apps => src}/systemcmds/top/top.c (100%) diff --git a/apps/systemcmds/calibration/Makefile b/apps/systemcmds/calibration/Makefile deleted file mode 100644 index a1735962e1..0000000000 --- a/apps/systemcmds/calibration/Makefile +++ /dev/null @@ -1,44 +0,0 @@ -############################################################################ -# -# Copyright (C) 2012 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Makefile to build the calibration tool -# - -APPNAME = calibration -PRIORITY = SCHED_PRIORITY_MAX - 1 -STACKSIZE = 4096 - -include $(APPDIR)/mk/app.mk - -MAXOPTIMIZATION = -Os diff --git a/apps/systemcmds/calibration/calibration.c b/apps/systemcmds/calibration/calibration.c deleted file mode 100755 index 7f8b9240f4..0000000000 --- a/apps/systemcmds/calibration/calibration.c +++ /dev/null @@ -1,147 +0,0 @@ -/**************************************************************************** - * calibration.c - * - * Copyright (C) 2012 Ivan Ovinnikov. All rights reserved. - * Authors: Nils Wenzler - * - * 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 NuttX 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. - * - ****************************************************************************/ - -/**************************************************************************** - * Included Files - ****************************************************************************/ -#include -#include -#include -#include "calibration.h" - -/**************************************************************************** - * Private Function Prototypes - ****************************************************************************/ - -static int calibrate_help(int argc, char *argv[]); -static int calibrate_all(int argc, char *argv[]); - -__EXPORT int calibration_main(int argc, char *argv[]); - -/**************************************************************************** - * Private Data - ****************************************************************************/ - -struct { - const char *name; - int (* fn)(int argc, char *argv[]); - unsigned options; -#define OPT_NOHELP (1<<0) -#define OPT_NOALLTEST (1<<1) -} calibrates[] = { - {"range", range_cal, 0}, - {"servo", servo_cal, 0}, - {"all", calibrate_all, OPT_NOALLTEST}, - {"help", calibrate_help, OPT_NOALLTEST | OPT_NOHELP}, - {NULL, NULL} -}; - -/**************************************************************************** - * Public Data - ****************************************************************************/ - -/**************************************************************************** - * Private Functions - ****************************************************************************/ - -static int -calibrate_help(int argc, char *argv[]) -{ - unsigned i; - - printf("Available calibration routines:\n"); - - for (i = 0; calibrates[i].name; i++) - printf(" %s\n", calibrates[i].name); - - return 0; -} - -static int -calibrate_all(int argc, char *argv[]) -{ - unsigned i; - char *args[2] = {"all", NULL}; - - printf("Running all calibration routines...\n\n"); - - for (i = 0; calibrates[i].name; i++) { - printf(" %s:\n", calibrates[i].name); - - if (calibrates[i].fn(1, args)) { - printf(" FAIL\n"); - - } else { - printf(" DONE\n"); - } - } - - return 0; -} - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -void press_enter(void) -{ - int c; - printf("Press CTRL+ENTER to continue... \n"); - fflush(stdout); - - do c = getchar(); while ((c != '\n') && (c != EOF)); -} - -/**************************************************************************** - * Name: calibrate_main - ****************************************************************************/ - -int calibration_main(int argc, char *argv[]) -{ - unsigned i; - - if (argc < 2) { - printf("calibration: missing name - 'calibration help' for a list of routines\n"); - return 1; - } - - for (i = 0; calibrates[i].name; i++) { - if (!strcmp(calibrates[i].name, argv[1])) - return calibrates[i].fn(argc - 1, argv + 1); - } - - printf("calibrate: no routines called '%s' - 'calibration help' for a list of routines\n", argv[1]); - return 1; -} diff --git a/apps/systemcmds/calibration/calibration.h b/apps/systemcmds/calibration/calibration.h deleted file mode 100644 index 028853ec8e..0000000000 --- a/apps/systemcmds/calibration/calibration.h +++ /dev/null @@ -1,60 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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. - * - ****************************************************************************/ - -#ifndef _CALIBRATION_H -#define _CALIBRATION_H - -/**************************************************************************** - * Included Files - ****************************************************************************/ - -/**************************************************************************** - * Definitions - ****************************************************************************/ - -/**************************************************************************** - * Public Types - ****************************************************************************/ - -/**************************************************************************** - * Public Variables - ****************************************************************************/ - -/**************************************************************************** - * Public Function Prototypes - ****************************************************************************/ - -extern int range_cal(int argc, char *argv[]); -extern int servo_cal(int argc, char *argv[]); - -#endif diff --git a/apps/systemcmds/calibration/channels_cal.c b/apps/systemcmds/calibration/channels_cal.c deleted file mode 100755 index 575138b120..0000000000 --- a/apps/systemcmds/calibration/channels_cal.c +++ /dev/null @@ -1,196 +0,0 @@ -/**************************************************************************** - * channels_cal.c - * - * Copyright (C) 2012 Nils Wenzler. All rights reserved. - * Authors: Nils Wenzler - * - * 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 NuttX 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. - * - ****************************************************************************/ - -/**************************************************************************** - * Included Files - ****************************************************************************/ -#include -#include -#include -#include "calibration.h" - -/**************************************************************************** - * Defines - ****************************************************************************/ -#define ABS(a) (((a) < 0) ? -(a) : (a)) -#define MIN(a,b) (((a)<(b))?(a):(b)) -#define MAX(a,b) (((a)>(b))?(a):(b)) - -/**************************************************************************** - * Private Data - ****************************************************************************/ -//Store the values here before writing them to global_data_rc_channels -uint16_t old_values[NR_CHANNELS]; -uint16_t cur_values[NR_CHANNELS]; -uint8_t function_map[NR_CHANNELS]; -char names[12][9]; - - - -/**************************************************************************** - * Private Functions - ****************************************************************************/ -void press_enter_2(void) -{ - int c; - printf("Press CTRL+ENTER to continue... \n"); - fflush(stdout); - - do c = getchar(); while ((c != '\n') && (c != EOF)); -} - -/**This gets all current values and writes them to min or max - */ -uint8_t get_val(uint16_t *buffer) -{ - if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) { - uint8_t i; - - for (i = 0; i < NR_CHANNELS; i++) { - printf("Channel: %i\t RAW Value: %i: \n", i, global_data_rc_channels->chan[i].raw); - buffer[i] = global_data_rc_channels->chan[i].raw; - } - - global_data_unlock(&global_data_rc_channels->access_conf); - return 0; - - } else - return -1; -} - -void set_channel(void) -{ - static uint8_t j = 0; - uint8_t i; - - for (i = 0; i < NR_CHANNELS; i++) { - if (ABS(old_values - cur_values) > 50) { - function_map[j] = i; - strcpy(names[i], global_data_rc_channels->function_names[j]); - j++; - } - } -} - -void write_dat(void) -{ - global_data_lock(&global_data_rc_channels->access_conf); - uint8_t i; - - for (i = 0; i < NR_CHANNELS; i++) { - global_data_rc_channels->function[i] = function_map[i]; - strcpy(global_data_rc_channels->chan[i].name, names[i]); - - printf("Channel %i\t Function %s\n", i, - global_data_rc_channels->chan[i].name); - } - - global_data_unlock(&global_data_rc_channels->access_conf); -} - - -/**************************************************************************** - * Public Functions - ****************************************************************************/ -int channels_cal(int argc, char *argv[]) -{ - - printf("This routine maps the input functions on the remote control\n"); - printf("to the corresponding function (Throttle, Roll,..)\n"); - printf("Always move the stick all the way\n"); - press_enter_2(); - - printf("Pull the THROTTLE stick down\n"); - press_enter_2(); - - while (get_val(old_values)); - - printf("Move the THROTTLE stick up\n "); - press_enter_2(); - - while (get_val(cur_values)); - - set_channel(); - - printf("Pull the PITCH stick down\n"); - press_enter_2(); - - while (get_val(old_values)); - - printf("Move the PITCH stick up\n "); - press_enter_2(); - - while (get_val(cur_values)); - - set_channel(); - - printf("Put the ROLL stick to the left\n"); - press_enter_2(); - - while (get_val(old_values)); - - printf("Put the ROLL stick to the right\n "); - press_enter_2(); - - while (get_val(cur_values)); - - set_channel(); - - printf("Put the YAW stick to the left\n"); - press_enter_2(); - - while (get_val(old_values)); - - printf("Put the YAW stick to the right\n "); - press_enter_2(); - - while (get_val(cur_values)); - - set_channel(); - - uint8_t k; - - for (k = 5; k < NR_CHANNELS; k++) { - function_map[k] = k; - strcpy(names[k], global_data_rc_channels->function_names[k]); - } - -//write the values to global_data_rc_channels - write_dat(); - - return 0; - -} - diff --git a/apps/systemcmds/calibration/range_cal.c b/apps/systemcmds/calibration/range_cal.c deleted file mode 100755 index 159a0d06b7..0000000000 --- a/apps/systemcmds/calibration/range_cal.c +++ /dev/null @@ -1,224 +0,0 @@ -/**************************************************************************** - * range_cal.c - * - * Copyright (C) 2012 Nils Wenzler. All rights reserved. - * Authors: Nils Wenzler - * - * 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 NuttX 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. - * - ****************************************************************************/ - -/**************************************************************************** - * Included Files - ****************************************************************************/ -#include -#include -#include "calibration.h" - -/**************************************************************************** - * Defines - ****************************************************************************/ - -/**************************************************************************** - * Private Data - ****************************************************************************/ -//Store the values here before writing them to global_data_rc_channels -uint16_t max_values[NR_CHANNELS]; -uint16_t min_values[NR_CHANNELS]; -uint16_t mid_values[NR_CHANNELS]; - - -/**************************************************************************** - * Private Functions - ****************************************************************************/ - -/**This sets the middle values - */ -uint8_t set_mid(void) -{ - if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) { - uint8_t i; - - for (i = 0; i < NR_CHANNELS; i++) { - if (i == global_data_rc_channels->function[ROLL] || - i == global_data_rc_channels->function[YAW] || - i == global_data_rc_channels->function[PITCH]) { - - mid_values[i] = global_data_rc_channels->chan[i].raw; - - } else { - mid_values[i] = (max_values[i] + min_values[i]) / 2; - } - - } - - global_data_unlock(&global_data_rc_channels->access_conf); - return 0; - - } else - return -1; -} - -/**This gets all current values and writes them to min or max - */ -uint8_t get_value(void) -{ - if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) { - uint8_t i; - - for (i = 0; i < NR_CHANNELS; i++) { - //see if the value is bigger or smaller than 1500 (roughly neutral) - //and write to the appropriate array - if (global_data_rc_channels->chan[i].raw != 0 && - global_data_rc_channels->chan[i].raw < 1500) { - min_values[i] = global_data_rc_channels->chan[i].raw; - - } else if (global_data_rc_channels->chan[i].raw != 0 && - global_data_rc_channels->chan[i].raw > 1500) { - max_values[i] = global_data_rc_channels->chan[i].raw; - - } else { - max_values[i] = 0; - min_values[i] = 0; - } - } - - global_data_unlock(&global_data_rc_channels->access_conf); - return 0; - - } else - return -1; -} - - -void write_data(void) -{ - // global_data_lock(&global_data_rc_channels->access_conf); - // uint8_t i; - // for(i=0; i < NR_CHANNELS; i++){ - // //Write the data to global_data_rc_channels (if not 0) - // if(mid_values[i]!=0 && min_values[i]!=0 && max_values[i]!=0){ - // global_data_rc_channels->chan[i].scaling_factor = - // 10000/((max_values[i] - min_values[i])/2); - // - // global_data_rc_channels->chan[i].mid = mid_values[i]; - // } - // - // printf("Channel %i\t Function %s \t\t Min %i\t\t Max %i\t\t Scaling Factor: %i\t Middle Value %i\n", - // i, - // global_data_rc_channels->function_name[global_data_rc_channels->function[i]], - // min_values[i], max_values[i], - // global_data_rc_channels->chan[i].scaling_factor, - // global_data_rc_channels->chan[i].mid); - // } - // global_data_unlock(&global_data_rc_channels->access_conf); - - //Write to the Parameter storage - - global_data_parameter_storage->pm.param_values[PARAM_RC1_MIN] = min_values[0]; - global_data_parameter_storage->pm.param_values[PARAM_RC2_MIN] = min_values[1]; - global_data_parameter_storage->pm.param_values[PARAM_RC3_MIN] = min_values[2]; - global_data_parameter_storage->pm.param_values[PARAM_RC4_MIN] = min_values[3]; - global_data_parameter_storage->pm.param_values[PARAM_RC5_MIN] = min_values[4]; - global_data_parameter_storage->pm.param_values[PARAM_RC6_MIN] = min_values[5]; - global_data_parameter_storage->pm.param_values[PARAM_RC7_MIN] = min_values[6]; - global_data_parameter_storage->pm.param_values[PARAM_RC8_MIN] = min_values[7]; - - - global_data_parameter_storage->pm.param_values[PARAM_RC1_MAX] = max_values[0]; - global_data_parameter_storage->pm.param_values[PARAM_RC2_MAX] = max_values[1]; - global_data_parameter_storage->pm.param_values[PARAM_RC3_MAX] = max_values[2]; - global_data_parameter_storage->pm.param_values[PARAM_RC4_MAX] = max_values[3]; - global_data_parameter_storage->pm.param_values[PARAM_RC5_MAX] = max_values[4]; - global_data_parameter_storage->pm.param_values[PARAM_RC6_MAX] = max_values[5]; - global_data_parameter_storage->pm.param_values[PARAM_RC7_MAX] = max_values[6]; - global_data_parameter_storage->pm.param_values[PARAM_RC8_MAX] = max_values[7]; - - - global_data_parameter_storage->pm.param_values[PARAM_RC1_TRIM] = mid_values[0]; - global_data_parameter_storage->pm.param_values[PARAM_RC2_TRIM] = mid_values[1]; - global_data_parameter_storage->pm.param_values[PARAM_RC3_TRIM] = mid_values[2]; - global_data_parameter_storage->pm.param_values[PARAM_RC4_TRIM] = mid_values[3]; - global_data_parameter_storage->pm.param_values[PARAM_RC5_TRIM] = mid_values[4]; - global_data_parameter_storage->pm.param_values[PARAM_RC6_TRIM] = mid_values[5]; - global_data_parameter_storage->pm.param_values[PARAM_RC7_TRIM] = mid_values[6]; - global_data_parameter_storage->pm.param_values[PARAM_RC8_TRIM] = mid_values[7]; - - usleep(3e6); - uint8_t i; - - for (i = 0; i < NR_CHANNELS; i++) { - printf("Channel %i:\t\t Min %i\t\t Max %i\t\t Scaling Factor: %i\t Middle Value %i\n", - i, - min_values[i], max_values[i], - global_data_rc_channels->chan[i].scaling_factor, - global_data_rc_channels->chan[i].mid); - } - - -} - -/**************************************************************************** - * Public Functions - ****************************************************************************/ -int range_cal(int argc, char *argv[]) -{ - - printf("The range calibration routine assumes you already did the channel\n"); - printf("assignment\n"); - printf("This routine chooses the minimum, maximum and middle\n"); - printf("value for each channel separatly. The ROLL, PITCH and YAW function\n"); - printf("get their middle value from the RC direct, for the rest it is\n"); - printf("calculated out of the min and max values.\n"); - press_enter(); - - printf("Hold both sticks in lower left corner and continue\n "); - press_enter(); - usleep(500000); - - while (get_value()); - - printf("Hold both sticks in upper right corner and continue\n"); - press_enter(); - usleep(500000); - - while (get_value()); - - printf("Set the trim to 0 and leave the sticks in the neutral position\n"); - press_enter(); - - //Loop until successfull - while (set_mid()); - - //write the values to global_data_rc_channels - write_data(); - - return 0; - -} - diff --git a/apps/systemcmds/calibration/servo_cal.c b/apps/systemcmds/calibration/servo_cal.c deleted file mode 100644 index 96b3045a95..0000000000 --- a/apps/systemcmds/calibration/servo_cal.c +++ /dev/null @@ -1,264 +0,0 @@ -/**************************************************************************** - * servo_cal.c - * - * Copyright (C) 2012 Nils Wenzler. All rights reserved. - * Authors: Nils Wenzler - * - * 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 NuttX 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. - * - ****************************************************************************/ - -/**************************************************************************** - * Included Files - ****************************************************************************/ -#include -#include -#include -#include -#include "calibration.h" - -/**************************************************************************** - * Defines - ****************************************************************************/ - -/**************************************************************************** - * Private Data - ****************************************************************************/ -//Store the values here before writing them to global_data_rc_channels -uint16_t max_values_servo[PWM_SERVO_MAX_CHANNELS]; -uint16_t min_values_servo[PWM_SERVO_MAX_CHANNELS]; -uint16_t mid_values_servo[PWM_SERVO_MAX_CHANNELS]; - -// Servo loop thread - -pthread_t servo_calib_thread; - -/**************************************************************************** - * Private Functions - ****************************************************************************/ - -/**This sets the middle values - */ -uint8_t set_mid_s(void) -{ - if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) { - uint8_t i; - - for (i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) { - if (i == global_data_rc_channels->function[ROLL] || - i == global_data_rc_channels->function[YAW] || - i == global_data_rc_channels->function[PITCH]) { - - mid_values_servo[i] = global_data_rc_channels->chan[i].raw; - - } else { - mid_values_servo[i] = (max_values_servo[i] + min_values_servo[i]) / 2; - } - - } - - global_data_unlock(&global_data_rc_channels->access_conf); - return 0; - - } else - return -1; -} - -/**This gets all current values and writes them to min or max - */ -uint8_t get_value_s(void) -{ - if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) { - uint8_t i; - - for (i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) { - //see if the value is bigger or smaller than 1500 (roughly neutral) - //and write to the appropriate array - if (global_data_rc_channels->chan[i].raw != 0 && - global_data_rc_channels->chan[i].raw < 1500) { - min_values_servo[i] = global_data_rc_channels->chan[i].raw; - - } else if (global_data_rc_channels->chan[i].raw != 0 && - global_data_rc_channels->chan[i].raw > 1500) { - max_values_servo[i] = global_data_rc_channels->chan[i].raw; - - } else { - max_values_servo[i] = 0; - min_values_servo[i] = 0; - } - } - - global_data_unlock(&global_data_rc_channels->access_conf); - return 0; - - } else - return -1; -} - - -void write_data_s(void) -{ - // global_data_lock(&global_data_rc_channels->access_conf); - // uint8_t i; - // for(i=0; i < NR_CHANNELS; i++){ - // //Write the data to global_data_rc_channels (if not 0) - // if(mid_values_servo[i]!=0 && min_values_servo[i]!=0 && max_values_servo[i]!=0){ - // global_data_rc_channels->chan[i].scaling_factor = - // 10000/((max_values_servo[i] - min_values_servo[i])/2); - // - // global_data_rc_channels->chan[i].mid = mid_values_servo[i]; - // } - // - // printf("Channel %i\t Function %s \t\t Min %i\t\t Max %i\t\t Scaling Factor: %i\t Middle Value %i\n", - // i, - // global_data_rc_channels->function_name[global_data_rc_channels->function[i]], - // min_values_servo[i], max_values_servo[i], - // global_data_rc_channels->chan[i].scaling_factor, - // global_data_rc_channels->chan[i].mid); - // } - // global_data_unlock(&global_data_rc_channels->access_conf); - - //Write to the Parameter storage - - - - global_data_lock(&global_data_parameter_storage->access_conf); - - global_data_parameter_storage->pm.param_values[PARAM_SERVO1_MIN] = min_values_servo[0]; - global_data_parameter_storage->pm.param_values[PARAM_SERVO2_MIN] = min_values_servo[1]; - global_data_parameter_storage->pm.param_values[PARAM_SERVO3_MIN] = min_values_servo[2]; - global_data_parameter_storage->pm.param_values[PARAM_SERVO4_MIN] = min_values_servo[3]; - - global_data_parameter_storage->pm.param_values[PARAM_SERVO1_MAX] = max_values_servo[0]; - global_data_parameter_storage->pm.param_values[PARAM_SERVO2_MAX] = max_values_servo[1]; - global_data_parameter_storage->pm.param_values[PARAM_SERVO3_MAX] = max_values_servo[2]; - global_data_parameter_storage->pm.param_values[PARAM_SERVO4_MAX] = max_values_servo[3]; - - global_data_parameter_storage->pm.param_values[PARAM_SERVO1_TRIM] = mid_values_servo[0]; - global_data_parameter_storage->pm.param_values[PARAM_SERVO2_TRIM] = mid_values_servo[1]; - global_data_parameter_storage->pm.param_values[PARAM_SERVO3_TRIM] = mid_values_servo[2]; - global_data_parameter_storage->pm.param_values[PARAM_SERVO4_TRIM] = mid_values_servo[3]; - - global_data_unlock(&global_data_parameter_storage->access_conf); - - usleep(3e6); - uint8_t i; - - for (i = 0; i < NR_CHANNELS; i++) { - printf("Channel %i:\t\t Min %i\t\t Max %i\t\t Scaling Factor: %i\t Middle Value %i\n", - i, - min_values_servo[i], max_values_servo[i], - global_data_rc_channels->chan[i].scaling_factor, - global_data_rc_channels->chan[i].mid); - } - -} - -static void *servo_loop(void *arg) -{ - - // Set thread name - prctl(1, "calibration servo", getpid()); - - // initialize servos - int fd; - servo_position_t data[PWM_SERVO_MAX_CHANNELS]; - - fd = open("/dev/pwm_servo", O_RDWR); - - if (fd < 0) { - printf("failed opening /dev/pwm_servo\n"); - } - - ioctl(fd, PWM_SERVO_ARM, 0); - - while (1) { - int i; - - for (i = 0; i < 4; i++) { - data[i] = global_data_rc_channels->chan[global_data_rc_channels->function[THROTTLE]].raw; - } - - int result = write(fd, &data, sizeof(data)); - - if (result != sizeof(data)) { - printf("failed bulk-reading channel values\n"); - } - - // 5Hz loop - usleep(200000); - } - - return NULL; -} - - -/**************************************************************************** - * Public Functions - ****************************************************************************/ -int servo_cal(int argc, char *argv[]) -{ - // pthread_attr_t servo_loop_attr; - // pthread_attr_init(&servo_loop_attr); - // pthread_attr_setstacksize(&servo_loop_attr, 1024); - pthread_create(&servo_calib_thread, NULL, servo_loop, NULL); - pthread_join(servo_calib_thread, NULL); - - printf("The servo calibration routine assumes you already did the channel\n"); - printf("assignment with 'calibration channels'\n"); - printf("This routine chooses the minimum, maximum and middle\n"); - printf("value for each channel separately. The ROLL, PITCH and YAW function\n"); - printf("get their middle value from the RC direct, for the rest it is\n"); - printf("calculated out of the min and max values.\n"); - press_enter(); - - printf("Hold both sticks in lower left corner and continue\n "); - press_enter(); - usleep(500000); - - while (get_value_s()); - - printf("Hold both sticks in upper right corner and continue\n"); - press_enter(); - usleep(500000); - - while (get_value_s()); - - printf("Set the trim to 0 and leave the sticks in the neutral position\n"); - press_enter(); - - //Loop until successfull - while (set_mid_s()); - - //write the values to global_data_rc_channels - write_data_s(); - - return 0; - -} - diff --git a/apps/systemcmds/top/Makefile b/apps/systemcmds/top/Makefile deleted file mode 100644 index f58f9212e7..0000000000 --- a/apps/systemcmds/top/Makefile +++ /dev/null @@ -1,44 +0,0 @@ -############################################################################ -# -# Copyright (C) 2012 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. -# -############################################################################ - -# -# realtime system performance display -# - -APPNAME = top -PRIORITY = SCHED_PRIORITY_DEFAULT - 10 -STACKSIZE = 3000 - -include $(APPDIR)/mk/app.mk - -MAXOPTIMIZATION = -Os diff --git a/makefiles/config_px4fmu_default.mk b/makefiles/config_px4fmu_default.mk index ada6b7ab7d..037ebe2b9b 100644 --- a/makefiles/config_px4fmu_default.mk +++ b/makefiles/config_px4fmu_default.mk @@ -15,7 +15,21 @@ MODULES += drivers/boards/px4fmu MODULES += drivers/lsm303d MODULES += drivers/l3gd20 MODULES += drivers/ardrone_interface + +# +# System commands +# MODULES += systemcmds/eeprom +MODULES += systemcmds/bl_update +MODULES += systemcmds/boardinfo +MODULES += systemcmds/i2c +MODULES += systemcmds/mixer +MODULES += systemcmds/param +MODULES += systemcmds/perf +MODULES += systemcmds/preflight_check +MODULES += systemcmds/pwm +MODULES += systemcmds/reboot +MODULES += systemcmds/top # # General system control @@ -44,12 +58,9 @@ endef # command priority stack entrypoint BUILTIN_COMMANDS := \ $(call _B, adc, , 2048, adc_main ) \ - $(call _B, bl_update, , 4096, bl_update_main ) \ $(call _B, blinkm, , 2048, blinkm_main ) \ $(call _B, bma180, , 2048, bma180_main ) \ - $(call _B, boardinfo, , 2048, boardinfo_main ) \ $(call _B, control_demo, , 2048, control_demo_main ) \ - $(call _B, delay_test, , 2048, delay_test_main ) \ $(call _B, fixedwing_att_control, SCHED_PRIORITY_MAX-30, 2048, fixedwing_att_control_main ) \ $(call _B, fixedwing_pos_control, SCHED_PRIORITY_MAX-30, 2048, fixedwing_pos_control_main ) \ $(call _B, gps, , 2048, gps_main ) \ @@ -58,23 +69,16 @@ BUILTIN_COMMANDS := \ $(call _B, hott_telemetry, , 2048, hott_telemetry_main ) \ $(call _B, kalman_demo, SCHED_PRIORITY_MAX-30, 2048, kalman_demo_main ) \ $(call _B, math_demo, , 8192, math_demo_main ) \ - $(call _B, mixer, , 4096, mixer_main ) \ $(call _B, mpu6000, , 4096, mpu6000_main ) \ $(call _B, ms5611, , 2048, ms5611_main ) \ $(call _B, multirotor_att_control, SCHED_PRIORITY_MAX-15, 2048, multirotor_att_control_main) \ $(call _B, multirotor_pos_control, SCHED_PRIORITY_MAX-25, 2048, multirotor_pos_control_main) \ - $(call _B, param, , 4096, param_main ) \ - $(call _B, perf, , 2048, perf_main ) \ $(call _B, position_estimator, , 4096, position_estimator_main ) \ - $(call _B, preflight_check, , 2048, preflight_check_main ) \ $(call _B, px4io, , 2048, px4io_main ) \ - $(call _B, reboot, , 2048, reboot_main ) \ $(call _B, sdlog, SCHED_PRIORITY_MAX-30, 2048, sdlog_main ) \ $(call _B, sensors, SCHED_PRIORITY_MAX-5, 4096, sensors_main ) \ $(call _B, sercon, , 2048, sercon_main ) \ $(call _B, serdis, , 2048, serdis_main ) \ $(call _B, tests, , 12000, tests_main ) \ $(call _B, tone_alarm, , 2048, tone_alarm_main ) \ - $(call _B, top, SCHED_PRIORITY_DEFAULT-10, 3000, top_main ) \ - $(call _B, param, SCHED_PRIORITY_DEFAULT-10, 2048, param_main ) \ $(call _B, uorb, , 4096, uorb_main ) diff --git a/makefiles/config_px4fmuv2_default.mk b/makefiles/config_px4fmuv2_default.mk index 9aa4ec3daf..5b3786ce2a 100644 --- a/makefiles/config_px4fmuv2_default.mk +++ b/makefiles/config_px4fmuv2_default.mk @@ -15,7 +15,20 @@ MODULES += drivers/lsm303d MODULES += drivers/l3gd20 MODULES += drivers/px4fmu MODULES += drivers/rgbled + +# +# System commands +# MODULES += systemcmds/ramtron +MODULES += systemcmds/bl_update +MODULES += systemcmds/boardinfo +MODULES += systemcmds/mixer +MODULES += systemcmds/param +MODULES += systemcmds/perf +MODULES += systemcmds/preflight_check +MODULES += systemcmds/pwm +MODULES += systemcmds/reboot +MODULES += systemcmds/top # # General system control @@ -44,12 +57,8 @@ endef # command priority stack entrypoint BUILTIN_COMMANDS := \ $(call _B, adc, , 2048, adc_main ) \ - $(call _B, bl_update, , 4096, bl_update_main ) \ $(call _B, blinkm, , 2048, blinkm_main ) \ - $(call _B, boardinfo, , 2048, boardinfo_main ) \ - $(call _B, commander, SCHED_PRIORITY_MAX-30, 2048, commander_main ) \ $(call _B, control_demo, , 2048, control_demo_main ) \ - $(call _B, delay_test, , 2048, delay_test_main ) \ $(call _B, fixedwing_att_control, SCHED_PRIORITY_MAX-30, 2048, fixedwing_att_control_main ) \ $(call _B, fixedwing_pos_control, SCHED_PRIORITY_MAX-30, 2048, fixedwing_pos_control_main ) \ $(call _B, gps, , 2048, gps_main ) \ @@ -57,19 +66,13 @@ BUILTIN_COMMANDS := \ $(call _B, hott_telemetry, , 2048, hott_telemetry_main ) \ $(call _B, kalman_demo, SCHED_PRIORITY_MAX-30, 2048, kalman_demo_main ) \ $(call _B, math_demo, , 8192, math_demo_main ) \ - $(call _B, mixer, , 4096, mixer_main ) \ $(call _B, multirotor_att_control, SCHED_PRIORITY_MAX-15, 2048, multirotor_att_control_main) \ $(call _B, multirotor_pos_control, SCHED_PRIORITY_MAX-25, 2048, multirotor_pos_control_main) \ - $(call _B, perf, , 2048, perf_main ) \ $(call _B, position_estimator, , 4096, position_estimator_main ) \ - $(call _B, preflight_check, , 2048, preflight_check_main ) \ - $(call _B, reboot, , 2048, reboot_main ) \ $(call _B, sdlog, SCHED_PRIORITY_MAX-30, 2048, sdlog_main ) \ $(call _B, sensors, SCHED_PRIORITY_MAX-5, 4096, sensors_main ) \ $(call _B, sercon, , 2048, sercon_main ) \ $(call _B, serdis, , 2048, serdis_main ) \ $(call _B, tests, , 12000, tests_main ) \ $(call _B, tone_alarm, , 2048, tone_alarm_main ) \ - $(call _B, top, SCHED_PRIORITY_DEFAULT-10, 3000, top_main ) \ - $(call _B, param, SCHED_PRIORITY_DEFAULT-10, 2048, param_main ) \ $(call _B, uorb, , 4096, uorb_main ) diff --git a/src/modules/mavlink_onboard/module.mk b/src/modules/mavlink_onboard/module.mk index c40fa042e8..a7a4980faf 100644 --- a/src/modules/mavlink_onboard/module.mk +++ b/src/modules/mavlink_onboard/module.mk @@ -37,6 +37,6 @@ MODULE_COMMAND = mavlink_onboard SRCS = mavlink.c \ - mavlink_receiver.c + mavlink_receiver.c INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/apps/systemcmds/bl_update/bl_update.c b/src/systemcmds/bl_update/bl_update.c similarity index 98% rename from apps/systemcmds/bl_update/bl_update.c rename to src/systemcmds/bl_update/bl_update.c index 45715b7918..0569d89f50 100644 --- a/apps/systemcmds/bl_update/bl_update.c +++ b/src/systemcmds/bl_update/bl_update.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/apps/systemcmds/bl_update/Makefile b/src/systemcmds/bl_update/module.mk similarity index 88% rename from apps/systemcmds/bl_update/Makefile rename to src/systemcmds/bl_update/module.mk index d05493577a..e8eea045e5 100644 --- a/apps/systemcmds/bl_update/Makefile +++ b/src/systemcmds/bl_update/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -32,13 +32,12 @@ ############################################################################ # -# Build the eeprom tool. +# Bootloader updater (flashes the FMU USB bootloader software) # -APPNAME = bl_update -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 4096 +MODULE_COMMAND = bl_update +SRCS = bl_update.c -include $(APPDIR)/mk/app.mk +MODULE_STACKSIZE = 4096 MAXOPTIMIZATION = -Os diff --git a/apps/systemcmds/boardinfo/.context b/src/systemcmds/boardinfo/.context similarity index 100% rename from apps/systemcmds/boardinfo/.context rename to src/systemcmds/boardinfo/.context diff --git a/apps/systemcmds/boardinfo/boardinfo.c b/src/systemcmds/boardinfo/boardinfo.c similarity index 99% rename from apps/systemcmds/boardinfo/boardinfo.c rename to src/systemcmds/boardinfo/boardinfo.c index fb8b07ef4f..3ff5a066c1 100644 --- a/apps/systemcmds/boardinfo/boardinfo.c +++ b/src/systemcmds/boardinfo/boardinfo.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/apps/systemcmds/preflight_check/Makefile b/src/systemcmds/boardinfo/module.mk similarity index 89% rename from apps/systemcmds/preflight_check/Makefile rename to src/systemcmds/boardinfo/module.mk index 98aadaa86f..6851d229b2 100644 --- a/apps/systemcmds/preflight_check/Makefile +++ b/src/systemcmds/boardinfo/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -32,13 +32,10 @@ ############################################################################ # -# Reboot command. +# Information about FMU and IO boards connected # -APPNAME = preflight_check -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 - -include $(APPDIR)/mk/app.mk +MODULE_COMMAND = boardinfo +SRCS = boardinfo.c MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/eeprom/module.mk b/src/systemcmds/eeprom/module.mk index 3b4fc04791..07f3945be2 100644 --- a/src/systemcmds/eeprom/module.mk +++ b/src/systemcmds/eeprom/module.mk @@ -1,3 +1,36 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + # # EEPROM file system driver # diff --git a/apps/systemcmds/i2c/i2c.c b/src/systemcmds/i2c/i2c.c similarity index 98% rename from apps/systemcmds/i2c/i2c.c rename to src/systemcmds/i2c/i2c.c index e1babdc12b..4da238039c 100644 --- a/apps/systemcmds/i2c/i2c.c +++ b/src/systemcmds/i2c/i2c.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. * Author: Lorenz Meier * * Redistribution and use in source and binary forms, with or without diff --git a/apps/systemcmds/i2c/Makefile b/src/systemcmds/i2c/module.mk similarity index 91% rename from apps/systemcmds/i2c/Makefile rename to src/systemcmds/i2c/module.mk index 046e747571..ab2357c7d0 100644 --- a/apps/systemcmds/i2c/Makefile +++ b/src/systemcmds/i2c/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -35,8 +35,7 @@ # Build the i2c test tool. # -APPNAME = i2c -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 4096 +MODULE_COMMAND = i2c +SRCS = i2c.c -include $(APPDIR)/mk/app.mk +MAXOPTIMIZATION = -Os diff --git a/apps/systemcmds/mixer/mixer.c b/src/systemcmds/mixer/mixer.c similarity index 100% rename from apps/systemcmds/mixer/mixer.c rename to src/systemcmds/mixer/mixer.c diff --git a/apps/systemcmds/mixer/Makefile b/src/systemcmds/mixer/module.mk similarity index 91% rename from apps/systemcmds/mixer/Makefile rename to src/systemcmds/mixer/module.mk index 3d8ac38cb7..d5a3f9f77b 100644 --- a/apps/systemcmds/mixer/Makefile +++ b/src/systemcmds/mixer/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -35,10 +35,9 @@ # Build the mixer tool. # -APPNAME = mixer -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 4096 +MODULE_COMMAND = mixer +SRCS = mixer.c -include $(APPDIR)/mk/app.mk +MODULE_STACKSIZE = 4096 MAXOPTIMIZATION = -Os diff --git a/apps/systemcmds/param/Makefile b/src/systemcmds/param/module.mk similarity index 91% rename from apps/systemcmds/param/Makefile rename to src/systemcmds/param/module.mk index f19cadbb60..63f15ad286 100644 --- a/apps/systemcmds/param/Makefile +++ b/src/systemcmds/param/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -35,10 +35,10 @@ # Build the parameters tool. # -APPNAME = param -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 4096 +MODULE_COMMAND = param +SRCS = param.c -include $(APPDIR)/mk/app.mk +MODULE_STACKSIZE = 4096 MAXOPTIMIZATION = -Os + diff --git a/apps/systemcmds/param/param.c b/src/systemcmds/param/param.c similarity index 100% rename from apps/systemcmds/param/param.c rename to src/systemcmds/param/param.c diff --git a/apps/systemcmds/perf/.context b/src/systemcmds/perf/.context similarity index 100% rename from apps/systemcmds/perf/.context rename to src/systemcmds/perf/.context diff --git a/apps/systemcmds/perf/Makefile b/src/systemcmds/perf/module.mk similarity index 91% rename from apps/systemcmds/perf/Makefile rename to src/systemcmds/perf/module.mk index f8bab41b65..77952842b3 100644 --- a/apps/systemcmds/perf/Makefile +++ b/src/systemcmds/perf/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -35,10 +35,7 @@ # perf_counter reporting tool # -APPNAME = perf -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 - -include $(APPDIR)/mk/app.mk +MODULE_COMMAND = perf +SRCS = perf.c MAXOPTIMIZATION = -Os diff --git a/apps/systemcmds/perf/perf.c b/src/systemcmds/perf/perf.c similarity index 97% rename from apps/systemcmds/perf/perf.c rename to src/systemcmds/perf/perf.c index 64443d019c..b69ea597b1 100644 --- a/apps/systemcmds/perf/perf.c +++ b/src/systemcmds/perf/perf.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/apps/systemcmds/boardinfo/Makefile b/src/systemcmds/preflight_check/module.mk similarity index 86% rename from apps/systemcmds/boardinfo/Makefile rename to src/systemcmds/preflight_check/module.mk index 6f1be149c6..7c3c887836 100644 --- a/apps/systemcmds/boardinfo/Makefile +++ b/src/systemcmds/preflight_check/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -32,13 +32,11 @@ ############################################################################ # -# Build the boardinfo tool. +# Pre-flight check. Locks down system for a few systems with blinking leds +# and buzzer if the sensors do not report an OK status. # -APPNAME = boardinfo -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 - -include $(APPDIR)/mk/app.mk +MODULE_COMMAND = preflight_check +SRCS = preflight_check.c MAXOPTIMIZATION = -Os diff --git a/apps/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c similarity index 98% rename from apps/systemcmds/preflight_check/preflight_check.c rename to src/systemcmds/preflight_check/preflight_check.c index 9ac6f0b5e0..42256be61e 100644 --- a/apps/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. * Author: @author Lorenz Meier * * Redistribution and use in source and binary forms, with or without diff --git a/apps/systemcmds/pwm/Makefile b/src/systemcmds/pwm/module.mk similarity index 91% rename from apps/systemcmds/pwm/Makefile rename to src/systemcmds/pwm/module.mk index 5ab105ed12..4a23bba90e 100644 --- a/apps/systemcmds/pwm/Makefile +++ b/src/systemcmds/pwm/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -35,8 +35,7 @@ # Build the pwm tool. # -APPNAME = pwm -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 4096 +MODULE_COMMAND = pwm +SRCS = pwm.c -include $(APPDIR)/mk/app.mk +MODULE_STACKSIZE = 4096 diff --git a/apps/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c similarity index 100% rename from apps/systemcmds/pwm/pwm.c rename to src/systemcmds/pwm/pwm.c diff --git a/apps/systemcmds/reboot/.context b/src/systemcmds/reboot/.context similarity index 100% rename from apps/systemcmds/reboot/.context rename to src/systemcmds/reboot/.context diff --git a/src/systemcmds/reboot/module.mk b/src/systemcmds/reboot/module.mk new file mode 100644 index 0000000000..19f64af54f --- /dev/null +++ b/src/systemcmds/reboot/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# reboot command. +# + +MODULE_COMMAND = reboot +SRCS = reboot.c + +MAXOPTIMIZATION = -Os diff --git a/apps/systemcmds/reboot/reboot.c b/src/systemcmds/reboot/reboot.c similarity index 100% rename from apps/systemcmds/reboot/reboot.c rename to src/systemcmds/reboot/reboot.c diff --git a/apps/systemcmds/top/.context b/src/systemcmds/top/.context similarity index 100% rename from apps/systemcmds/top/.context rename to src/systemcmds/top/.context diff --git a/apps/systemcmds/reboot/Makefile b/src/systemcmds/top/module.mk similarity index 90% rename from apps/systemcmds/reboot/Makefile rename to src/systemcmds/top/module.mk index 15dd199829..9239aafc31 100644 --- a/apps/systemcmds/reboot/Makefile +++ b/src/systemcmds/top/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -32,13 +32,13 @@ ############################################################################ # -# Reboot command. +# Build top memory / cpu status tool. # -APPNAME = reboot -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 +MODULE_COMMAND = top +SRCS = top.c -include $(APPDIR)/mk/app.mk +MODULE_STACKSIZE = 3000 MAXOPTIMIZATION = -Os + diff --git a/apps/systemcmds/top/top.c b/src/systemcmds/top/top.c similarity index 100% rename from apps/systemcmds/top/top.c rename to src/systemcmds/top/top.c From a85eb8cdc12305b17a6d7de5f00327f2a8e096ff Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 27 Apr 2013 13:40:11 +0200 Subject: [PATCH 099/486] Updated config file to reflect current app state --- makefiles/config_px4fmuv2_default.mk | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/makefiles/config_px4fmuv2_default.mk b/makefiles/config_px4fmuv2_default.mk index 5b3786ce2a..d786f9dc0e 100644 --- a/makefiles/config_px4fmuv2_default.mk +++ b/makefiles/config_px4fmuv2_default.mk @@ -10,11 +10,12 @@ ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common # # Board support modules # +MODULES += drivers/px4fmu MODULES += drivers/boards/px4fmuv2 +MODULES += drivers/rgbled MODULES += drivers/lsm303d MODULES += drivers/l3gd20 -MODULES += drivers/px4fmu -MODULES += drivers/rgbled +MODULES += modules/sensors # # System commands @@ -29,6 +30,7 @@ MODULES += systemcmds/preflight_check MODULES += systemcmds/pwm MODULES += systemcmds/reboot MODULES += systemcmds/top +MODULES += systemcmds/tests # # General system control @@ -41,6 +43,12 @@ MODULES += modules/mavlink_onboard # Estimation modules (EKF / other filters) # MODULES += modules/attitude_estimator_ekf +MODULES += modules/position_estimator_mc + +# +# Logging +# +MODULES += modules/sdlog # # Transitional support - add commands from the NuttX export archive. @@ -69,10 +77,7 @@ BUILTIN_COMMANDS := \ $(call _B, multirotor_att_control, SCHED_PRIORITY_MAX-15, 2048, multirotor_att_control_main) \ $(call _B, multirotor_pos_control, SCHED_PRIORITY_MAX-25, 2048, multirotor_pos_control_main) \ $(call _B, position_estimator, , 4096, position_estimator_main ) \ - $(call _B, sdlog, SCHED_PRIORITY_MAX-30, 2048, sdlog_main ) \ - $(call _B, sensors, SCHED_PRIORITY_MAX-5, 4096, sensors_main ) \ $(call _B, sercon, , 2048, sercon_main ) \ $(call _B, serdis, , 2048, serdis_main ) \ - $(call _B, tests, , 12000, tests_main ) \ $(call _B, tone_alarm, , 2048, tone_alarm_main ) \ $(call _B, uorb, , 4096, uorb_main ) From f03ba89557417f9805ba95c13936d358a26af7d9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 27 Apr 2013 18:27:17 +0200 Subject: [PATCH 100/486] Config adjustments for v2 --- makefiles/config_px4fmuv2_default.mk | 19 +++++++++++++++++++ nuttx/configs/px4fmuv2/include/board.h | 1 + 2 files changed, 20 insertions(+) diff --git a/makefiles/config_px4fmuv2_default.mk b/makefiles/config_px4fmuv2_default.mk index d786f9dc0e..68e5fa9166 100644 --- a/makefiles/config_px4fmuv2_default.mk +++ b/makefiles/config_px4fmuv2_default.mk @@ -15,6 +15,14 @@ MODULES += drivers/boards/px4fmuv2 MODULES += drivers/rgbled MODULES += drivers/lsm303d MODULES += drivers/l3gd20 +MODULES += drivers/mpu6000 +MODULES += drivers/hmc5883 +MODULES += drivers/ms5611 +MODULES += drivers/mb12xx +MODULES += drivers/gps +MODULES += drivers/hil +MODULES += drivers/hott_telemetry +MODULES += drivers/blinkm MODULES += modules/sensors # @@ -44,6 +52,17 @@ MODULES += modules/mavlink_onboard # MODULES += modules/attitude_estimator_ekf MODULES += modules/position_estimator_mc +MODULES += modules/position_estimator +MODULES += modules/att_pos_estimator_ekf + +# +# Vehicle Control +# +MODULES += modules/fixedwing_backside +MODULES += modules/fixedwing_att_control +MODULES += modules/fixedwing_pos_control +MODULES += modules/multirotor_att_control +MODULES += modules/multirotor_pos_control # # Logging diff --git a/nuttx/configs/px4fmuv2/include/board.h b/nuttx/configs/px4fmuv2/include/board.h index be4cdcdfdc..0055d65e17 100755 --- a/nuttx/configs/px4fmuv2/include/board.h +++ b/nuttx/configs/px4fmuv2/include/board.h @@ -295,6 +295,7 @@ * Note that these are unshifted addresses. */ #define PX4_I2C_OBDEV_LED 0x55 +#define PX4_I2C_OBDEV_HMC5883 0x1e /* * SPI From 86cf4d075dae90db4a681261f4e378d41d8bcf49 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 27 Apr 2013 19:50:25 +0200 Subject: [PATCH 101/486] Makefile cleanup --- makefiles/config_px4fmuv2_default.mk | 15 ++------------- 1 file changed, 2 insertions(+), 13 deletions(-) diff --git a/makefiles/config_px4fmuv2_default.mk b/makefiles/config_px4fmuv2_default.mk index 68e5fa9166..47cd097486 100644 --- a/makefiles/config_px4fmuv2_default.mk +++ b/makefiles/config_px4fmuv2_default.mk @@ -15,9 +15,9 @@ MODULES += drivers/boards/px4fmuv2 MODULES += drivers/rgbled MODULES += drivers/lsm303d MODULES += drivers/l3gd20 -MODULES += drivers/mpu6000 +#MODULES += drivers/mpu6000 MODULES += drivers/hmc5883 -MODULES += drivers/ms5611 +#MODULES += drivers/ms5611 MODULES += drivers/mb12xx MODULES += drivers/gps MODULES += drivers/hil @@ -84,18 +84,7 @@ endef # command priority stack entrypoint BUILTIN_COMMANDS := \ $(call _B, adc, , 2048, adc_main ) \ - $(call _B, blinkm, , 2048, blinkm_main ) \ - $(call _B, control_demo, , 2048, control_demo_main ) \ - $(call _B, fixedwing_att_control, SCHED_PRIORITY_MAX-30, 2048, fixedwing_att_control_main ) \ - $(call _B, fixedwing_pos_control, SCHED_PRIORITY_MAX-30, 2048, fixedwing_pos_control_main ) \ - $(call _B, gps, , 2048, gps_main ) \ - $(call _B, hil, , 2048, hil_main ) \ - $(call _B, hott_telemetry, , 2048, hott_telemetry_main ) \ - $(call _B, kalman_demo, SCHED_PRIORITY_MAX-30, 2048, kalman_demo_main ) \ $(call _B, math_demo, , 8192, math_demo_main ) \ - $(call _B, multirotor_att_control, SCHED_PRIORITY_MAX-15, 2048, multirotor_att_control_main) \ - $(call _B, multirotor_pos_control, SCHED_PRIORITY_MAX-25, 2048, multirotor_pos_control_main) \ - $(call _B, position_estimator, , 4096, position_estimator_main ) \ $(call _B, sercon, , 2048, sercon_main ) \ $(call _B, serdis, , 2048, serdis_main ) \ $(call _B, tone_alarm, , 2048, tone_alarm_main ) \ From 13d58afd0a57698710c18fd1ffc6192d6aabe07a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 28 Apr 2013 11:03:25 +0200 Subject: [PATCH 102/486] Updated FMUv2 config --- makefiles/config_px4fmuv2_default.mk | 36 ++++++++++++++++++++++++---- 1 file changed, 31 insertions(+), 5 deletions(-) diff --git a/makefiles/config_px4fmuv2_default.mk b/makefiles/config_px4fmuv2_default.mk index 47cd097486..ab1f19314e 100644 --- a/makefiles/config_px4fmuv2_default.mk +++ b/makefiles/config_px4fmuv2_default.mk @@ -10,6 +10,10 @@ ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common # # Board support modules # +MODULES += drivers/device +MODULES += drivers/stm32 +MODULES += drivers/stm32/adc +MODULES += drivers/stm32/tone_alarm MODULES += drivers/px4fmu MODULES += drivers/boards/px4fmuv2 MODULES += drivers/rgbled @@ -69,6 +73,32 @@ MODULES += modules/multirotor_pos_control # MODULES += modules/sdlog +# +# Libraries +# +MODULES += modules/systemlib +MODULES += modules/systemlib/mixer +MODULES += modules/mathlib +MODULES += modules/mathlib/CMSIS +MODULES += modules/controllib +MODULES += modules/uORB + +# +# Demo apps +# +#MODULES += examples/math_demo +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/hello_sky +#MODULES += examples/px4_simple_app + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/daemon +#MODULES += examples/px4_daemon_app + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/debug_values +#MODULES += examples/px4_mavlink_debug + # # Transitional support - add commands from the NuttX export archive. # @@ -83,9 +113,5 @@ endef # command priority stack entrypoint BUILTIN_COMMANDS := \ - $(call _B, adc, , 2048, adc_main ) \ - $(call _B, math_demo, , 8192, math_demo_main ) \ $(call _B, sercon, , 2048, sercon_main ) \ - $(call _B, serdis, , 2048, serdis_main ) \ - $(call _B, tone_alarm, , 2048, tone_alarm_main ) \ - $(call _B, uorb, , 4096, uorb_main ) + $(call _B, serdis, , 2048, serdis_main ) From 1df5e98aa507c7a89f1491254d7f34f94c04ede6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 28 Apr 2013 14:50:05 +0200 Subject: [PATCH 103/486] XXX: WIP: Disabled mixer on IOv2 due to CXX compile issue --- Images/px4iov2.prototype | 12 + apps/drivers/boards/px4iov2/Makefile | 41 -- makefiles/board_px4iov2.mk | 10 + makefiles/config_px4iov2_default.mk | 10 + nuttx/configs/px4fmuv2/nsh/appconfig | 86 --- nuttx/configs/px4iov2/include/board.h | 11 + .../configs/px4iov2/include/drv_i2c_device.h | 42 -- nuttx/configs/px4iov2/io/appconfig | 8 - nuttx/configs/px4iov2/io/defconfig | 2 +- nuttx/configs/px4iov2/src/drv_i2c_device.c | 618 ------------------ src/drivers/boards/px4iov2/module.mk | 6 + src/drivers/boards/px4iov2/px4iov2_init.c | 171 +++++ src/drivers/boards/px4iov2/px4iov2_internal.h | 112 ++++ .../boards/px4iov2/px4iov2_pwm_servo.c | 123 ++++ 14 files changed, 456 insertions(+), 796 deletions(-) create mode 100644 Images/px4iov2.prototype delete mode 100644 apps/drivers/boards/px4iov2/Makefile create mode 100644 makefiles/board_px4iov2.mk create mode 100644 makefiles/config_px4iov2_default.mk delete mode 100644 nuttx/configs/px4iov2/include/drv_i2c_device.h delete mode 100644 nuttx/configs/px4iov2/src/drv_i2c_device.c create mode 100644 src/drivers/boards/px4iov2/module.mk create mode 100644 src/drivers/boards/px4iov2/px4iov2_init.c create mode 100755 src/drivers/boards/px4iov2/px4iov2_internal.h create mode 100644 src/drivers/boards/px4iov2/px4iov2_pwm_servo.c diff --git a/Images/px4iov2.prototype b/Images/px4iov2.prototype new file mode 100644 index 0000000000..af87924e90 --- /dev/null +++ b/Images/px4iov2.prototype @@ -0,0 +1,12 @@ +{ + "board_id": 10, + "magic": "PX4FWv2", + "description": "Firmware for the PX4IOv2 board", + "image": "", + "build_time": 0, + "summary": "PX4IOv2", + "version": "2.0", + "image_size": 0, + "git_identity": "", + "board_revision": 0 +} diff --git a/apps/drivers/boards/px4iov2/Makefile b/apps/drivers/boards/px4iov2/Makefile deleted file mode 100644 index 85806fe6f5..0000000000 --- a/apps/drivers/boards/px4iov2/Makefile +++ /dev/null @@ -1,41 +0,0 @@ -############################################################################ -# -# Copyright (C) 2012 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. -# -############################################################################ - -# -# Board-specific startup code for the PX4IO -# - -INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common -LIBNAME = brd_px4io - -include $(APPDIR)/mk/app.mk diff --git a/makefiles/board_px4iov2.mk b/makefiles/board_px4iov2.mk new file mode 100644 index 0000000000..ee6b6125e0 --- /dev/null +++ b/makefiles/board_px4iov2.mk @@ -0,0 +1,10 @@ +# +# Board-specific definitions for the PX4IOv2 +# + +# +# Configure the toolchain +# +CONFIG_ARCH = CORTEXM3 + +include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk diff --git a/makefiles/config_px4iov2_default.mk b/makefiles/config_px4iov2_default.mk new file mode 100644 index 0000000000..f9f35d629d --- /dev/null +++ b/makefiles/config_px4iov2_default.mk @@ -0,0 +1,10 @@ +# +# Makefile for the px4iov2_default configuration +# + +# +# Board support modules +# +MODULES += drivers/stm32 +MODULES += drivers/boards/px4iov2 +MODULES += modules/px4iofirmware \ No newline at end of file diff --git a/nuttx/configs/px4fmuv2/nsh/appconfig b/nuttx/configs/px4fmuv2/nsh/appconfig index d945f3d889..0e18aa8ef1 100644 --- a/nuttx/configs/px4fmuv2/nsh/appconfig +++ b/nuttx/configs/px4fmuv2/nsh/appconfig @@ -41,92 +41,6 @@ CONFIGURED_APPS += examples/nsh CONFIGURED_APPS += nshlib CONFIGURED_APPS += system/readline -# System library - utility functions -CONFIGURED_APPS += systemlib -CONFIGURED_APPS += systemlib/mixer - -# Math library -ifneq ($(CONFIG_APM),y) -CONFIGURED_APPS += mathlib -CONFIGURED_APPS += mathlib/CMSIS -CONFIGURED_APPS += examples/math_demo -endif - -# Control library -ifneq ($(CONFIG_APM),y) -CONFIGURED_APPS += controllib -CONFIGURED_APPS += examples/control_demo -CONFIGURED_APPS += examples/kalman_demo -endif - -# System utility commands -CONFIGURED_APPS += systemcmds/reboot -CONFIGURED_APPS += systemcmds/perf -CONFIGURED_APPS += systemcmds/top -CONFIGURED_APPS += systemcmds/boardinfo -CONFIGURED_APPS += systemcmds/mixer -CONFIGURED_APPS += systemcmds/param -CONFIGURED_APPS += systemcmds/pwm -CONFIGURED_APPS += systemcmds/bl_update -CONFIGURED_APPS += systemcmds/preflight_check -CONFIGURED_APPS += systemcmds/delay_test - -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/hello_sky -# CONFIGURED_APPS += examples/px4_simple_app - -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/deamon -# CONFIGURED_APPS += examples/px4_deamon_app - -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/debug_values -# CONFIGURED_APPS += examples/px4_mavlink_debug - -# Shared object broker; required by many parts of the system. -CONFIGURED_APPS += uORB - -CONFIGURED_APPS += mavlink -CONFIGURED_APPS += mavlink_onboard -CONFIGURED_APPS += commander -CONFIGURED_APPS += sdlog -CONFIGURED_APPS += sensors - -ifneq ($(CONFIG_APM),y) -CONFIGURED_APPS += multirotor_att_control -CONFIGURED_APPS += multirotor_pos_control -CONFIGURED_APPS += fixedwing_att_control -CONFIGURED_APPS += fixedwing_pos_control -CONFIGURED_APPS += position_estimator -CONFIGURED_APPS += attitude_estimator_ekf -CONFIGURED_APPS += hott_telemetry -endif - -# Hacking tools -# XXX needs updating for new i2c config -#CONFIGURED_APPS += systemcmds/i2c - -# Communication and Drivers -CONFIGURED_APPS += drivers/boards/px4fmuv2 -CONFIGURED_APPS += drivers/device -# XXX needs conversion to SPI -#CONFIGURED_APPS += drivers/ms5611 -# XXX needs conversion to serial -#CONFIGURED_APPS += drivers/px4io -CONFIGURED_APPS += drivers/stm32 -#CONFIGURED_APPS += drivers/led -CONFIGURED_APPS += drivers/blinkm -CONFIGURED_APPS += drivers/stm32/tone_alarm -CONFIGURED_APPS += drivers/stm32/adc -#CONFIGURED_APPS += drivers/px4fmu -CONFIGURED_APPS += drivers/hil -CONFIGURED_APPS += drivers/gps -CONFIGURED_APPS += drivers/mb12xx - -# Testing stuff -CONFIGURED_APPS += px4/sensors_bringup -CONFIGURED_APPS += px4/tests - ifeq ($(CONFIG_CAN),y) #CONFIGURED_APPS += examples/can endif diff --git a/nuttx/configs/px4iov2/include/board.h b/nuttx/configs/px4iov2/include/board.h index b8dc711442..21aacda871 100755 --- a/nuttx/configs/px4iov2/include/board.h +++ b/nuttx/configs/px4iov2/include/board.h @@ -100,12 +100,19 @@ * Some of the USART pins are not available; override the GPIO * definitions with an invalid pin configuration. */ +#undef GPIO_USART2_CTS #define GPIO_USART2_CTS 0xffffffff +#undef GPIO_USART2_RTS #define GPIO_USART2_RTS 0xffffffff +#undef GPIO_USART2_CK #define GPIO_USART2_CK 0xffffffff +#undef GPIO_USART3_TX #define GPIO_USART3_TX 0xffffffff +#undef GPIO_USART3_CK #define GPIO_USART3_CK 0xffffffff +#undef GPIO_USART3_CTS #define GPIO_USART3_CTS 0xffffffff +#undef GPIO_USART3_RTS #define GPIO_USART3_RTS 0xffffffff /* @@ -168,6 +175,10 @@ extern "C" { ************************************************************************************/ EXTERN void stm32_boardinitialize(void); + +#if defined(__cplusplus) +} +#endif #endif /* __ASSEMBLY__ */ #endif /* __ARCH_BOARD_BOARD_H */ diff --git a/nuttx/configs/px4iov2/include/drv_i2c_device.h b/nuttx/configs/px4iov2/include/drv_i2c_device.h deleted file mode 100644 index 02582bc092..0000000000 --- a/nuttx/configs/px4iov2/include/drv_i2c_device.h +++ /dev/null @@ -1,42 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 A simple, polled I2C slave-mode driver. - * - * The master writes to and reads from a byte buffer, which the caller - * can update inbetween calls to the FSM. - */ - -extern void i2c_fsm_init(uint8_t *buffer, size_t buffer_size); -extern bool i2c_fsm(void); diff --git a/nuttx/configs/px4iov2/io/appconfig b/nuttx/configs/px4iov2/io/appconfig index 628607a515..48a41bcdb8 100644 --- a/nuttx/configs/px4iov2/io/appconfig +++ b/nuttx/configs/px4iov2/io/appconfig @@ -30,11 +30,3 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ - -CONFIGURED_APPS += drivers/boards/px4io -CONFIGURED_APPS += drivers/stm32 - -CONFIGURED_APPS += px4io - -# Mixer library from systemlib -CONFIGURED_APPS += systemlib/mixer diff --git a/nuttx/configs/px4iov2/io/defconfig b/nuttx/configs/px4iov2/io/defconfig index bb937cf4ee..c73f0df896 100755 --- a/nuttx/configs/px4iov2/io/defconfig +++ b/nuttx/configs/px4iov2/io/defconfig @@ -127,7 +127,7 @@ CONFIG_STM32_WWDG=n CONFIG_STM32_SPI2=n CONFIG_STM32_USART2=y CONFIG_STM32_USART3=y -CONFIG_STM32_I2C1=y +CONFIG_STM32_I2C1=n CONFIG_STM32_I2C2=n CONFIG_STM32_BKP=n CONFIG_STM32_PWR=n diff --git a/nuttx/configs/px4iov2/src/drv_i2c_device.c b/nuttx/configs/px4iov2/src/drv_i2c_device.c deleted file mode 100644 index 1f5931ae5e..0000000000 --- a/nuttx/configs/px4iov2/src/drv_i2c_device.c +++ /dev/null @@ -1,618 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 A simple, polled I2C slave-mode driver. - * - * The master writes to and reads from a byte buffer, which the caller - * can update inbetween calls to the FSM. - */ - -#include - -#include "stm32_i2c.h" - -#include - -/* - * I2C register definitions. - */ -#define I2C_BASE STM32_I2C1_BASE - -#define REG(_reg) (*(volatile uint32_t *)(I2C_BASE + _reg)) - -#define rCR1 REG(STM32_I2C_CR1_OFFSET) -#define rCR2 REG(STM32_I2C_CR2_OFFSET) -#define rOAR1 REG(STM32_I2C_OAR1_OFFSET) -#define rOAR2 REG(STM32_I2C_OAR2_OFFSET) -#define rDR REG(STM32_I2C_DR_OFFSET) -#define rSR1 REG(STM32_I2C_SR1_OFFSET) -#define rSR2 REG(STM32_I2C_SR2_OFFSET) -#define rCCR REG(STM32_I2C_CCR_OFFSET) -#define rTRISE REG(STM32_I2C_TRISE_OFFSET) - -/* - * "event" values (cr2 << 16 | cr1) as described in the ST DriverLib - */ -#define I2C_EVENT_SLAVE_RECEIVER_ADDRESS_MATCHED ((uint32_t)0x00020002) /* BUSY and ADDR flags */ -#define I2C_EVENT_SLAVE_TRANSMITTER_ADDRESS_MATCHED ((uint32_t)0x00060082) /* TRA, BUSY, TXE and ADDR flags */ -#define I2C_EVENT_SLAVE_BYTE_RECEIVED ((uint32_t)0x00020040) /* BUSY and RXNE flags */ -#define I2C_EVENT_SLAVE_STOP_DETECTED ((uint32_t)0x00000010) /* STOPF flag */ -#define I2C_EVENT_SLAVE_BYTE_TRANSMITTED ((uint32_t)0x00060084) /* TRA, BUSY, TXE and BTF flags */ -#define I2C_EVENT_SLAVE_BYTE_TRANSMITTING ((uint32_t)0x00060080) /* TRA, BUSY and TXE flags */ -#define I2C_EVENT_SLAVE_ACK_FAILURE ((uint32_t)0x00000400) /* AF flag */ - -/** - * States implemented by the I2C FSM. - */ -enum fsm_state { - BAD_PHASE, // must be zero, default exit on a bad state transition - - WAIT_FOR_MASTER, - - /* write from master */ - WAIT_FOR_COMMAND, - RECEIVE_COMMAND, - RECEIVE_DATA, - HANDLE_COMMAND, - - /* read from master */ - WAIT_TO_SEND, - SEND_STATUS, - SEND_DATA, - - NUM_STATES -}; - -/** - * Events recognised by the I2C FSM. - */ -enum fsm_event { - /* automatic transition */ - AUTO, - - /* write from master */ - ADDRESSED_WRITE, - BYTE_RECEIVED, - STOP_RECEIVED, - - /* read from master */ - ADDRESSED_READ, - BYTE_SENDABLE, - ACK_FAILED, - - NUM_EVENTS -}; - -/** - * Context for the I2C FSM - */ -static struct fsm_context { - enum fsm_state state; - - /* XXX want to eliminate these */ - uint8_t command; - uint8_t status; - - uint8_t *data_ptr; - uint32_t data_count; - - size_t buffer_size; - uint8_t *buffer; -} context; - -/** - * Structure defining one FSM state and its outgoing transitions. - */ -struct fsm_transition { - void (*handler)(void); - enum fsm_state next_state[NUM_EVENTS]; -}; - -static bool i2c_command_received; - -static void fsm_event(enum fsm_event event); - -static void go_bad(void); -static void go_wait_master(void); - -static void go_wait_command(void); -static void go_receive_command(void); -static void go_receive_data(void); -static void go_handle_command(void); - -static void go_wait_send(void); -static void go_send_status(void); -static void go_send_buffer(void); - -/** - * The FSM state graph. - */ -static const struct fsm_transition fsm[NUM_STATES] = { - [BAD_PHASE] = { - .handler = go_bad, - .next_state = { - [AUTO] = WAIT_FOR_MASTER, - }, - }, - - [WAIT_FOR_MASTER] = { - .handler = go_wait_master, - .next_state = { - [ADDRESSED_WRITE] = WAIT_FOR_COMMAND, - [ADDRESSED_READ] = WAIT_TO_SEND, - }, - }, - - /* write from master*/ - [WAIT_FOR_COMMAND] = { - .handler = go_wait_command, - .next_state = { - [BYTE_RECEIVED] = RECEIVE_COMMAND, - [STOP_RECEIVED] = WAIT_FOR_MASTER, - }, - }, - [RECEIVE_COMMAND] = { - .handler = go_receive_command, - .next_state = { - [BYTE_RECEIVED] = RECEIVE_DATA, - [STOP_RECEIVED] = HANDLE_COMMAND, - }, - }, - [RECEIVE_DATA] = { - .handler = go_receive_data, - .next_state = { - [BYTE_RECEIVED] = RECEIVE_DATA, - [STOP_RECEIVED] = HANDLE_COMMAND, - }, - }, - [HANDLE_COMMAND] = { - .handler = go_handle_command, - .next_state = { - [AUTO] = WAIT_FOR_MASTER, - }, - }, - - /* buffer send */ - [WAIT_TO_SEND] = { - .handler = go_wait_send, - .next_state = { - [BYTE_SENDABLE] = SEND_STATUS, - }, - }, - [SEND_STATUS] = { - .handler = go_send_status, - .next_state = { - [BYTE_SENDABLE] = SEND_DATA, - [ACK_FAILED] = WAIT_FOR_MASTER, - }, - }, - [SEND_DATA] = { - .handler = go_send_buffer, - .next_state = { - [BYTE_SENDABLE] = SEND_DATA, - [ACK_FAILED] = WAIT_FOR_MASTER, - }, - }, -}; - - -/* debug support */ -#if 1 -struct fsm_logentry { - char kind; - uint32_t code; -}; - -#define LOG_ENTRIES 32 -static struct fsm_logentry fsm_log[LOG_ENTRIES]; -int fsm_logptr; -#define LOG_NEXT(_x) (((_x) + 1) % LOG_ENTRIES) -#define LOGx(_kind, _code) \ - do { \ - fsm_log[fsm_logptr].kind = _kind; \ - fsm_log[fsm_logptr].code = _code; \ - fsm_logptr = LOG_NEXT(fsm_logptr); \ - fsm_log[fsm_logptr].kind = 0; \ - } while(0) - -#define LOG(_kind, _code) \ - do {\ - if (fsm_logptr < LOG_ENTRIES) { \ - fsm_log[fsm_logptr].kind = _kind; \ - fsm_log[fsm_logptr].code = _code; \ - fsm_logptr++;\ - }\ - }while(0) - -#else -#define LOG(_kind, _code) -#endif - - -static void i2c_setclock(uint32_t frequency); - -/** - * Initialise I2C - * - */ -void -i2c_fsm_init(uint8_t *buffer, size_t buffer_size) -{ - /* save the buffer */ - context.buffer = buffer; - context.buffer_size = buffer_size; - - // initialise the FSM - context.status = 0; - context.command = 0; - context.state = BAD_PHASE; - fsm_event(AUTO); - -#if 0 - // enable the i2c block clock and reset it - modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_I2C1EN); - modifyreg32(STM32_RCC_APB1RSTR, 0, RCC_APB1RSTR_I2C1RST); - modifyreg32(STM32_RCC_APB1RSTR, RCC_APB1RSTR_I2C1RST, 0); - - // configure the i2c GPIOs - stm32_configgpio(GPIO_I2C1_SCL); - stm32_configgpio(GPIO_I2C1_SDA); - - // set the peripheral clock to match the APB clock - rCR2 = STM32_PCLK1_FREQUENCY / 1000000; - - // configure for 100kHz operation - i2c_setclock(100000); - - // enable i2c - rCR1 = I2C_CR1_PE; -#endif -} - -/** - * Run the I2C FSM for some period. - * - * @return True if the buffer has been updated by a command. - */ -bool -i2c_fsm(void) -{ - uint32_t event; - int idle_iterations = 0; - - for (;;) { - // handle bus error states by discarding the current operation - if (rSR1 & I2C_SR1_BERR) { - context.state = WAIT_FOR_MASTER; - rSR1 = ~I2C_SR1_BERR; - } - - // we do not anticipate over/underrun errors as clock-stretching is enabled - - // fetch the most recent event - event = ((rSR2 << 16) | rSR1) & 0x00ffffff; - - // generate FSM events based on I2C events - switch (event) { - case I2C_EVENT_SLAVE_RECEIVER_ADDRESS_MATCHED: - LOG('w', 0); - fsm_event(ADDRESSED_WRITE); - break; - - case I2C_EVENT_SLAVE_TRANSMITTER_ADDRESS_MATCHED: - LOG('r', 0); - fsm_event(ADDRESSED_READ); - break; - - case I2C_EVENT_SLAVE_BYTE_RECEIVED: - LOG('R', 0); - fsm_event(BYTE_RECEIVED); - break; - - case I2C_EVENT_SLAVE_STOP_DETECTED: - LOG('s', 0); - fsm_event(STOP_RECEIVED); - break; - - case I2C_EVENT_SLAVE_BYTE_TRANSMITTING: - //case I2C_EVENT_SLAVE_BYTE_TRANSMITTED: - LOG('T', 0); - fsm_event(BYTE_SENDABLE); - break; - - case I2C_EVENT_SLAVE_ACK_FAILURE: - LOG('a', 0); - fsm_event(ACK_FAILED); - break; - - default: - idle_iterations++; -// if ((event) && (event != 0x00020000)) -// LOG('e', event); - break; - } - - /* if we have just received something, drop out and let the caller handle it */ - if (i2c_command_received) { - i2c_command_received = false; - return true; - } - - /* if we have done nothing recently, drop out and let the caller have a slice */ - if (idle_iterations > 1000) - return false; - } -} - -/** - * Update the FSM with an event - * - * @param event New event. - */ -static void -fsm_event(enum fsm_event event) -{ - // move to the next state - context.state = fsm[context.state].next_state[event]; - - LOG('f', context.state); - - // call the state entry handler - if (fsm[context.state].handler) { - fsm[context.state].handler(); - } -} - -static void -go_bad() -{ - LOG('B', 0); - fsm_event(AUTO); -} - -/** - * Wait for the master to address us. - * - */ -static void -go_wait_master() -{ - // We currently don't have a command byte. - // - context.command = '\0'; - - // The data pointer starts pointing to the start of the data buffer. - // - context.data_ptr = context.buffer; - - // The data count is either: - // - the size of the data buffer - // - some value less than or equal the size of the data buffer during a write or a read - // - context.data_count = context.buffer_size; - - // (re)enable the peripheral, clear the stop event flag in - // case we just finished receiving data - rCR1 |= I2C_CR1_PE; - - // clear the ACK failed flag in case we just finished sending data - rSR1 = ~I2C_SR1_AF; -} - -/** - * Prepare to receive a command byte. - * - */ -static void -go_wait_command() -{ - // NOP -} - -/** - * Command byte has been received, save it and prepare to handle the data. - * - */ -static void -go_receive_command() -{ - - // fetch the command byte - context.command = (uint8_t)rDR; - LOG('c', context.command); - -} - -/** - * Receive a data byte. - * - */ -static void -go_receive_data() -{ - uint8_t d; - - // fetch the byte - d = (uint8_t)rDR; - LOG('d', d); - - // if we have somewhere to put it, do so - if (context.data_count) { - *context.data_ptr++ = d; - context.data_count--; - } -} - -/** - * Handle a command once the host is done sending it to us. - * - */ -static void -go_handle_command() -{ - // presume we are happy with the command - context.status = 0; - - // make a note that the buffer contains a fresh command - i2c_command_received = true; - - // kick along to the next state - fsm_event(AUTO); -} - -/** - * Wait to be able to send the status byte. - * - */ -static void -go_wait_send() -{ - // NOP -} - -/** - * Send the status byte. - * - */ -static void -go_send_status() -{ - rDR = context.status; - LOG('?', context.status); -} - -/** - * Send a data or pad byte. - * - */ -static void -go_send_buffer() -{ - if (context.data_count) { - LOG('D', *context.data_ptr); - rDR = *(context.data_ptr++); - context.data_count--; - } else { - LOG('-', 0); - rDR = 0xff; - } -} - -/* cribbed directly from the NuttX master driver */ -static void -i2c_setclock(uint32_t frequency) -{ - uint16_t cr1; - uint16_t ccr; - uint16_t trise; - uint16_t freqmhz; - uint16_t speed; - - /* Disable the selected I2C peripheral to configure TRISE */ - - cr1 = rCR1; - rCR1 &= ~I2C_CR1_PE; - - /* Update timing and control registers */ - - freqmhz = (uint16_t)(STM32_PCLK1_FREQUENCY / 1000000); - ccr = 0; - - /* Configure speed in standard mode */ - - if (frequency <= 100000) { - /* Standard mode speed calculation */ - - speed = (uint16_t)(STM32_PCLK1_FREQUENCY / (frequency << 1)); - - /* The CCR fault must be >= 4 */ - - if (speed < 4) { - /* Set the minimum allowed value */ - - speed = 4; - } - ccr |= speed; - - /* Set Maximum Rise Time for standard mode */ - - trise = freqmhz + 1; - - /* Configure speed in fast mode */ - } else { /* (frequency <= 400000) */ - /* Fast mode speed calculation with Tlow/Thigh = 16/9 */ - -#ifdef CONFIG_I2C_DUTY16_9 - speed = (uint16_t)(STM32_PCLK1_FREQUENCY / (frequency * 25)); - - /* Set DUTY and fast speed bits */ - - ccr |= (I2C_CCR_DUTY|I2C_CCR_FS); -#else - /* Fast mode speed calculation with Tlow/Thigh = 2 */ - - speed = (uint16_t)(STM32_PCLK1_FREQUENCY / (frequency * 3)); - - /* Set fast speed bit */ - - ccr |= I2C_CCR_FS; -#endif - - /* Verify that the CCR speed value is nonzero */ - - if (speed < 1) { - /* Set the minimum allowed value */ - - speed = 1; - } - ccr |= speed; - - /* Set Maximum Rise Time for fast mode */ - - trise = (uint16_t)(((freqmhz * 300) / 1000) + 1); - } - - /* Write the new values of the CCR and TRISE registers */ - - rCCR = ccr; - rTRISE = trise; - - /* Bit 14 of OAR1 must be configured and kept at 1 */ - - rOAR1 = I2C_OAR1_ONE); - - /* Re-enable the peripheral (or not) */ - - rCR1 = cr1; -} diff --git a/src/drivers/boards/px4iov2/module.mk b/src/drivers/boards/px4iov2/module.mk new file mode 100644 index 0000000000..85f94e8be1 --- /dev/null +++ b/src/drivers/boards/px4iov2/module.mk @@ -0,0 +1,6 @@ +# +# Board-specific startup code for the PX4IOv2 +# + +SRCS = px4iov2_init.c \ + px4iov2_pwm_servo.c diff --git a/src/drivers/boards/px4iov2/px4iov2_init.c b/src/drivers/boards/px4iov2/px4iov2_init.c new file mode 100644 index 0000000000..09469d7e80 --- /dev/null +++ b/src/drivers/boards/px4iov2/px4iov2_init.c @@ -0,0 +1,171 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 px4iov2_init.c + * + * PX4FMU-specific early startup code. This file implements the + * nsh_archinitialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialisation. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include + +#include + +#include "stm32_internal.h" +#include "px4iov2_internal.h" + +#include + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +/* Debug ********************************************************************/ + +#ifdef CONFIG_CPP_HAVE_VARARGS +# ifdef CONFIG_DEBUG +# define message(...) lowsyslog(__VA_ARGS__) +# else +# define message(...) printf(__VA_ARGS__) +# endif +#else +# ifdef CONFIG_DEBUG +# define message lowsyslog +# else +# define message printf +# endif +#endif + +/**************************************************************************** + * Protected Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void stm32_boardinitialize(void) +{ + + /* configure GPIOs */ + + /* turn off - all leds are active low */ + stm32_gpiowrite(GPIO_LED1, true); + stm32_gpiowrite(GPIO_LED2, true); + stm32_gpiowrite(GPIO_LED3, true); + stm32_configgpio(GPIO_LED1); + stm32_configgpio(GPIO_LED2); + stm32_configgpio(GPIO_LED3); + + + stm32_configgpio(GPIO_BTN_SAFETY); + + /* spektrum power enable is active high - disable it by default */ + stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, false); + stm32_configgpio(GPIO_SPEKTRUM_PWR_EN); + + /* servo power enable is active low, and has a pull down resistor + * to keep it low during boot (since it may power the whole board.) + */ + stm32_gpiowrite(GPIO_SERVO_PWR_EN, false); + stm32_configgpio(GPIO_SERVO_PWR_EN); + + stm32_configgpio(GPIO_SERVO_FAULT_DETECT); + + stm32_configgpio(GPIO_TIM_RSSI); /* xxx alternate function */ + stm32_configgpio(GPIO_ADC_RSSI); + stm32_configgpio(GPIO_ADC_VSERVO); + + stm32_configgpio(GPIO_SBUS_INPUT); /* xxx alternate function */ + + stm32_gpiowrite(GPIO_SBUS_OUTPUT, false); + stm32_configgpio(GPIO_SBUS_OUTPUT); + + /* sbus output enable is active low - disable it by default */ + stm32_gpiowrite(GPIO_SBUS_OENABLE, true); + stm32_configgpio(GPIO_SBUS_OENABLE); + + + stm32_configgpio(GPIO_PPM); /* xxx alternate function */ + + stm32_gpiowrite(GPIO_PWM1, false); + stm32_configgpio(GPIO_PWM1); + + stm32_gpiowrite(GPIO_PWM2, false); + stm32_configgpio(GPIO_PWM2); + + stm32_gpiowrite(GPIO_PWM3, false); + stm32_configgpio(GPIO_PWM3); + + stm32_gpiowrite(GPIO_PWM4, false); + stm32_configgpio(GPIO_PWM4); + + stm32_gpiowrite(GPIO_PWM5, false); + stm32_configgpio(GPIO_PWM5); + + stm32_gpiowrite(GPIO_PWM6, false); + stm32_configgpio(GPIO_PWM6); + + stm32_gpiowrite(GPIO_PWM7, false); + stm32_configgpio(GPIO_PWM7); + + stm32_gpiowrite(GPIO_PWM8, false); + stm32_configgpio(GPIO_PWM8); + +// message("[boot] Successfully initialized px4iov2 gpios\n"); +} diff --git a/src/drivers/boards/px4iov2/px4iov2_internal.h b/src/drivers/boards/px4iov2/px4iov2_internal.h new file mode 100755 index 0000000000..c4c592fe44 --- /dev/null +++ b/src/drivers/boards/px4iov2/px4iov2_internal.h @@ -0,0 +1,112 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 px4iov2_internal.h + * + * PX4IOV2 internal definitions + */ + +#pragma once + +/****************************************************************************** + * Included Files + ******************************************************************************/ + +#include +#include +#include + +/* these headers are not C++ safe */ +#include + + +/****************************************************************************** + * Definitions + ******************************************************************************/ +/* Configuration **************************************************************/ + +/****************************************************************************** + * GPIOS + ******************************************************************************/ + +/* LEDS **********************************************************************/ + +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14) +#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15) +#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN13) + +#define GPIO_LED_BLUE BOARD_GPIO_LED1 +#define GPIO_LED_AMBER BOARD_GPIO_LED2 +#define GPIO_LED_SAFETY BOARD_GPIO_LED3 + +/* Safety switch button *******************************************************/ + +#define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5) + +/* Power switch controls ******************************************************/ + +#define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) + +#define GPIO_SERVO_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN15) + +#define GPIO_SERVO_FAULT_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN13) + + +/* Analog inputs **************************************************************/ + +#define GPIO_ADC_VSERVO (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN4) +/* the same rssi signal goes to both an adc and a timer input */ +#define GPIO_ADC_RSSI (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN5) +/* floating pin */ +#define GPIO_TIM_RSSI (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN12) + +/* PWM pins **************************************************************/ + +#define GPIO_PPM (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN8) + +#define GPIO_PWM1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0) +#define GPIO_PWM2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1) +#define GPIO_PWM3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8) +#define GPIO_PWM4 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9) +#define GPIO_PWM5 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN6) +#define GPIO_PWM6 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7) +#define GPIO_PWM7 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0) +#define GPIO_PWM8 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1) + +/* SBUS pins *************************************************************/ + +#define GPIO_SBUS_INPUT (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN11) +#define GPIO_SBUS_OUTPUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) +#define GPIO_SBUS_OENABLE (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN4) + diff --git a/src/drivers/boards/px4iov2/px4iov2_pwm_servo.c b/src/drivers/boards/px4iov2/px4iov2_pwm_servo.c new file mode 100644 index 0000000000..5e1aafa494 --- /dev/null +++ b/src/drivers/boards/px4iov2/px4iov2_pwm_servo.c @@ -0,0 +1,123 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 px4iov2_pwm_servo.c + * + * Configuration data for the stm32 pwm_servo driver. + * + * Note that these arrays must always be fully-sized. + */ + +#include + +#include + +#include +#include + +#include +#include +#include + +__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = { + { + .base = STM32_TIM2_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM2EN, + .clock_freq = STM32_APB1_TIM2_CLKIN + }, + { + .base = STM32_TIM3_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM3EN, + .clock_freq = STM32_APB1_TIM3_CLKIN + }, + { + .base = STM32_TIM4_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM4EN, + .clock_freq = STM32_APB1_TIM4_CLKIN + } +}; + +__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = { + { + .gpio = GPIO_TIM2_CH1OUT, + .timer_index = 0, + .timer_channel = 1, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM2_CH2OUT, + .timer_index = 0, + .timer_channel = 2, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM4_CH3OUT, + .timer_index = 2, + .timer_channel = 3, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM4_CH4OUT, + .timer_index = 2, + .timer_channel = 4, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM3_CH1OUT, + .timer_index = 1, + .timer_channel = 1, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM3_CH2OUT, + .timer_index = 1, + .timer_channel = 2, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM3_CH3OUT, + .timer_index = 1, + .timer_channel = 3, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM3_CH4OUT, + .timer_index = 1, + .timer_channel = 4, + .default_value = 1000, + } +}; From 00daa29b1f6a2989e8d732aabf075a6a912ec3ee Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 28 Apr 2013 12:37:33 -0700 Subject: [PATCH 104/486] Kill old iov2 board support code --- apps/drivers/boards/px4iov2/module.mk | 5 - apps/drivers/boards/px4iov2/px4iov2_init.c | 172 ------------------ .../drivers/boards/px4iov2/px4iov2_internal.h | 135 -------------- 3 files changed, 312 deletions(-) delete mode 100644 apps/drivers/boards/px4iov2/module.mk delete mode 100644 apps/drivers/boards/px4iov2/px4iov2_init.c delete mode 100644 apps/drivers/boards/px4iov2/px4iov2_internal.h diff --git a/apps/drivers/boards/px4iov2/module.mk b/apps/drivers/boards/px4iov2/module.mk deleted file mode 100644 index d596ce4db9..0000000000 --- a/apps/drivers/boards/px4iov2/module.mk +++ /dev/null @@ -1,5 +0,0 @@ -# -# Board-specific startup code for the PX4IOv2 -# - -SRCS = px4iov2_init.c diff --git a/apps/drivers/boards/px4iov2/px4iov2_init.c b/apps/drivers/boards/px4iov2/px4iov2_init.c deleted file mode 100644 index 711bee4257..0000000000 --- a/apps/drivers/boards/px4iov2/px4iov2_init.c +++ /dev/null @@ -1,172 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 px4iov2_init.c - * - * PX4FMU-specific early startup code. This file implements the - * nsh_archinitialize() function that is called early by nsh during startup. - * - * Code here is run before the rcS script is invoked; it should start required - * subsystems and perform board-specific initialisation. - */ - -/**************************************************************************** - * Included Files - ****************************************************************************/ - -#include - -#include -#include -#include -#include - -#include - -#include "stm32_internal.h" -#include "px4iov2_internal.h" - -#include - -/**************************************************************************** - * Pre-Processor Definitions - ****************************************************************************/ - -/* Configuration ************************************************************/ - -/* Debug ********************************************************************/ - -#ifdef CONFIG_CPP_HAVE_VARARGS -# ifdef CONFIG_DEBUG -# define message(...) lowsyslog(__VA_ARGS__) -# else -# define message(...) printf(__VA_ARGS__) -# endif -#else -# ifdef CONFIG_DEBUG -# define message lowsyslog -# else -# define message printf -# endif -#endif - -/**************************************************************************** - * Protected Functions - ****************************************************************************/ - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/************************************************************************************ - * Name: stm32_boardinitialize - * - * Description: - * All STM32 architectures must provide the following entry point. This entry point - * is called early in the intitialization -- after all memory has been configured - * and mapped but before any devices have been initialized. - * - ************************************************************************************/ - -__EXPORT void stm32_boardinitialize(void) -{ - - /* configure GPIOs */ - - /* turn off - all leds are active low */ - stm32_gpiowrite(BOARD_GPIO_LED1, true); - stm32_gpiowrite(BOARD_GPIO_LED2, true); - stm32_gpiowrite(BOARD_GPIO_LED3, true); - stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_LED1)); - stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_LED2)); - stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_LED3)); - - - stm32_configgpio(BOARD_GPIO_INPUT_FLOAT(BOARD_GPIO_BTN_SAFETY)); - - /* spektrum power enable is active high - disable it by default */ - stm32_gpiowrite(BOARD_GPIO_SPEKTRUM_PWR_EN, false); - stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_SPEKTRUM_PWR_EN)); - - /* servo power enable is active low, and has a pull down resistor - * to keep it low during boot (since it may power the whole board.) - */ - stm32_gpiowrite(BOARD_GPIO_SERVO_PWR_EN, false); - stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_SERVO_PWR_EN)); - - stm32_configgpio(BOARD_GPIO_INPUT_PUP(BOARD_GPIO_SERVO_FAULT_DETECT)); - - stm32_configgpio(BOARD_GPIO_INPUT_FLOAT(BOARD_GPIO_TIM_RSSI)); /* xxx alternate function */ - stm32_configgpio(BOARD_GPIO_INPUT_ANALOG(BOARD_GPIO_ADC_RSSI)); - stm32_configgpio(BOARD_GPIO_INPUT_ANALOG(BOARD_GPIO_ADC_VSERVO)); - - stm32_configgpio(BOARD_GPIO_INPUT_FLOAT(BOARD_GPIO_SBUS_INPUT)); /* xxx alternate function */ - - stm32_gpiowrite(BOARD_GPIO_SBUS_OUTPUT, false); - stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_SBUS_OUTPUT)); - /* sbus output enable is active low - disable it by default */ - stm32_gpiowrite(BOARD_GPIO_SBUS_OENABLE, true); - stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_SBUS_OENABLE)); - - - stm32_configgpio(BOARD_GPIO_INPUT_FLOAT(BOARD_GPIO_PPM)); /* xxx alternate function */ - - stm32_gpiowrite(BOARD_GPIO_PWM1, false); - stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_PWM1)); - - stm32_gpiowrite(BOARD_GPIO_PWM2, false); - stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_PWM2)); - - stm32_gpiowrite(BOARD_GPIO_PWM3, false); - stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_PWM3)); - - stm32_gpiowrite(BOARD_GPIO_PWM4, false); - stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_PWM4)); - - stm32_gpiowrite(BOARD_GPIO_PWM5, false); - stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_PWM5)); - - stm32_gpiowrite(BOARD_GPIO_PWM6, false); - stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_PWM6)); - - stm32_gpiowrite(BOARD_GPIO_PWM7, false); - stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_PWM7)); - - stm32_gpiowrite(BOARD_GPIO_PWM8, false); - stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_PWM8)); - -// message("[boot] Successfully initialized px4iov2 gpios\n"); - - return OK; -} diff --git a/apps/drivers/boards/px4iov2/px4iov2_internal.h b/apps/drivers/boards/px4iov2/px4iov2_internal.h deleted file mode 100644 index 9675c6f366..0000000000 --- a/apps/drivers/boards/px4iov2/px4iov2_internal.h +++ /dev/null @@ -1,135 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 px4iov2_internal.h - * - * PX4IOV2 internal definitions - */ - -#pragma once - -/**************************************************************************************************** - * Included Files - ****************************************************************************************************/ - -#include -#include -#include - -__BEGIN_DECLS - -/* these headers are not C++ safe */ -#include - - -/**************************************************************************************************** - * Definitions - ****************************************************************************************************/ -/* Configuration ************************************************************************************/ - -/****************************************************************************** - * GPIOS - ******************************************************************************/ - -#define BOARD_GPIO_OUTPUT(pin) (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ - GPIO_OUTPUT_CLEAR|(pin)) -#define BOARD_GPIO_INPUT_FLOAT(pin) (GPIO_INPUT|GPIO_CNF_INFLOAT|\ - GPIO_MODE_INPUT|(pin)) -#define BOARD_GPIO_INPUT_PUP(pin) (GPIO_INPUT|GPIO_CNF_INPULLUP|\ - GPIO_MODE_INPUT|(pin)) -#define BOARD_GPIO_INPUT_ANALOG(pin) (GPIO_INPUT|GPIO_CNF_ANALOGIN|\ - GPIO_MODE_INPUT|(pin)) - -/* LEDS **********************************************************************/ - -#define BOARD_GPIO_LED1 (GPIO_PORTB|GPIO_PIN14) -#define BOARD_GPIO_LED2 (GPIO_PORTB|GPIO_PIN15) -#define BOARD_GPIO_LED3 (GPIO_PORTB|GPIO_PIN13) - -#define BOARD_GPIO_LED_BLUE BOARD_GPIO_LED1 -#define BOARD_GPIO_LED_AMBER BOARD_GPIO_LED2 -#define BOARD_GPIO_LED_SAFETY BOARD_GPIO_LED3 - -/* Safety switch button *******************************************************/ - -#define BOARD_GPIO_BTN_SAFETY (GPIO_PORTB|GPIO_PIN5) - -/* Power switch controls ******************************************************/ - -#define BOARD_GPIO_SPEKTRUM_PWR_EN (GPIO_PORTC|GPIO_PIN13) - -#define BOARD_GPIO_SERVO_PWR_EN (GPIO_PORTC|GPIO_PIN15) - -#define BOARD_GPIO_SERVO_FAULT_DETECT (GPIO_PORTB|GPIO_PIN13) - - -/* Analog inputs **************************************************************/ - -#define BOARD_GPIO_ADC_VSERVO (GPIO_PORTA|GPIO_PIN4) -/* the same rssi signal goes to both an adc and a timer input */ -#define BOARD_GPIO_ADC_RSSI (GPIO_PORTA|GPIO_PIN5) -#define BOARD_GPIO_TIM_RSSI (GPIO_PORTA|GPIO_PIN12) - -/* PWM pins **************************************************************/ - -#define BOARD_GPIO_PPM (GPIO_PORTA|GPIO_PIN8) - -#define BOARD_GPIO_PWM1 (GPIO_PORTA|GPIO_PIN0) -#define BOARD_GPIO_PWM2 (GPIO_PORTA|GPIO_PIN1) -#define BOARD_GPIO_PWM3 (GPIO_PORTB|GPIO_PIN8) -#define BOARD_GPIO_PWM4 (GPIO_PORTB|GPIO_PIN9) -#define BOARD_GPIO_PWM5 (GPIO_PORTA|GPIO_PIN6) -#define BOARD_GPIO_PWM6 (GPIO_PORTA|GPIO_PIN7) -#define BOARD_GPIO_PWM7 (GPIO_PORTB|GPIO_PIN0) -#define BOARD_GPIO_PWM8 (GPIO_PORTB|GPIO_PIN1) - -/* SBUS pins *************************************************************/ - -#define BOARD_GPIO_SBUS_INPUT (GPIO_PORTB|GPIO_PIN11) -#define BOARD_GPIO_SBUS_OUTPUT (GPIO_PORTB|GPIO_PIN10) -#define BOARD_GPIO_SBUS_OENABLE (GPIO_PORTB|GPIO_PIN4) - -/**************************************************************************************************** - * Public Types - ****************************************************************************************************/ - -/**************************************************************************************************** - * Public data - ****************************************************************************************************/ - -#ifndef __ASSEMBLY__ - -#endif /* __ASSEMBLY__ */ - -__END_DECLS From 03eb16f874a8db7443e2b7f7a47d05c1a0c87357 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 28 Apr 2013 12:47:34 -0700 Subject: [PATCH 105/486] Remove some naked command invocations. --- makefiles/module.mk | 2 +- makefiles/setup.mk | 2 ++ makefiles/upload.mk | 13 ++++++++----- 3 files changed, 11 insertions(+), 6 deletions(-) diff --git a/makefiles/module.mk b/makefiles/module.mk index 86810627b2..59c67f574d 100644 --- a/makefiles/module.mk +++ b/makefiles/module.mk @@ -153,7 +153,7 @@ $(MODULE_COMMAND_FILES): exclude = $(dir $@)COMMAND.$(command).* $(MODULE_COMMAND_FILES): $(GLOBAL_DEPS) @$(REMOVE) -f $(exclude) @$(MKDIR) -p $(dir $@) - @echo "CMD: $(command)" + @$(ECHO) "CMD: $(command)" $(Q) $(TOUCH) $@ endif diff --git a/makefiles/setup.mk b/makefiles/setup.mk index 8072ec7915..1111930937 100644 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -72,6 +72,8 @@ export TOUCH = touch export MKDIR = mkdir export ECHO = echo export UNZIP_CMD = unzip +export PYTHON = python +export OPENOCD = openocd # # Host-specific paths, hacks and fixups diff --git a/makefiles/upload.mk b/makefiles/upload.mk index a1159d157a..c9a317caf5 100644 --- a/makefiles/upload.mk +++ b/makefiles/upload.mk @@ -25,7 +25,10 @@ endif all: upload-$(METHOD)-$(BOARD) upload-serial-px4fmu: $(BUNDLE) $(UPLOADER) - @python -u $(UPLOADER) --port $(SERIAL_PORTS) $(BUNDLE) + $(Q) $(PYTHON) -u $(UPLOADER) --port $(SERIAL_PORTS) $(BUNDLE) + +upload-serial-px4fmuv2: $(BUNDLE) $(UPLOADER) + $(Q) $(PYTHON) -u $(UPLOADER) --port $(SERIAL_PORTS) $(BUNDLE) upload-serial-px4fmuv2: $(BUNDLE) $(UPLOADER) @python -u $(UPLOADER) --port $(SERIAL_PORTS) $(BUNDLE) @@ -36,9 +39,9 @@ upload-serial-px4fmuv2: $(BUNDLE) $(UPLOADER) JTAGCONFIG ?= interface/olimex-jtag-tiny.cfg upload-jtag-px4fmu: all - @echo Attempting to flash PX4FMU board via JTAG - @openocd -f $(JTAGCONFIG) -f ../Bootloader/stm32f4x.cfg -c init -c "reset halt" -c "flash write_image erase nuttx/nuttx" -c "flash write_image erase ../Bootloader/px4fmu_bl.elf" -c "reset run" -c shutdown + @$(ECHO) Attempting to flash PX4FMU board via JTAG + $(Q) $(OPENOCD) -f $(JTAGCONFIG) -f ../Bootloader/stm32f4x.cfg -c init -c "reset halt" -c "flash write_image erase nuttx/nuttx" -c "flash write_image erase ../Bootloader/px4fmu_bl.elf" -c "reset run" -c shutdown upload-jtag-px4io: all - @echo Attempting to flash PX4IO board via JTAG - @openocd -f $(JTAGCONFIG) -f ../Bootloader/stm32f1x.cfg -c init -c "reset halt" -c "flash write_image erase nuttx/nuttx" -c "flash write_image erase ../Bootloader/px4io_bl.elf" -c "reset run" -c shutdown + @$(ECHO) Attempting to flash PX4IO board via JTAG + $(Q) $(OPENOCD) -f $(JTAGCONFIG) -f ../Bootloader/stm32f1x.cfg -c init -c "reset halt" -c "flash write_image erase nuttx/nuttx" -c "flash write_image erase ../Bootloader/px4io_bl.elf" -c "reset run" -c shutdown From c6b7eb1224426d9ec2e6d59a3df4c7443449109a Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 28 Apr 2013 13:00:49 -0700 Subject: [PATCH 106/486] Remove obsoleted file. --- src/modules/attitude_estimator_ekf/Makefile | 57 --------------------- 1 file changed, 57 deletions(-) delete mode 100755 src/modules/attitude_estimator_ekf/Makefile diff --git a/src/modules/attitude_estimator_ekf/Makefile b/src/modules/attitude_estimator_ekf/Makefile deleted file mode 100755 index 46a54c6607..0000000000 --- a/src/modules/attitude_estimator_ekf/Makefile +++ /dev/null @@ -1,57 +0,0 @@ -############################################################################ -# -# Copyright (C) 2012 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. -# -############################################################################ - -APPNAME = attitude_estimator_ekf -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 - -CXXSRCS = attitude_estimator_ekf_main.cpp - -CSRCS = attitude_estimator_ekf_params.c \ - codegen/eye.c \ - codegen/attitudeKalmanfilter.c \ - codegen/mrdivide.c \ - codegen/rdivide.c \ - codegen/attitudeKalmanfilter_initialize.c \ - codegen/attitudeKalmanfilter_terminate.c \ - codegen/rt_nonfinite.c \ - codegen/rtGetInf.c \ - codegen/rtGetNaN.c \ - codegen/norm.c \ - codegen/cross.c - - -# XXX this is *horribly* broken -INCLUDES += $(TOPDIR)/../mavlink/include/mavlink - -include $(APPDIR)/mk/app.mk From 301b600b0a0fbb816b692f53224d1ab6c5958741 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 28 Apr 2013 13:22:36 -0700 Subject: [PATCH 107/486] Fix board name defines in v2 config. --- nuttx/configs/px4iov2/io/defconfig | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx/configs/px4iov2/io/defconfig b/nuttx/configs/px4iov2/io/defconfig index c73f0df896..1eaf4f2d11 100755 --- a/nuttx/configs/px4iov2/io/defconfig +++ b/nuttx/configs/px4iov2/io/defconfig @@ -74,8 +74,8 @@ CONFIG_ARCH_ARM=y CONFIG_ARCH_CORTEXM3=y CONFIG_ARCH_CHIP="stm32" CONFIG_ARCH_CHIP_STM32F100C8=y -CONFIG_ARCH_BOARD="px4io" -CONFIG_ARCH_BOARD_PX4IO=y +CONFIG_ARCH_BOARD="px4iov2" +CONFIG_ARCH_BOARD_PX4IOV2=y CONFIG_BOARD_LOOPSPERMSEC=2000 CONFIG_DRAM_SIZE=0x00002000 CONFIG_DRAM_START=0x20000000 From 2423c54e0e5500a8a9e2ef482b95b351a4a6071e Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 28 Apr 2013 13:45:21 -0700 Subject: [PATCH 108/486] Build the right config for IOv2 --- Makefile | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Makefile b/Makefile index 8f566a0020..2a5abb741c 100644 --- a/Makefile +++ b/Makefile @@ -140,9 +140,9 @@ ifneq ($(filter archives,$(MAKECMDGOALS)),) endif $(ARCHIVE_DIR)%.export: board = $(notdir $(basename $@)) -$(ARCHIVE_DIR)%.export: configuration = $(if $(filter $(board),px4io),io,nsh) +$(ARCHIVE_DIR)%.export: configuration = $(if $(findstring px4io,$(board)),io,nsh) $(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) $(NUTTX_APPS) - @echo %% Configuring NuttX for $(board) + @echo %% Configuring NuttX for $(board)/$(configuration) $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) $(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean $(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(board)/$(configuration)) From 8f7200e0114d6bd9fcac7ec088b125e54761c2e0 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 28 Apr 2013 13:51:33 -0700 Subject: [PATCH 109/486] Frame up the configuration for the serial interface on IOv2 --- src/modules/px4iofirmware/module.mk | 11 +++++++++-- src/modules/px4iofirmware/px4io.c | 13 +++++-------- 2 files changed, 14 insertions(+), 10 deletions(-) diff --git a/src/modules/px4iofirmware/module.mk b/src/modules/px4iofirmware/module.mk index 6379366f43..016be6d3b6 100644 --- a/src/modules/px4iofirmware/module.mk +++ b/src/modules/px4iofirmware/module.mk @@ -3,7 +3,6 @@ SRCS = adc.c \ controls.c \ dsm.c \ - i2c.c \ px4io.c \ registers.c \ safety.c \ @@ -16,4 +15,12 @@ SRCS = adc.c \ ../systemlib/mixer/mixer_group.cpp \ ../systemlib/mixer/mixer_multirotor.cpp \ ../systemlib/mixer/mixer_simple.cpp \ - \ No newline at end of file + +ifneq ($(CONFIG_ARCH_BOARD_PX4IO),) +SRCS += i2c.c +EXTRADEFINES += -DINTERFACE_I2C +endif +ifneq ($(CONFIG_ARCH_BOARD_PX4IOV2),) +#SRCS += serial.c +EXTRADEFINES += -DINTERFACE_SERIAL +endif diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index bc8dfc1167..39f05c112f 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -64,11 +64,6 @@ struct sys_state_s system_state; static struct hrt_call serial_dma_call; -#ifdef CONFIG_STM32_I2C1 -/* store i2c reset count XXX this should be a register, together with other error counters */ -volatile uint32_t i2c_loop_resets = 0; -#endif - /* * a set of debug buffers to allow us to send debug information from ISRs */ @@ -159,10 +154,13 @@ user_start(int argc, char *argv[]) /* initialise the control inputs */ controls_init(); -#ifdef CONFIG_STM32_I2C1 +#ifdef INTERFACE_I2C /* start the i2c handler */ i2c_init(); #endif +#ifdef INTERFACE_SERIAL + /* start the serial interface */ +#endif /* add a performance counter for mixing */ perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix"); @@ -223,12 +221,11 @@ user_start(int argc, char *argv[]) struct mallinfo minfo = mallinfo(); - isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u m=%u", + isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG], (unsigned)r_status_flags, (unsigned)r_setup_arming, (unsigned)r_setup_features, - (unsigned)i2c_loop_resets, (unsigned)minfo.mxordblk); last_debug_time = hrt_absolute_time(); } From e67022f874f0fa091ed7dffd617c70c0c0253b5c Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 28 Apr 2013 18:14:46 -0700 Subject: [PATCH 110/486] Serial interface for IOv2 --- src/modules/px4iofirmware/i2c.c | 15 +++- src/modules/px4iofirmware/module.mk | 10 +-- src/modules/px4iofirmware/protocol.h | 39 ++++++-- src/modules/px4iofirmware/px4io.c | 19 ++-- src/modules/px4iofirmware/px4io.h | 22 ++--- src/modules/px4iofirmware/serial.c | 129 +++++++++++++++++++++++++++ src/modules/systemlib/hx_stream.c | 42 ++++++++- src/modules/systemlib/hx_stream.h | 18 +++- 8 files changed, 249 insertions(+), 45 deletions(-) create mode 100644 src/modules/px4iofirmware/serial.c diff --git a/src/modules/px4iofirmware/i2c.c b/src/modules/px4iofirmware/i2c.c index 4485daa5b1..10aeb5c9fa 100644 --- a/src/modules/px4iofirmware/i2c.c +++ b/src/modules/px4iofirmware/i2c.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2012,2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -69,6 +69,7 @@ static void i2c_rx_setup(void); static void i2c_tx_setup(void); static void i2c_rx_complete(void); static void i2c_tx_complete(void); +static void i2c_dump(void); static DMA_HANDLE rx_dma; static DMA_HANDLE tx_dma; @@ -92,7 +93,7 @@ enum { } direction; void -i2c_init(void) +interface_init(void) { debug("i2c init"); @@ -148,12 +149,18 @@ i2c_init(void) #endif } +void +interface_tick() +{ +} + /* reset the I2C bus used to recover from lockups */ -void i2c_reset(void) +void +i2c_reset(void) { rCR1 |= I2C_CR1_SWRST; rCR1 = 0; @@ -330,7 +337,7 @@ i2c_tx_complete(void) i2c_tx_setup(); } -void +static void i2c_dump(void) { debug("CR1 0x%08x CR2 0x%08x", rCR1, rCR2); diff --git a/src/modules/px4iofirmware/module.mk b/src/modules/px4iofirmware/module.mk index 016be6d3b6..4dd1aa8d73 100644 --- a/src/modules/px4iofirmware/module.mk +++ b/src/modules/px4iofirmware/module.mk @@ -8,7 +8,6 @@ SRCS = adc.c \ safety.c \ sbus.c \ ../systemlib/up_cxxinitialize.c \ - ../systemlib/hx_stream.c \ ../systemlib/perf_counter.c \ mixer.cpp \ ../systemlib/mixer/mixer.cpp \ @@ -16,11 +15,10 @@ SRCS = adc.c \ ../systemlib/mixer/mixer_multirotor.cpp \ ../systemlib/mixer/mixer_simple.cpp \ -ifneq ($(CONFIG_ARCH_BOARD_PX4IO),) +ifeq ($(BOARD),px4io) SRCS += i2c.c -EXTRADEFINES += -DINTERFACE_I2C endif -ifneq ($(CONFIG_ARCH_BOARD_PX4IOV2),) -#SRCS += serial.c -EXTRADEFINES += -DINTERFACE_SERIAL +ifeq ($(BOARD),px4iov2) +SRCS += serial.c \ + ../systemlib/hx_stream.c endif diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 8d8b7e941b..1a687bd2a4 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -36,7 +36,7 @@ /** * @file protocol.h * - * PX4IO I2C interface protocol. + * PX4IO interface protocol. * * Communication is performed via writes to and reads from 16-bit virtual * registers organised into pages of 255 registers each. @@ -62,6 +62,27 @@ * Note that the implementation of readable pages prefers registers within * readable pages to be densely packed. Page numbers do not need to be * packed. + * + * PX4IO I2C interface notes: + * + * Register read/write operations are mapped directly to PX4IO register + * read/write operations. + * + * PX4IO Serial interface notes: + * + * The MSB of the page number is used to distinguish between read and + * write operations. If set, the operation is a write and additional + * data is expected to follow in the packet as for I2C writes. + * + * If clear, the packet is expected to contain a single byte giving the + * number of bytes to be read. PX4IO will respond with a packet containing + * the same header (page, offset) and the requested data. + * + * If a read is requested when PX4IO does not have buffer space to store + * the reply, the request will be dropped. PX4IO is always configured with + * enough space to receive one full-sized write and one read request, and + * to send one full-sized read response. + * */ #define PX4IO_CONTROL_CHANNELS 8 @@ -75,12 +96,14 @@ #define REG_TO_FLOAT(_reg) ((float)REG_TO_SIGNED(_reg) / 10000.0f) #define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)((_float) * 10000.0f)) +#define PX4IO_PAGE_WRITE (1<<7) + /* static configuration page */ #define PX4IO_PAGE_CONFIG 0 #define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* magic numbers TBD */ #define PX4IO_P_CONFIG_SOFTWARE_VERSION 1 /* magic numbers TBD */ #define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */ -#define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum I2C transfer size */ +#define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum packet transfer size */ #define PX4IO_P_CONFIG_CONTROL_COUNT 4 /* hardcoded max control count supported */ #define PX4IO_P_CONFIG_ACTUATOR_COUNT 5 /* hardcoded max actuator output count */ #define PX4IO_P_CONFIG_RC_INPUT_COUNT 6 /* hardcoded max R/C input count supported */ @@ -141,7 +164,7 @@ #define PX4IO_RATE_MAP_BASE 0 /* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */ /* setup page */ -#define PX4IO_PAGE_SETUP 100 +#define PX4IO_PAGE_SETUP 64 #define PX4IO_P_SETUP_FEATURES 0 #define PX4IO_P_SETUP_ARMING 1 /* arming controls */ @@ -160,13 +183,13 @@ #define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ /* autopilot control values, -10000..10000 */ -#define PX4IO_PAGE_CONTROLS 101 /* 0..CONFIG_CONTROL_COUNT */ +#define PX4IO_PAGE_CONTROLS 65 /* 0..CONFIG_CONTROL_COUNT */ /* raw text load to the mixer parser - ignores offset */ -#define PX4IO_PAGE_MIXERLOAD 102 +#define PX4IO_PAGE_MIXERLOAD 66 /* see px4io_mixdata structure below */ /* R/C channel config */ -#define PX4IO_PAGE_RC_CONFIG 103 /* R/C input configuration */ +#define PX4IO_PAGE_RC_CONFIG 67 /* R/C input configuration */ #define PX4IO_P_RC_CONFIG_MIN 0 /* lowest input value */ #define PX4IO_P_RC_CONFIG_CENTER 1 /* center input value */ #define PX4IO_P_RC_CONFIG_MAX 2 /* highest input value */ @@ -178,10 +201,10 @@ #define PX4IO_P_RC_CONFIG_STRIDE 6 /* spacing between channel config data */ /* PWM output - overrides mixer */ -#define PX4IO_PAGE_DIRECT_PWM 104 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +#define PX4IO_PAGE_DIRECT_PWM 68 /* 0..CONFIG_ACTUATOR_COUNT-1 */ /* PWM failsafe values - zero disables the output */ -#define PX4IO_PAGE_FAILSAFE_PWM 105 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +#define PX4IO_PAGE_FAILSAFE_PWM 69 /* 0..CONFIG_ACTUATOR_COUNT-1 */ /** * As-needed mixer data upload. diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 39f05c112f..385920d53b 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -142,7 +142,7 @@ user_start(int argc, char *argv[]) LED_BLUE(false); LED_SAFETY(false); - /* turn on servo power */ + /* turn on servo power (if supported) */ POWER_SERVO(true); /* start the safety switch handler */ @@ -154,13 +154,11 @@ user_start(int argc, char *argv[]) /* initialise the control inputs */ controls_init(); -#ifdef INTERFACE_I2C - /* start the i2c handler */ - i2c_init(); -#endif -#ifdef INTERFACE_SERIAL - /* start the serial interface */ -#endif + /* start the FMU interface */ + interface_init(); + + /* add a performance counter for the interface */ + perf_counter_t interface_perf = perf_alloc(PC_ELAPSED, "interface"); /* add a performance counter for mixing */ perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix"); @@ -203,6 +201,11 @@ user_start(int argc, char *argv[]) /* track the rate at which the loop is running */ perf_count(loop_perf); + /* kick the interface */ + perf_begin(interface_perf); + interface_tick(); + perf_end(interface_perf); + /* kick the mixer */ perf_begin(mixer_perf); mixer_tick(); diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 272cdb7bf4..2077e6244d 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -125,16 +125,16 @@ extern struct sys_state_s system_state; #define POWER_SERVO(_s) stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s)) #ifdef GPIO_ACC1_PWR_EN - #define POWER_ACC1(_s) stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s)) +# define POWER_ACC1(_s) stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s)) #endif #ifdef GPIO_ACC2_PWR_EN - #define POWER_ACC2(_s) stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s)) +# define POWER_ACC2(_s) stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s)) #endif #ifdef GPIO_RELAY1_EN - #define POWER_RELAY1(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s)) +# define POWER_RELAY1(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s)) #endif #ifdef GPIO_RELAY2_EN - #define POWER_RELAY2(_s) stm32_gpiowrite(GPIO_RELAY2_EN, (_s)) +# define POWER_RELAY2(_s) stm32_gpiowrite(GPIO_RELAY2_EN, (_s)) #endif #define OVERCURRENT_ACC (!stm32_gpioread(GPIO_ACC_OC_DETECT)) @@ -156,12 +156,11 @@ extern void mixer_handle_text(const void *buffer, size_t length); */ extern void safety_init(void); -#ifdef CONFIG_STM32_I2C1 /** * FMU communications */ -extern void i2c_init(void); -#endif +extern void interface_init(void); +extern void interface_tick(void); /** * Register space @@ -190,10 +189,5 @@ extern bool sbus_input(uint16_t *values, uint16_t *num_values); /** global debug level for isr_debug() */ extern volatile uint8_t debug_level; -/* send a debug message to the console */ -extern void isr_debug(uint8_t level, const char *fmt, ...); - -#ifdef CONFIG_STM32_I2C1 -void i2c_dump(void); -void i2c_reset(void); -#endif +/** send a debug message to the console */ +extern void isr_debug(uint8_t level, const char *fmt, ...); diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c new file mode 100644 index 0000000000..a12d58aca2 --- /dev/null +++ b/src/modules/px4iofirmware/serial.c @@ -0,0 +1,129 @@ +/**************************************************************************** + * + * Copyright (C) 2012,2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file serial.c + * + * Serial communication for the PX4IO module. + */ + +#include +#include +#include +#include +#include + +#include +#include + +#include + +//#define DEBUG +#include "px4io.h" + +static uint8_t tx_buf[66]; /* XXX hardcoded magic number */ + +static hx_stream_t if_stream; + +static void serial_callback(void *arg, const void *data, unsigned length); + +void +interface_init(void) +{ + + int fd = open("/dev/ttyS1", O_RDWR, O_NONBLOCK); + if (fd < 0) { + debug("serial fail"); + return; + } + + /* configure serial port - XXX increase port speed? */ + struct termios t; + tcgetattr(fd, &t); + cfsetspeed(&t, 115200); + t.c_cflag &= ~(CSTOPB | PARENB); + tcsetattr(fd, TCSANOW, &t); + + /* allocate the HX stream we'll use for communication */ + if_stream = hx_stream_init(fd, serial_callback, NULL); + + /* XXX add stream stats counters? */ + + debug("serial init"); +} + +void +interface_tick() +{ + /* process incoming bytes */ + hx_stream_rx(if_stream); +} + +static void +serial_callback(void *arg, const void *data, unsigned length) +{ + const uint8_t *message = (const uint8_t *)data; + + /* malformed frame, ignore it */ + if (length < 2) + return; + + /* it's a write operation, pass it to the register API */ + if (message[0] & PX4IO_PAGE_WRITE) { + registers_set(message[0] & ~PX4IO_PAGE_WRITE, message[1], + (const uint16_t *)&message[2], + (length - 2) / 2); + + return; + } + + /* it's a read */ + uint16_t *registers; + unsigned count; + + tx_buf[0] = message[0]; + tx_buf[1] = message[1]; + + /* get registers for response, send an empty reply on error */ + if (registers_get(message[0], message[1], ®isters, &count) < 0) + count = 0; + + /* fill buffer with message */ +#define TX_MAX ((sizeof(tx_buf) - 2) / 2) + if (count > TX_MAX) + count = TX_MAX; + memcpy(&tx_buf[2], registers, count * 2); + + /* try to send the message */ + hx_stream_send(if_stream, tx_buf, count * 2 + 2); +} \ No newline at end of file diff --git a/src/modules/systemlib/hx_stream.c b/src/modules/systemlib/hx_stream.c index 8d77f14a8d..88f7f762ca 100644 --- a/src/modules/systemlib/hx_stream.c +++ b/src/modules/systemlib/hx_stream.c @@ -76,6 +76,7 @@ struct hx_stream { static void hx_tx_raw(hx_stream_t stream, uint8_t c); static void hx_tx_raw(hx_stream_t stream, uint8_t c); static int hx_rx_frame(hx_stream_t stream); +static bool hx_rx_char(hx_stream_t stream, uint8_t c); static void hx_tx_raw(hx_stream_t stream, uint8_t c) @@ -224,13 +225,13 @@ hx_stream_send(hx_stream_t stream, return 0; } -void -hx_stream_rx(hx_stream_t stream, uint8_t c) +static bool +hx_rx_char(hx_stream_t stream, uint8_t c) { /* frame end? */ if (c == FBO) { hx_rx_frame(stream); - return; + return true; } /* escaped? */ @@ -241,10 +242,43 @@ hx_stream_rx(hx_stream_t stream, uint8_t c) } else if (c == CEO) { /* now escaped, ignore the byte */ stream->escaped = true; - return; + return false; } /* save for later */ if (stream->frame_bytes < sizeof(stream->buf)) stream->buf[stream->frame_bytes++] = c; + + return false; +} + +void +hx_stream_rx_char(hx_stream_t stream, uint8_t c) +{ + hx_rx_char(stream, c); +} + +int +hx_stream_rx(hx_stream_t stream) +{ + uint16_t buf[16]; + ssize_t len; + + /* read bytes */ + len = read(stream->fd, buf, sizeof(buf)); + if (len <= 0) { + + /* nonblocking stream and no data */ + if (errno == EWOULDBLOCK) + return 0; + + /* error or EOF */ + return -errno; + } + + /* process received characters */ + for (int i = 0; i < len; i++) + hx_rx_char(stream, buf[i]); + + return 0; } diff --git a/src/modules/systemlib/hx_stream.h b/src/modules/systemlib/hx_stream.h index 128689953a..be4850f745 100644 --- a/src/modules/systemlib/hx_stream.h +++ b/src/modules/systemlib/hx_stream.h @@ -114,9 +114,25 @@ __EXPORT extern int hx_stream_send(hx_stream_t stream, * @param stream A handle returned from hx_stream_init. * @param c The character to process. */ -__EXPORT extern void hx_stream_rx(hx_stream_t stream, +__EXPORT extern void hx_stream_rx_char(hx_stream_t stream, uint8_t c); +/** + * Handle received bytes from the stream. + * + * Note that this interface should only be used with blocking streams + * when it is OK for the call to block until a frame is received. + * + * When used with a non-blocking stream, it will typically return + * immediately, or after handling a received frame. + * + * @param stream A handle returned from hx_stream_init. + * @return -errno on error, nonzero if a frame + * has been received, or if not enough + * bytes are available to complete a frame. + */ +__EXPORT extern int hx_stream_rx(hx_stream_t stream); + __END_DECLS #endif From 7570d9082ca4291b598f3e34be291189678685ec Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 2 May 2013 17:41:25 +0400 Subject: [PATCH 111/486] Use common accel calibration. Cleanup. --- apps/position_estimator_inav/Makefile | 5 - .../acceleration_transform.c | 130 ------------ .../acceleration_transform.h | 19 -- .../position_estimator_inav_main.c | 185 +++--------------- .../position_estimator_inav_params.c | 53 ----- .../position_estimator_inav_params.h | 19 -- 6 files changed, 22 insertions(+), 389 deletions(-) delete mode 100644 apps/position_estimator_inav/acceleration_transform.c delete mode 100644 apps/position_estimator_inav/acceleration_transform.h diff --git a/apps/position_estimator_inav/Makefile b/apps/position_estimator_inav/Makefile index 192ec18255..566bb9f431 100644 --- a/apps/position_estimator_inav/Makefile +++ b/apps/position_estimator_inav/Makefile @@ -39,9 +39,4 @@ APPNAME = position_estimator_inav PRIORITY = SCHED_PRIORITY_DEFAULT STACKSIZE = 4096 -CSRCS = position_estimator_inav_main.c \ - position_estimator_inav_params.c \ - kalman_filter_inertial.c \ - acceleration_transform.c - include $(APPDIR)/mk/app.mk diff --git a/apps/position_estimator_inav/acceleration_transform.c b/apps/position_estimator_inav/acceleration_transform.c deleted file mode 100644 index 3aba9b403d..0000000000 --- a/apps/position_estimator_inav/acceleration_transform.c +++ /dev/null @@ -1,130 +0,0 @@ -/* - * acceleration_transform.c - * - * Copyright (C) 2013 Anton Babushkin. All rights reserved. - * Author: Anton Babushkin - * - * Transform acceleration vector to true orientation and scale - * - * * * * Model * * * - * accel_corr = accel_T * (accel_raw - accel_offs) - * - * accel_corr[3] - fully corrected acceleration vector in UAV frame - * accel_T[3][3] - accelerometers transform matrix, rotation and scaling transform - * accel_raw[3] - raw acceleration vector - * accel_offs[3] - true acceleration offset vector, don't use sensors offset because - * it's caused not only by real offset but also by sensor rotation - * - * * * * Calibration * * * - * - * Reference vectors - * accel_corr_ref[6][3] = [ g 0 0 ] // positive x - * | -g 0 0 | // negative x - * | 0 g 0 | // positive y - * | 0 -g 0 | // negative y - * | 0 0 g | // positive z - * [ 0 0 -g ] // negative z - * accel_raw_ref[6][3] - * - * accel_corr_ref[i] = accel_T * (accel_raw_ref[i] - accel_offs), i = 0...5 - * - * 6 reference vectors * 3 axes = 18 equations - * 9 (accel_T) + 3 (accel_offs) = 12 unknown constants - * - * Find accel_offs - * - * accel_offs[i] = (accel_raw_ref[i*2][i] + accel_raw_ref[i*2+1][i]) / 2 - * - * - * Find accel_T - * - * 9 unknown constants - * need 9 equations -> use 3 of 6 measurements -> 3 * 3 = 9 equations - * - * accel_corr_ref[i*2] = accel_T * (accel_raw_ref[i*2] - accel_offs), i = 0...2 - * - * Solve separate system for each row of accel_T: - * - * accel_corr_ref[j*2][i] = accel_T[i] * (accel_raw_ref[j*2] - accel_offs), j = 0...2 - * - * A x = b - * - * x = [ accel_T[0][i] ] - * | accel_T[1][i] | - * [ accel_T[2][i] ] - * - * b = [ accel_corr_ref[0][i] ] // One measurement per axis is enough - * | accel_corr_ref[2][i] | - * [ accel_corr_ref[4][i] ] - * - * a[i][j] = accel_raw_ref[i][j] - accel_offs[j], i = 0;2;4, j = 0...2 - * - * Matrix A is common for all three systems: - * A = [ a[0][0] a[0][1] a[0][2] ] - * | a[2][0] a[2][1] a[2][2] | - * [ a[4][0] a[4][1] a[4][2] ] - * - * x = A^-1 b - * - * accel_T = A^-1 * g - * g = 9.81 - */ - -#include "acceleration_transform.h" - -void acceleration_correct(float accel_corr[3], int16_t accel_raw[3], - float accel_T[3][3], int16_t accel_offs[3]) { - for (int i = 0; i < 3; i++) { - accel_corr[i] = 0.0f; - for (int j = 0; j < 3; j++) { - accel_corr[i] += accel_T[i][j] * (accel_raw[j] - accel_offs[j]); - } - } -} - -int mat_invert3(float src[3][3], float dst[3][3]) { - float det = src[0][0] * (src[1][1] * src[2][2] - src[1][2] * src[2][1]) - - src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) + - src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]); - if (det == 0.0) - return -1; // Singular matrix - dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det; - dst[1][0] = (src[1][2] * src[2][0] - src[1][0] * src[2][2]) / det; - dst[2][0] = (src[1][0] * src[2][1] - src[1][1] * src[2][0]) / det; - dst[0][1] = (src[0][2] * src[2][1] - src[0][1] * src[2][2]) / det; - dst[1][1] = (src[0][0] * src[2][2] - src[0][2] * src[2][0]) / det; - dst[2][1] = (src[0][1] * src[2][0] - src[0][0] * src[2][1]) / det; - dst[0][2] = (src[0][1] * src[1][2] - src[0][2] * src[1][1]) / det; - dst[1][2] = (src[0][2] * src[1][0] - src[0][0] * src[1][2]) / det; - dst[2][2] = (src[0][0] * src[1][1] - src[0][1] * src[1][0]) / det; - return 0; -} - -int calculate_calibration_values(int16_t accel_raw_ref[6][3], - float accel_T[3][3], int16_t accel_offs[3], float g) { - /* calculate raw offsets */ - for (int i = 0; i < 3; i++) { - accel_offs[i] = (int16_t) (((int32_t) (accel_raw_ref[i * 2][i]) - + (int32_t) (accel_raw_ref[i * 2 + 1][i])) / 2); - } - /* fill matrix A for linear equations system*/ - float mat_A[3][3]; - memset(mat_A, 0, sizeof(mat_A)); - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - float a = (accel_raw_ref[i * 2][j] - accel_offs[j]); - mat_A[i][j] = a; - } - } - /* calculate inverse matrix for A */ - float mat_A_inv[3][3]; - mat_invert3(mat_A, mat_A_inv); - for (int i = 0; i < 3; i++) { - /* copy results to accel_T */ - for (int j = 0; j < 3; j++) { - /* simplify matrices mult because b has only one non-zero element == g at index i */ - accel_T[j][i] = mat_A_inv[j][i] * g; - } - } - return 0; -} diff --git a/apps/position_estimator_inav/acceleration_transform.h b/apps/position_estimator_inav/acceleration_transform.h deleted file mode 100644 index 4d1299db5d..0000000000 --- a/apps/position_estimator_inav/acceleration_transform.h +++ /dev/null @@ -1,19 +0,0 @@ -/* - * acceleration_transform.h - * - * Copyright (C) 2013 Anton Babushkin. All rights reserved. - * Author: Anton Babushkin - */ - -#ifndef ACCELERATION_TRANSFORM_H_ -#define ACCELERATION_TRANSFORM_H_ - -#include - -void acceleration_correct(float accel_corr[3], int16_t accel_raw[3], - float accel_T[3][3], int16_t accel_offs[3]); - -int calculate_calibration_values(int16_t accel_raw_ref[6][3], - float accel_T[3][3], int16_t accel_offs[3], float g); - -#endif /* ACCELERATION_TRANSFORM_H_ */ diff --git a/apps/position_estimator_inav/position_estimator_inav_main.c b/apps/position_estimator_inav/position_estimator_inav_main.c index 6e8c0ab5fe..071ec6ad4d 100644 --- a/apps/position_estimator_inav/position_estimator_inav_main.c +++ b/apps/position_estimator_inav/position_estimator_inav_main.c @@ -62,22 +62,19 @@ #include #include #include +#include #include #include "position_estimator_inav_params.h" #include "kalman_filter_inertial.h" -#include "acceleration_transform.h" static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int position_estimator_inav_task; /**< Handle of deamon task / thread */ static bool verbose_mode = false; -const static float const_earth_gravity = 9.81f; __EXPORT int position_estimator_inav_main(int argc, char *argv[]); -void do_accelerometer_calibration(); - int position_estimator_inav_thread_main(int argc, char *argv[]); static void usage(const char *reason); @@ -89,18 +86,18 @@ static void usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); fprintf(stderr, - "usage: position_estimator_inav {start|stop|status|calibrate} [-v]\n\n"); + "usage: position_estimator_inav {start|stop|status} [-v]\n\n"); exit(1); } - /** - * The position_estimator_inav_thread only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ +/** + * The position_estimator_inav_thread only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ int position_estimator_inav_main(int argc, char *argv[]) { if (argc < 1) usage("missing command"); @@ -136,136 +133,10 @@ int position_estimator_inav_main(int argc, char *argv[]) { exit(0); } - if (!strcmp(argv[1], "calibrate")) { - do_accelerometer_calibration(); - exit(0); - } - usage("unrecognized command"); exit(1); } -void wait_for_input() { - printf("press any key to continue, 'Q' to abort\n"); - while (true ) { - int c = getchar(); - if (c == 'q' || c == 'Q') { - exit(0); - } else { - return; - } - } -} - -void read_accelerometer_raw_avg(int sensor_combined_sub, int16_t accel_avg[3], - int samples) { - printf("[position_estimator_inav] measuring...\n"); - struct pollfd fds[1] = { { .fd = sensor_combined_sub, .events = POLLIN } }; - int count = 0; - int32_t accel_sum[3] = { 0, 0, 0 }; - while (count < samples) { - int poll_ret = poll(fds, 1, 1000); - if (poll_ret == 1) { - struct sensor_combined_s sensor; - orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - accel_sum[0] += sensor.accelerometer_raw[0]; - accel_sum[1] += sensor.accelerometer_raw[1]; - accel_sum[2] += sensor.accelerometer_raw[2]; - count++; - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - printf("[position_estimator_inav] no accelerometer data for 1s\n"); - exit(1); - } else { - printf("[position_estimator_inav] poll error\n"); - exit(1); - } - } - for (int i = 0; i < 3; i++) { - accel_avg[i] = (accel_sum[i] + count / 2) / count; - } - printf("[position_estimator_inav] raw data: [ %i %i %i ]\n", accel_avg[0], - accel_avg[1], accel_avg[2]); -} - -void do_accelerometer_calibration() { - printf("[position_estimator_inav] calibration started\n"); - const int calibration_samples = 1000; - int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); - int16_t accel_raw_ref[6][3]; // Reference measurements - printf("[position_estimator_inav] place vehicle level, "); - wait_for_input(); - read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[5][0]), - calibration_samples); - printf("[position_estimator_inav] place vehicle on it's back, "); - wait_for_input(); - read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[4][0]), - calibration_samples); - printf("[position_estimator_inav] place vehicle on right side, "); - wait_for_input(); - read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[3][0]), - calibration_samples); - printf("[position_estimator_inav] place vehicle on left side, "); - wait_for_input(); - read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[2][0]), - calibration_samples); - printf("[position_estimator_inav] place vehicle nose down, "); - wait_for_input(); - read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[1][0]), - calibration_samples); - printf("[position_estimator_inav] place vehicle nose up, "); - wait_for_input(); - read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[0][0]), - calibration_samples); - close(sensor_combined_sub); - printf("[position_estimator_inav] reference data collection done\n"); - - int16_t accel_offs[3]; - float accel_T[3][3]; - int res = calculate_calibration_values(accel_raw_ref, accel_T, accel_offs, - const_earth_gravity); - if (res != 0) { - printf( - "[position_estimator_inav] calibration parameters calculation error\n"); - exit(1); - - } - printf( - "[position_estimator_inav] accelerometers raw offsets: [ %i %i %i ]\n", - accel_offs[0], accel_offs[1], accel_offs[2]); - printf( - "[position_estimator_inav] accelerometers transform matrix:\n [ %0.6f %0.6f %0.6f ]\n | %0.6f %0.6f %0.6f |\n [ %0.6f %0.6f %0.6f ]\n", - accel_T[0][0], accel_T[0][1], accel_T[0][2], accel_T[1][0], - accel_T[1][1], accel_T[1][2], accel_T[2][0], accel_T[2][1], - accel_T[2][2]); - int32_t accel_offs_int32[3] = - { accel_offs[0], accel_offs[1], accel_offs[2] }; - - if (param_set(param_find("INAV_ACC_OFFS_0"), &(accel_offs_int32[0])) - || param_set(param_find("INAV_ACC_OFFS_1"), &(accel_offs_int32[1])) - || param_set(param_find("INAV_ACC_OFFS_2"), &(accel_offs_int32[2])) - || param_set(param_find("INAV_ACC_T_00"), &(accel_T[0][0])) - || param_set(param_find("INAV_ACC_T_01"), &(accel_T[0][1])) - || param_set(param_find("INAV_ACC_T_02"), &(accel_T[0][2])) - || param_set(param_find("INAV_ACC_T_10"), &(accel_T[1][0])) - || param_set(param_find("INAV_ACC_T_11"), &(accel_T[1][1])) - || param_set(param_find("INAV_ACC_T_12"), &(accel_T[1][2])) - || param_set(param_find("INAV_ACC_T_20"), &(accel_T[2][0])) - || param_set(param_find("INAV_ACC_T_21"), &(accel_T[2][1])) - || param_set(param_find("INAV_ACC_T_22"), &(accel_T[2][2]))) { - printf("[position_estimator_inav] setting parameters failed\n"); - exit(1); - } - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - if (save_ret != 0) { - printf( - "[position_estimator_inav] auto-save of parameters to storage failed\n"); - exit(1); - } - printf("[position_estimator_inav] calibration done\n"); -} - /**************************************************************************** * main ****************************************************************************/ @@ -277,18 +148,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { mavlink_log_info(mavlink_fd, "[position_estimator_inav] started"); /* initialize values */ - static float x_est[3] = { 0.0f, 0.0f, 0.0f }; - static float y_est[3] = { 0.0f, 0.0f, 0.0f }; - static float z_est[3] = { 0.0f, 0.0f, 0.0f }; - float accel_offs_est[3] = { 0.0f, 0.0f, 0.0f }; + float x_est[3] = { 0.0f, 0.0f, 0.0f }; + float y_est[3] = { 0.0f, 0.0f, 0.0f }; + float z_est[3] = { 0.0f, 0.0f, 0.0f }; int baro_loop_cnt = 0; int baro_loop_end = 70; /* measurement for 1 second */ - float baro_alt0 = 0.0f; /* to determin while start up */ + float baro_alt0 = 0.0f; /* to determine while start up */ - static double lat_current = 0.0d; //[°]] --> 47.0 - static double lon_current = 0.0d; //[°]] -->8.5 - static double alt_current = 0.0d; //[m] above MSL + static double lat_current = 0.0; //[°]] --> 47.0 + static double lon_current = 0.0; //[°]] -->8.5 + static double alt_current = 0.0; //[m] above MSL /* declare and safely initialize all structs */ struct vehicle_status_s vehicle_status; @@ -473,26 +343,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { float dt = (t - last_time) / 1000000.0; last_time = t; if (att.R_valid) { - /* calculate corrected acceleration vector in UAV frame */ - float accel_corr[3]; - acceleration_correct(accel_corr, sensor.accelerometer_raw, - params.acc_T, params.acc_offs); /* transform acceleration vector from UAV frame to NED frame */ float accel_NED[3]; for (int i = 0; i < 3; i++) { accel_NED[i] = 0.0f; for (int j = 0; j < 3; j++) { - accel_NED[i] += att.R[i][j] * (accel_corr[j] - accel_offs_est[j]); - } - } - accel_NED[2] += const_earth_gravity; - /* accelerometers offset drift correction: rotate acceleration error back to UAV frame and integrate */ - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - /* the inverse of a rotation matrix is its transpose, just swap i and j */ - accel_offs_est[i] += att.R[j][i] * accel_NED[j] * params.acc_offs_w * dt; + accel_NED[i] += att.R[i][j] * sensor.accelerometer_m_s2[j]; } } + accel_NED[2] += CONSTANTS_ONE_G; /* kalman filter prediction */ kalman_filter_inertial_predict(dt, z_est); /* prepare vectors for kalman filter correction */ @@ -531,9 +390,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } if (t - pub_last > pub_interval) { pub_last = t; - pos.x = accel_offs_est[0]; - pos.vx = accel_offs_est[1]; - pos.y = accel_offs_est[2]; + pos.x = 0.0f; + pos.vx = 0.0f; + pos.y = 0.0f; pos.vy = 0.0f; pos.z = z_est[0]; pos.vz = z_est[1]; diff --git a/apps/position_estimator_inav/position_estimator_inav_params.c b/apps/position_estimator_inav/position_estimator_inav_params.c index 8cd9844c7d..bb2b014813 100644 --- a/apps/position_estimator_inav/position_estimator_inav_params.c +++ b/apps/position_estimator_inav/position_estimator_inav_params.c @@ -39,7 +39,6 @@ */ #include "position_estimator_inav_params.h" -#include "acceleration_transform.h" /* Kalman Filter covariances */ /* gps measurement noise standard deviation */ @@ -52,22 +51,6 @@ PARAM_DEFINE_FLOAT(INAV_K_ALT_11, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_ALT_20, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_ALT_21, 0.0f); -PARAM_DEFINE_FLOAT(INAV_ACC_OFFS_W, 0.0f); - -PARAM_DEFINE_INT32(INAV_ACC_OFFS_0, 0); -PARAM_DEFINE_INT32(INAV_ACC_OFFS_1, 0); -PARAM_DEFINE_INT32(INAV_ACC_OFFS_2, 0); - -PARAM_DEFINE_FLOAT(INAV_ACC_T_00, 0.0021f); -PARAM_DEFINE_FLOAT(INAV_ACC_T_01, 0.0f); -PARAM_DEFINE_FLOAT(INAV_ACC_T_02, 0.0f); -PARAM_DEFINE_FLOAT(INAV_ACC_T_10, 0.0f); -PARAM_DEFINE_FLOAT(INAV_ACC_T_11, 0.0021f); -PARAM_DEFINE_FLOAT(INAV_ACC_T_12, 0.0f); -PARAM_DEFINE_FLOAT(INAV_ACC_T_20, 0.0f); -PARAM_DEFINE_FLOAT(INAV_ACC_T_21, 0.0f); -PARAM_DEFINE_FLOAT(INAV_ACC_T_22, 0.0021f); - int parameters_init(struct position_estimator_inav_param_handles *h) { h->use_gps = param_find("INAV_USE_GPS"); @@ -78,21 +61,6 @@ int parameters_init(struct position_estimator_inav_param_handles *h) { h->k_alt_20 = param_find("INAV_K_ALT_20"); h->k_alt_21 = param_find("INAV_K_ALT_21"); - h->acc_offs_w = param_find("INAV_ACC_OFFS_W"); - - h->acc_offs_0 = param_find("INAV_ACC_OFFS_0"); - h->acc_offs_1 = param_find("INAV_ACC_OFFS_1"); - h->acc_offs_2 = param_find("INAV_ACC_OFFS_2"); - - h->acc_t_00 = param_find("INAV_ACC_T_00"); - h->acc_t_01 = param_find("INAV_ACC_T_01"); - h->acc_t_02 = param_find("INAV_ACC_T_02"); - h->acc_t_10 = param_find("INAV_ACC_T_10"); - h->acc_t_11 = param_find("INAV_ACC_T_11"); - h->acc_t_12 = param_find("INAV_ACC_T_12"); - h->acc_t_20 = param_find("INAV_ACC_T_20"); - h->acc_t_21 = param_find("INAV_ACC_T_21"); - h->acc_t_22 = param_find("INAV_ACC_T_22"); return OK; } @@ -106,26 +74,5 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->k_alt_20, &(p->k[2][0])); param_get(h->k_alt_21, &(p->k[2][1])); - param_get(h->acc_offs_w, &(p->acc_offs_w)); - - /* read int32 and cast to int16 */ - int32_t t; - param_get(h->acc_offs_0, &t); - p->acc_offs[0] = t; - param_get(h->acc_offs_1, &t); - p->acc_offs[1] = t; - param_get(h->acc_offs_2, &t); - p->acc_offs[2] = t; - - param_get(h->acc_t_00, &(p->acc_T[0][0])); - param_get(h->acc_t_01, &(p->acc_T[0][1])); - param_get(h->acc_t_02, &(p->acc_T[0][2])); - param_get(h->acc_t_10, &(p->acc_T[1][0])); - param_get(h->acc_t_11, &(p->acc_T[1][1])); - param_get(h->acc_t_12, &(p->acc_T[1][2])); - param_get(h->acc_t_20, &(p->acc_T[2][0])); - param_get(h->acc_t_21, &(p->acc_T[2][1])); - param_get(h->acc_t_22, &(p->acc_T[2][2])); - return OK; } diff --git a/apps/position_estimator_inav/position_estimator_inav_params.h b/apps/position_estimator_inav/position_estimator_inav_params.h index 2c6fb3df2e..eaa1bbc953 100644 --- a/apps/position_estimator_inav/position_estimator_inav_params.h +++ b/apps/position_estimator_inav/position_estimator_inav_params.h @@ -43,9 +43,6 @@ struct position_estimator_inav_params { int use_gps; float k[3][2]; - float acc_offs_w; - int16_t acc_offs[3]; - float acc_T[3][3]; }; struct position_estimator_inav_param_handles { @@ -57,22 +54,6 @@ struct position_estimator_inav_param_handles { param_t k_alt_11; param_t k_alt_20; param_t k_alt_21; - - param_t acc_offs_w; - - param_t acc_offs_0; - param_t acc_offs_1; - param_t acc_offs_2; - - param_t acc_t_00; - param_t acc_t_01; - param_t acc_t_02; - param_t acc_t_10; - param_t acc_t_11; - param_t acc_t_12; - param_t acc_t_20; - param_t acc_t_21; - param_t acc_t_22; }; /** From 861a21ef7559386d8301c6b8860a13ac2fc0ef64 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 2 May 2013 19:02:32 +0400 Subject: [PATCH 112/486] Position estimation using accel+GPS implemented --- .../position_estimator_inav_main.c | 51 ++++++++++++++----- .../position_estimator_inav_params.c | 35 ++++++++++--- .../position_estimator_inav_params.h | 10 +++- 3 files changed, 74 insertions(+), 22 deletions(-) diff --git a/apps/position_estimator_inav/position_estimator_inav_main.c b/apps/position_estimator_inav/position_estimator_inav_main.c index 071ec6ad4d..2b485f8950 100644 --- a/apps/position_estimator_inav/position_estimator_inav_main.c +++ b/apps/position_estimator_inav/position_estimator_inav_main.c @@ -261,6 +261,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { while (!thread_should_exit) { bool accelerometer_updated = false; bool baro_updated = false; + bool gps_updated = false; + float local_pos_gps[3] = { 0.0f, 0.0f, 0.0f }; + int ret = poll(fds, params.use_gps ? 5 : 4, 10); // wait maximal this 10 ms = 100 Hz minimum rate if (ret < 0) { /* poll error */ @@ -322,14 +325,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { /* vehicle GPS position */ if (fds[4].revents & POLLIN) { /* new GPS value */ - orb_copy(ORB_ID(vehicle_gps_position), - vehicle_gps_position_sub, &gps); - static float local_pos_gps[3] = { 0.0f, 0.0f, 0.0f }; /* output variables from tangent plane mapping */ + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); /* Project gps lat lon (Geographic coordinate system) to plane */ map_projection_project(((double) (gps.lat)) * 1e-7, ((double) (gps.lon)) * 1e-7, &(local_pos_gps[0]), &(local_pos_gps[1])); local_pos_gps[2] = (float) (gps.alt * 1e-3); + gps_updated = true; pos.valid = gps.fix_type >= 3; gps_updates++; } @@ -352,7 +354,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } } accel_NED[2] += CONSTANTS_ONE_G; - /* kalman filter prediction */ + + /* kalman filter for altitude */ kalman_filter_inertial_predict(dt, z_est); /* prepare vectors for kalman filter correction */ float z_meas[2]; // position, acceleration @@ -367,8 +370,32 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } if (use_z[0] || use_z[1]) { /* correction */ - kalman_filter_inertial_update(z_est, z_meas, params.k, - use_z); + kalman_filter_inertial_update(z_est, z_meas, params.k_alt, use_z); + } + + if (params.use_gps) { + /* kalman filter for position */ + kalman_filter_inertial_predict(dt, x_est); + kalman_filter_inertial_predict(dt, y_est); + /* prepare vectors for kalman filter correction */ + float x_meas[2]; // position, acceleration + float y_meas[2]; // position, acceleration + bool use_xy[2] = { false, false }; + if (gps_updated) { + x_meas[0] = local_pos_gps[0]; + y_meas[0] = local_pos_gps[1]; + use_xy[0] = true; + } + if (accelerometer_updated) { + x_meas[1] = accel_NED[0]; + y_meas[1] = accel_NED[1]; + use_xy[1] = true; + } + if (use_xy[0] || use_xy[1]) { + /* correction */ + kalman_filter_inertial_update(x_est, x_meas, params.k_pos, use_xy); + kalman_filter_inertial_update(y_est, y_meas, params.k_pos, use_xy); + } } } if (verbose_mode) { @@ -390,10 +417,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } if (t - pub_last > pub_interval) { pub_last = t; - pos.x = 0.0f; - pos.vx = 0.0f; - pos.y = 0.0f; - pos.vy = 0.0f; + pos.x = x_est[0]; + pos.vx = x_est[1]; + pos.y = y_est[0]; + pos.vy = y_est[1]; pos.z = z_est[0]; pos.vz = z_est[1]; pos.timestamp = hrt_absolute_time(); @@ -402,9 +429,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { && (isfinite(pos.vy)) && (isfinite(pos.z)) && (isfinite(pos.vz))) { - orb_publish(ORB_ID( - vehicle_local_position), vehicle_local_position_pub, - &pos); + orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &pos); } } } diff --git a/apps/position_estimator_inav/position_estimator_inav_params.c b/apps/position_estimator_inav/position_estimator_inav_params.c index bb2b014813..8466bcd0a3 100644 --- a/apps/position_estimator_inav/position_estimator_inav_params.c +++ b/apps/position_estimator_inav/position_estimator_inav_params.c @@ -40,8 +40,6 @@ #include "position_estimator_inav_params.h" -/* Kalman Filter covariances */ -/* gps measurement noise standard deviation */ PARAM_DEFINE_INT32(INAV_USE_GPS, 0); PARAM_DEFINE_FLOAT(INAV_K_ALT_00, 0.0f); @@ -51,6 +49,13 @@ PARAM_DEFINE_FLOAT(INAV_K_ALT_11, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_ALT_20, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_ALT_21, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_POS_00, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_POS_01, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_POS_10, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_POS_11, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_POS_20, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_POS_21, 0.0f); + int parameters_init(struct position_estimator_inav_param_handles *h) { h->use_gps = param_find("INAV_USE_GPS"); @@ -61,18 +66,32 @@ int parameters_init(struct position_estimator_inav_param_handles *h) { h->k_alt_20 = param_find("INAV_K_ALT_20"); h->k_alt_21 = param_find("INAV_K_ALT_21"); + h->k_pos_00 = param_find("INAV_K_POS_00"); + h->k_pos_01 = param_find("INAV_K_POS_01"); + h->k_pos_10 = param_find("INAV_K_POS_10"); + h->k_pos_11 = param_find("INAV_K_POS_11"); + h->k_pos_20 = param_find("INAV_K_POS_20"); + h->k_pos_21 = param_find("INAV_K_POS_21"); + return OK; } int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) { param_get(h->use_gps, &(p->use_gps)); - param_get(h->k_alt_00, &(p->k[0][0])); - param_get(h->k_alt_01, &(p->k[0][1])); - param_get(h->k_alt_10, &(p->k[1][0])); - param_get(h->k_alt_11, &(p->k[1][1])); - param_get(h->k_alt_20, &(p->k[2][0])); - param_get(h->k_alt_21, &(p->k[2][1])); + param_get(h->k_alt_00, &(p->k_alt[0][0])); + param_get(h->k_alt_01, &(p->k_alt[0][1])); + param_get(h->k_alt_10, &(p->k_alt[1][0])); + param_get(h->k_alt_11, &(p->k_alt[1][1])); + param_get(h->k_alt_20, &(p->k_alt[2][0])); + param_get(h->k_alt_21, &(p->k_alt[2][1])); + + param_get(h->k_pos_00, &(p->k_pos[0][0])); + param_get(h->k_pos_01, &(p->k_pos[0][1])); + param_get(h->k_pos_10, &(p->k_pos[1][0])); + param_get(h->k_pos_11, &(p->k_pos[1][1])); + param_get(h->k_pos_20, &(p->k_pos[2][0])); + param_get(h->k_pos_21, &(p->k_pos[2][1])); return OK; } diff --git a/apps/position_estimator_inav/position_estimator_inav_params.h b/apps/position_estimator_inav/position_estimator_inav_params.h index eaa1bbc953..8cdc2e10f3 100644 --- a/apps/position_estimator_inav/position_estimator_inav_params.h +++ b/apps/position_estimator_inav/position_estimator_inav_params.h @@ -42,7 +42,8 @@ struct position_estimator_inav_params { int use_gps; - float k[3][2]; + float k_alt[3][2]; + float k_pos[3][2]; }; struct position_estimator_inav_param_handles { @@ -54,6 +55,13 @@ struct position_estimator_inav_param_handles { param_t k_alt_11; param_t k_alt_20; param_t k_alt_21; + + param_t k_pos_00; + param_t k_pos_01; + param_t k_pos_10; + param_t k_pos_11; + param_t k_pos_20; + param_t k_pos_21; }; /** From c21ba6c5007530f22db704924e79e9bb1ea79130 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 17 May 2013 12:32:07 +0400 Subject: [PATCH 113/486] position_estimator_inav added to config_px4fmu_default.mk --- makefiles/config_px4fmu_default.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/makefiles/config_px4fmu_default.mk b/makefiles/config_px4fmu_default.mk index ae62b70347..23700b0ac7 100644 --- a/makefiles/config_px4fmu_default.mk +++ b/makefiles/config_px4fmu_default.mk @@ -64,6 +64,7 @@ MODULES += modules/attitude_estimator_ekf MODULES += modules/position_estimator_mc MODULES += modules/position_estimator MODULES += modules/att_pos_estimator_ekf +MODULES += modules/position_estimator_inav # # Vehicle Control From 5842c2212334876979f329b9b6bceba9609d91af Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 20 May 2013 19:47:38 +0400 Subject: [PATCH 114/486] Use GPS velocity in position estimator --- .../kalman_filter_inertial.c | 17 ++++++++++++- .../kalman_filter_inertial.h | 4 ++- .../position_estimator_inav_main.c | 25 +++++++++++-------- .../position_estimator_inav_params.c | 9 +++++++ .../position_estimator_inav_params.h | 5 +++- 5 files changed, 46 insertions(+), 14 deletions(-) diff --git a/src/modules/position_estimator_inav/kalman_filter_inertial.c b/src/modules/position_estimator_inav/kalman_filter_inertial.c index 64031ee7bb..390e1b7208 100644 --- a/src/modules/position_estimator_inav/kalman_filter_inertial.c +++ b/src/modules/position_estimator_inav/kalman_filter_inertial.c @@ -12,7 +12,7 @@ void kalman_filter_inertial_predict(float dt, float x[3]) { x[1] += x[2] * dt; } -void kalman_filter_inertial_update(float x[3], float z[2], float k[3][2], bool use[2]) { +void kalman_filter_inertial2_update(float x[3], float z[2], float k[3][2], bool use[2]) { float y[2]; // y = z - H x y[0] = z[0] - x[0]; @@ -25,3 +25,18 @@ void kalman_filter_inertial_update(float x[3], float z[2], float k[3][2], bool u } } } + +void kalman_filter_inertial3_update(float x[3], float z[3], float k[3][3], bool use[3]) { + float y[2]; + // y = z - H x + y[0] = z[0] - x[0]; + y[1] = z[1] - x[1]; + y[2] = z[2] - x[2]; + // x = x + K * y + for (int i = 0; i < 3; i++) { // Row + for (int j = 0; j < 3; j++) { // Column + if (use[j]) + x[i] += k[i][j] * y[j]; + } + } +} diff --git a/src/modules/position_estimator_inav/kalman_filter_inertial.h b/src/modules/position_estimator_inav/kalman_filter_inertial.h index 3e5134c33e..d34f582980 100644 --- a/src/modules/position_estimator_inav/kalman_filter_inertial.h +++ b/src/modules/position_estimator_inav/kalman_filter_inertial.h @@ -9,4 +9,6 @@ void kalman_filter_inertial_predict(float dt, float x[3]); -void kalman_filter_inertial_update(float x[3], float z[2], float k[3][2], bool use[2]); +void kalman_filter_inertial2_update(float x[3], float z[2], float k[3][2], bool use[2]); + +void kalman_filter_inertial3_update(float x[3], float z[3], float k[3][3], bool use[3]); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 2b485f8950..6ecdfe01d9 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -370,7 +370,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } if (use_z[0] || use_z[1]) { /* correction */ - kalman_filter_inertial_update(z_est, z_meas, params.k_alt, use_z); + kalman_filter_inertial2_update(z_est, z_meas, params.k_alt, use_z); } if (params.use_gps) { @@ -378,23 +378,26 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { kalman_filter_inertial_predict(dt, x_est); kalman_filter_inertial_predict(dt, y_est); /* prepare vectors for kalman filter correction */ - float x_meas[2]; // position, acceleration - float y_meas[2]; // position, acceleration - bool use_xy[2] = { false, false }; + float x_meas[3]; // position, velocity, acceleration + float y_meas[3]; // position, velocity, acceleration + bool use_xy[3] = { false, false, false }; if (gps_updated) { x_meas[0] = local_pos_gps[0]; y_meas[0] = local_pos_gps[1]; + x_meas[1] = gps.vel_n_m_s; + y_meas[1] = gps.vel_e_m_s; use_xy[0] = true; - } - if (accelerometer_updated) { - x_meas[1] = accel_NED[0]; - y_meas[1] = accel_NED[1]; use_xy[1] = true; } - if (use_xy[0] || use_xy[1]) { + if (accelerometer_updated) { + x_meas[2] = accel_NED[0]; + y_meas[2] = accel_NED[1]; + use_xy[2] = true; + } + if (use_xy[0] || use_xy[2]) { /* correction */ - kalman_filter_inertial_update(x_est, x_meas, params.k_pos, use_xy); - kalman_filter_inertial_update(y_est, y_meas, params.k_pos, use_xy); + kalman_filter_inertial3_update(x_est, x_meas, params.k_pos, use_xy); + kalman_filter_inertial3_update(y_est, y_meas, params.k_pos, use_xy); } } } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 8466bcd0a3..20142b70c5 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -51,10 +51,13 @@ PARAM_DEFINE_FLOAT(INAV_K_ALT_21, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_POS_00, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_POS_01, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_POS_02, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_POS_10, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_POS_11, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_POS_12, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_POS_20, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_POS_21, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_POS_22, 0.0f); int parameters_init(struct position_estimator_inav_param_handles *h) { h->use_gps = param_find("INAV_USE_GPS"); @@ -68,10 +71,13 @@ int parameters_init(struct position_estimator_inav_param_handles *h) { h->k_pos_00 = param_find("INAV_K_POS_00"); h->k_pos_01 = param_find("INAV_K_POS_01"); + h->k_pos_02 = param_find("INAV_K_POS_02"); h->k_pos_10 = param_find("INAV_K_POS_10"); h->k_pos_11 = param_find("INAV_K_POS_11"); + h->k_pos_12 = param_find("INAV_K_POS_12"); h->k_pos_20 = param_find("INAV_K_POS_20"); h->k_pos_21 = param_find("INAV_K_POS_21"); + h->k_pos_22 = param_find("INAV_K_POS_22"); return OK; } @@ -88,10 +94,13 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->k_pos_00, &(p->k_pos[0][0])); param_get(h->k_pos_01, &(p->k_pos[0][1])); + param_get(h->k_pos_02, &(p->k_pos[0][2])); param_get(h->k_pos_10, &(p->k_pos[1][0])); param_get(h->k_pos_11, &(p->k_pos[1][1])); + param_get(h->k_pos_12, &(p->k_pos[1][2])); param_get(h->k_pos_20, &(p->k_pos[2][0])); param_get(h->k_pos_21, &(p->k_pos[2][1])); + param_get(h->k_pos_22, &(p->k_pos[2][2])); return OK; } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index 8cdc2e10f3..c0e0042b6e 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -43,7 +43,7 @@ struct position_estimator_inav_params { int use_gps; float k_alt[3][2]; - float k_pos[3][2]; + float k_pos[3][3]; }; struct position_estimator_inav_param_handles { @@ -58,10 +58,13 @@ struct position_estimator_inav_param_handles { param_t k_pos_00; param_t k_pos_01; + param_t k_pos_02; param_t k_pos_10; param_t k_pos_11; + param_t k_pos_12; param_t k_pos_20; param_t k_pos_21; + param_t k_pos_22; }; /** From cb1fbecd09d0e98ac11a18342f8670d7eb71ec47 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 22 May 2013 12:25:13 +0200 Subject: [PATCH 115/486] Merged master from main repo --- makefiles/config_px4fmuv2_default.mk | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/makefiles/config_px4fmuv2_default.mk b/makefiles/config_px4fmuv2_default.mk index ab1f19314e..26c2499013 100644 --- a/makefiles/config_px4fmuv2_default.mk +++ b/makefiles/config_px4fmuv2_default.mk @@ -74,15 +74,19 @@ MODULES += modules/multirotor_pos_control MODULES += modules/sdlog # -# Libraries +# Library modules # MODULES += modules/systemlib MODULES += modules/systemlib/mixer MODULES += modules/mathlib -MODULES += modules/mathlib/CMSIS MODULES += modules/controllib MODULES += modules/uORB +# +# Libraries +# +LIBRARIES += modules/mathlib/CMSIS + # # Demo apps # From 78d29045c4da2d5ed4cea50fc2184b57dea01951 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 22 May 2013 20:12:16 +0200 Subject: [PATCH 116/486] Fix configuration selection for px4iov2; still doesn't build completely, but it's better. --- Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Makefile b/Makefile index dea3443901..ad10ed7c81 100644 --- a/Makefile +++ b/Makefile @@ -145,7 +145,7 @@ ifneq ($(filter archives,$(MAKECMDGOALS)),) endif $(ARCHIVE_DIR)%.export: board = $(notdir $(basename $@)) -$(ARCHIVE_DIR)%.export: configuration = $(if $(filter $(board),px4io),io,nsh) +$(ARCHIVE_DIR)%.export: configuration = $(if $(filter px4io px4iov2,$(board)),io,nsh) $(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) $(NUTTX_APPS) @echo %% Configuring NuttX for $(board) $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) From 308ec6001a2e1ac31ea818b1d482a34b8ed0099b Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 22 May 2013 22:09:00 +0200 Subject: [PATCH 117/486] Add serial read-length handling. --- src/modules/px4iofirmware/protocol.h | 11 +++++++---- src/modules/px4iofirmware/serial.c | 8 ++++++-- 2 files changed, 13 insertions(+), 6 deletions(-) diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 29107f79f3..6cdf86b2b3 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -36,7 +36,8 @@ /** * @file protocol.h * - * PX4IO interface protocol. + * PX4IO interface protocol + * ======================== * * Communication is performed via writes to and reads from 16-bit virtual * registers organised into pages of 255 registers each. @@ -63,19 +64,21 @@ * readable pages to be densely packed. Page numbers do not need to be * packed. * - * PX4IO I2C interface notes: + * PX4IO I2C interface notes + * ------------------------- * * Register read/write operations are mapped directly to PX4IO register * read/write operations. * - * PX4IO Serial interface notes: + * PX4IO Serial interface notes + * ---------------------------- * * The MSB of the page number is used to distinguish between read and * write operations. If set, the operation is a write and additional * data is expected to follow in the packet as for I2C writes. * * If clear, the packet is expected to contain a single byte giving the - * number of bytes to be read. PX4IO will respond with a packet containing + * number of registers to be read. PX4IO will respond with a packet containing * the same header (page, offset) and the requested data. * * If a read is requested when PX4IO does not have buffer space to store diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index a12d58aca2..bf9456e942 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -107,7 +107,9 @@ serial_callback(void *arg, const void *data, unsigned length) return; } - /* it's a read */ + /* it's a read - must contain length byte */ + if (length != 3) + return; uint16_t *registers; unsigned count; @@ -118,10 +120,12 @@ serial_callback(void *arg, const void *data, unsigned length) if (registers_get(message[0], message[1], ®isters, &count) < 0) count = 0; - /* fill buffer with message */ + /* fill buffer with message, limited by length */ #define TX_MAX ((sizeof(tx_buf) - 2) / 2) if (count > TX_MAX) count = TX_MAX; + if (count > message[2]) + count = message[2]; memcpy(&tx_buf[2], registers, count * 2); /* try to send the message */ From 4bf49cfc35e86528a7b321dc0b8fb55c36fad510 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 2 Jun 2013 19:28:25 +0400 Subject: [PATCH 118/486] multirotor_pos_control cleanup --- .../multirotor_pos_control/position_control.c | 235 ------------------ .../multirotor_pos_control/position_control.h | 50 ---- 2 files changed, 285 deletions(-) delete mode 100644 src/modules/multirotor_pos_control/position_control.c delete mode 100644 src/modules/multirotor_pos_control/position_control.h diff --git a/src/modules/multirotor_pos_control/position_control.c b/src/modules/multirotor_pos_control/position_control.c deleted file mode 100644 index 9c53a5bf6c..0000000000 --- a/src/modules/multirotor_pos_control/position_control.c +++ /dev/null @@ -1,235 +0,0 @@ -// /**************************************************************************** -// * -// * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. -// * Author: @author Lorenz Meier -// * @author Laurens Mackay -// * @author Tobias Naegeli -// * @author Martin Rutschmann -// * -// * 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 multirotor_position_control.c -// * Implementation of the position control for a multirotor VTOL -// */ - -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include - -// #include "multirotor_position_control.h" - -// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual, -// const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos, -// const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp) -// { -// static PID_t distance_controller; - -// static int read_ret; -// static global_data_position_t position_estimated; - -// static uint16_t counter; - -// static bool initialized; -// static uint16_t pm_counter; - -// static float lat_next; -// static float lon_next; - -// static float pitch_current; - -// static float thrust_total; - - -// if (initialized == false) { - -// pid_init(&distance_controller, -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU], -// PID_MODE_DERIVATIV_CALC, 150);//150 - -// // pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM]; -// // pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM]; - -// thrust_total = 0.0f; - -// /* Position initialization */ -// /* Wait for new position estimate */ -// do { -// read_ret = read_lock_position(&position_estimated); -// } while (read_ret != 0); - -// lat_next = position_estimated.lat; -// lon_next = position_estimated.lon; - -// /* attitude initialization */ -// global_data_lock(&global_data_attitude->access_conf); -// pitch_current = global_data_attitude->pitch; -// global_data_unlock(&global_data_attitude->access_conf); - -// initialized = true; -// } - -// /* load new parameters with 10Hz */ -// if (counter % 50 == 0) { -// if (global_data_trylock(&global_data_parameter_storage->access_conf) == 0) { -// /* check whether new parameters are available */ -// if (global_data_parameter_storage->counter > pm_counter) { -// pid_set_parameters(&distance_controller, -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU]); - -// // -// // pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM]; -// // pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM]; - -// pm_counter = global_data_parameter_storage->counter; -// printf("Position controller changed pid parameters\n"); -// } -// } - -// global_data_unlock(&global_data_parameter_storage->access_conf); -// } - - -// /* Wait for new position estimate */ -// do { -// read_ret = read_lock_position(&position_estimated); -// } while (read_ret != 0); - -// /* Get next waypoint */ //TODO: add local copy - -// if (0 == global_data_trylock(&global_data_position_setpoint->access_conf)) { -// lat_next = global_data_position_setpoint->x; -// lon_next = global_data_position_setpoint->y; -// global_data_unlock(&global_data_position_setpoint->access_conf); -// } - -// /* Get distance to waypoint */ -// float distance_to_waypoint = get_distance_to_next_waypoint(position_estimated.lat , position_estimated.lon, lat_next, lon_next); -// // if(counter % 5 == 0) -// // printf("distance_to_waypoint: %.4f\n", distance_to_waypoint); - -// /* Get bearing to waypoint (direction on earth surface to next waypoint) */ -// float bearing = get_bearing_to_next_waypoint(position_estimated.lat, position_estimated.lon, lat_next, lon_next); - -// if (counter % 5 == 0) -// printf("bearing: %.4f\n", bearing); - -// /* Calculate speed in direction of bearing (needed for controller) */ -// float speed_norm = sqrtf(position_estimated.vx * position_estimated.vx + position_estimated.vy * position_estimated.vy); -// // if(counter % 5 == 0) -// // printf("speed_norm: %.4f\n", speed_norm); -// float speed_to_waypoint = 0; //(position_estimated.vx * cosf(bearing) + position_estimated.vy * sinf(bearing))/speed_norm; //FIXME, TODO: re-enable this once we have a full estimate of the speed, then we can do a PID for the distance controller - -// /* Control Thrust in bearing direction */ -// float horizontal_thrust = -pid_calculate(&distance_controller, 0, distance_to_waypoint, speed_to_waypoint, -// CONTROL_PID_POSITION_INTERVAL); //TODO: maybe this "-" sign is an error somewhere else - -// // if(counter % 5 == 0) -// // printf("horizontal thrust: %.4f\n", horizontal_thrust); - -// /* Get total thrust (from remote for now) */ -// if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) { -// thrust_total = (float)global_data_rc_channels->chan[THROTTLE].scale; //TODO: how should we use the RC_CHANNELS_FUNCTION enum? -// global_data_unlock(&global_data_rc_channels->access_conf); -// } - -// const float max_gas = 500.0f; -// thrust_total *= max_gas / 20000.0f; //TODO: check this -// thrust_total += max_gas / 2.0f; - - -// if (horizontal_thrust > thrust_total) { -// horizontal_thrust = thrust_total; - -// } else if (horizontal_thrust < -thrust_total) { -// horizontal_thrust = -thrust_total; -// } - - - -// //TODO: maybe we want to add a speed controller later... - -// /* Calclulate thrust in east and north direction */ -// float thrust_north = cosf(bearing) * horizontal_thrust; -// float thrust_east = sinf(bearing) * horizontal_thrust; - -// if (counter % 10 == 0) { -// printf("thrust north: %.4f\n", thrust_north); -// printf("thrust east: %.4f\n", thrust_east); -// fflush(stdout); -// } - -// /* Get current attitude */ -// if (0 == global_data_trylock(&global_data_attitude->access_conf)) { -// pitch_current = global_data_attitude->pitch; -// global_data_unlock(&global_data_attitude->access_conf); -// } - -// /* Get desired pitch & roll */ -// float pitch_desired = 0.0f; -// float roll_desired = 0.0f; - -// if (thrust_total != 0) { -// float pitch_fraction = -thrust_north / thrust_total; -// float roll_fraction = thrust_east / (cosf(pitch_current) * thrust_total); - -// if (roll_fraction < -1) { -// roll_fraction = -1; - -// } else if (roll_fraction > 1) { -// roll_fraction = 1; -// } - -// // if(counter % 5 == 0) -// // { -// // printf("pitch_fraction: %.4f, roll_fraction: %.4f\n",pitch_fraction, roll_fraction); -// // fflush(stdout); -// // } - -// pitch_desired = asinf(pitch_fraction); -// roll_desired = asinf(roll_fraction); -// } - -// att_sp.roll = roll_desired; -// att_sp.pitch = pitch_desired; - -// counter++; -// } diff --git a/src/modules/multirotor_pos_control/position_control.h b/src/modules/multirotor_pos_control/position_control.h deleted file mode 100644 index 2144ebc343..0000000000 --- a/src/modules/multirotor_pos_control/position_control.h +++ /dev/null @@ -1,50 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * @author Laurens Mackay - * @author Tobias Naegeli - * @author Martin Rutschmann - * - * 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 multirotor_position_control.h - * Definition of the position control for a multirotor VTOL - */ - -// #ifndef POSITION_CONTROL_H_ -// #define POSITION_CONTROL_H_ - -// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual, -// const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos, -// const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp); - -// #endif /* POSITION_CONTROL_H_ */ From 8567134d64f5d8e3c7aba7cebfdf56ffe56b44ed Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 7 Jun 2013 19:41:41 +0200 Subject: [PATCH 119/486] Made pwm command sending continously, improved failsafe logic --- src/modules/px4iofirmware/mixer.cpp | 16 +++++----- src/systemcmds/pwm/pwm.c | 48 +++++++++++++++++++++++++---- 2 files changed, 49 insertions(+), 15 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index a2193b526b..123eb67786 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -98,7 +98,7 @@ mixer_tick(void) if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) { isr_debug(1, "AP RX timeout"); } - r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM); + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK); r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST; } else { @@ -112,12 +112,11 @@ mixer_tick(void) * Decide which set of controls we're using. */ - /* do not mix if mixer is invalid or if RAW_PWM mode is on and FMU is good */ + /* do not mix if RAW_PWM mode is on and FMU is good */ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) && - !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { + (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { - /* don't actually mix anything - we already have raw PWM values or - not a valid mixer. */ + /* don't actually mix anything - we already have raw PWM values */ source = MIX_NONE; } else { @@ -196,10 +195,9 @@ mixer_tick(void) bool should_arm = ( /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && - /* there is valid input via direct PWM or mixer */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) && - /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) && - /* FMU is available or FMU is not available but override is an option */ - ((r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) || (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) )) + /* there is valid input via direct PWM or mixer */ ((r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) || + /* or failsafe was set manually */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)) && + /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) ); if (should_arm && !mixer_servos_armed) { diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index e150b5a74b..570ca6aa93 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -180,7 +180,8 @@ pwm_main(int argc, char *argv[]) } /* iterate remaining arguments */ - unsigned channel = 0; + unsigned nchannels = 0; + unsigned channel[8] = {0}; while (argc--) { const char *arg = argv[0]; argv++; @@ -204,13 +205,15 @@ pwm_main(int argc, char *argv[]) } unsigned pwm_value = strtol(arg, &ep, 0); if (*ep == '\0') { - ret = ioctl(fd, PWM_SERVO_SET(channel), pwm_value); - if (ret != OK) - err(1, "PWM_SERVO_SET(%d)", channel); - channel++; + channel[nchannels] = pwm_value; + nchannels++; + + if (nchannels >= sizeof(channel) / sizeof(channel[0])) + err(1, "too many pwm values (max %d)", sizeof(channel) / sizeof(channel[0])); + continue; } - usage("unrecognised option"); + usage("unrecognized option"); } /* print verbose info */ @@ -250,5 +253,38 @@ pwm_main(int argc, char *argv[]) } fflush(stdout); } + + /* perform PWM output */ + if (nchannels) { + + /* Open console directly to grab CTRL-C signal */ + int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY); + if (!console) + err(1, "failed opening console"); + + warnx("Press CTRL-C or 'c' to abort."); + + while (1) { + for (int i = 0; i < nchannels; i++) { + ret = ioctl(fd, PWM_SERVO_SET(i), channel[i]); + if (ret != OK) + err(1, "PWM_SERVO_SET(%d)", i); + } + + /* abort on user request */ + char c; + if (read(console, &c, 1) == 1) { + if (c == 0x03 || c == 0x63) { + warnx("User abort\n"); + close(console); + exit(0); + } + } + + /* rate limit to ~ 20 Hz */ + usleep(50000); + } + } + exit(0); } \ No newline at end of file From d2c5990d6f0b1c3f4183a193c1c51250cbdfa127 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 9 Jun 2013 12:41:47 +0200 Subject: [PATCH 120/486] Fixed pwm count check --- src/systemcmds/pwm/pwm.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 570ca6aa93..619bd2c784 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -205,12 +205,12 @@ pwm_main(int argc, char *argv[]) } unsigned pwm_value = strtol(arg, &ep, 0); if (*ep == '\0') { + if (nchannels > sizeof(channel) / sizeof(channel[0])) + err(1, "too many pwm values (max %d)", sizeof(channel) / sizeof(channel[0])); + channel[nchannels] = pwm_value; nchannels++; - if (nchannels >= sizeof(channel) / sizeof(channel[0])) - err(1, "too many pwm values (max %d)", sizeof(channel) / sizeof(channel[0])); - continue; } usage("unrecognized option"); From 4ef87206eccd292eb5111bba7d5f39dd03f7e20c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 9 Jun 2013 14:03:49 +0200 Subject: [PATCH 121/486] Code formatting and warning fixes --- src/drivers/blinkm/blinkm.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 3fabfd9a54..9fed1149d4 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -486,15 +486,15 @@ BlinkM::led() /* get number of used satellites in navigation */ num_of_used_sats = 0; - //for(int satloop=0; satloop<20; satloop++) { - for(int satloop=0; satloop checking cells\n"); for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) { From b12678014ff9b500912ec44f6f9c771af3bdd217 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 9 Jun 2013 14:04:13 +0200 Subject: [PATCH 122/486] Fixed chan count logic --- src/systemcmds/pwm/pwm.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 619bd2c784..c42a36c7f3 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -274,7 +274,7 @@ pwm_main(int argc, char *argv[]) /* abort on user request */ char c; if (read(console, &c, 1) == 1) { - if (c == 0x03 || c == 0x63) { + if (c == 0x03 || c == 0x63 || c == 'q') { warnx("User abort\n"); close(console); exit(0); From 1deced7629e7d140a931c42657f75da512696c7e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 9 Jun 2013 14:09:09 +0200 Subject: [PATCH 123/486] Added safety status feedback, disallow arming of a rotary wing with engaged safety --- src/drivers/px4io/px4io.cpp | 33 +++++++- src/modules/commander/commander.c | 82 +++++++++++++------- src/modules/commander/state_machine_helper.c | 35 ++++++++- src/modules/commander/state_machine_helper.h | 4 + src/modules/uORB/objects_common.cpp | 4 + src/modules/uORB/topics/actuator_controls.h | 7 +- src/modules/uORB/topics/safety.h | 60 ++++++++++++++ src/modules/uORB/topics/vehicle_status.h | 2 + 8 files changed, 189 insertions(+), 38 deletions(-) create mode 100644 src/modules/uORB/topics/safety.h diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 19163cebe3..bc65c4f259 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -80,6 +80,7 @@ #include #include #include +#include #include #include @@ -161,6 +162,7 @@ private: orb_advert_t _to_actuators_effective; ///< effective actuator controls topic orb_advert_t _to_outputs; ///< mixed servo outputs topic orb_advert_t _to_battery; ///< battery status / voltage + orb_advert_t _to_safety; ///< status of safety actuator_outputs_s _outputs; ///< mixed outputs actuator_controls_effective_s _controls_effective; ///< effective controls @@ -883,6 +885,25 @@ PX4IO::io_handle_status(uint16_t status) _status = status; } + /** + * Get and handle the safety status + */ + struct safety_s safety; + safety.timestamp = hrt_absolute_time(); + + if (status & PX4IO_P_STATUS_FLAGS_ARMED) { + safety.status = SAFETY_STATUS_UNLOCKED; + } else { + safety.status = SAFETY_STATUS_SAFE; + } + + /* lazily publish the safety status */ + if (_to_safety > 0) { + orb_publish(ORB_ID(safety), _to_safety, &safety); + } else { + _to_safety = orb_advertise(ORB_ID(safety), &safety); + } + return ret; } @@ -912,7 +933,7 @@ PX4IO::io_get_status() io_handle_status(regs[0]); io_handle_alarms(regs[1]); - + /* only publish if battery has a valid minimum voltage */ if (regs[2] > 3300) { battery_status_s battery_status; @@ -946,6 +967,7 @@ PX4IO::io_get_status() _to_battery = orb_advertise(ORB_ID(battery_status), &battery_status); } } + return ret; } @@ -1273,7 +1295,7 @@ PX4IO::print_status() uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS); printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s\n", flags, - ((flags & PX4IO_P_STATUS_FLAGS_ARMED) ? " ARMED" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_ARMED) ? " SAFETY_UNLOCKED" : ""), ((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""), ((flags & PX4IO_P_STATUS_FLAGS_RC_OK) ? " RC_OK" : " RC_FAIL"), ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM) ? " PPM" : ""), @@ -1634,6 +1656,11 @@ test(void) if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count)) err(1, "failed to get servo count"); + /* tell IO that its ok to disable its safety with the switch */ + ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); + if (ret != OK) + err(1, "PWM_SERVO_SET_ARM_OK"); + if (ioctl(fd, PWM_SERVO_ARM, 0)) err(1, "failed to arm servos"); @@ -1682,7 +1709,7 @@ test(void) /* Check if user wants to quit */ char c; if (read(console, &c, 1) == 1) { - if (c == 0x03 || c == 0x63) { + if (c == 0x03 || c == 0x63 || c == 'q') { warnx("User abort\n"); close(console); exit(0); diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index aab8f3e04d..937c515e69 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -81,6 +81,7 @@ #include #include #include +#include #include #include @@ -1281,6 +1282,8 @@ int commander_thread_main(int argc, char *argv[]) current_status.flag_vector_flight_mode_ok = false; /* set battery warning flag */ current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; + /* set safety device detection flag */ + current_status.flag_safety_present = false; /* advertise to ORB */ stat_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); @@ -1377,6 +1380,12 @@ int commander_thread_main(int argc, char *argv[]) memset(&battery, 0, sizeof(battery)); battery.voltage_v = 0.0f; + /* subscribe to safety topic */ + int safety_sub = orb_subscribe(ORB_ID(safety)); + struct safety_s safety; + memset(&safety, 0, sizeof(safety)); + safety.status = SAFETY_STATUS_NOT_PRESENT; + // uint8_t vehicle_state_previous = current_status.state_machine; float voltage_previous = 0.0f; @@ -1396,6 +1405,39 @@ int commander_thread_main(int argc, char *argv[]) /* Get current values */ bool new_data; + + /* update parameters */ + orb_check(param_changed_sub, &new_data); + + if (new_data || param_init_forced) { + param_init_forced = false; + /* parameters changed */ + orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); + + + /* update parameters */ + if (!current_status.flag_system_armed) { + if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { + warnx("failed setting new system type"); + } + + /* disable manual override for all systems that rely on electronic stabilization */ + if (current_status.system_type == VEHICLE_TYPE_QUADROTOR || + current_status.system_type == VEHICLE_TYPE_HEXAROTOR || + current_status.system_type == VEHICLE_TYPE_OCTOROTOR) { + current_status.flag_external_manual_override_ok = false; + + } else { + current_status.flag_external_manual_override_ok = true; + } + + /* check and update system / component ID */ + param_get(_param_system_id, &(current_status.system_id)); + param_get(_param_component_id, &(current_status.component_id)); + + } + } + orb_check(sp_man_sub, &new_data); if (new_data) { @@ -1431,36 +1473,17 @@ int commander_thread_main(int argc, char *argv[]) handle_command(stat_pub, ¤t_status, &cmd); } - /* update parameters */ - orb_check(param_changed_sub, &new_data); + orb_check(safety_sub, &new_data); - if (new_data || param_init_forced) { - param_init_forced = false; - /* parameters changed */ - orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); + if (new_data) { + /* got safety change */ + orb_copy(ORB_ID(safety), safety_sub, &safety); - - /* update parameters */ - if (!current_status.flag_system_armed) { - if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { - warnx("failed setting new system type"); - } - - /* disable manual override for all systems that rely on electronic stabilization */ - if (current_status.system_type == VEHICLE_TYPE_QUADROTOR || - current_status.system_type == VEHICLE_TYPE_HEXAROTOR || - current_status.system_type == VEHICLE_TYPE_OCTOROTOR) { - current_status.flag_external_manual_override_ok = false; - - } else { - current_status.flag_external_manual_override_ok = true; - } - - /* check and update system / component ID */ - param_get(_param_system_id, &(current_status.system_id)); - param_get(_param_component_id, &(current_status.component_id)); - - } + /* handle it */ + current_status.flag_safety_present = (safety.status != SAFETY_STATUS_NOT_PRESENT); + + if (current_status.flag_safety_present) + current_status.flag_safety_safe = (safety.status == SAFETY_STATUS_SAFE); } /* update global position estimate */ @@ -1699,7 +1722,8 @@ int commander_thread_main(int argc, char *argv[]) state_changed = true; } - if (orb_check(ORB_ID(vehicle_gps_position), &new_data)) { + orb_check(ORB_ID(vehicle_gps_position), &new_data); + if (new_data) { orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index ab728c7bbb..ac603abfdc 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -65,6 +65,18 @@ static const char *system_state_txt[] = { }; +bool is_multirotor(const struct vehicle_status_s *current_status) +{ + return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status->system_type == VEHICLE_TYPE_OCTOROTOR)); +} + +bool is_rotary_wing(const struct vehicle_status_s *current_status) +{ + return is_multirotor(current_status); +} + /** * Transition from one state to another */ @@ -513,6 +525,25 @@ void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_ void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { if (current_status->state_machine == SYSTEM_STATE_STANDBY) { + + /* reject arming if safety status is wrong */ + if (current_status->flag_safety_present) { + + /* + * The operator should not touch the vehicle if + * its armed, so we don't allow arming in the + * first place + */ + if (is_rotary_wing(current_status)) { + + /* safety is in safe position, disallow arming */ + if (current_status->flag_safety_safe) { + mavlink_log_critical(mavlink_fd, "DISENGAGE SAFETY BEFORE ARMING!"); + } + + } + } + printf("[cmd] arming\n"); do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); } @@ -538,9 +569,7 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c current_status->flag_control_manual_enabled = true; /* set behaviour based on airframe */ - if ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status->system_type == VEHICLE_TYPE_OCTOROTOR)) { + if (is_rotary_wing(current_status)) { /* assuming a rotary wing, set to SAS */ current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 2f2ccc7298..95b48d3261 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -47,6 +47,10 @@ #include #include +bool is_multirotor(const struct vehicle_status_s *current_status); + +bool is_rotary_wing(const struct vehicle_status_s *current_status); + /** * Switch to new state with no checking. * diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 4197f6fb2d..e2df31651f 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -154,3 +154,7 @@ ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s); #include "topics/debug_key_value.h" ORB_DEFINE(debug_key_value, struct debug_key_value_s); + +/* status of the system safety device */ +#include "topics/safety.h" +ORB_DEFINE(safety, struct safety_s); diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/uORB/topics/actuator_controls.h index b7c4196c0a..745c5bc874 100644 --- a/src/modules/uORB/topics/actuator_controls.h +++ b/src/modules/uORB/topics/actuator_controls.h @@ -68,9 +68,10 @@ ORB_DECLARE(actuator_controls_3); /** global 'actuator output is live' control. */ struct actuator_armed_s { - bool armed; /**< Set to true if system is armed */ - bool ready_to_arm; /**< Set to true if system is ready to be armed */ - bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ + bool armed; /**< Set to true if system is armed */ + bool ready_to_arm; /**< Set to true if system is ready to be armed */ + bool throttle_locked; /**< Set to true if the trottle is locked to zero */ + bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ }; ORB_DECLARE(actuator_armed); diff --git a/src/modules/uORB/topics/safety.h b/src/modules/uORB/topics/safety.h new file mode 100644 index 0000000000..19e8e8d459 --- /dev/null +++ b/src/modules/uORB/topics/safety.h @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file safety.h + * + * Status of an attached safety device + */ + +#ifndef TOPIC_SAFETY_H +#define TOPIC_SAFETY_H + +#include +#include "../uORB.h" + +enum SAFETY_STATUS { + SAFETY_STATUS_NOT_PRESENT, + SAFETY_STATUS_SAFE, + SAFETY_STATUS_UNLOCKED +}; + +struct safety_s { + uint64_t timestamp; /**< output timestamp in us since system boot */ + enum SAFETY_STATUS status; +}; + +/* actuator output sets; this list can be expanded as more drivers emerge */ +ORB_DECLARE(safety); + +#endif /* TOPIC_SAFETY_H */ diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index c7c1048f60..07660614bb 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -214,6 +214,8 @@ struct vehicle_status_s bool flag_valid_launch_position; /**< indicates a valid launch position */ bool flag_valid_home_position; /**< indicates a valid home position (a valid home position is not always a valid launch) */ bool flag_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */ + bool flag_safety_present; /**< indicates that a safety switch is present */ + bool flag_safety_safe; /**< safety switch is in safe position */ }; /** From 8b67f88331a9dc65e5c947da177701317d77f8bd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 9 Jun 2013 14:12:17 +0200 Subject: [PATCH 124/486] Play warning tune --- src/modules/commander/state_machine_helper.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index ac603abfdc..e18c0edc3c 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -49,6 +49,7 @@ #include #include "state_machine_helper.h" +#include "commander.h" static const char *system_state_txt[] = { "SYSTEM_STATE_PREFLIGHT", @@ -539,6 +540,9 @@ void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_s /* safety is in safe position, disallow arming */ if (current_status->flag_safety_safe) { mavlink_log_critical(mavlink_fd, "DISENGAGE SAFETY BEFORE ARMING!"); + + /* play warning tune */ + tune_error(); } } From 1028bd932cfd08366dd0dcb8c189ebcf88cce53a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 10 Jun 2013 07:39:12 +0200 Subject: [PATCH 125/486] Extended vehicle detection --- src/modules/commander/state_machine_helper.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index e18c0edc3c..0a6cfd0b5a 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -70,12 +70,14 @@ bool is_multirotor(const struct vehicle_status_s *current_status) { return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status->system_type == VEHICLE_TYPE_OCTOROTOR)); + (current_status->system_type == VEHICLE_TYPE_OCTOROTOR) || + (current_status->system_type == VEHICLE_TYPE_TRICOPTER)); } bool is_rotary_wing(const struct vehicle_status_s *current_status) { - return is_multirotor(current_status); + return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER) + || (current_status->system_type == VEHICLE_TYPE_COAXIAL); } /** From 4256e43de7ea4c9cad98e8bfc9a811310bfb3d77 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 10 Jun 2013 23:16:04 +0400 Subject: [PATCH 126/486] Complete position estimator implemented (GPS + Baro + Accel) --- .../position_estimator_inav/inertial_filter.c | 28 ++ .../position_estimator_inav/inertial_filter.h | 13 + .../kalman_filter_inertial.c | 42 --- .../kalman_filter_inertial.h | 14 - src/modules/position_estimator_inav/module.mk | 2 +- .../position_estimator_inav_main.c | 242 ++++++++++-------- .../position_estimator_inav_params.c | 76 ++---- .../position_estimator_inav_params.h | 31 +-- 8 files changed, 207 insertions(+), 241 deletions(-) create mode 100644 src/modules/position_estimator_inav/inertial_filter.c create mode 100644 src/modules/position_estimator_inav/inertial_filter.h delete mode 100644 src/modules/position_estimator_inav/kalman_filter_inertial.c delete mode 100644 src/modules/position_estimator_inav/kalman_filter_inertial.h diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c new file mode 100644 index 0000000000..b70d3504e6 --- /dev/null +++ b/src/modules/position_estimator_inav/inertial_filter.c @@ -0,0 +1,28 @@ +/* + * inertial_filter.c + * + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin + */ + +#include "inertial_filter.h" + +void inertial_filter_predict(float dt, float x[3]) +{ + x[0] += x[1] * dt + x[2] * dt * dt / 2.0f; + x[1] += x[2] * dt; +} + +void inertial_filter_correct(float dt, float x[3], int i, float z, float w) +{ + float e = z - x[i]; + x[i] += e * w * dt; + + if (i == 0) { + x[1] += e * w * w * dt; + //x[2] += e * w * w * w * dt / 3.0; // ~ 0.0 + + } else if (i == 1) { + x[2] += e * w * w * dt; + } +} diff --git a/src/modules/position_estimator_inav/inertial_filter.h b/src/modules/position_estimator_inav/inertial_filter.h new file mode 100644 index 0000000000..18c194abf2 --- /dev/null +++ b/src/modules/position_estimator_inav/inertial_filter.h @@ -0,0 +1,13 @@ +/* + * inertial_filter.h + * + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin + */ + +#include +#include + +void inertial_filter_predict(float dt, float x[3]); + +void inertial_filter_correct(float dt, float x[3], int i, float z, float w); diff --git a/src/modules/position_estimator_inav/kalman_filter_inertial.c b/src/modules/position_estimator_inav/kalman_filter_inertial.c deleted file mode 100644 index 390e1b7208..0000000000 --- a/src/modules/position_estimator_inav/kalman_filter_inertial.c +++ /dev/null @@ -1,42 +0,0 @@ -/* - * kalman_filter_inertial.c - * - * Copyright (C) 2013 Anton Babushkin. All rights reserved. - * Author: Anton Babushkin - */ - -#include "kalman_filter_inertial.h" - -void kalman_filter_inertial_predict(float dt, float x[3]) { - x[0] += x[1] * dt + x[2] * dt * dt / 2.0f; - x[1] += x[2] * dt; -} - -void kalman_filter_inertial2_update(float x[3], float z[2], float k[3][2], bool use[2]) { - float y[2]; - // y = z - H x - y[0] = z[0] - x[0]; - y[1] = z[1] - x[2]; - // x = x + K * y - for (int i = 0; i < 3; i++) { // Row - for (int j = 0; j < 2; j++) { // Column - if (use[j]) - x[i] += k[i][j] * y[j]; - } - } -} - -void kalman_filter_inertial3_update(float x[3], float z[3], float k[3][3], bool use[3]) { - float y[2]; - // y = z - H x - y[0] = z[0] - x[0]; - y[1] = z[1] - x[1]; - y[2] = z[2] - x[2]; - // x = x + K * y - for (int i = 0; i < 3; i++) { // Row - for (int j = 0; j < 3; j++) { // Column - if (use[j]) - x[i] += k[i][j] * y[j]; - } - } -} diff --git a/src/modules/position_estimator_inav/kalman_filter_inertial.h b/src/modules/position_estimator_inav/kalman_filter_inertial.h deleted file mode 100644 index d34f582980..0000000000 --- a/src/modules/position_estimator_inav/kalman_filter_inertial.h +++ /dev/null @@ -1,14 +0,0 @@ -/* - * kalman_filter_inertial.h - * - * Copyright (C) 2013 Anton Babushkin. All rights reserved. - * Author: Anton Babushkin - */ - -#include - -void kalman_filter_inertial_predict(float dt, float x[3]); - -void kalman_filter_inertial2_update(float x[3], float z[2], float k[3][2], bool use[2]); - -void kalman_filter_inertial3_update(float x[3], float z[3], float k[3][3], bool use[3]); diff --git a/src/modules/position_estimator_inav/module.mk b/src/modules/position_estimator_inav/module.mk index 8ac701c535..939d768495 100644 --- a/src/modules/position_estimator_inav/module.mk +++ b/src/modules/position_estimator_inav/module.mk @@ -38,4 +38,4 @@ MODULE_COMMAND = position_estimator_inav SRCS = position_estimator_inav_main.c \ position_estimator_inav_params.c \ - kalman_filter_inertial.c + inertial_filter.c diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 6ecdfe01d9..306ebe5ccc 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -66,7 +66,7 @@ #include #include "position_estimator_inav_params.h" -#include "kalman_filter_inertial.h" +#include "inertial_filter.h" static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ @@ -82,13 +82,15 @@ static void usage(const char *reason); /** * Print the correct usage. */ -static void usage(const char *reason) { +static void usage(const char *reason) +{ if (reason) fprintf(stderr, "%s\n", reason); - fprintf(stderr, - "usage: position_estimator_inav {start|stop|status} [-v]\n\n"); - exit(1); - } + + fprintf(stderr, + "usage: position_estimator_inav {start|stop|status} [-v]\n\n"); + exit(1); +} /** * The position_estimator_inav_thread only briefly exists to start @@ -98,7 +100,8 @@ static void usage(const char *reason) { * The actual stack size should be set in the call * to task_create(). */ -int position_estimator_inav_main(int argc, char *argv[]) { +int position_estimator_inav_main(int argc, char *argv[]) +{ if (argc < 1) usage("missing command"); @@ -108,17 +111,19 @@ int position_estimator_inav_main(int argc, char *argv[]) { /* this is not an error */ exit(0); } + if (argc > 1) if (!strcmp(argv[2], "-v")) verbose_mode = true; thread_should_exit = false; position_estimator_inav_task = task_spawn("position_estimator_inav", - SCHED_RR, SCHED_PRIORITY_MAX - 5, 4096, - position_estimator_inav_thread_main, - (argv) ? (const char **) &argv[2] : (const char **) NULL ); + SCHED_RR, SCHED_PRIORITY_MAX - 5, 4096, + position_estimator_inav_thread_main, + (argv) ? (const char **) &argv[2] : (const char **) NULL); exit(0); } + if (!strcmp(argv[1], "stop")) { thread_should_exit = true; exit(0); @@ -127,9 +132,11 @@ int position_estimator_inav_main(int argc, char *argv[]) { if (!strcmp(argv[1], "status")) { if (thread_running) { printf("\tposition_estimator_inav is running\n"); + } else { printf("\tposition_estimator_inav not started\n"); } + exit(0); } @@ -140,10 +147,10 @@ int position_estimator_inav_main(int argc, char *argv[]) { /**************************************************************************** * main ****************************************************************************/ -int position_estimator_inav_thread_main(int argc, char *argv[]) { - /* welcome user */ - printf("[position_estimator_inav] started\n"); - static int mavlink_fd; +int position_estimator_inav_thread_main(int argc, char *argv[]) +{ + warnx("started."); + int mavlink_fd; mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); mavlink_log_info(mavlink_fd, "[position_estimator_inav] started"); @@ -156,9 +163,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { int baro_loop_end = 70; /* measurement for 1 second */ float baro_alt0 = 0.0f; /* to determine while start up */ - static double lat_current = 0.0; //[°]] --> 47.0 - static double lon_current = 0.0; //[°]] -->8.5 - static double alt_current = 0.0; //[m] above MSL + double lat_current = 0.0; //[°]] --> 47.0 + double lon_current = 0.0; //[°]] -->8.5 + double alt_current = 0.0; //[m] above MSL /* declare and safely initialize all structs */ struct vehicle_status_s vehicle_status; @@ -188,40 +195,42 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { /* initialize parameter handles */ parameters_init(&pos_inav_param_handles); - bool local_flag_baroINITdone = false; /* in any case disable baroINITdone */ - /* FIRST PARAMETER READ at START UP*/ + bool local_flag_baroINITdone = false; + /* first parameters read at start up */ struct parameter_update_s param_update; - orb_copy(ORB_ID(parameter_update), parameter_update_sub, ¶m_update); /* read from param to clear updated flag */ - /* FIRST PARAMETER UPDATE */ + orb_copy(ORB_ID(parameter_update), parameter_update_sub, ¶m_update); /* read from param topic to clear updated flag */ + /* first parameters update */ parameters_update(&pos_inav_param_handles, ¶ms); - /* END FIRST PARAMETER UPDATE */ - /* wait until gps signal turns valid, only then can we initialize the projection */ + /* wait for GPS fix, only then can we initialize the projection */ if (params.use_gps) { struct pollfd fds_init[1] = { - { .fd = vehicle_gps_position_sub, .events = POLLIN } + { .fd = vehicle_gps_position_sub, .events = POLLIN } }; while (gps.fix_type < 3) { - if (poll(fds_init, 1, 5000)) { /* poll only two first subscriptions */ + if (poll(fds_init, 1, 5000)) { if (fds_init[0].revents & POLLIN) { /* Wait for the GPS update to propagate (we have some time) */ usleep(5000); orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); } } + static int printcounter = 0; + if (printcounter == 100) { printcounter = 0; - printf("[position_estimator_inav] wait for GPS fix type 3\n"); + warnx("waiting for GPS fix type 3..."); } + printcounter++; } /* get GPS position for first initialization */ orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - lat_current = ((double) (gps.lat)) * 1e-7; - lon_current = ((double) (gps.lon)) * 1e-7; + lat_current = ((double)(gps.lat)) * 1e-7; + lon_current = ((double)(gps.lon)) * 1e-7; alt_current = gps.alt * 1e-3; pos.home_lat = lat_current * 1e7; @@ -232,15 +241,18 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { map_projection_init(lat_current, lon_current); /* publish global position messages only after first GPS message */ } - printf( - "[position_estimator_inav] initialized projection with: lat: %.10f, lon:%.10f\n", - lat_current, lon_current); - hrt_abstime last_time = 0; + warnx("initialized projection with: lat: %.10f, lon:%.10f", lat_current, lon_current); + + hrt_abstime t_prev = 0; thread_running = true; - uint32_t accelerometer_counter = 0; + uint32_t accel_counter = 0; + hrt_abstime accel_t = 0; + float accel_dt = 0.0f; uint32_t baro_counter = 0; - uint16_t accelerometer_updates = 0; + hrt_abstime baro_t = 0; + hrt_abstime gps_t = 0; + uint16_t accel_updates = 0; uint16_t baro_updates = 0; uint16_t gps_updates = 0; uint16_t attitude_updates = 0; @@ -251,173 +263,184 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { /* main loop */ struct pollfd fds[5] = { - { .fd = parameter_update_sub, .events = POLLIN }, - { .fd = vehicle_status_sub, .events = POLLIN }, - { .fd = vehicle_attitude_sub, .events = POLLIN }, - { .fd = sensor_combined_sub, .events = POLLIN }, - { .fd = vehicle_gps_position_sub, .events = POLLIN } + { .fd = parameter_update_sub, .events = POLLIN }, + { .fd = vehicle_status_sub, .events = POLLIN }, + { .fd = vehicle_attitude_sub, .events = POLLIN }, + { .fd = sensor_combined_sub, .events = POLLIN }, + { .fd = vehicle_gps_position_sub, .events = POLLIN } }; printf("[position_estimator_inav] main loop started\n"); + while (!thread_should_exit) { bool accelerometer_updated = false; bool baro_updated = false; bool gps_updated = false; - float local_pos_gps[3] = { 0.0f, 0.0f, 0.0f }; + float proj_pos_gps[3] = { 0.0f, 0.0f, 0.0f }; int ret = poll(fds, params.use_gps ? 5 : 4, 10); // wait maximal this 10 ms = 100 Hz minimum rate + hrt_abstime t = hrt_absolute_time(); + if (ret < 0) { /* poll error */ - printf("[position_estimator_inav] subscriptions poll error\n"); + warnx("subscriptions poll error."); thread_should_exit = true; continue; + } else if (ret > 0) { /* parameter update */ if (fds[0].revents & POLLIN) { /* read from param to clear updated flag */ struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), parameter_update_sub, - &update); + &update); /* update parameters */ parameters_update(&pos_inav_param_handles, ¶ms); } + /* vehicle status */ if (fds[1].revents & POLLIN) { orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, - &vehicle_status); + &vehicle_status); } + /* vehicle attitude */ if (fds[2].revents & POLLIN) { orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); attitude_updates++; } + /* sensor combined */ if (fds[3].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - if (sensor.accelerometer_counter > accelerometer_counter) { + + if (sensor.accelerometer_counter > accel_counter) { accelerometer_updated = true; - accelerometer_counter = sensor.accelerometer_counter; - accelerometer_updates++; + accel_counter = sensor.accelerometer_counter; + accel_updates++; + accel_dt = accel_t > 0 ? (t - accel_t) / 1000000.0f : 0.0f; + accel_t = t; } + if (sensor.baro_counter > baro_counter) { baro_updated = true; baro_counter = sensor.baro_counter; baro_updates++; } - // barometric pressure estimation at start up + + /* barometric pressure estimation at start up */ if (!local_flag_baroINITdone && baro_updated) { - // mean calculation over several measurements + /* mean calculation over several measurements */ if (baro_loop_cnt < baro_loop_end) { baro_alt0 += sensor.baro_alt_meter; baro_loop_cnt++; + } else { - baro_alt0 /= (float) (baro_loop_cnt); + baro_alt0 /= (float)(baro_loop_cnt); local_flag_baroINITdone = true; - char str[80]; - sprintf(str, - "[position_estimator_inav] baro_alt0 = %.2f", - baro_alt0); - printf("%s\n", str); - mavlink_log_info(mavlink_fd, str); + mavlink_log_info(mavlink_fd, "[position_estimator_inav] baro_alt0 = %.2f", baro_alt0); } } } + if (params.use_gps) { /* vehicle GPS position */ if (fds[4].revents & POLLIN) { /* new GPS value */ orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - /* Project gps lat lon (Geographic coordinate system) to plane */ - map_projection_project(((double) (gps.lat)) * 1e-7, - ((double) (gps.lon)) * 1e-7, &(local_pos_gps[0]), - &(local_pos_gps[1])); - local_pos_gps[2] = (float) (gps.alt * 1e-3); + /* project GPS lat lon (Geographic coordinate system) to plane */ + map_projection_project(((double)(gps.lat)) * 1e-7, + ((double)(gps.lon)) * 1e-7, &(proj_pos_gps[0]), + &(proj_pos_gps[1])); + proj_pos_gps[2] = (float)(gps.alt * 1e-3); gps_updated = true; pos.valid = gps.fix_type >= 3; gps_updates++; } + } else { pos.valid = true; } - } /* end of poll return value check */ + } + + /* end of poll return value check */ + + float dt = t_prev > 0 ? (t - t_prev) / 1000000.0 : 0.0f; + t_prev = t; - hrt_abstime t = hrt_absolute_time(); - float dt = (t - last_time) / 1000000.0; - last_time = t; if (att.R_valid) { /* transform acceleration vector from UAV frame to NED frame */ float accel_NED[3]; + for (int i = 0; i < 3; i++) { accel_NED[i] = 0.0f; + for (int j = 0; j < 3; j++) { accel_NED[i] += att.R[i][j] * sensor.accelerometer_m_s2[j]; } } + accel_NED[2] += CONSTANTS_ONE_G; - /* kalman filter for altitude */ - kalman_filter_inertial_predict(dt, z_est); - /* prepare vectors for kalman filter correction */ - float z_meas[2]; // position, acceleration - bool use_z[2] = { false, false }; + /* inertial filter prediction for altitude */ + inertial_filter_predict(dt, z_est); + + /* inertial filter correction for altitude */ if (local_flag_baroINITdone && baro_updated) { - z_meas[0] = baro_alt0 - sensor.baro_alt_meter; // Z = -alt - use_z[0] = true; + if (baro_t > 0) { + inertial_filter_correct((t - baro_t) / 1000000.0f, z_est, 0, baro_alt0 - sensor.baro_alt_meter, params.w_alt_baro); + } + + baro_t = t; } + if (accelerometer_updated) { - z_meas[1] = accel_NED[2]; - use_z[1] = true; - } - if (use_z[0] || use_z[1]) { - /* correction */ - kalman_filter_inertial2_update(z_est, z_meas, params.k_alt, use_z); + inertial_filter_correct(accel_dt, z_est, 2, accel_NED[2], params.w_alt_acc); } if (params.use_gps) { - /* kalman filter for position */ - kalman_filter_inertial_predict(dt, x_est); - kalman_filter_inertial_predict(dt, y_est); - /* prepare vectors for kalman filter correction */ - float x_meas[3]; // position, velocity, acceleration - float y_meas[3]; // position, velocity, acceleration - bool use_xy[3] = { false, false, false }; + /* inertial filter prediction for position */ + inertial_filter_predict(dt, x_est); + inertial_filter_predict(dt, y_est); + + /* inertial filter correction for position */ if (gps_updated) { - x_meas[0] = local_pos_gps[0]; - y_meas[0] = local_pos_gps[1]; - x_meas[1] = gps.vel_n_m_s; - y_meas[1] = gps.vel_e_m_s; - use_xy[0] = true; - use_xy[1] = true; + if (gps_t > 0) { + float gps_dt = (t - gps_t) / 1000000.0f; + inertial_filter_correct(gps_dt, x_est, 0, proj_pos_gps[0], params.w_pos_gps_p); + inertial_filter_correct(gps_dt, x_est, 1, gps.vel_n_m_s, params.w_pos_gps_v); + inertial_filter_correct(gps_dt, y_est, 0, proj_pos_gps[1], params.w_pos_gps_p); + inertial_filter_correct(gps_dt, y_est, 1, gps.vel_e_m_s, params.w_pos_gps_v); + } + + gps_t = t; } + if (accelerometer_updated) { - x_meas[2] = accel_NED[0]; - y_meas[2] = accel_NED[1]; - use_xy[2] = true; - } - if (use_xy[0] || use_xy[2]) { - /* correction */ - kalman_filter_inertial3_update(x_est, x_meas, params.k_pos, use_xy); - kalman_filter_inertial3_update(y_est, y_meas, params.k_pos, use_xy); + inertial_filter_correct(accel_dt, x_est, 2, accel_NED[0], params.w_pos_acc); + inertial_filter_correct(accel_dt, y_est, 2, accel_NED[1], params.w_pos_acc); } } } + if (verbose_mode) { /* print updates rate */ if (t - updates_counter_start > updates_counter_len) { float updates_dt = (t - updates_counter_start) * 0.000001f; printf( - "[position_estimator_inav] updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s\n", - accelerometer_updates / updates_dt, - baro_updates / updates_dt, - gps_updates / updates_dt, - attitude_updates / updates_dt); + "[position_estimator_inav] updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s\n", + accel_updates / updates_dt, + baro_updates / updates_dt, + gps_updates / updates_dt, + attitude_updates / updates_dt); updates_counter_start = t; - accelerometer_updates = 0; + accel_updates = 0; baro_updates = 0; gps_updates = 0; attitude_updates = 0; } } + if (t - pub_last > pub_interval) { pub_last = t; pos.x = x_est[0]; @@ -427,17 +450,18 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { pos.z = z_est[0]; pos.vz = z_est[1]; pos.timestamp = hrt_absolute_time(); + if ((isfinite(pos.x)) && (isfinite(pos.vx)) - && (isfinite(pos.y)) - && (isfinite(pos.vy)) - && (isfinite(pos.z)) - && (isfinite(pos.vz))) { + && (isfinite(pos.y)) + && (isfinite(pos.vy)) + && (isfinite(pos.z)) + && (isfinite(pos.vz))) { orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &pos); } } } - printf("[position_estimator_inav] exiting.\n"); + warnx("exiting."); mavlink_log_info(mavlink_fd, "[position_estimator_inav] exiting"); thread_running = false; return 0; diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 20142b70c5..8dd0ceff82 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -34,73 +34,39 @@ /* * @file position_estimator_inav_params.c - * + * * Parameters for position_estimator_inav */ #include "position_estimator_inav_params.h" -PARAM_DEFINE_INT32(INAV_USE_GPS, 0); +PARAM_DEFINE_INT32(INAV_USE_GPS, 1); +PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 1.0f); +PARAM_DEFINE_FLOAT(INAV_W_ALT_ACC, 50.0f); +PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_P, 1.0f); +PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 1.0f); +PARAM_DEFINE_FLOAT(INAV_W_POS_ACC, 50.0f); -PARAM_DEFINE_FLOAT(INAV_K_ALT_00, 0.0f); -PARAM_DEFINE_FLOAT(INAV_K_ALT_01, 0.0f); -PARAM_DEFINE_FLOAT(INAV_K_ALT_10, 0.0f); -PARAM_DEFINE_FLOAT(INAV_K_ALT_11, 0.0f); -PARAM_DEFINE_FLOAT(INAV_K_ALT_20, 0.0f); -PARAM_DEFINE_FLOAT(INAV_K_ALT_21, 0.0f); - -PARAM_DEFINE_FLOAT(INAV_K_POS_00, 0.0f); -PARAM_DEFINE_FLOAT(INAV_K_POS_01, 0.0f); -PARAM_DEFINE_FLOAT(INAV_K_POS_02, 0.0f); -PARAM_DEFINE_FLOAT(INAV_K_POS_10, 0.0f); -PARAM_DEFINE_FLOAT(INAV_K_POS_11, 0.0f); -PARAM_DEFINE_FLOAT(INAV_K_POS_12, 0.0f); -PARAM_DEFINE_FLOAT(INAV_K_POS_20, 0.0f); -PARAM_DEFINE_FLOAT(INAV_K_POS_21, 0.0f); -PARAM_DEFINE_FLOAT(INAV_K_POS_22, 0.0f); - -int parameters_init(struct position_estimator_inav_param_handles *h) { +int parameters_init(struct position_estimator_inav_param_handles *h) +{ h->use_gps = param_find("INAV_USE_GPS"); - - h->k_alt_00 = param_find("INAV_K_ALT_00"); - h->k_alt_01 = param_find("INAV_K_ALT_01"); - h->k_alt_10 = param_find("INAV_K_ALT_10"); - h->k_alt_11 = param_find("INAV_K_ALT_11"); - h->k_alt_20 = param_find("INAV_K_ALT_20"); - h->k_alt_21 = param_find("INAV_K_ALT_21"); - - h->k_pos_00 = param_find("INAV_K_POS_00"); - h->k_pos_01 = param_find("INAV_K_POS_01"); - h->k_pos_02 = param_find("INAV_K_POS_02"); - h->k_pos_10 = param_find("INAV_K_POS_10"); - h->k_pos_11 = param_find("INAV_K_POS_11"); - h->k_pos_12 = param_find("INAV_K_POS_12"); - h->k_pos_20 = param_find("INAV_K_POS_20"); - h->k_pos_21 = param_find("INAV_K_POS_21"); - h->k_pos_22 = param_find("INAV_K_POS_22"); + h->w_alt_baro = param_find("INAV_W_ALT_BARO"); + h->w_alt_acc = param_find("INAV_W_ALT_ACC"); + h->w_pos_gps_p = param_find("INAV_W_POS_GPS_P"); + h->w_pos_gps_v = param_find("INAV_W_POS_GPS_V"); + h->w_pos_acc = param_find("INAV_W_POS_ACC"); return OK; } -int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) { +int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) +{ param_get(h->use_gps, &(p->use_gps)); - - param_get(h->k_alt_00, &(p->k_alt[0][0])); - param_get(h->k_alt_01, &(p->k_alt[0][1])); - param_get(h->k_alt_10, &(p->k_alt[1][0])); - param_get(h->k_alt_11, &(p->k_alt[1][1])); - param_get(h->k_alt_20, &(p->k_alt[2][0])); - param_get(h->k_alt_21, &(p->k_alt[2][1])); - - param_get(h->k_pos_00, &(p->k_pos[0][0])); - param_get(h->k_pos_01, &(p->k_pos[0][1])); - param_get(h->k_pos_02, &(p->k_pos[0][2])); - param_get(h->k_pos_10, &(p->k_pos[1][0])); - param_get(h->k_pos_11, &(p->k_pos[1][1])); - param_get(h->k_pos_12, &(p->k_pos[1][2])); - param_get(h->k_pos_20, &(p->k_pos[2][0])); - param_get(h->k_pos_21, &(p->k_pos[2][1])); - param_get(h->k_pos_22, &(p->k_pos[2][2])); + param_get(h->w_alt_baro, &(p->w_alt_baro)); + param_get(h->w_alt_acc, &(p->w_alt_acc)); + param_get(h->w_pos_gps_p, &(p->w_pos_gps_p)); + param_get(h->w_pos_gps_v, &(p->w_pos_gps_v)); + param_get(h->w_pos_acc, &(p->w_pos_acc)); return OK; } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index c0e0042b6e..870227fefc 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -34,7 +34,7 @@ /* * @file position_estimator_inav_params.h - * + * * Parameters for Position Estimator */ @@ -42,29 +42,20 @@ struct position_estimator_inav_params { int use_gps; - float k_alt[3][2]; - float k_pos[3][3]; + float w_alt_baro; + float w_alt_acc; + float w_pos_gps_p; + float w_pos_gps_v; + float w_pos_acc; }; struct position_estimator_inav_param_handles { param_t use_gps; - - param_t k_alt_00; - param_t k_alt_01; - param_t k_alt_10; - param_t k_alt_11; - param_t k_alt_20; - param_t k_alt_21; - - param_t k_pos_00; - param_t k_pos_01; - param_t k_pos_02; - param_t k_pos_10; - param_t k_pos_11; - param_t k_pos_12; - param_t k_pos_20; - param_t k_pos_21; - param_t k_pos_22; + param_t w_alt_baro; + param_t w_alt_acc; + param_t w_pos_gps_p; + param_t w_pos_gps_v; + param_t w_pos_acc; }; /** From eb76d116cc67c6354c29fa41e49b4cf9df1a472d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Jun 2013 12:30:42 +0200 Subject: [PATCH 127/486] Minor state machine improvements and fixes for IO safety / in-air restart handling --- src/drivers/px4io/px4io.cpp | 70 +++++++++++++++----- src/modules/commander/state_machine_helper.c | 3 + src/modules/mavlink/orb_listener.c | 2 +- src/modules/px4iofirmware/mixer.cpp | 11 +-- src/modules/px4iofirmware/protocol.h | 5 +- src/modules/px4iofirmware/registers.c | 13 ++-- src/modules/px4iofirmware/safety.c | 10 +-- 7 files changed, 78 insertions(+), 36 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index bc65c4f259..f033382a63 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -336,6 +336,7 @@ PX4IO::PX4IO() : _to_actuators_effective(0), _to_outputs(0), _to_battery(0), + _to_safety(0), _primary_pwm_device(false), _battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor _battery_amp_bias(0), @@ -389,6 +390,30 @@ PX4IO::init() */ _retries = 2; + /* get IO's last seen FMU state */ + int val = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING); + if (val == _io_reg_get_error) { + mavlink_log_emergency(_mavlink_fd, "[IO] ERROR! FAILED READING STATE"); + } + uint16_t arming = val; + + /* get basic software version */ + /* basic configuration */ + usleep(5000); + + unsigned proto_version = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); + unsigned sw_version = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_SOFTWARE_VERSION); + + if (proto_version != PX4IO_P_CONFIG_PROTOCOL_VERSION) { + mavlink_log_emergency(_mavlink_fd, "[IO] ERROR! PROTO VER MISMATCH: v%u vs v%u\n", + proto_version, + PX4IO_P_CONFIG_PROTOCOL_VERSION); + + mavlink_log_emergency(_mavlink_fd, "[IO] Please update PX4IO firmware."); + log("protocol version mismatch (v%u on IO vs v%u on FMU)", proto_version, PX4IO_P_CONFIG_PROTOCOL_VERSION); + return 1; + } + /* get some parameters */ _max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT); _max_controls = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT); @@ -414,21 +439,23 @@ PX4IO::init() * in this case. */ - uint16_t reg; + printf("arming 0x%04x%s%s%s%s\n", + arming, + ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : ""), + ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : "")); - /* get IO's last seen FMU state */ - ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, ®, sizeof(reg)); - if (ret != OK) - return ret; /* * in-air restart is only tried if the IO board reports it is * already armed, and has been configured for in-air restart */ - if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) && - (reg & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + if ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) && + (arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART"); + log("INAIR RESTART RECOVERY (needs commander app running)"); /* WARNING: COMMANDER app/vehicle status must be initialized. * If this fails (or the app is not started), worst-case IO @@ -482,7 +509,7 @@ PX4IO::init() cmd.confirmation = 1; /* send command once */ - (void)orb_advertise(ORB_ID(vehicle_command), &cmd); + orb_advert_t pub = orb_advertise(ORB_ID(vehicle_command), &cmd); /* spin here until IO's state has propagated into the system */ do { @@ -492,16 +519,22 @@ PX4IO::init() orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status); } - /* wait 10 ms */ - usleep(10000); + /* wait 50 ms */ + usleep(50000); /* abort after 5s */ - if ((hrt_absolute_time() - try_start_time)/1000 > 50000) { + if ((hrt_absolute_time() - try_start_time)/1000 > 2000) { log("failed to recover from in-air restart (2), aborting IO driver init."); return 1; } - /* keep waiting for state change for 10 s */ + /* re-send if necessary */ + if (!status.flag_system_armed) { + orb_publish(ORB_ID(vehicle_command), pub, &cmd); + log("re-sending arm cmd"); + } + + /* keep waiting for state change for 2 s */ } while (!status.flag_system_armed); /* regular boot, no in-air restart, init IO */ @@ -540,7 +573,7 @@ PX4IO::init() return -errno; } - mavlink_log_info(_mavlink_fd, "[IO] init ok"); + mavlink_log_info(_mavlink_fd, "[IO] init ok (sw v.%u)", sw_version); return OK; } @@ -863,14 +896,14 @@ PX4IO::io_handle_status(uint16_t status) */ /* check for IO reset - force it back to armed if necessary */ - if (_status & PX4IO_P_STATUS_FLAGS_ARMED && !(status & PX4IO_P_STATUS_FLAGS_ARMED) + if (_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF && !(status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && !(status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { /* set the arming flag */ - ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARMED | PX4IO_P_STATUS_FLAGS_ARM_SYNC); + ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_SAFETY_OFF | PX4IO_P_STATUS_FLAGS_ARM_SYNC); /* set new status */ _status = status; - _status &= PX4IO_P_STATUS_FLAGS_ARMED; + _status &= PX4IO_P_STATUS_FLAGS_SAFETY_OFF; } else if (!(_status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { /* set the sync flag */ @@ -891,7 +924,7 @@ PX4IO::io_handle_status(uint16_t status) struct safety_s safety; safety.timestamp = hrt_absolute_time(); - if (status & PX4IO_P_STATUS_FLAGS_ARMED) { + if (status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { safety.status = SAFETY_STATUS_UNLOCKED; } else { safety.status = SAFETY_STATUS_SAFE; @@ -1295,7 +1328,8 @@ PX4IO::print_status() uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS); printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s\n", flags, - ((flags & PX4IO_P_STATUS_FLAGS_ARMED) ? " SAFETY_UNLOCKED" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED) ? " OUTPUTS_ARMED" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ? " SAFETY_OFF" : " SAFETY_SAFE"), ((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""), ((flags & PX4IO_P_STATUS_FLAGS_RC_OK) ? " RC_OK" : " RC_FAIL"), ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM) ? " PPM" : ""), diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index 0a6cfd0b5a..f42caad57b 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -545,6 +545,9 @@ void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_s /* play warning tune */ tune_error(); + + /* abort */ + return; } } diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 295cd5e28c..9b2d984f09 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -659,7 +659,7 @@ uorb_receive_thread(void *arg) /* handle the poll result */ if (poll_ret == 0) { - mavlink_missionlib_send_gcs_string("[mavlink] No telemetry data for 1 s"); + /* silent */ } else if (poll_ret < 0) { mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data"); diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 123eb67786..b654bdec49 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -194,7 +194,7 @@ mixer_tick(void) */ bool should_arm = ( /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && - /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && + /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && /* there is valid input via direct PWM or mixer */ ((r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) || /* or failsafe was set manually */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)) && /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) @@ -204,11 +204,15 @@ mixer_tick(void) /* need to arm, but not armed */ up_pwm_servo_arm(true); mixer_servos_armed = true; + r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED; + isr_debug(5, "> armed"); } else if (!should_arm && mixer_servos_armed) { /* armed but need to disarm */ up_pwm_servo_arm(false); mixer_servos_armed = false; + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED); + isr_debug(5, "> disarmed"); } if (mixer_servos_armed) { @@ -263,9 +267,8 @@ static unsigned mixer_text_length = 0; void mixer_handle_text(const void *buffer, size_t length) { - /* do not allow a mixer change while fully armed */ - if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && - /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { + /* do not allow a mixer change while outputs armed */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { return; } diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 674f9dddd6..497e6af8e2 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -77,7 +77,7 @@ /* static configuration page */ #define PX4IO_PAGE_CONFIG 0 -#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* magic numbers TBD */ +#define PX4IO_P_CONFIG_PROTOCOL_VERSION 2 /* magic numbers TBD */ #define PX4IO_P_CONFIG_SOFTWARE_VERSION 1 /* magic numbers TBD */ #define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */ #define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum I2C transfer size */ @@ -93,7 +93,7 @@ #define PX4IO_P_STATUS_CPULOAD 1 #define PX4IO_P_STATUS_FLAGS 2 /* monitoring flags */ -#define PX4IO_P_STATUS_FLAGS_ARMED (1 << 0) /* arm-ok and locally armed */ +#define PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED (1 << 0) /* arm-ok and locally armed */ #define PX4IO_P_STATUS_FLAGS_OVERRIDE (1 << 1) /* in manual override */ #define PX4IO_P_STATUS_FLAGS_RC_OK (1 << 2) /* RC input is valid */ #define PX4IO_P_STATUS_FLAGS_RC_PPM (1 << 3) /* PPM input is valid */ @@ -105,6 +105,7 @@ #define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 9) /* the arming state between IO and FMU is in sync */ #define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */ #define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */ +#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 12) /* safety is off */ #define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */ #define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* VBatt is very close to regulator dropout */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index df7d6dcd35..a092fc93b5 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -317,9 +317,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) * so that an in-air reset of FMU can not lead to a * lockup of the IO arming state. */ - if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED; - } + + // XXX do not reset IO's safety state by FMU for now + // if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + // r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED; + // } r_setup_arming = value; @@ -367,9 +369,8 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_PAGE_RC_CONFIG: { - /* do not allow a RC config change while fully armed */ - if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && - /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { + /* do not allow a RC config change while outputs armed */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { break; } diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c index 4dbecc2744..95335f038a 100644 --- a/src/modules/px4iofirmware/safety.c +++ b/src/modules/px4iofirmware/safety.c @@ -110,7 +110,7 @@ safety_check_button(void *arg) * state machine, keep ARM_COUNTER_THRESHOLD the same * length in all cases of the if/else struct below. */ - if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && + if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && (r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)) { if (counter < ARM_COUNTER_THRESHOLD) { @@ -118,18 +118,18 @@ safety_check_button(void *arg) } else if (counter == ARM_COUNTER_THRESHOLD) { /* switch to armed state */ - r_status_flags |= PX4IO_P_STATUS_FLAGS_ARMED; + r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF; counter++; } - } else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { + } else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) { if (counter < ARM_COUNTER_THRESHOLD) { counter++; } else if (counter == ARM_COUNTER_THRESHOLD) { /* change to disarmed state and notify the FMU */ - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED; + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF; counter++; } @@ -140,7 +140,7 @@ safety_check_button(void *arg) /* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */ uint16_t pattern = LED_PATTERN_FMU_REFUSE_TO_ARM; - if (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) { + if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { if (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) { pattern = LED_PATTERN_IO_FMU_ARMED; From ec08dec8bae403f463ebf9e9a7b71b399ed7b97a Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 12 Jun 2013 12:47:00 +0200 Subject: [PATCH 128/486] Two hacks here to make it compile --- src/modules/gpio_led/gpio_led.c | 7 ++++--- src/modules/sdlog2/sdlog2.c | 20 +++++++++++++------- 2 files changed, 17 insertions(+), 10 deletions(-) diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 43cbe74c74..542821e957 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -171,7 +171,8 @@ void gpio_led_cycle(FAR void *arg) /* select pattern for current status */ int pattern = 0; - if (priv->status.flag_system_armed) { + /* XXX fmu armed correct? */ + if (priv->status.flag_fmu_armed) { if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { pattern = 0x3f; // ****** solid (armed) @@ -180,10 +181,10 @@ void gpio_led_cycle(FAR void *arg) } } else { - if (priv->status.state_machine == SYSTEM_STATE_PREFLIGHT) { + if (priv->status.arming_state == ARMING_STATE_STANDBY) { pattern = 0x00; // ______ off (disarmed, preflight check) - } else if (priv->status.state_machine == SYSTEM_STATE_STANDBY && + } else if (priv->status.arming_state == ARMING_STATE_STANDBY && priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { pattern = 0x38; // ***___ slow blink (disarmed, ready) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index a14bd6f80e..8c5ec092d1 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -861,11 +861,16 @@ int sdlog2_thread_main(int argc, char *argv[]) if (fds[ifds++].revents & POLLIN) { // Don't orb_copy, it's already done few lines above log_msg.msg_type = LOG_STAT_MSG; - log_msg.body.log_STAT.state = (unsigned char) buf_status.state_machine; - log_msg.body.log_STAT.flight_mode = (unsigned char) buf_status.flight_mode; - log_msg.body.log_STAT.manual_control_mode = (unsigned char) buf_status.manual_control_mode; - log_msg.body.log_STAT.manual_sas_mode = (unsigned char) buf_status.manual_sas_mode; - log_msg.body.log_STAT.armed = (unsigned char) buf_status.flag_system_armed; + // XXX fix this + // log_msg.body.log_STAT.state = (unsigned char) buf_status.state_machine; + // log_msg.body.log_STAT.flight_mode = (unsigned char) buf_status.flight_mode; + // log_msg.body.log_STAT.manual_control_mode = (unsigned char) buf_status.manual_control_mode; + // log_msg.body.log_STAT.manual_sas_mode = (unsigned char) buf_status.manual_sas_mode; + log_msg.body.log_STAT.state = 0; + log_msg.body.log_STAT.flight_mode = 0; + log_msg.body.log_STAT.manual_control_mode = 0; + log_msg.body.log_STAT.manual_sas_mode = 0; + log_msg.body.log_STAT.armed = (unsigned char) buf_status.flag_fmu_armed; /* XXX fmu armed correct? */ log_msg.body.log_STAT.battery_voltage = buf_status.voltage_battery; log_msg.body.log_STAT.battery_current = buf_status.current_battery; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; @@ -1165,8 +1170,9 @@ void handle_command(struct vehicle_command_s *cmd) void handle_status(struct vehicle_status_s *status) { - if (status->flag_system_armed != flag_system_armed) { - flag_system_armed = status->flag_system_armed; + /* XXX fmu armed correct? */ + if (status->flag_fmu_armed != flag_system_armed) { + flag_system_armed = status->flag_fmu_armed; if (flag_system_armed) { sdlog2_start_log(); From c3a8f177b6316a9cefd814e312742f47d3049739 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Jun 2013 12:58:17 +0200 Subject: [PATCH 129/486] Software version check fixes --- src/drivers/px4io/px4io.cpp | 16 +++++++++++++--- src/modules/px4iofirmware/protocol.h | 7 +++++-- src/modules/px4iofirmware/registers.c | 4 ++-- 3 files changed, 20 insertions(+), 7 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index f033382a63..925041e0cd 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -404,13 +404,23 @@ PX4IO::init() unsigned proto_version = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); unsigned sw_version = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_SOFTWARE_VERSION); - if (proto_version != PX4IO_P_CONFIG_PROTOCOL_VERSION) { + if (proto_version != PX4IO_P_CONFIG_PROTOCOL_VERSION_MAGIC) { mavlink_log_emergency(_mavlink_fd, "[IO] ERROR! PROTO VER MISMATCH: v%u vs v%u\n", proto_version, - PX4IO_P_CONFIG_PROTOCOL_VERSION); + PX4IO_P_CONFIG_PROTOCOL_VERSION_MAGIC); mavlink_log_emergency(_mavlink_fd, "[IO] Please update PX4IO firmware."); - log("protocol version mismatch (v%u on IO vs v%u on FMU)", proto_version, PX4IO_P_CONFIG_PROTOCOL_VERSION); + log("protocol version mismatch (v%u on IO vs v%u on FMU)", proto_version, PX4IO_P_CONFIG_PROTOCOL_VERSION_MAGIC); + return 1; + } + + if (sw_version != PX4IO_P_CONFIG_SOFTWARE_VERSION_MAGIC) { + mavlink_log_emergency(_mavlink_fd, "[IO] ERROR! SOFTWARE VER MISMATCH: v%u vs v%u\n", + proto_version, + PX4IO_P_CONFIG_SOFTWARE_VERSION_MAGIC); + + mavlink_log_emergency(_mavlink_fd, "[IO] Please update PX4IO firmware."); + log("software version mismatch (v%u on IO vs v%u on FMU)", sw_version, PX4IO_P_CONFIG_SOFTWARE_VERSION_MAGIC); return 1; } diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 497e6af8e2..6c6c7b4e27 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -75,10 +75,13 @@ #define REG_TO_FLOAT(_reg) ((float)REG_TO_SIGNED(_reg) / 10000.0f) #define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)((_float) * 10000.0f)) +#define PX4IO_P_CONFIG_PROTOCOL_VERSION_MAGIC 2 +#define PX4IO_P_CONFIG_SOFTWARE_VERSION_MAGIC 2 + /* static configuration page */ #define PX4IO_PAGE_CONFIG 0 -#define PX4IO_P_CONFIG_PROTOCOL_VERSION 2 /* magic numbers TBD */ -#define PX4IO_P_CONFIG_SOFTWARE_VERSION 1 /* magic numbers TBD */ +#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* magic numbers */ +#define PX4IO_P_CONFIG_SOFTWARE_VERSION 1 /* magic numbers */ #define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */ #define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum I2C transfer size */ #define PX4IO_P_CONFIG_CONTROL_COUNT 4 /* hardcoded max control count supported */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index a092fc93b5..148514455a 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -57,8 +57,8 @@ static void pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t alt * Static configuration parameters. */ static const uint16_t r_page_config[] = { - [PX4IO_P_CONFIG_PROTOCOL_VERSION] = 1, /* XXX hardcoded magic number */ - [PX4IO_P_CONFIG_SOFTWARE_VERSION] = 1, /* XXX hardcoded magic number */ + [PX4IO_P_CONFIG_PROTOCOL_VERSION] = PX4IO_P_CONFIG_PROTOCOL_VERSION_MAGIC, + [PX4IO_P_CONFIG_SOFTWARE_VERSION] = PX4IO_P_CONFIG_SOFTWARE_VERSION_MAGIC, [PX4IO_P_CONFIG_BOOTLOADER_VERSION] = 3, /* XXX hardcoded magic number */ [PX4IO_P_CONFIG_MAX_TRANSFER] = 64, /* XXX hardcoded magic number */ [PX4IO_P_CONFIG_CONTROL_COUNT] = PX4IO_CONTROL_CHANNELS, From 4860c73008c0bdfe69503fbbfa4e717a144fc3e0 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 13 Jun 2013 06:48:24 +0400 Subject: [PATCH 130/486] multirotor_pos_control: position controller implemented --- .../multirotor_pos_control.c | 128 +++++++++++++----- .../multirotor_pos_control_params.c | 22 ++- .../multirotor_pos_control_params.h | 10 ++ 3 files changed, 126 insertions(+), 34 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 6fe129d849..ad5670edca 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -42,6 +42,7 @@ #include #include #include +#include #include #include #include @@ -59,7 +60,6 @@ #include #include #include -#include #include #include @@ -82,6 +82,12 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]); */ static void usage(const char *reason); +static float scale_control(float ctl, float end, float dz); + +static float limit_value(float v, float limit); + +static float norm(float x, float y); + static void usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); @@ -137,9 +143,32 @@ int multirotor_pos_control_main(int argc, char *argv[]) { exit(1); } +static float scale_control(float ctl, float end, float dz) { + if (ctl > dz) { + return (ctl - dz) / (end - dz); + } else if (ctl < -dz) { + return (ctl + dz) / (end - dz); + } else { + return 0.0f; + } +} + +static float limit_value(float v, float limit) { + if (v > limit) { + v = limit; + } else if (v < -limit) { + v = -limit; + } + return v; +} + +static float norm(float x, float y) { + return sqrtf(x * x + y * y); +} + static int multirotor_pos_control_thread_main(int argc, char *argv[]) { /* welcome user */ - printf("[multirotor_pos_control] started\n"); + warnx("started."); static int mavlink_fd; mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); mavlink_log_info(mavlink_fd, "[multirotor_pos_control] started"); @@ -175,7 +204,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { bool reset_sp_pos = true; hrt_abstime t_prev = 0; float alt_integral = 0.0f; - float alt_ctl_dz = 0.2f; + /* integrate in NED frame to estimate wind but not attitude offset */ + float pos_x_integral = 0.0f; + float pos_y_integral = 0.0f; + const float alt_ctl_dz = 0.2f; + const float pos_ctl_dz = 0.05f; thread_running = true; @@ -235,64 +268,97 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { reset_sp_alt = false; local_pos_sp.z = local_pos.z; alt_integral = manual.throttle; - char str[80]; - sprintf(str, "reset alt setpoint: z = %.2f, throttle = %.2f", local_pos_sp.z, manual.throttle); - mavlink_log_info(mavlink_fd, str); + mavlink_log_info(mavlink_fd, "reset alt setpoint: z = %.2f, throttle = %.2f", local_pos_sp.z, manual.throttle); } if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE && reset_sp_pos) { reset_sp_pos = false; local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; - char str[80]; - sprintf(str, "reset pos setpoint: x = %.2f, y = %.2f", local_pos_sp.x, local_pos_sp.y); - mavlink_log_info(mavlink_fd, str); + pos_x_integral = 0.0f; + pos_y_integral = 0.0f; + mavlink_log_info(mavlink_fd, "reset pos setpoint: x = %.2f, y = %.2f", local_pos_sp.x, local_pos_sp.y); } - float err_linear_limit = params.alt_d / params.alt_p * params.alt_rate_max; + float alt_err_linear_limit = params.alt_d / params.alt_p * params.alt_rate_max; + float pos_err_linear_limit = params.pos_d / params.pos_p * params.pos_rate_max; if (status.flag_control_manual_enabled) { /* move altitude setpoint with manual controls */ - float alt_ctl = manual.throttle - 0.5f; - if (fabs(alt_ctl) < alt_ctl_dz) { - alt_ctl = 0.0f; - } else { - if (alt_ctl > 0.0f) - alt_ctl = (alt_ctl - alt_ctl_dz) / (0.5f - alt_ctl_dz); - else - alt_ctl = (alt_ctl + alt_ctl_dz) / (0.5f - alt_ctl_dz); - local_pos_sp.z -= alt_ctl * params.alt_rate_max * dt; - if (local_pos_sp.z > local_pos.z + err_linear_limit) - local_pos_sp.z = local_pos.z + err_linear_limit; - else if (local_pos_sp.z < local_pos.z - err_linear_limit) - local_pos_sp.z = local_pos.z - err_linear_limit; + float alt_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz); + if (alt_sp_ctl != 0.0f) { + local_pos_sp.z -= alt_sp_ctl * params.alt_rate_max * dt; + if (local_pos_sp.z > local_pos.z + alt_err_linear_limit) { + local_pos_sp.z = local_pos.z + alt_err_linear_limit; + } else if (local_pos_sp.z < local_pos.z - alt_err_linear_limit) { + local_pos_sp.z = local_pos.z - alt_err_linear_limit; + } } if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE) { - // TODO move position setpoint with manual controls + /* move position setpoint with manual controls */ + float pos_x_sp_ctl = scale_control(-manual.pitch, 1.0f, pos_ctl_dz); + float pos_y_sp_ctl = scale_control(manual.roll, 1.0f, pos_ctl_dz); + if (pos_x_sp_ctl != 0.0f || pos_y_sp_ctl != 0.0f) { + /* calculate direction and increment of control in NED frame */ + float dir_sp_ctl = att.yaw + atan2f(pos_y_sp_ctl, pos_x_sp_ctl); + float d_sp_ctl = norm(pos_x_sp_ctl, pos_y_sp_ctl) * params.pos_rate_max * dt; + local_pos_sp.x += cosf(dir_sp_ctl) * d_sp_ctl; + local_pos_sp.y += sinf(dir_sp_ctl) * d_sp_ctl; + /* limit maximum setpoint from position offset and preserve direction */ + float pos_vec_x = local_pos_sp.x - local_pos.x; + float pos_vec_y = local_pos_sp.y - local_pos.y; + float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / pos_err_linear_limit; + if (pos_vec_norm > 1.0f) { + local_pos_sp.x = local_pos.x + pos_vec_x / pos_vec_norm; + local_pos_sp.y = local_pos.y + pos_vec_y / pos_vec_norm; + } + } } } /* PID for altitude */ - float alt_err = local_pos.z - local_pos_sp.z; /* don't accelerate more than ALT_RATE_MAX, limit error to corresponding value */ - if (alt_err > err_linear_limit) { - alt_err = err_linear_limit; - } else if (alt_err < -err_linear_limit) { - alt_err = -err_linear_limit; - } + float alt_err = limit_value(local_pos.z - local_pos_sp.z, alt_err_linear_limit); /* P and D components */ float thrust_ctl_pd = alt_err * params.alt_p + local_pos.vz * params.alt_d; /* add I component */ float thrust_ctl = thrust_ctl_pd + alt_integral; + /* integrate */ alt_integral += thrust_ctl_pd / params.alt_p * params.alt_i * dt; + if (alt_integral < params.thr_min) { + alt_integral = params.thr_min; + } else if (alt_integral > params.thr_max) { + alt_integral = params.thr_max; + } if (thrust_ctl < params.thr_min) { thrust_ctl = params.thr_min; } else if (thrust_ctl > params.thr_max) { thrust_ctl = params.thr_max; } if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) { - // TODO add position controller + /* PID for position */ + /* don't accelerate more than POS_RATE_MAX, limit error to corresponding value */ + float pos_x_err = limit_value(local_pos.x - local_pos_sp.x, pos_err_linear_limit); + float pos_y_err = limit_value(local_pos.y - local_pos_sp.y, pos_err_linear_limit); + /* P and D components */ + float pos_x_ctl_pd = - pos_x_err * params.pos_p - local_pos.vx * params.pos_d; + float pos_y_ctl_pd = - pos_y_err * params.pos_p - local_pos.vy * params.pos_d; + /* add I component */ + float pos_x_ctl = pos_x_ctl_pd + pos_x_integral; + float pos_y_ctl = pos_y_ctl_pd + pos_y_integral; + /* integrate */ + pos_x_integral = limit_value(pos_x_integral + pos_x_ctl_pd / params.pos_p * params.pos_i * dt, params.slope_max); + pos_y_integral = limit_value(pos_y_integral + pos_y_ctl_pd / params.pos_p * params.pos_i * dt, params.slope_max); + /* calculate direction and slope in NED frame */ + float dir = atan2f(pos_y_ctl, pos_x_ctl); + /* use approximation: slope ~ sin(slope) = force */ + float slope = limit_value(sqrtf(pos_x_ctl * pos_x_ctl + pos_y_ctl * pos_y_ctl), params.slope_max); + /* convert direction to body frame */ + dir -= att.yaw; + /* calculate roll and pitch */ + att_sp.pitch_body = -cosf(dir) * slope; // reverse pitch + att_sp.roll_body = sinf(dir) * slope; } else { reset_sp_pos = true; } diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c index 90dd820f43..9610ef9f78 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -45,19 +45,29 @@ PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.3f); PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.7f); PARAM_DEFINE_FLOAT(MPC_ALT_P, 0.1f); -PARAM_DEFINE_FLOAT(MPC_ALT_I, 0.01f); -PARAM_DEFINE_FLOAT(MPC_ALT_D, 0.2f); +PARAM_DEFINE_FLOAT(MPC_ALT_I, 0.1f); +PARAM_DEFINE_FLOAT(MPC_ALT_D, 0.1f); PARAM_DEFINE_FLOAT(MPC_ALT_RATE_MAX, 3.0f); +PARAM_DEFINE_FLOAT(MPC_POS_P, 0.1f); +PARAM_DEFINE_FLOAT(MPC_POS_I, 0.0f); +PARAM_DEFINE_FLOAT(MPC_POS_D, 0.2f); +PARAM_DEFINE_FLOAT(MPC_POS_RATE_MAX, 3.0f); +PARAM_DEFINE_FLOAT(MPC_SLOPE_MAX, 0.3f); int parameters_init(struct multirotor_position_control_param_handles *h) { h->thr_min = param_find("MPC_THR_MIN"); h->thr_max = param_find("MPC_THR_MAX"); - /* PID parameters */ h->alt_p = param_find("MPC_ALT_P"); h->alt_i = param_find("MPC_ALT_I"); h->alt_d = param_find("MPC_ALT_D"); h->alt_rate_max = param_find("MPC_ALT_RATE_MAX"); + h->pos_p = param_find("MPC_POS_P"); + h->pos_i = param_find("MPC_POS_I"); + h->pos_d = param_find("MPC_POS_D"); + h->pos_rate_max = param_find("MPC_POS_RATE_MAX"); + h->slope_max = param_find("MPC_SLOPE_MAX"); + return OK; } @@ -69,5 +79,11 @@ int parameters_update(const struct multirotor_position_control_param_handles *h, param_get(h->alt_i, &(p->alt_i)); param_get(h->alt_d, &(p->alt_d)); param_get(h->alt_rate_max, &(p->alt_rate_max)); + param_get(h->pos_p, &(p->pos_p)); + param_get(h->pos_i, &(p->pos_i)); + param_get(h->pos_d, &(p->pos_d)); + param_get(h->pos_rate_max, &(p->pos_rate_max)); + param_get(h->slope_max, &(p->slope_max)); + return OK; } diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h index 849055cd16..589ee92a1d 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h @@ -48,6 +48,11 @@ struct multirotor_position_control_params { float alt_i; float alt_d; float alt_rate_max; + float pos_p; + float pos_i; + float pos_d; + float pos_rate_max; + float slope_max; }; struct multirotor_position_control_param_handles { @@ -57,6 +62,11 @@ struct multirotor_position_control_param_handles { param_t alt_i; param_t alt_d; param_t alt_rate_max; + param_t pos_p; + param_t pos_i; + param_t pos_d; + param_t pos_rate_max; + param_t slope_max; }; /** From 4cdee2be03b693b843c4dec93e67eadec903f5d8 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 13 Jun 2013 06:49:17 +0400 Subject: [PATCH 131/486] position_estimator_inav cosmetic changes --- .../position_estimator_inav_main.c | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 306ebe5ccc..07ea7cd5cc 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -152,7 +152,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) warnx("started."); int mavlink_fd; mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(mavlink_fd, "[position_estimator_inav] started"); + mavlink_log_info(mavlink_fd, "[inav] started"); /* initialize values */ float x_est[3] = { 0.0f, 0.0f, 0.0f }; @@ -241,8 +241,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) map_projection_init(lat_current, lon_current); /* publish global position messages only after first GPS message */ } - - warnx("initialized projection with: lat: %.10f, lon:%.10f", lat_current, lon_current); + warnx("initialized projection with: lat = %.10f, lon = %.10f", lat_current, lon_current); + mavlink_log_info(mavlink_fd, "[inav] home: lat = %.10f, lon = %.10f", lat_current, lon_current); hrt_abstime t_prev = 0; thread_running = true; @@ -269,7 +269,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { .fd = sensor_combined_sub, .events = POLLIN }, { .fd = vehicle_gps_position_sub, .events = POLLIN } }; - printf("[position_estimator_inav] main loop started\n"); + warnx("main loop started."); while (!thread_should_exit) { bool accelerometer_updated = false; @@ -337,7 +337,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else { baro_alt0 /= (float)(baro_loop_cnt); local_flag_baroINITdone = true; - mavlink_log_info(mavlink_fd, "[position_estimator_inav] baro_alt0 = %.2f", baro_alt0); + warnx("baro_alt0 = %.2f", baro_alt0); + mavlink_log_info(mavlink_fd, "[inav] baro_alt0 = %.2f", baro_alt0); + pos.home_alt = baro_alt0; } } } @@ -428,7 +430,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (t - updates_counter_start > updates_counter_len) { float updates_dt = (t - updates_counter_start) * 0.000001f; printf( - "[position_estimator_inav] updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s\n", + "[inav] updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s\n", accel_updates / updates_dt, baro_updates / updates_dt, gps_updates / updates_dt, @@ -462,7 +464,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } warnx("exiting."); - mavlink_log_info(mavlink_fd, "[position_estimator_inav] exiting"); + mavlink_log_info(mavlink_fd, "[inav] exiting"); thread_running = false; return 0; } From 236053a600f5708aee0e5849f4fefc2380e7d101 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 13 Jun 2013 15:04:16 +0200 Subject: [PATCH 132/486] Fixed param save --- src/modules/commander/commander.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index a34e526a8b..c2a7242d11 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -955,7 +955,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* check if no other task is scheduled */ if(low_prio_task == LOW_PRIO_TASK_NONE) { - low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; + low_prio_task = LOW_PRIO_TASK_PARAM_SAVE; result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; From e4b25f857016302fa254d41a964e586da49dd4b3 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 13 Jun 2013 17:12:13 +0400 Subject: [PATCH 133/486] Default parameters updated for position_estimator_inav and multirotor_pos_control --- .../multirotor_pos_control/multirotor_pos_control_params.c | 4 ++-- .../position_estimator_inav_params.c | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c index 9610ef9f78..7f2ba3db8a 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -51,8 +51,8 @@ PARAM_DEFINE_FLOAT(MPC_ALT_RATE_MAX, 3.0f); PARAM_DEFINE_FLOAT(MPC_POS_P, 0.1f); PARAM_DEFINE_FLOAT(MPC_POS_I, 0.0f); PARAM_DEFINE_FLOAT(MPC_POS_D, 0.2f); -PARAM_DEFINE_FLOAT(MPC_POS_RATE_MAX, 3.0f); -PARAM_DEFINE_FLOAT(MPC_SLOPE_MAX, 0.3f); +PARAM_DEFINE_FLOAT(MPC_POS_RATE_MAX, 10.0f); +PARAM_DEFINE_FLOAT(MPC_SLOPE_MAX, 0.5f); int parameters_init(struct multirotor_position_control_param_handles *h) { diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 8dd0ceff82..49dc7f51ff 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -43,9 +43,9 @@ PARAM_DEFINE_INT32(INAV_USE_GPS, 1); PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 1.0f); PARAM_DEFINE_FLOAT(INAV_W_ALT_ACC, 50.0f); -PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_P, 1.0f); -PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 1.0f); -PARAM_DEFINE_FLOAT(INAV_W_POS_ACC, 50.0f); +PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_P, 4.0f); +PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 0.0f); +PARAM_DEFINE_FLOAT(INAV_W_POS_ACC, 10.0f); int parameters_init(struct position_estimator_inav_param_handles *h) { From 53f29a25b6c011d4cf4992a9eb1207a344ee75ae Mon Sep 17 00:00:00 2001 From: Sam Kelly Date: Thu, 13 Jun 2013 12:51:50 -0700 Subject: [PATCH 134/486] Added l3gd20h detection --- src/drivers/l3gd20/l3gd20.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 98098c83bf..f474818233 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -78,6 +78,7 @@ static const int ERROR = -1; /* register addresses */ #define ADDR_WHO_AM_I 0x0F +#define WHO_I_AM_H 0xD7 #define WHO_I_AM 0xD4 #define ADDR_CTRL_REG1 0x20 @@ -351,7 +352,7 @@ L3GD20::probe() (void)read_reg(ADDR_WHO_AM_I); /* verify that the device is attached and functioning */ - if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) + if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM || read_reg(ADDR_WHO_AM_I) == WHO_I_AM_H) return OK; return -EIO; From 90f5e30f2a177bed2ac08e76699ec3029292d640 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 14 Jun 2013 13:53:26 +0200 Subject: [PATCH 135/486] Introduced new actuator_safety topic --- .../ardrone_interface/ardrone_interface.c | 15 +- src/drivers/blinkm/blinkm.cpp | 21 ++- src/drivers/hil/hil.cpp | 17 +- src/drivers/mkblctrl/mkblctrl.cpp | 19 +- src/drivers/px4fmu/fmu.cpp | 27 +-- src/drivers/px4io/px4io.cpp | 41 ++--- src/modules/commander/commander.c | 170 ++++++++++-------- src/modules/commander/state_machine_helper.c | 25 ++- src/modules/commander/state_machine_helper.h | 3 +- src/modules/gpio_led/gpio_led.c | 16 +- src/modules/mavlink/orb_listener.c | 18 +- src/modules/mavlink/orb_topics.h | 3 +- src/modules/mavlink_onboard/mavlink.c | 8 +- src/modules/mavlink_onboard/orb_topics.h | 3 +- src/modules/mavlink_onboard/util.h | 2 +- .../multirotor_att_control_main.c | 21 ++- src/modules/sdlog2/sdlog2.c | 23 ++- src/modules/uORB/objects_common.cpp | 4 +- src/modules/uORB/topics/actuator_controls.h | 15 +- src/modules/uORB/topics/actuator_safety.h | 65 +++++++ .../uORB/topics/vehicle_control_mode.h | 96 ++++++++++ src/modules/uORB/topics/vehicle_status.h | 5 +- 22 files changed, 424 insertions(+), 193 deletions(-) create mode 100644 src/modules/uORB/topics/actuator_safety.h create mode 100644 src/modules/uORB/topics/vehicle_control_mode.h diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c index 264041e65a..4a6f30a4f1 100644 --- a/src/drivers/ardrone_interface/ardrone_interface.c +++ b/src/drivers/ardrone_interface/ardrone_interface.c @@ -55,6 +55,7 @@ #include #include #include +#include #include @@ -247,13 +248,13 @@ int ardrone_interface_thread_main(int argc, char *argv[]) memset(&state, 0, sizeof(state)); struct actuator_controls_s actuator_controls; memset(&actuator_controls, 0, sizeof(actuator_controls)); - struct actuator_armed_s armed; - armed.armed = false; + struct actuator_safety_s safety; + safety.armed = false; /* subscribe to attitude, motor setpoints and system state */ int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); int state_sub = orb_subscribe(ORB_ID(vehicle_status)); - int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); printf("[ardrone_interface] Motors initialized - ready.\n"); fflush(stdout); @@ -322,18 +323,14 @@ int ardrone_interface_thread_main(int argc, char *argv[]) } else { /* MAIN OPERATION MODE */ - /* get a local copy of the vehicle state */ - orb_copy(ORB_ID(vehicle_status), state_sub, &state); /* get a local copy of the actuator controls */ orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls); - orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); + orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); /* for now only spin if armed and immediately shut down * if in failsafe */ - //XXX fix this - //if (armed.armed && !armed.lockdown) { - if (state.flag_fmu_armed) { + if (safety.armed && !safety.lockdown) { ardrone_mixing_and_output(ardrone_write, &actuator_controls); } else { diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 7a64b72a4e..1cfdcfbedd 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -116,6 +116,7 @@ #include #include #include +#include #include static const float MAX_CELL_VOLTAGE = 4.3f; @@ -376,6 +377,7 @@ BlinkM::led() static int vehicle_status_sub_fd; static int vehicle_gps_position_sub_fd; + static int actuator_safety_sub_fd; static int num_of_cells = 0; static int detected_cells_runcount = 0; @@ -386,6 +388,7 @@ BlinkM::led() static int led_interval = 1000; static int no_data_vehicle_status = 0; + static int no_data_actuator_safety = 0; static int no_data_vehicle_gps_position = 0; static bool topic_initialized = false; @@ -398,6 +401,9 @@ BlinkM::led() vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status)); orb_set_interval(vehicle_status_sub_fd, 1000); + actuator_safety_sub_fd = orb_subscribe(ORB_ID(actuator_safety)); + orb_set_interval(actuator_safety_sub_fd, 1000); + vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position)); orb_set_interval(vehicle_gps_position_sub_fd, 1000); @@ -452,12 +458,14 @@ BlinkM::led() if (led_thread_runcount == 15) { /* obtained data for the first file descriptor */ struct vehicle_status_s vehicle_status_raw; + struct actuator_safety_s actuator_safety; struct vehicle_gps_position_s vehicle_gps_position_raw; memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw)); memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw)); bool new_data_vehicle_status; + bool new_data_actuator_safety; bool new_data_vehicle_gps_position; orb_check(vehicle_status_sub_fd, &new_data_vehicle_status); @@ -471,6 +479,17 @@ BlinkM::led() no_data_vehicle_status = 3; } + orb_check(actuator_safety_sub_fd, &new_data_actuator_safety); + + if (new_data_actuator_safety) { + orb_copy(ORB_ID(actuator_safety), actuator_safety_sub_fd, &actuator_safety); + no_data_actuator_safety = 0; + } else { + no_data_actuator_safety++; + if(no_data_vehicle_status >= 3) + no_data_vehicle_status = 3; + } + orb_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position); if (new_data_vehicle_gps_position) { @@ -530,7 +549,7 @@ BlinkM::led() } else { /* no battery warnings here */ - if(vehicle_status_raw.flag_fmu_armed == false) { + if(actuator_safety.armed == false) { /* system not armed */ led_color_1 = LED_RED; led_color_2 = LED_RED; diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index d9aa772d4d..e851d8a52b 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -75,6 +75,7 @@ #include #include +#include #include #include @@ -108,7 +109,7 @@ private: int _current_update_rate; int _task; int _t_actuators; - int _t_armed; + int _t_safety; orb_advert_t _t_outputs; unsigned _num_outputs; bool _primary_pwm_device; @@ -161,7 +162,7 @@ HIL::HIL() : _current_update_rate(0), _task(-1), _t_actuators(-1), - _t_armed(-1), + _t_safety(-1), _t_outputs(0), _num_outputs(0), _primary_pwm_device(false), @@ -321,8 +322,8 @@ HIL::task_main() /* force a reset of the update rate */ _current_update_rate = 0; - _t_armed = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(_t_armed, 200); /* 5Hz update rate */ + _t_safety = orb_subscribe(ORB_ID(actuator_safety)); + orb_set_interval(_t_safety, 200); /* 5Hz update rate */ /* advertise the mixed control outputs */ actuator_outputs_s outputs; @@ -334,7 +335,7 @@ HIL::task_main() pollfd fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; - fds[1].fd = _t_armed; + fds[1].fd = _t_safety; fds[1].events = POLLIN; unsigned num_outputs; @@ -426,15 +427,15 @@ HIL::task_main() /* how about an arming update? */ if (fds[1].revents & POLLIN) { - actuator_armed_s aa; + actuator_safety_s aa; /* get new value */ - orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); + orb_copy(ORB_ID(actuator_safety), _t_safety, &aa); } } ::close(_t_actuators); - ::close(_t_armed); + ::close(_t_safety); /* make sure servos are off */ // up_pwm_servo_deinit(); diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index c67276f8af..093b53d426 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -74,6 +74,7 @@ #include #include #include +#include #include @@ -132,7 +133,7 @@ private: int _current_update_rate; int _task; int _t_actuators; - int _t_armed; + int _t_actuator_safety; unsigned int _motor; int _px4mode; int _frametype; @@ -240,7 +241,7 @@ MK::MK(int bus) : _update_rate(50), _task(-1), _t_actuators(-1), - _t_armed(-1), + _t_actuator_safety(-1), _t_outputs(0), _t_actuators_effective(0), _num_outputs(0), @@ -496,8 +497,8 @@ MK::task_main() /* force a reset of the update rate */ _current_update_rate = 0; - _t_armed = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(_t_armed, 200); /* 5Hz update rate */ + _t_actuator_safety = orb_subscribe(ORB_ID(actuator_safety)); + orb_set_interval(_t_actuator_safety, 200); /* 5Hz update rate */ /* advertise the mixed control outputs */ actuator_outputs_s outputs; @@ -519,7 +520,7 @@ MK::task_main() pollfd fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; - fds[1].fd = _t_armed; + fds[1].fd = _t_actuator_safety; fds[1].events = POLLIN; log("starting"); @@ -625,13 +626,13 @@ MK::task_main() /* how about an arming update? */ if (fds[1].revents & POLLIN) { - actuator_armed_s aa; + actuator_safety_s as; /* get new value */ - orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); + orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &as); /* update PWM servo armed status if armed and not locked down */ - mk_servo_arm(aa.armed && !aa.lockdown); + mk_servo_arm(as.armed && !as.lockdown); } @@ -639,7 +640,7 @@ MK::task_main() ::close(_t_actuators); ::close(_t_actuators_effective); - ::close(_t_armed); + ::close(_t_actuator_safety); /* make sure servos are off */ diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index bf72892ebe..db4c87ddc7 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -69,6 +69,7 @@ #include #include #include +#include #include #include @@ -104,7 +105,7 @@ private: unsigned _current_update_rate; int _task; int _t_actuators; - int _t_armed; + int _t_actuator_safety; orb_advert_t _t_outputs; orb_advert_t _t_actuators_effective; unsigned _num_outputs; @@ -174,7 +175,7 @@ PX4FMU::PX4FMU() : _current_update_rate(0), _task(-1), _t_actuators(-1), - _t_armed(-1), + _t_actuator_safety(-1), _t_outputs(0), _t_actuators_effective(0), _num_outputs(0), @@ -376,8 +377,8 @@ PX4FMU::task_main() /* force a reset of the update rate */ _current_update_rate = 0; - _t_armed = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(_t_armed, 200); /* 5Hz update rate */ + _t_actuator_safety = orb_subscribe(ORB_ID(actuator_safety)); + orb_set_interval(_t_actuator_safety, 200); /* 5Hz update rate */ /* advertise the mixed control outputs */ actuator_outputs_s outputs; @@ -396,7 +397,7 @@ PX4FMU::task_main() pollfd fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; - fds[1].fd = _t_armed; + fds[1].fd = _t_actuator_safety; fds[1].events = POLLIN; unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4; @@ -499,13 +500,13 @@ PX4FMU::task_main() /* how about an arming update? */ if (fds[1].revents & POLLIN) { - actuator_armed_s aa; + actuator_safety_s as; /* get new value */ - orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); + orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &as); /* update PWM servo armed status if armed and not locked down */ - bool set_armed = aa.armed && !aa.lockdown; + bool set_armed = as.armed && !as.lockdown; if (set_armed != _armed) { _armed = set_armed; up_pwm_servo_arm(set_armed); @@ -535,7 +536,7 @@ PX4FMU::task_main() ::close(_t_actuators); ::close(_t_actuators_effective); - ::close(_t_armed); + ::close(_t_actuator_safety); /* make sure servos are off */ up_pwm_servo_deinit(); @@ -1021,12 +1022,12 @@ fake(int argc, char *argv[]) if (handle < 0) errx(1, "advertise failed"); - actuator_armed_s aa; + actuator_safety_s as; - aa.armed = true; - aa.lockdown = false; + as.armed = true; + as.lockdown = false; - handle = orb_advertise(ORB_ID(actuator_armed), &aa); + handle = orb_advertise(ORB_ID(actuator_safety), &as); if (handle < 0) errx(1, "advertise failed 2"); diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 962a91c7fe..28a3eb917b 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -75,6 +75,7 @@ #include #include #include +#include #include #include #include @@ -152,7 +153,7 @@ private: /* subscribed topics */ int _t_actuators; ///< actuator controls topic - int _t_armed; ///< system armed control topic + int _t_actuator_safety; ///< system armed control topic int _t_vstatus; ///< system / vehicle status int _t_param; ///< parameter update topic @@ -327,7 +328,7 @@ PX4IO::PX4IO() : _status(0), _alarms(0), _t_actuators(-1), - _t_armed(-1), + _t_actuator_safety(-1), _t_vstatus(-1), _t_param(-1), _to_input_rc(0), @@ -433,20 +434,20 @@ PX4IO::init() * remains untouched (so manual override is still available). */ - int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); /* fill with initial values, clear updated flag */ - vehicle_status_s status; + struct actuator_safety_s armed; uint64_t try_start_time = hrt_absolute_time(); bool updated = false; - /* keep checking for an update, ensure we got a recent state, + /* keep checking for an update, ensure we got a arming information, not something that was published a long time ago. */ do { - orb_check(vstatus_sub, &updated); + orb_check(safety_sub, &updated); if (updated) { /* got data, copy and exit loop */ - orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status); + orb_copy(ORB_ID(actuator_safety), safety_sub, &armed); break; } @@ -472,10 +473,10 @@ PX4IO::init() cmd.param6 = 0; cmd.param7 = 0; cmd.command = VEHICLE_CMD_COMPONENT_ARM_DISARM; - cmd.target_system = status.system_id; - cmd.target_component = status.component_id; - cmd.source_system = status.system_id; - cmd.source_component = status.component_id; + // cmd.target_system = status.system_id; + // cmd.target_component = status.component_id; + // cmd.source_system = status.system_id; + // cmd.source_component = status.component_id; /* ask to confirm command */ cmd.confirmation = 1; @@ -484,10 +485,10 @@ PX4IO::init() /* spin here until IO's state has propagated into the system */ do { - orb_check(vstatus_sub, &updated); + orb_check(safety_sub, &updated); if (updated) { - orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status); + orb_copy(ORB_ID(actuator_safety), safety_sub, &armed); } /* wait 10 ms */ @@ -500,7 +501,7 @@ PX4IO::init() } /* keep waiting for state change for 10 s */ - } while (!status.flag_fmu_armed); + } while (!armed.armed); /* regular boot, no in-air restart, init IO */ } else { @@ -564,8 +565,8 @@ PX4IO::task_main() ORB_ID(actuator_controls_1)); orb_set_interval(_t_actuators, 20); /* default to 50Hz */ - _t_armed = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(_t_armed, 200); /* 5Hz update rate */ + _t_actuator_safety = orb_subscribe(ORB_ID(actuator_safety)); + orb_set_interval(_t_actuator_safety, 200); /* 5Hz update rate */ _t_vstatus = orb_subscribe(ORB_ID(vehicle_status)); orb_set_interval(_t_vstatus, 200); /* 5Hz update rate max. */ @@ -577,7 +578,7 @@ PX4IO::task_main() pollfd fds[4]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; - fds[1].fd = _t_armed; + fds[1].fd = _t_actuator_safety; fds[1].events = POLLIN; fds[2].fd = _t_vstatus; fds[2].events = POLLIN; @@ -713,16 +714,16 @@ PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len) int PX4IO::io_set_arming_state() { - actuator_armed_s armed; ///< system armed state + actuator_safety_s safety; ///< system armed state vehicle_status_s vstatus; ///< overall system state - orb_copy(ORB_ID(actuator_armed), _t_armed, &armed); + orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &safety); orb_copy(ORB_ID(vehicle_status), _t_vstatus, &vstatus); uint16_t set = 0; uint16_t clear = 0; - if (armed.armed && !armed.lockdown) { + if (safety.armed && !safety.lockdown) { set |= PX4IO_P_SETUP_ARMING_FMU_ARMED; } else { clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED; diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index c2a7242d11..6812fb1fb8 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -73,8 +73,10 @@ #include #include #include +#include #include #include +#include #include #include #include @@ -137,10 +139,6 @@ static bool thread_should_exit = false; /**< daemon exit flag */ static bool thread_running = false; /**< daemon status flag */ static int daemon_task; /**< Handle of daemon task / thread */ -/* Main state machine */ -static struct vehicle_status_s current_status; -static orb_advert_t stat_pub; - /* timout until lowlevel failsafe */ static unsigned int failsafe_lowlevel_timeout_ms; @@ -187,7 +185,7 @@ void do_rc_calibration(void); void do_airspeed_calibration(void); -void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd); +void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd, int safety_pub, struct actuator_safety_s *safety); int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state); @@ -304,10 +302,11 @@ void do_rc_calibration() { mavlink_log_info(mavlink_fd, "trim calibration starting"); - if (current_status.offboard_control_signal_lost) { - mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal."); - return; - } + /* XXX fix this */ + // if (current_status.offboard_control_signal_lost) { + // mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal."); + // return; + // } int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint)); struct manual_control_setpoint_s sp; @@ -718,7 +717,7 @@ void do_airspeed_calibration() close(diff_pres_sub); } -void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd) +void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd, int safety_pub, struct actuator_safety_s *safety) { /* result of the command */ uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; @@ -738,13 +737,13 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } if ((int)cmd->param1 & VEHICLE_MODE_FLAG_SAFETY_ARMED) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, safety_pub, safety, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; } } else { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, safety_pub, safety, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -757,14 +756,14 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request to arm */ if ((int)cmd->param1 == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, safety_pub, safety, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; } /* request to disarm */ } else if ((int)cmd->param1 == 0) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, safety_pub, safety, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -777,7 +776,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request for an autopilot reboot */ if ((int)cmd->param1 == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_REBOOT, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_REBOOT, safety_pub, safety, mavlink_fd)) { /* reboot is executed later, after positive tune is sent */ result = VEHICLE_CMD_RESULT_ACCEPTED; tune_positive(); @@ -826,7 +825,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION; @@ -845,7 +844,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION; @@ -865,7 +864,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta // if(low_prio_task == LOW_PRIO_TASK_NONE) { // /* try to go to INIT/PREFLIGHT arming state */ - // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { // result = VEHICLE_CMD_RESULT_ACCEPTED; // /* now set the task for the low prio thread */ // low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION; @@ -886,7 +885,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta // if(low_prio_task == LOW_PRIO_TASK_NONE) { // /* try to go to INIT/PREFLIGHT arming state */ - // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { // result = VEHICLE_CMD_RESULT_ACCEPTED; // /* now set the task for the low prio thread */ // low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION; @@ -906,7 +905,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION; @@ -925,7 +924,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION; @@ -1189,17 +1188,30 @@ int commander_thread_main(int argc, char *argv[]) if (mavlink_fd < 0) { warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first."); } - + + /* Main state machine */ + struct vehicle_status_s current_status; + orb_advert_t status_pub; /* make sure we are in preflight state */ memset(¤t_status, 0, sizeof(current_status)); + /* safety topic */ + struct actuator_safety_s safety; + orb_advert_t safety_pub; + /* Initialize safety with all false */ + memset(&safety, 0, sizeof(safety)); + + + /* flags for control apps */ + struct vehicle_control_mode_s control_mode; + orb_advert_t control_mode_pub; + + /* Initialize all flags to false */ + memset(&control_mode, 0, sizeof(control_mode)); current_status.navigation_state = NAVIGATION_STATE_INIT; current_status.arming_state = ARMING_STATE_INIT; current_status.hil_state = HIL_STATE_OFF; - current_status.flag_hil_enabled = false; - current_status.flag_fmu_armed = false; - current_status.flag_io_armed = false; // XXX read this from somewhere /* neither manual nor offboard control commands have been received */ current_status.offboard_control_signal_found_once = false; @@ -1222,16 +1234,18 @@ int commander_thread_main(int argc, char *argv[]) current_status.condition_system_sensors_initialized = true; /* advertise to ORB */ - stat_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); + status_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); /* publish current state machine */ - state_machine_publish(stat_pub, ¤t_status, mavlink_fd); + state_machine_publish(status_pub, ¤t_status, mavlink_fd); + + safety_pub = orb_advertise(ORB_ID(actuator_safety), &safety); /* home position */ orb_advert_t home_pub = -1; struct home_position_s home; memset(&home, 0, sizeof(home)); - if (stat_pub < 0) { + if (status_pub < 0) { warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n"); warnx("exiting."); exit(ERROR); @@ -1333,6 +1347,7 @@ int commander_thread_main(int argc, char *argv[]) memset(&battery, 0, sizeof(battery)); battery.voltage_v = 0.0f; + // uint8_t vehicle_state_previous = current_status.state_machine; float voltage_previous = 0.0f; @@ -1386,9 +1401,11 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - handle_command(stat_pub, ¤t_status, &cmd); + handle_command(status_pub, ¤t_status, &cmd, safety_pub, &safety); } + + /* update parameters */ orb_check(param_changed_sub, &new_data); @@ -1397,9 +1414,8 @@ int commander_thread_main(int argc, char *argv[]) /* parameters changed */ orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); - /* update parameters */ - if (!current_status.flag_fmu_armed) { + if (!safety.armed) { if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { warnx("failed setting new system type"); } @@ -1417,7 +1433,6 @@ int commander_thread_main(int argc, char *argv[]) /* check and update system / component ID */ param_get(_param_system_id, &(current_status.system_id)); param_get(_param_component_id, &(current_status.component_id)); - } } @@ -1564,7 +1579,7 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!"); current_status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; // XXX implement this - // state_machine_emergency(stat_pub, ¤t_status, mavlink_fd); + // state_machine_emergency(status_pub, ¤t_status, mavlink_fd); } critical_voltage_counter++; @@ -1579,7 +1594,7 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (current_status.arming_state == ARMING_STATE_INIT) { // XXX check for sensors - arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd); } else { // XXX: Add emergency stuff if sensors are lost } @@ -1694,7 +1709,7 @@ int commander_thread_main(int argc, char *argv[]) && (vdop_m < vdop_threshold_m) // XXX note that vdop is 0 for mtk && !home_position_set && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) - && !current_status.flag_fmu_armed) { + && !safety.armed) { warnx("setting home position"); /* copy position data to uORB home message, store it locally as well */ @@ -1734,24 +1749,22 @@ int commander_thread_main(int argc, char *argv[]) warnx("mode sw not finite"); /* no valid stick position, go to default */ + current_status.mode_switch = MODE_SWITCH_MANUAL; } else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) { /* bottom stick position, go to manual mode */ current_status.mode_switch = MODE_SWITCH_MANUAL; - printf("mode switch: manual\n"); } else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) { /* top stick position, set auto/mission for all vehicle types */ current_status.mode_switch = MODE_SWITCH_AUTO; - printf("mode switch: auto\n"); } else { /* center stick position, set seatbelt/simple control */ current_status.mode_switch = MODE_SWITCH_SEATBELT; - printf("mode switch: seatbelt\n"); } // warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode); @@ -1811,7 +1824,7 @@ int commander_thread_main(int argc, char *argv[]) /* just manual, XXX this might depend on the return switch */ if (current_status.mode_switch == MODE_SWITCH_MANUAL) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); } @@ -1819,9 +1832,9 @@ int commander_thread_main(int argc, char *argv[]) /* Try seatbelt or fallback to manual */ } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); } @@ -1830,11 +1843,11 @@ int commander_thread_main(int argc, char *argv[]) /* Try auto or fallback to seatbelt or even manual */ } else if (current_status.mode_switch == MODE_SWITCH_AUTO) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_STANDBY, mavlink_fd) != OK) { // first fallback to SEATBELT_STANDY - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) { // or fallback to MANUAL_STANDBY - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); } @@ -1850,7 +1863,7 @@ int commander_thread_main(int argc, char *argv[]) /* Always accept manual mode */ if (current_status.mode_switch == MODE_SWITCH_MANUAL) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -1859,9 +1872,9 @@ int commander_thread_main(int argc, char *argv[]) } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT && current_status.return_switch == RETURN_SWITCH_NONE) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -1871,9 +1884,9 @@ int commander_thread_main(int argc, char *argv[]) } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT && current_status.return_switch == RETURN_SWITCH_RETURN) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -1885,19 +1898,19 @@ int commander_thread_main(int argc, char *argv[]) && current_status.mission_switch == MISSION_SWITCH_NONE) { /* we might come from the disarmed state AUTO_STANDBY */ - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_READY, mavlink_fd) != OK) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_READY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } } /* or from some other armed state like SEATBELT or MANUAL */ - } else if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_LOITER, mavlink_fd) != OK) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + } else if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_LOITER, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -1909,10 +1922,10 @@ int commander_thread_main(int argc, char *argv[]) && current_status.return_switch == RETURN_SWITCH_NONE && current_status.mission_switch == MISSION_SWITCH_MISSION) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_MISSION, mavlink_fd) != OK) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_MISSION, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -1924,10 +1937,10 @@ int commander_thread_main(int argc, char *argv[]) && current_status.return_switch == RETURN_SWITCH_RETURN && (current_status.mission_switch == MISSION_SWITCH_NONE || current_status.mission_switch == MISSION_SWITCH_MISSION)) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_RTL, mavlink_fd) != OK) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_RTL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -1975,13 +1988,22 @@ int commander_thread_main(int argc, char *argv[]) * Check if left stick is in lower left position --> switch to standby state. * Do this only for multirotors, not for fixed wing aircraft. */ - if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) - ) && - (sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { + // printf("checking\n"); + if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); + + + printf("System Type: %d\n", current_status.system_type); + + if((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) + ) { + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd); + + } else { + mavlink_log_critical(mavlink_fd, "STICK DISARM not allowed"); + } stick_off_counter = 0; } else { @@ -1993,7 +2015,7 @@ int commander_thread_main(int argc, char *argv[]) /* check if left stick is in lower right position --> arm */ if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.2f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_ARMED, mavlink_fd); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, safety_pub, &safety, mavlink_fd); stick_on_counter = 0; } else { @@ -2095,13 +2117,13 @@ int commander_thread_main(int argc, char *argv[]) // XXX check if this is correct /* arm / disarm on request */ - if (sp_offboard.armed && !current_status.flag_fmu_armed) { + if (sp_offboard.armed && !safety.armed) { - arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_ARMED, mavlink_fd); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, safety_pub, &safety, mavlink_fd); - } else if (!sp_offboard.armed && current_status.flag_fmu_armed) { + } else if (!sp_offboard.armed && safety.armed) { - arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd); } } else { @@ -2143,13 +2165,13 @@ int commander_thread_main(int argc, char *argv[]) // current_status.flag_preflight_mag_calibration == false && // current_status.flag_preflight_accel_calibration == false) { // /* All ok, no calibration going on, go to standby */ - // do_state_update(stat_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + // do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); // } /* publish at least with 1 Hz */ if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) { - orb_publish(ORB_ID(vehicle_status), stat_pub, ¤t_status); + orb_publish(ORB_ID(vehicle_status), status_pub, ¤t_status); state_changed = false; } diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index daed81553d..fea7ee840b 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -54,7 +54,7 @@ #include "state_machine_helper.h" -int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, const int mavlink_fd) { +int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int safety_pub, struct actuator_safety_s *safety, const int mavlink_fd) { int ret = ERROR; @@ -69,7 +69,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* allow going back from INIT for calibration */ if (current_state->arming_state == ARMING_STATE_STANDBY) { ret = OK; - current_state->flag_fmu_armed = false; + safety->armed = false; } break; case ARMING_STATE_STANDBY: @@ -81,7 +81,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* sensors need to be initialized for STANDBY state */ if (current_state->condition_system_sensors_initialized) { ret = OK; - current_state->flag_fmu_armed = false; + safety->armed = false; } else { mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized"); } @@ -95,7 +95,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* XXX conditions for arming? */ ret = OK; - current_state->flag_fmu_armed = true; + safety->armed = true; } break; case ARMING_STATE_ARMED_ERROR: @@ -105,7 +105,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* XXX conditions for an error state? */ ret = OK; - current_state->flag_fmu_armed = true; + safety->armed = true; } break; case ARMING_STATE_STANDBY_ERROR: @@ -114,7 +114,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta || current_state->arming_state == ARMING_STATE_INIT || current_state->arming_state == ARMING_STATE_ARMED_ERROR) { ret = OK; - current_state->flag_fmu_armed = false; + safety->armed = false; } break; case ARMING_STATE_REBOOT: @@ -125,7 +125,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta || current_state->arming_state == ARMING_STATE_STANDBY_ERROR) { ret = OK; - current_state->flag_fmu_armed = false; + safety->armed = false; } break; @@ -139,7 +139,12 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta if (ret == OK) { current_state->arming_state = new_arming_state; - state_machine_publish(status_pub, current_state, mavlink_fd); + current_state->counter++; + current_state->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_status), status_pub, current_state); + + safety->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(actuator_safety), safety_pub, safety); } } @@ -460,7 +465,9 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current if (ret == OK) { current_state->navigation_state = new_navigation_state; - state_machine_publish(status_pub, current_state, mavlink_fd); + current_state->counter++; + current_state->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_status), status_pub, current_state); } } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 5b57cffb73..824a7529f0 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -46,6 +46,7 @@ #include #include +#include void navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); @@ -53,7 +54,7 @@ void navigation_state_update(int status_pub, struct vehicle_status_s *current_st void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); -int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, const int mavlink_fd); +int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int safety_pub, struct actuator_safety_s *safety, const int mavlink_fd); int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, const int mavlink_fd); int hil_state_transition(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state); diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 542821e957..618095d0b8 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -51,6 +51,7 @@ #include #include #include +#include #include #include @@ -60,6 +61,8 @@ struct gpio_led_s { int pin; struct vehicle_status_s status; int vehicle_status_sub; + struct actuator_safety_s safety; + int actuator_safety_sub; bool led_state; int counter; }; @@ -168,11 +171,15 @@ void gpio_led_cycle(FAR void *arg) if (status_updated) orb_copy(ORB_ID(vehicle_status), priv->vehicle_status_sub, &priv->status); + orb_check(priv->vehicle_status_sub, &status_updated); + + if (status_updated) + orb_copy(ORB_ID(actuator_safety), priv->actuator_safety_sub, &priv->safety); + /* select pattern for current status */ int pattern = 0; - /* XXX fmu armed correct? */ - if (priv->status.flag_fmu_armed) { + if (priv->safety.armed) { if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { pattern = 0x3f; // ****** solid (armed) @@ -181,11 +188,10 @@ void gpio_led_cycle(FAR void *arg) } } else { - if (priv->status.arming_state == ARMING_STATE_STANDBY) { + if (priv->safety.ready_to_arm) { pattern = 0x00; // ______ off (disarmed, preflight check) - } else if (priv->status.arming_state == ARMING_STATE_STANDBY && - priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { + } else if (priv->safety.ready_to_arm && priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { pattern = 0x38; // ***___ slow blink (disarmed, ready) } else { diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index d84406e7a2..a4a2ca3e5f 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -71,7 +71,7 @@ struct vehicle_local_position_s local_pos; struct vehicle_status_s v_status; struct rc_channels_s rc; struct rc_input_values rc_raw; -struct actuator_armed_s armed; +struct actuator_safety_s safety; struct mavlink_subscriptions mavlink_subs; @@ -109,7 +109,7 @@ static void l_global_position_setpoint(const struct listener *l); static void l_local_position_setpoint(const struct listener *l); static void l_attitude_setpoint(const struct listener *l); static void l_actuator_outputs(const struct listener *l); -static void l_actuator_armed(const struct listener *l); +static void l_actuator_safety(const struct listener *l); static void l_manual_control_setpoint(const struct listener *l); static void l_vehicle_attitude_controls(const struct listener *l); static void l_debug_key_value(const struct listener *l); @@ -133,7 +133,7 @@ static const struct listener listeners[] = { {l_actuator_outputs, &mavlink_subs.act_1_sub, 1}, {l_actuator_outputs, &mavlink_subs.act_2_sub, 2}, {l_actuator_outputs, &mavlink_subs.act_3_sub, 3}, - {l_actuator_armed, &mavlink_subs.armed_sub, 0}, + {l_actuator_safety, &mavlink_subs.safety_sub, 0}, {l_manual_control_setpoint, &mavlink_subs.man_control_sp_sub, 0}, {l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0}, {l_debug_key_value, &mavlink_subs.debug_key_value, 0}, @@ -269,7 +269,7 @@ l_vehicle_status(const struct listener *l) { /* immediately communicate state changes back to user */ orb_copy(ORB_ID(vehicle_status), status_sub, &v_status); - orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); + orb_copy(ORB_ID(actuator_safety), mavlink_subs.safety_sub, &safety); /* enable or disable HIL */ set_hil_on_off(v_status.flag_hil_enabled); @@ -466,7 +466,7 @@ l_actuator_outputs(const struct listener *l) act_outputs.output[7]); /* only send in HIL mode */ - if (mavlink_hil_enabled && armed.armed) { + if (mavlink_hil_enabled && safety.armed) { /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; @@ -538,9 +538,9 @@ l_actuator_outputs(const struct listener *l) } void -l_actuator_armed(const struct listener *l) +l_actuator_safety(const struct listener *l) { - orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); + orb_copy(ORB_ID(actuator_safety), mavlink_subs.safety_sub, &safety); } void @@ -745,8 +745,8 @@ uorb_receive_start(void) orb_set_interval(mavlink_subs.act_3_sub, 100); /* 10Hz updates */ /* --- ACTUATOR ARMED VALUE --- */ - mavlink_subs.armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(mavlink_subs.armed_sub, 100); /* 10Hz updates */ + mavlink_subs.safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + orb_set_interval(mavlink_subs.safety_sub, 100); /* 10Hz updates */ /* --- MAPPED MANUAL CONTROL INPUTS --- */ mavlink_subs.man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h index d61cd43dce..e647b090ae 100644 --- a/src/modules/mavlink/orb_topics.h +++ b/src/modules/mavlink/orb_topics.h @@ -58,6 +58,7 @@ #include #include #include +#include #include #include #include @@ -72,7 +73,7 @@ struct mavlink_subscriptions { int act_3_sub; int gps_sub; int man_control_sp_sub; - int armed_sub; + int safety_sub; int actuators_sub; int local_pos_sub; int spa_sub; diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c index 408a850d8c..4b6d811139 100644 --- a/src/modules/mavlink_onboard/mavlink.c +++ b/src/modules/mavlink_onboard/mavlink.c @@ -274,7 +274,7 @@ void mavlink_update_system(void) } void -get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_armed_s *armed, +get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_safety_s *safety, uint8_t *mavlink_state, uint8_t *mavlink_mode) { /* reset MAVLink mode bitfield */ @@ -290,7 +290,7 @@ get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct } /* set arming state */ - if (v_status->flag_fmu_armed) { + if (safety->armed) { *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } else { *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; @@ -369,7 +369,7 @@ int mavlink_thread_main(int argc, char *argv[]) baudrate = 57600; struct vehicle_status_s v_status; - struct actuator_armed_s armed; + struct actuator_safety_s safety; /* work around some stupidity in task_create's argv handling */ argc -= 2; @@ -437,7 +437,7 @@ int mavlink_thread_main(int argc, char *argv[]) /* translate the current system state to mavlink state and mode */ uint8_t mavlink_state = 0; uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode); + get_mavlink_mode_and_state(&v_status, &safety, &mavlink_state, &mavlink_mode); /* send heartbeat */ mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state); diff --git a/src/modules/mavlink_onboard/orb_topics.h b/src/modules/mavlink_onboard/orb_topics.h index f18f562439..fee5580b30 100644 --- a/src/modules/mavlink_onboard/orb_topics.h +++ b/src/modules/mavlink_onboard/orb_topics.h @@ -55,6 +55,7 @@ #include #include #include +#include #include #include #include @@ -69,7 +70,7 @@ struct mavlink_subscriptions { int act_3_sub; int gps_sub; int man_control_sp_sub; - int armed_sub; + int safety_sub; int actuators_sub; int local_pos_sub; int spa_sub; diff --git a/src/modules/mavlink_onboard/util.h b/src/modules/mavlink_onboard/util.h index 38a4db372b..17c3ba8201 100644 --- a/src/modules/mavlink_onboard/util.h +++ b/src/modules/mavlink_onboard/util.h @@ -50,5 +50,5 @@ extern volatile bool thread_should_exit; /** * Translate the custom state into standard mavlink modes and state. */ -extern void get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_armed_s *armed, +extern void get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_safety_s *safety, uint8_t *mavlink_state, uint8_t *mavlink_mode); diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 3329c5c48e..44c2fb1d80 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -58,6 +58,7 @@ #include #include #include +#include #include #include #include @@ -84,13 +85,16 @@ static bool motor_test_mode = false; static orb_advert_t actuator_pub; -static struct vehicle_status_s state; + static int mc_thread_main(int argc, char *argv[]) { /* declare and safely initialize all structs */ + struct vehicle_status_s state; memset(&state, 0, sizeof(state)); + struct actuator_safety_s safety; + memset(&safety, 0, sizeof(safety)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_attitude_setpoint_s att_sp; @@ -113,6 +117,7 @@ mc_thread_main(int argc, char *argv[]) int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); int state_sub = orb_subscribe(ORB_ID(vehicle_status)); + int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); @@ -150,7 +155,7 @@ mc_thread_main(int argc, char *argv[]) /* store last control mode to detect mode switches */ bool flag_control_manual_enabled = false; bool flag_control_attitude_enabled = false; - bool flag_fmu_armed = false; + bool flag_armed = false; bool flag_control_position_enabled = false; bool flag_control_velocity_enabled = false; @@ -200,6 +205,12 @@ mc_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_status), state_sub, &state); } + orb_check(safety_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + } + /* get a local copy of manual setpoint */ orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); /* get a local copy of attitude */ @@ -248,7 +259,7 @@ mc_thread_main(int argc, char *argv[]) /* initialize to current yaw if switching to manual or att control */ if (state.flag_control_attitude_enabled != flag_control_attitude_enabled || state.flag_control_manual_enabled != flag_control_manual_enabled || - state.flag_fmu_armed != flag_fmu_armed) { + safety.armed != flag_armed) { att_sp.yaw_body = att.yaw; } @@ -292,7 +303,7 @@ mc_thread_main(int argc, char *argv[]) att_sp.pitch_body = manual.pitch; /* set attitude if arming */ - if (!flag_control_attitude_enabled && state.flag_fmu_armed) { + if (!flag_control_attitude_enabled && safety.armed) { att_sp.yaw_body = att.yaw; } @@ -399,7 +410,7 @@ mc_thread_main(int argc, char *argv[]) flag_control_manual_enabled = state.flag_control_manual_enabled; flag_control_position_enabled = state.flag_control_position_enabled; flag_control_velocity_enabled = state.flag_control_velocity_enabled; - flag_fmu_armed = state.flag_fmu_armed; + flag_armed = safety.armed; perf_end(mc_loop_perf); } /* end of poll call for attitude updates */ diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 8c5ec092d1..41c2f67e56 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -60,6 +60,7 @@ #include #include +#include #include #include #include @@ -190,7 +191,7 @@ static int file_copy(const char *file_old, const char *file_new); static void handle_command(struct vehicle_command_s *cmd); -static void handle_status(struct vehicle_status_s *cmd); +static void handle_status(struct actuator_safety_s *safety); /** * Create folder for current logging session. Store folder name in 'log_folder'. @@ -623,6 +624,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* warning! using union here to save memory, elements should be used separately! */ union { + struct actuator_safety_s safety; struct vehicle_command_s cmd; struct sensor_combined_s sensor; struct vehicle_attitude_s att; @@ -643,6 +645,7 @@ int sdlog2_thread_main(int argc, char *argv[]) memset(&buf, 0, sizeof(buf)); struct { + int safety_sub; int cmd_sub; int status_sub; int sensor_sub; @@ -690,6 +693,12 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- SAFETY STATUS --- */ + subs.safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + fds[fdsc_count].fd = subs.safety_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* --- VEHICLE STATUS --- */ subs.status_sub = orb_subscribe(ORB_ID(vehicle_status)); fds[fdsc_count].fd = subs.status_sub; @@ -826,6 +835,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int ifds = 0; int handled_topics = 0; + /* --- VEHICLE COMMAND - LOG MANAGEMENT --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd); @@ -838,7 +848,7 @@ int sdlog2_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf_status); if (log_when_armed) { - handle_status(&buf_status); + handle_status(&buf.safety); } handled_topics++; @@ -870,7 +880,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_STAT.flight_mode = 0; log_msg.body.log_STAT.manual_control_mode = 0; log_msg.body.log_STAT.manual_sas_mode = 0; - log_msg.body.log_STAT.armed = (unsigned char) buf_status.flag_fmu_armed; /* XXX fmu armed correct? */ + log_msg.body.log_STAT.armed = (unsigned char) buf.safety.armed; /* XXX fmu armed correct? */ log_msg.body.log_STAT.battery_voltage = buf_status.voltage_battery; log_msg.body.log_STAT.battery_current = buf_status.current_battery; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; @@ -1168,11 +1178,10 @@ void handle_command(struct vehicle_command_s *cmd) } } -void handle_status(struct vehicle_status_s *status) +void handle_status(struct actuator_safety_s *safety) { - /* XXX fmu armed correct? */ - if (status->flag_fmu_armed != flag_system_armed) { - flag_system_armed = status->flag_fmu_armed; + if (safety->armed != flag_system_armed) { + flag_system_armed = safety->armed; if (flag_system_armed) { sdlog2_start_log(); diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 4197f6fb2d..2674354c32 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -137,7 +137,9 @@ ORB_DEFINE(actuator_controls_0, struct actuator_controls_s); ORB_DEFINE(actuator_controls_1, struct actuator_controls_s); ORB_DEFINE(actuator_controls_2, struct actuator_controls_s); ORB_DEFINE(actuator_controls_3, struct actuator_controls_s); -ORB_DEFINE(actuator_armed, struct actuator_armed_s); + +#include "topics/actuator_safety.h" +ORB_DEFINE(actuator_safety, struct actuator_safety_s); /* actuator controls, as set by actuators / mixers after limiting */ #include "topics/actuator_controls_effective.h" diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/uORB/topics/actuator_controls.h index b7c4196c0a..26b967237c 100644 --- a/src/modules/uORB/topics/actuator_controls.h +++ b/src/modules/uORB/topics/actuator_controls.h @@ -52,6 +52,9 @@ #define NUM_ACTUATOR_CONTROLS 8 #define NUM_ACTUATOR_CONTROL_GROUPS 4 /**< for sanity checking */ +/* control sets with pre-defined applications */ +#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0) + struct actuator_controls_s { uint64_t timestamp; float control[NUM_ACTUATOR_CONTROLS]; @@ -63,16 +66,4 @@ ORB_DECLARE(actuator_controls_1); ORB_DECLARE(actuator_controls_2); ORB_DECLARE(actuator_controls_3); -/* control sets with pre-defined applications */ -#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0) - -/** global 'actuator output is live' control. */ -struct actuator_armed_s { - bool armed; /**< Set to true if system is armed */ - bool ready_to_arm; /**< Set to true if system is ready to be armed */ - bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ -}; - -ORB_DECLARE(actuator_armed); - #endif \ No newline at end of file diff --git a/src/modules/uORB/topics/actuator_safety.h b/src/modules/uORB/topics/actuator_safety.h new file mode 100644 index 0000000000..b78eb4b7e8 --- /dev/null +++ b/src/modules/uORB/topics/actuator_safety.h @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 actuator_controls.h + * + * Actuator control topics - mixer inputs. + * + * Values published to these topics are the outputs of the vehicle control + * system, and are expected to be mixed and used to drive the actuators + * (servos, speed controls, etc.) that operate the vehicle. + * + * Each topic can be published by a single controller + */ + +#ifndef TOPIC_ACTUATOR_SAFETY_H +#define TOPIC_ACTUATOR_SAFETY_H + +#include +#include "../uORB.h" + +/** global 'actuator output is live' control. */ +struct actuator_safety_s { + + uint64_t timestamp; + + bool safety_off; /**< Set to true if safety is off */ + bool armed; /**< Set to true if system is armed */ + bool ready_to_arm; /**< Set to true if system is ready to be armed */ + bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ +}; + +ORB_DECLARE(actuator_safety); + +#endif \ No newline at end of file diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h new file mode 100644 index 0000000000..eb35d08840 --- /dev/null +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -0,0 +1,96 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier + * @author Petri Tanskanen + * @author Thomas Gubler + * @author Julian Oes + * + * 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 vehicle_control_mode.h + * Definition of the vehicle_control_mode uORB topic. + * + * All control apps should depend their actions based on the flags set here. + */ + +#ifndef VEHICLE_CONTROL_MODE +#define VEHICLE_CONTROL_MODE + +#include +#include +#include "../uORB.h" + +/** + * @addtogroup topics @{ + */ + + +/** + * state machine / state of vehicle. + * + * Encodes the complete system state and is set by the commander app. + */ +struct vehicle_control_mode_s +{ + /* use of a counter and timestamp recommended (but not necessary) */ + + uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ + uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ + + bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ + + bool flag_hil_enabled; /**< true if hardware in the loop simulation is enabled */ + bool flag_armed; /**< true is motors / actuators are armed */ + bool flag_safety_off; /**< true if safety is off */ + bool flag_system_emergency; + bool flag_preflight_calibration; + + bool flag_control_manual_enabled; /**< true if manual input is mixed in */ + bool flag_control_offboard_enabled; /**< true if offboard control input is on */ + bool flag_auto_enabled; + bool flag_control_rates_enabled; /**< true if rates are stabilized */ + bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ + bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ + bool flag_control_position_enabled; /**< true if position is controlled */ + + bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ + bool failsave_highlevel; +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_status); + +#endif diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 6d3f8a8632..b19075921f 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -199,8 +199,8 @@ struct vehicle_status_s bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ bool flag_hil_enabled; /**< true if hardware in the loop simulation is enabled */ - bool flag_fmu_armed; /**< true is motors / actuators of FMU are armed */ - bool flag_io_armed; /**< true is motors / actuators of IO are armed */ + bool flag_armed; /**< true is motors / actuators are armed */ + bool flag_safety_off; /**< true if safety is off */ bool flag_system_emergency; bool flag_preflight_calibration; @@ -245,7 +245,6 @@ struct vehicle_status_s uint16_t errors_count2; uint16_t errors_count3; uint16_t errors_count4; - }; /** From 5b21362e1ffefe4e28579eb7a853fe5d22288760 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 14 Jun 2013 16:04:23 +0200 Subject: [PATCH 136/486] Arming with IO working now --- src/drivers/px4io/px4io.cpp | 11 +++++------ src/modules/commander/state_machine_helper.c | 4 ++++ 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 28a3eb917b..d728b7b762 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -728,12 +728,11 @@ PX4IO::io_set_arming_state() } else { clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED; } - // TODO fix this -// if (armed.ready_to_arm) { -// set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; -// } else { -// clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; -// } + if (safety.ready_to_arm) { + set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; + } else { + clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; + } if (vstatus.flag_external_manual_override_ok) { set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index fea7ee840b..4f2fbc9840 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -70,6 +70,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta if (current_state->arming_state == ARMING_STATE_STANDBY) { ret = OK; safety->armed = false; + safety->ready_to_arm = false; } break; case ARMING_STATE_STANDBY: @@ -82,6 +83,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta if (current_state->condition_system_sensors_initialized) { ret = OK; safety->armed = false; + safety->ready_to_arm = true; } else { mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized"); } @@ -115,6 +117,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta || current_state->arming_state == ARMING_STATE_ARMED_ERROR) { ret = OK; safety->armed = false; + safety->ready_to_arm = false; } break; case ARMING_STATE_REBOOT: @@ -126,6 +129,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta ret = OK; safety->armed = false; + safety->ready_to_arm = false; } break; From e556649f2ff6922a7a3b7751b68cdedd0d6254aa Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 14 Jun 2013 16:48:41 +0200 Subject: [PATCH 137/486] Beep when mode is not possible --- src/modules/commander/commander.c | 3 --- src/modules/commander/state_machine_helper.c | 19 +++++++++++++++++++ 2 files changed, 19 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index 6812fb1fb8..1d3f908073 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -1991,9 +1991,6 @@ int commander_thread_main(int argc, char *argv[]) // printf("checking\n"); if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - - - printf("System Type: %d\n", current_status.system_type); if((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index 4f2fbc9840..dcaf775b9d 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -198,6 +198,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to be disarmed first */ if (current_state->arming_state != ARMING_STATE_STANDBY) { mavlink_log_critical(mavlink_fd, "Rej. MANUAL_STANDBY: not disarmed"); + tune_negative(); } else { ret = OK; current_state->flag_control_rates_enabled = true; @@ -214,6 +215,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to be armed first */ if (current_state->arming_state != ARMING_STATE_ARMED) { mavlink_log_critical(mavlink_fd, "Rej. MANUAL: not armed"); + tune_negative(); } else { ret = OK; current_state->flag_control_rates_enabled = true; @@ -236,8 +238,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to be disarmed and have a position estimate */ if (current_state->arming_state != ARMING_STATE_STANDBY) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: not disarmed"); + tune_negative(); } else if (!current_state->condition_local_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: no position estimate"); + tune_negative(); } else { ret = OK; current_state->flag_control_rates_enabled = true; @@ -265,8 +269,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to be armed and have a position estimate */ if (current_state->arming_state != ARMING_STATE_ARMED) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: not armed"); + tune_negative(); } else if (!current_state->condition_local_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate"); + tune_negative(); } else { ret = OK; current_state->flag_control_rates_enabled = true; @@ -293,8 +299,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to be armed and have a position estimate */ if (current_state->arming_state != ARMING_STATE_ARMED) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: not armed"); + tune_negative(); } else if (!current_state->condition_local_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no pos estimate"); + tune_negative(); } else { ret = OK; current_state->flag_control_rates_enabled = true; @@ -317,10 +325,13 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to be disarmed and have a position and home lock */ if (current_state->arming_state != ARMING_STATE_STANDBY) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: not disarmed"); + tune_negative(); } else if (!current_state->condition_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no pos lock"); + tune_negative(); } else if (!current_state->condition_home_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no home pos"); + tune_negative(); } else { ret = OK; current_state->flag_control_rates_enabled = true; @@ -343,6 +354,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to be armed and have a position and home lock */ if (current_state->arming_state != ARMING_STATE_ARMED) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_READY: not armed"); + tune_negative(); } else { ret = OK; current_state->flag_control_rates_enabled = true; @@ -380,8 +392,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to have a position and home lock */ if (!current_state->condition_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no pos lock"); + tune_negative(); } else if (!current_state->condition_home_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no home pos"); + tune_negative(); } else { ret = OK; current_state->flag_control_rates_enabled = true; @@ -405,6 +419,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to have a mission ready */ if (!current_state-> condition_auto_mission_available) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_MISSION: no mission available"); + tune_negative(); } else { ret = OK; current_state->flag_control_rates_enabled = true; @@ -428,8 +443,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to have a position and home lock */ if (!current_state->condition_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no pos lock"); + tune_negative(); } else if (!current_state->condition_home_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no home pos"); + tune_negative(); } else { ret = OK; current_state->flag_control_rates_enabled = true; @@ -450,8 +467,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to have a position and home lock */ if (!current_state->condition_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no pos lock"); + tune_negative(); } else if (!current_state->condition_home_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no home pos"); + tune_negative(); } else { ret = OK; current_state->flag_control_rates_enabled = true; From 38ca3bd78a9b415df4a260a8cba43e2c13703972 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 15 Jun 2013 11:36:26 +0400 Subject: [PATCH 138/486] multirotor_pos_control fixes, introduced HARD control mode (disabled by default) --- .../multirotor_pos_control.c | 43 ++++++++++++------- .../multirotor_pos_control_params.c | 11 +++++ .../multirotor_pos_control_params.h | 10 +++++ 3 files changed, 49 insertions(+), 15 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index ad5670edca..c94972e7df 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -283,11 +283,16 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { float alt_err_linear_limit = params.alt_d / params.alt_p * params.alt_rate_max; float pos_err_linear_limit = params.pos_d / params.pos_p * params.pos_rate_max; + float pos_sp_speed_x = 0.0f; + float pos_sp_speed_y = 0.0f; + float pos_sp_speed_z = 0.0f; + if (status.flag_control_manual_enabled) { /* move altitude setpoint with manual controls */ float alt_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz); if (alt_sp_ctl != 0.0f) { - local_pos_sp.z -= alt_sp_ctl * params.alt_rate_max * dt; + pos_sp_speed_z = -alt_sp_ctl * params.alt_rate_max; + local_pos_sp.z += pos_sp_speed_z * dt; if (local_pos_sp.z > local_pos.z + alt_err_linear_limit) { local_pos_sp.z = local_pos.z + alt_err_linear_limit; } else if (local_pos_sp.z < local_pos.z - alt_err_linear_limit) { @@ -297,14 +302,16 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE) { /* move position setpoint with manual controls */ - float pos_x_sp_ctl = scale_control(-manual.pitch, 1.0f, pos_ctl_dz); - float pos_y_sp_ctl = scale_control(manual.roll, 1.0f, pos_ctl_dz); + float pos_x_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz); + float pos_y_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz); if (pos_x_sp_ctl != 0.0f || pos_y_sp_ctl != 0.0f) { /* calculate direction and increment of control in NED frame */ float dir_sp_ctl = att.yaw + atan2f(pos_y_sp_ctl, pos_x_sp_ctl); - float d_sp_ctl = norm(pos_x_sp_ctl, pos_y_sp_ctl) * params.pos_rate_max * dt; - local_pos_sp.x += cosf(dir_sp_ctl) * d_sp_ctl; - local_pos_sp.y += sinf(dir_sp_ctl) * d_sp_ctl; + float d_sp_ctl = norm(pos_x_sp_ctl, pos_y_sp_ctl) * params.pos_rate_max; + pos_sp_speed_x = cosf(dir_sp_ctl) * d_sp_ctl; + pos_sp_speed_x = sinf(dir_sp_ctl) * d_sp_ctl; + local_pos_sp.x += pos_sp_speed_x * dt; + local_pos_sp.y += pos_sp_speed_y * dt; /* limit maximum setpoint from position offset and preserve direction */ float pos_vec_x = local_pos_sp.x - local_pos.x; float pos_vec_y = local_pos_sp.y - local_pos.y; @@ -315,15 +322,19 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { } } } + + if (params.hard == 0) { + pos_sp_speed_x = 0.0f; + pos_sp_speed_y = 0.0f; + pos_sp_speed_z = 0.0f; + } } /* PID for altitude */ /* don't accelerate more than ALT_RATE_MAX, limit error to corresponding value */ - float alt_err = limit_value(local_pos.z - local_pos_sp.z, alt_err_linear_limit); + float alt_err = limit_value(local_pos_sp.z - local_pos.z, alt_err_linear_limit); /* P and D components */ - float thrust_ctl_pd = alt_err * params.alt_p + local_pos.vz * params.alt_d; - /* add I component */ - float thrust_ctl = thrust_ctl_pd + alt_integral; + float thrust_ctl_pd = -(alt_err * params.alt_p + (pos_sp_speed_z - local_pos.vz) * params.alt_d); // altitude = -z /* integrate */ alt_integral += thrust_ctl_pd / params.alt_p * params.alt_i * dt; if (alt_integral < params.thr_min) { @@ -331,6 +342,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { } else if (alt_integral > params.thr_max) { alt_integral = params.thr_max; } + /* add I component */ + float thrust_ctl = thrust_ctl_pd + alt_integral; if (thrust_ctl < params.thr_min) { thrust_ctl = params.thr_min; } else if (thrust_ctl > params.thr_max) { @@ -342,14 +355,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { float pos_x_err = limit_value(local_pos.x - local_pos_sp.x, pos_err_linear_limit); float pos_y_err = limit_value(local_pos.y - local_pos_sp.y, pos_err_linear_limit); /* P and D components */ - float pos_x_ctl_pd = - pos_x_err * params.pos_p - local_pos.vx * params.pos_d; - float pos_y_ctl_pd = - pos_y_err * params.pos_p - local_pos.vy * params.pos_d; - /* add I component */ - float pos_x_ctl = pos_x_ctl_pd + pos_x_integral; - float pos_y_ctl = pos_y_ctl_pd + pos_y_integral; + float pos_x_ctl_pd = - pos_x_err * params.pos_p + (pos_sp_speed_x - local_pos.vx) * params.pos_d; + float pos_y_ctl_pd = - pos_y_err * params.pos_p + (pos_sp_speed_y - local_pos.vy) * params.pos_d; /* integrate */ pos_x_integral = limit_value(pos_x_integral + pos_x_ctl_pd / params.pos_p * params.pos_i * dt, params.slope_max); pos_y_integral = limit_value(pos_y_integral + pos_y_ctl_pd / params.pos_p * params.pos_i * dt, params.slope_max); + /* add I component */ + float pos_x_ctl = pos_x_ctl_pd + pos_x_integral; + float pos_y_ctl = pos_y_ctl_pd + pos_y_integral; /* calculate direction and slope in NED frame */ float dir = atan2f(pos_y_ctl, pos_x_ctl); /* use approximation: slope ~ sin(slope) = force */ diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c index 7f2ba3db8a..7c3296cae3 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -53,6 +53,7 @@ PARAM_DEFINE_FLOAT(MPC_POS_I, 0.0f); PARAM_DEFINE_FLOAT(MPC_POS_D, 0.2f); PARAM_DEFINE_FLOAT(MPC_POS_RATE_MAX, 10.0f); PARAM_DEFINE_FLOAT(MPC_SLOPE_MAX, 0.5f); +PARAM_DEFINE_INT32(MPC_HARD, 0); int parameters_init(struct multirotor_position_control_param_handles *h) { @@ -67,6 +68,11 @@ int parameters_init(struct multirotor_position_control_param_handles *h) h->pos_d = param_find("MPC_POS_D"); h->pos_rate_max = param_find("MPC_POS_RATE_MAX"); h->slope_max = param_find("MPC_SLOPE_MAX"); + h->slope_max = param_find("MPC_HARD"); + + h->rc_scale_pitch = param_find("RC_SCALE_PITCH"); + h->rc_scale_roll = param_find("RC_SCALE_ROLL"); + h->rc_scale_yaw = param_find("RC_SCALE_YAW"); return OK; } @@ -84,6 +90,11 @@ int parameters_update(const struct multirotor_position_control_param_handles *h, param_get(h->pos_d, &(p->pos_d)); param_get(h->pos_rate_max, &(p->pos_rate_max)); param_get(h->slope_max, &(p->slope_max)); + param_get(h->hard, &(p->hard)); + + param_get(h->rc_scale_pitch, &(p->rc_scale_pitch)); + param_get(h->rc_scale_roll, &(p->rc_scale_roll)); + param_get(h->rc_scale_yaw, &(p->rc_scale_yaw)); return OK; } diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h index 589ee92a1d..13b667ad30 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h @@ -53,6 +53,11 @@ struct multirotor_position_control_params { float pos_d; float pos_rate_max; float slope_max; + int hard; + + float rc_scale_pitch; + float rc_scale_roll; + float rc_scale_yaw; }; struct multirotor_position_control_param_handles { @@ -67,6 +72,11 @@ struct multirotor_position_control_param_handles { param_t pos_d; param_t pos_rate_max; param_t slope_max; + param_t hard; + + param_t rc_scale_pitch; + param_t rc_scale_roll; + param_t rc_scale_yaw; }; /** From 9f5565de3221718ba12800a54ca1a0c06b7491ef Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 15 Jun 2013 19:41:54 +0200 Subject: [PATCH 139/486] Controllers should not access state machine anymore but access the vehicle_control_mode flags --- src/modules/commander/commander.c | 113 ++++++++------- src/modules/commander/state_machine_helper.c | 136 +++++++++--------- src/modules/commander/state_machine_helper.h | 4 +- src/modules/mavlink/orb_topics.h | 1 + src/modules/mavlink_onboard/mavlink.c | 12 +- src/modules/mavlink_onboard/orb_topics.h | 1 + src/modules/mavlink_onboard/util.h | 3 +- .../multirotor_att_control_main.c | 45 +++--- src/modules/uORB/objects_common.cpp | 3 + src/modules/uORB/topics/actuator_safety.h | 1 + .../uORB/topics/vehicle_control_mode.h | 9 +- src/modules/uORB/topics/vehicle_status.h | 18 +-- 12 files changed, 180 insertions(+), 166 deletions(-) diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index 1d3f908073..7853d2e790 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -1233,6 +1233,9 @@ int commander_thread_main(int argc, char *argv[]) // XXX for now just set sensors as initialized current_status.condition_system_sensors_initialized = true; + // XXX just disable offboard control for now + control_mode.flag_control_offboard_enabled = false; + /* advertise to ORB */ status_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); /* publish current state machine */ @@ -1240,6 +1243,8 @@ int commander_thread_main(int argc, char *argv[]) safety_pub = orb_advertise(ORB_ID(actuator_safety), &safety); + control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); + /* home position */ orb_advert_t home_pub = -1; struct home_position_s home; @@ -1824,7 +1829,7 @@ int commander_thread_main(int argc, char *argv[]) /* just manual, XXX this might depend on the return switch */ if (current_status.mode_switch == MODE_SWITCH_MANUAL) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); } @@ -1832,9 +1837,9 @@ int commander_thread_main(int argc, char *argv[]) /* Try seatbelt or fallback to manual */ } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); } @@ -1843,11 +1848,11 @@ int commander_thread_main(int argc, char *argv[]) /* Try auto or fallback to seatbelt or even manual */ } else if (current_status.mode_switch == MODE_SWITCH_AUTO) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { // first fallback to SEATBELT_STANDY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { // or fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); } @@ -1863,7 +1868,7 @@ int commander_thread_main(int argc, char *argv[]) /* Always accept manual mode */ if (current_status.mode_switch == MODE_SWITCH_MANUAL) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -1872,9 +1877,9 @@ int commander_thread_main(int argc, char *argv[]) } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT && current_status.return_switch == RETURN_SWITCH_NONE) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -1884,9 +1889,9 @@ int commander_thread_main(int argc, char *argv[]) } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT && current_status.return_switch == RETURN_SWITCH_RETURN) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -1898,19 +1903,19 @@ int commander_thread_main(int argc, char *argv[]) && current_status.mission_switch == MISSION_SWITCH_NONE) { /* we might come from the disarmed state AUTO_STANDBY */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_READY, mavlink_fd) != OK) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_READY, control_mode_pub, &control_mode, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } } /* or from some other armed state like SEATBELT or MANUAL */ - } else if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_LOITER, mavlink_fd) != OK) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + } else if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_LOITER, control_mode_pub, &control_mode, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -1922,10 +1927,10 @@ int commander_thread_main(int argc, char *argv[]) && current_status.return_switch == RETURN_SWITCH_NONE && current_status.mission_switch == MISSION_SWITCH_MISSION) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_MISSION, mavlink_fd) != OK) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_MISSION, control_mode_pub, &control_mode, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -1937,10 +1942,10 @@ int commander_thread_main(int argc, char *argv[]) && current_status.return_switch == RETURN_SWITCH_RETURN && (current_status.mission_switch == MISSION_SWITCH_NONE || current_status.mission_switch == MISSION_SWITCH_MISSION)) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_RTL, mavlink_fd) != OK) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_RTL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -2070,43 +2075,43 @@ int commander_thread_main(int argc, char *argv[]) if (!current_status.rc_signal_found_once && sp_offboard.timestamp != 0) { if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) { - /* decide about attitude control flag, enable in att/pos/vel */ - bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE || - sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || - sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION); + // /* decide about attitude control flag, enable in att/pos/vel */ + // bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE || + // sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || + // sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION); - /* decide about rate control flag, enable it always XXX (for now) */ - bool rates_ctrl_enabled = true; + // /* decide about rate control flag, enable it always XXX (for now) */ + // bool rates_ctrl_enabled = true; - /* set up control mode */ - if (current_status.flag_control_attitude_enabled != attitude_ctrl_enabled) { - current_status.flag_control_attitude_enabled = attitude_ctrl_enabled; - state_changed = true; - } + // /* set up control mode */ + // if (current_status.flag_control_attitude_enabled != attitude_ctrl_enabled) { + // current_status.flag_control_attitude_enabled = attitude_ctrl_enabled; + // state_changed = true; + // } - if (current_status.flag_control_rates_enabled != rates_ctrl_enabled) { - current_status.flag_control_rates_enabled = rates_ctrl_enabled; - state_changed = true; - } + // if (current_status.flag_control_rates_enabled != rates_ctrl_enabled) { + // current_status.flag_control_rates_enabled = rates_ctrl_enabled; + // state_changed = true; + // } - /* handle the case where offboard control signal was regained */ - if (!current_status.offboard_control_signal_found_once) { - current_status.offboard_control_signal_found_once = true; - /* enable offboard control, disable manual input */ - current_status.flag_control_manual_enabled = false; - current_status.flag_control_offboard_enabled = true; - state_changed = true; - tune_positive(); + // /* handle the case where offboard control signal was regained */ + // if (!current_status.offboard_control_signal_found_once) { + // current_status.offboard_control_signal_found_once = true; + // /* enable offboard control, disable manual input */ + // current_status.flag_control_manual_enabled = false; + // current_status.flag_control_offboard_enabled = true; + // state_changed = true; + // tune_positive(); - mavlink_log_critical(mavlink_fd, "DETECTED OFFBOARD SIGNAL FIRST"); + // mavlink_log_critical(mavlink_fd, "DETECTED OFFBOARD SIGNAL FIRST"); - } else { - if (current_status.offboard_control_signal_lost) { - mavlink_log_critical(mavlink_fd, "RECOVERY OFFBOARD CONTROL"); - state_changed = true; - tune_positive(); - } - } + // } else { + // if (current_status.offboard_control_signal_lost) { + // mavlink_log_critical(mavlink_fd, "RECOVERY OFFBOARD CONTROL"); + // state_changed = true; + // tune_positive(); + // } + // } current_status.offboard_control_signal_weak = false; current_status.offboard_control_signal_lost = false; diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index dcaf775b9d..394ee67cc7 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -46,6 +46,7 @@ #include #include #include +#include #include #include #include @@ -161,7 +162,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta * This functions does not evaluate any input flags but only checks if the transitions * are valid. */ -int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, const int mavlink_fd) { +int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, int control_mode_pub, struct vehicle_control_mode_s *control_mode, const int mavlink_fd) { int ret = ERROR; @@ -179,11 +180,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY) { ret = OK; - current_state->flag_control_rates_enabled = false; - current_state->flag_control_attitude_enabled = false; - current_state->flag_control_velocity_enabled = false; - current_state->flag_control_position_enabled = false; - current_state->flag_control_manual_enabled = false; + control_mode->flag_control_rates_enabled = false; + control_mode->flag_control_attitude_enabled = false; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = false; } break; @@ -201,11 +202,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current tune_negative(); } else { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = false; - current_state->flag_control_position_enabled = false; - current_state->flag_control_manual_enabled = true; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = true; } } break; @@ -218,11 +219,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current tune_negative(); } else { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = false; - current_state->flag_control_position_enabled = false; - current_state->flag_control_manual_enabled = true; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = true; } break; @@ -244,11 +245,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current tune_negative(); } else { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = true; - current_state->flag_control_position_enabled = false; - current_state->flag_control_manual_enabled = false; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = false; } } break; @@ -275,11 +276,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current tune_negative(); } else { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = true; - current_state->flag_control_position_enabled = false; - current_state->flag_control_manual_enabled = false; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = false; } } break; @@ -305,11 +306,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current tune_negative(); } else { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = true; - current_state->flag_control_position_enabled = false; - current_state->flag_control_manual_enabled = false; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = false; } } break; @@ -334,11 +335,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current tune_negative(); } else { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = true; - current_state->flag_control_position_enabled = true; - current_state->flag_control_manual_enabled = false; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; } } break; @@ -357,11 +358,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current tune_negative(); } else { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = true; - current_state->flag_control_position_enabled = true; - current_state->flag_control_manual_enabled = false; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; } } break; @@ -372,11 +373,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current if (current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = true; - current_state->flag_control_position_enabled = true; - current_state->flag_control_manual_enabled = false; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; } break; @@ -398,11 +399,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current tune_negative(); } else { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = true; - current_state->flag_control_position_enabled = true; - current_state->flag_control_manual_enabled = false; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; } } break; @@ -422,11 +423,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current tune_negative(); } else { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = true; - current_state->flag_control_position_enabled = true; - current_state->flag_control_manual_enabled = false; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; } } break; @@ -449,11 +450,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current tune_negative(); } else { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = true; - current_state->flag_control_position_enabled = true; - current_state->flag_control_manual_enabled = false; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; } } break; @@ -473,11 +474,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current tune_negative(); } else { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = true; - current_state->flag_control_position_enabled = true; - current_state->flag_control_manual_enabled = false; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; } } break; @@ -491,6 +492,9 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->counter++; current_state->timestamp = hrt_absolute_time(); orb_publish(ORB_ID(vehicle_status), status_pub, current_state); + + control_mode->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, control_mode); } } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 824a7529f0..8d85360905 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -47,6 +47,7 @@ #include #include #include +#include void navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); @@ -54,8 +55,7 @@ void navigation_state_update(int status_pub, struct vehicle_status_s *current_st void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); -int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int safety_pub, struct actuator_safety_s *safety, const int mavlink_fd); -int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, const int mavlink_fd); +int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, int control_mode_pub, struct vehicle_control_mode_s *control_mode, const int mavlink_fd); int hil_state_transition(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state); diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h index e647b090ae..221957d465 100644 --- a/src/modules/mavlink/orb_topics.h +++ b/src/modules/mavlink/orb_topics.h @@ -54,6 +54,7 @@ #include #include #include +#include #include #include #include diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c index 4b6d811139..dbea2be083 100644 --- a/src/modules/mavlink_onboard/mavlink.c +++ b/src/modules/mavlink_onboard/mavlink.c @@ -274,18 +274,18 @@ void mavlink_update_system(void) } void -get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_safety_s *safety, +get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_safety_s *safety, uint8_t *mavlink_state, uint8_t *mavlink_mode) { /* reset MAVLink mode bitfield */ *mavlink_mode = 0; /* set mode flags independent of system state */ - if (v_status->flag_control_manual_enabled) { + if (control_mode->flag_control_manual_enabled) { *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; } - if (v_status->flag_hil_enabled) { + if (safety->hil_enabled) { *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; } @@ -296,7 +296,7 @@ get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; } - if (v_status->flag_control_velocity_enabled) { + if (control_mode->flag_control_velocity_enabled) { *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; } else { *mavlink_mode &= ~MAV_MODE_FLAG_GUIDED_ENABLED; @@ -368,7 +368,9 @@ int mavlink_thread_main(int argc, char *argv[]) char *device_name = "/dev/ttyS1"; baudrate = 57600; + /* XXX this is never written? */ struct vehicle_status_s v_status; + struct vehicle_control_mode_s control_mode; struct actuator_safety_s safety; /* work around some stupidity in task_create's argv handling */ @@ -437,7 +439,7 @@ int mavlink_thread_main(int argc, char *argv[]) /* translate the current system state to mavlink state and mode */ uint8_t mavlink_state = 0; uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&v_status, &safety, &mavlink_state, &mavlink_mode); + get_mavlink_mode_and_state(&control_mode, &safety, &mavlink_state, &mavlink_mode); /* send heartbeat */ mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state); diff --git a/src/modules/mavlink_onboard/orb_topics.h b/src/modules/mavlink_onboard/orb_topics.h index fee5580b30..f01f09e12d 100644 --- a/src/modules/mavlink_onboard/orb_topics.h +++ b/src/modules/mavlink_onboard/orb_topics.h @@ -52,6 +52,7 @@ #include #include #include +#include #include #include #include diff --git a/src/modules/mavlink_onboard/util.h b/src/modules/mavlink_onboard/util.h index 17c3ba8201..c6a2e52bf0 100644 --- a/src/modules/mavlink_onboard/util.h +++ b/src/modules/mavlink_onboard/util.h @@ -50,5 +50,6 @@ extern volatile bool thread_should_exit; /** * Translate the custom state into standard mavlink modes and state. */ -extern void get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_safety_s *safety, +extern void +get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_safety_s *safety, uint8_t *mavlink_state, uint8_t *mavlink_mode); diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 44c2fb1d80..b4d7fb630a 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -57,7 +57,7 @@ #include #include #include -#include +#include #include #include #include @@ -91,8 +91,8 @@ static int mc_thread_main(int argc, char *argv[]) { /* declare and safely initialize all structs */ - struct vehicle_status_s state; - memset(&state, 0, sizeof(state)); + struct vehicle_control_mode_s control_mode; + memset(&control_mode, 0, sizeof(control_mode)); struct actuator_safety_s safety; memset(&safety, 0, sizeof(safety)); struct vehicle_attitude_s att; @@ -116,7 +116,7 @@ mc_thread_main(int argc, char *argv[]) int param_sub = orb_subscribe(ORB_ID(parameter_update)); int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); - int state_sub = orb_subscribe(ORB_ID(vehicle_status)); + int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); @@ -181,7 +181,6 @@ mc_thread_main(int argc, char *argv[]) } else if (ret == 0) { /* no return value, ignore */ } else { - /* only update parameters if they changed */ if (fds[1].revents & POLLIN) { /* read from param to clear updated flag */ @@ -199,10 +198,10 @@ mc_thread_main(int argc, char *argv[]) /* get a local copy of system state */ bool updated; - orb_check(state_sub, &updated); + orb_check(control_mode_sub, &updated); if (updated) { - orb_copy(ORB_ID(vehicle_status), state_sub, &state); + orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); } orb_check(safety_sub, &updated); @@ -227,9 +226,8 @@ mc_thread_main(int argc, char *argv[]) /* get a local copy of the current sensor values */ orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); - /** STEP 1: Define which input is the dominating control input */ - if (state.flag_control_offboard_enabled) { + if (control_mode.flag_control_offboard_enabled) { /* offboard inputs */ if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) { rates_sp.roll = offboard_sp.p1; @@ -252,13 +250,11 @@ mc_thread_main(int argc, char *argv[]) } - } else if (state.flag_control_manual_enabled) { - - if (state.flag_control_attitude_enabled) { - + } else if (control_mode.flag_control_manual_enabled) { + if (control_mode.flag_control_attitude_enabled) { /* initialize to current yaw if switching to manual or att control */ - if (state.flag_control_attitude_enabled != flag_control_attitude_enabled || - state.flag_control_manual_enabled != flag_control_manual_enabled || + if (control_mode.flag_control_attitude_enabled != flag_control_attitude_enabled || + control_mode.flag_control_manual_enabled != flag_control_manual_enabled || safety.armed != flag_armed) { att_sp.yaw_body = att.yaw; } @@ -266,6 +262,8 @@ mc_thread_main(int argc, char *argv[]) static bool rc_loss_first_time = true; /* if the RC signal is lost, try to stay level and go slowly back down to ground */ +/* XXX fix this */ +#if 0 if (state.rc_signal_lost) { /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ param_get(failsafe_throttle_handle, &failsafe_throttle); @@ -297,6 +295,7 @@ mc_thread_main(int argc, char *argv[]) rc_loss_first_time = false; } else { +#endif rc_loss_first_time = true; att_sp.roll_body = manual.roll; @@ -344,7 +343,9 @@ mc_thread_main(int argc, char *argv[]) att_sp.thrust = manual.throttle; att_sp.timestamp = hrt_absolute_time(); +#if 0 } +#endif /* STEP 2: publish the controller output */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); @@ -378,7 +379,7 @@ mc_thread_main(int argc, char *argv[]) } /** STEP 3: Identify the controller setup to run and set up the inputs correctly */ - if (state.flag_control_attitude_enabled) { + if (control_mode.flag_control_attitude_enabled) { multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position); orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); @@ -405,11 +406,11 @@ mc_thread_main(int argc, char *argv[]) multirotor_control_rates(&rates_sp, gyro, &actuators); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - /* update state */ - flag_control_attitude_enabled = state.flag_control_attitude_enabled; - flag_control_manual_enabled = state.flag_control_manual_enabled; - flag_control_position_enabled = state.flag_control_position_enabled; - flag_control_velocity_enabled = state.flag_control_velocity_enabled; + /* update control_mode */ + flag_control_attitude_enabled = control_mode.flag_control_attitude_enabled; + flag_control_manual_enabled = control_mode.flag_control_manual_enabled; + flag_control_position_enabled = control_mode.flag_control_position_enabled; + flag_control_velocity_enabled = control_mode.flag_control_velocity_enabled; flag_armed = safety.armed; perf_end(mc_loop_perf); @@ -427,7 +428,7 @@ mc_thread_main(int argc, char *argv[]) close(att_sub); - close(state_sub); + close(control_mode_sub); close(manual_sub); close(actuator_pub); close(att_sp_pub); diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 2674354c32..313fbf6419 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -98,6 +98,9 @@ ORB_DEFINE(rc_channels, struct rc_channels_s); #include "topics/vehicle_command.h" ORB_DEFINE(vehicle_command, struct vehicle_command_s); +#include "topics/vehicle_control_mode.h" +ORB_DEFINE(vehicle_control_mode, struct vehicle_control_mode_s); + #include "topics/vehicle_local_position_setpoint.h" ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoint_s); diff --git a/src/modules/uORB/topics/actuator_safety.h b/src/modules/uORB/topics/actuator_safety.h index b78eb4b7e8..3a107d41aa 100644 --- a/src/modules/uORB/topics/actuator_safety.h +++ b/src/modules/uORB/topics/actuator_safety.h @@ -58,6 +58,7 @@ struct actuator_safety_s { bool armed; /**< Set to true if system is armed */ bool ready_to_arm; /**< Set to true if system is ready to be armed */ bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ + bool hil_enabled; /**< Set to true if hardware-in-the-loop (HIL) is enabled */ }; ORB_DECLARE(actuator_safety); diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index eb35d08840..177e30898b 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -61,16 +61,11 @@ */ struct vehicle_control_mode_s { - /* use of a counter and timestamp recommended (but not necessary) */ - uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ - bool flag_hil_enabled; /**< true if hardware in the loop simulation is enabled */ - bool flag_armed; /**< true is motors / actuators are armed */ - bool flag_safety_off; /**< true if safety is off */ bool flag_system_emergency; bool flag_preflight_calibration; @@ -83,7 +78,7 @@ struct vehicle_control_mode_s bool flag_control_position_enabled; /**< true if position is controlled */ bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ - bool failsave_highlevel; + bool failsave_highlevel; /**< Set to true if high-level failsafe mode is enabled */ }; /** @@ -91,6 +86,6 @@ struct vehicle_control_mode_s */ /* register this as object request broker structure */ -ORB_DECLARE(vehicle_status); +ORB_DECLARE(vehicle_control_mode); #endif diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index b19075921f..2bcd57f4bb 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -199,18 +199,18 @@ struct vehicle_status_s bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ bool flag_hil_enabled; /**< true if hardware in the loop simulation is enabled */ - bool flag_armed; /**< true is motors / actuators are armed */ - bool flag_safety_off; /**< true if safety is off */ + //bool flag_armed; /**< true is motors / actuators are armed */ + //bool flag_safety_off; /**< true if safety is off */ bool flag_system_emergency; bool flag_preflight_calibration; - bool flag_control_manual_enabled; /**< true if manual input is mixed in */ - bool flag_control_offboard_enabled; /**< true if offboard control input is on */ - bool flag_auto_enabled; - bool flag_control_rates_enabled; /**< true if rates are stabilized */ - bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ - bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ - bool flag_control_position_enabled; /**< true if position is controlled */ + // bool flag_control_manual_enabled; /**< true if manual input is mixed in */ + // bool flag_control_offboard_enabled; /**< true if offboard control input is on */ + // bool flag_auto_enabled; + // bool flag_control_rates_enabled; /**< true if rates are stabilized */ + // bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ + // bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ + // bool flag_control_position_enabled; /**< true if position is controlled */ // bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */ // bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */ From 2b9fa731efd08a01effd87a636ae8e53994944f7 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 12 Mar 2013 12:13:02 -0700 Subject: [PATCH 140/486] Use the pid library in the rate controller and change de implementation of the D part Conflicts: src/modules/multirotor_att_control/multirotor_rate_control.c src/modules/systemlib/pid/pid.c src/modules/systemlib/pid/pid.h --- .../multirotor_attitude_control.c | 47 ++----------------- .../multirotor_rate_control.c | 35 +++++++++----- src/modules/systemlib/pid/pid.c | 26 ++++++---- src/modules/systemlib/pid/pid.h | 3 +- 4 files changed, 45 insertions(+), 66 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index 76dbb36d34..1053ac7a31 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -57,50 +57,29 @@ PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 0.3f); PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.15f); PARAM_DEFINE_FLOAT(MC_YAWPOS_D, 0.0f); -//PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f); -//PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f); PARAM_DEFINE_FLOAT(MC_ATT_P, 0.2f); PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f); PARAM_DEFINE_FLOAT(MC_ATT_D, 0.05f); -//PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.05f); -//PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.4f); - -//PARAM_DEFINE_FLOAT(MC_ATT_XOFF, 0.0f); -//PARAM_DEFINE_FLOAT(MC_ATT_YOFF, 0.0f); struct mc_att_control_params { float yaw_p; float yaw_i; float yaw_d; - //float yaw_awu; - //float yaw_lim; float att_p; float att_i; float att_d; - //float att_awu; - //float att_lim; - - //float att_xoff; - //float att_yoff; }; struct mc_att_control_param_handles { param_t yaw_p; param_t yaw_i; param_t yaw_d; - //param_t yaw_awu; - //param_t yaw_lim; param_t att_p; param_t att_i; param_t att_d; - //param_t att_awu; - //param_t att_lim; - - //param_t att_xoff; - //param_t att_yoff; }; /** @@ -122,17 +101,10 @@ static int parameters_init(struct mc_att_control_param_handles *h) h->yaw_p = param_find("MC_YAWPOS_P"); h->yaw_i = param_find("MC_YAWPOS_I"); h->yaw_d = param_find("MC_YAWPOS_D"); - //h->yaw_awu = param_find("MC_YAWPOS_AWU"); - //h->yaw_lim = param_find("MC_YAWPOS_LIM"); h->att_p = param_find("MC_ATT_P"); h->att_i = param_find("MC_ATT_I"); h->att_d = param_find("MC_ATT_D"); - //h->att_awu = param_find("MC_ATT_AWU"); - //h->att_lim = param_find("MC_ATT_LIM"); - - //h->att_xoff = param_find("MC_ATT_XOFF"); - //h->att_yoff = param_find("MC_ATT_YOFF"); return OK; } @@ -142,17 +114,10 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc param_get(h->yaw_p, &(p->yaw_p)); param_get(h->yaw_i, &(p->yaw_i)); param_get(h->yaw_d, &(p->yaw_d)); - //param_get(h->yaw_awu, &(p->yaw_awu)); - //param_get(h->yaw_lim, &(p->yaw_lim)); param_get(h->att_p, &(p->att_p)); param_get(h->att_i, &(p->att_i)); param_get(h->att_d, &(p->att_d)); - //param_get(h->att_awu, &(p->att_awu)); - //param_get(h->att_lim, &(p->att_lim)); - - //param_get(h->att_xoff, &(p->att_xoff)); - //param_get(h->att_yoff, &(p->att_yoff)); return OK; } @@ -170,8 +135,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s last_input = att_sp->timestamp; } - static int sensor_delay; - sensor_delay = hrt_absolute_time() - att->timestamp; +// static int sensor_delay; +// sensor_delay = hrt_absolute_time() - att->timestamp; static int motor_skip_counter = 0; @@ -190,10 +155,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_init(&h); parameters_update(&h, &p); - pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, - 1000.0f, PID_MODE_DERIVATIV_SET); - pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, - 1000.0f, PID_MODE_DERIVATIV_SET); + pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET); + pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET); initialized = true; } @@ -205,7 +168,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s /* apply parameters */ pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); - pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); + pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); } /* reset integral if on ground */ diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index deba1ac037..4ab92b9557 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -150,13 +150,11 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, const float rates[], struct actuator_controls_s *actuators) { - static float roll_control_last = 0; - static float pitch_control_last = 0; static uint64_t last_run = 0; const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; static uint64_t last_input = 0; - float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f; +// float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f; if (last_input != rate_sp->timestamp) { last_input = rate_sp->timestamp; @@ -166,9 +164,15 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, static int motor_skip_counter = 0; + static PID_t pitch_rate_controller; + static PID_t roll_rate_controller; + static struct mc_rate_control_params p; static struct mc_rate_control_param_handles h; + float pitch_control_last = 0.0f; + float roll_control_last = 0.0f; + static bool initialized = false; /* initialize the pid controllers when the function is called for the first time */ @@ -176,39 +180,44 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, parameters_init(&h); parameters_update(&h, &p); initialized = true; + + pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, + 1000.0f, PID_MODE_DERIVATIV_CALC); + pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, + 1000.0f, PID_MODE_DERIVATIV_CALC); } /* load new parameters with lower rate */ if (motor_skip_counter % 2500 == 0) { /* update parameters from storage */ parameters_update(&h, &p); - // warnx("rate ctrl: p.yawrate_p: %8.4f, loop: %d Hz, input: %d Hz", - // (double)p.yawrate_p, (int)(1.0f/deltaT), (int)(1.0f/dT_input)); + pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f); + pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f); } - /* calculate current control outputs */ - /* control pitch (forward) output */ - float pitch_control = p.attrate_p * (rate_sp->pitch - rates[1]) - (p.attrate_d * pitch_control_last); + float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch , + rates[1], 0.0f, deltaT); + + /* control roll (left/right) output */ + float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll , + rates[0], 0.0f, deltaT); /* increase resilience to faulty control inputs */ if (isfinite(pitch_control)) { pitch_control_last = pitch_control; } else { - pitch_control = 0.0f; + pitch_control = pitch_control_last; warnx("rej. NaN ctrl pitch"); } - /* control roll (left/right) output */ - float roll_control = p.attrate_p * (rate_sp->roll - rates[0]) - (p.attrate_d * roll_control_last); - /* increase resilience to faulty control inputs */ if (isfinite(roll_control)) { roll_control_last = roll_control; } else { - roll_control = 0.0f; + roll_control = roll_control_last; warnx("rej. NaN ctrl roll"); } diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 49315cdc9f..08e0ca46fa 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -51,13 +51,14 @@ __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, pid->intmax = intmax; pid->limit = limit; pid->mode = mode; - pid->count = 0; - pid->saturated = 0; - pid->last_output = 0; + pid->count = 0.0f; + pid->saturated = 0.0f; + pid->last_output = 0.0f; - pid->sp = 0; - pid->error_previous = 0; - pid->integral = 0; + pid->sp = 0.0f; + pid->error_previous_filtered = 0.0f; + pid->control_previous = 0.0f; + pid->integral = 0.0f; } __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit) { @@ -136,15 +137,15 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo // Calculated current error value float error = pid->sp - val; - if (isfinite(error)) { // Why is this necessary? DEW - pid->error_previous = error; - } + float error_filtered = 0.2f*error + 0.8f*pid->error_previous_filtered; // Calculate or measured current error derivative if (pid->mode == PID_MODE_DERIVATIV_CALC) { - d = (error - pid->error_previous) / dt; +// d = (error_filtered - pid->error_previous_filtered) / dt; + d = pid->error_previous_filtered - error_filtered; + pid->error_previous_filtered = error_filtered; } else if (pid->mode == PID_MODE_DERIVATIV_SET) { d = -val_dot; @@ -180,6 +181,11 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo pid->last_output = output; } + pid->control_previous = pid->last_output; + + // if (isfinite(error)) { // Why is this necessary? DEW + // pid->error_previous = error; + // } return pid->last_output; } diff --git a/src/modules/systemlib/pid/pid.h b/src/modules/systemlib/pid/pid.h index 64d6688677..5790459c81 100644 --- a/src/modules/systemlib/pid/pid.h +++ b/src/modules/systemlib/pid/pid.h @@ -59,7 +59,8 @@ typedef struct { float intmax; float sp; float integral; - float error_previous; + float error_previous_filtered; + float control_previous; float last_output; float limit; uint8_t mode; From 8559315f4f8b6515f2ba9ceadf2aa3b73af41bdc Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 13 Mar 2013 10:53:20 -0700 Subject: [PATCH 141/486] Added a filter parameter to the pid function Conflicts: apps/multirotor_pos_control/multirotor_pos_control.c --- .../fixedwing_att_control_att.c | 8 ++++---- .../fixedwing_att_control_rate.c | 12 ++++++------ .../fixedwing_pos_control_main.c | 16 ++++++++-------- .../multirotor_attitude_control.c | 8 ++++---- .../multirotor_rate_control.c | 8 ++++---- src/modules/systemlib/pid/pid.c | 14 +++++++++++--- src/modules/systemlib/pid/pid.h | 5 +++-- 7 files changed, 40 insertions(+), 31 deletions(-) diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c index 769b8b0a8d..a226757e0c 100644 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c +++ b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c @@ -128,8 +128,8 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att if (!initialized) { parameters_init(&h); parameters_update(&h, &p); - pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, PID_MODE_DERIVATIV_NONE); //P Controller - pid_init(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, PID_MODE_DERIVATIV_NONE); //P Controller + pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, 0, PID_MODE_DERIVATIV_NONE); //P Controller + pid_init(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, 0, PID_MODE_DERIVATIV_NONE); //P Controller initialized = true; } @@ -137,8 +137,8 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att if (counter % 100 == 0) { /* update parameters from storage */ parameters_update(&h, &p); - pid_set_parameters(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim); - pid_set_parameters(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim); + pid_set_parameters(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, 0); + pid_set_parameters(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, 0); } /* Roll (P) */ diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c index 4eccc118cb..79194e5151 100644 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c +++ b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c @@ -179,9 +179,9 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, if (!initialized) { parameters_init(&h); parameters_update(&h, &p); - pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the controller layout is with a PI rate controller - pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher - pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher + pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, 0, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the controller layout is with a PI rate controller + pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1, 0, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher + pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1, 0, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher initialized = true; } @@ -189,9 +189,9 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, if (counter % 100 == 0) { /* update parameters from storage */ parameters_update(&h, &p); - pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1); - pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1); - pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1); + pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, 0); + pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1, 0); + pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1, 0); } diff --git a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c index 71c78f5b86..48ec3603ff 100644 --- a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c +++ b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c @@ -239,10 +239,10 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) parameters_init(&h); parameters_update(&h, &p); - pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f, 10000.0f, PID_MODE_DERIVATIV_NONE); //arbitrary high limit - pid_init(&heading_rate_controller, p.headingr_p, p.headingr_i, 0.0f, 0.0f, p.roll_lim, PID_MODE_DERIVATIV_NONE); - pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f, p.pitch_lim, PID_MODE_DERIVATIV_NONE); - pid_init(&offtrack_controller, p.xtrack_p, 0.0f, 0.0f, 0.0f , 60.0f * M_DEG_TO_RAD, PID_MODE_DERIVATIV_NONE); //TODO: remove hardcoded value + pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f, 10000.0f, 0.0f, PID_MODE_DERIVATIV_NONE); //arbitrary high limit + pid_init(&heading_rate_controller, p.headingr_p, p.headingr_i, 0.0f, 0.0f, p.roll_lim, 0.0f, PID_MODE_DERIVATIV_NONE); + pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f, p.pitch_lim, 0.0f, PID_MODE_DERIVATIV_NONE); + pid_init(&offtrack_controller, p.xtrack_p, 0.0f, 0.0f, 0.0f , 60.0f * M_DEG_TO_RAD_F, 0.0f, PID_MODE_DERIVATIV_NONE); //TODO: remove hardcoded value /* error and performance monitoring */ perf_counter_t fw_interval_perf = perf_alloc(PC_INTERVAL, "fixedwing_pos_control_interval"); @@ -268,10 +268,10 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) /* update parameters from storage */ parameters_update(&h, &p); - pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, 10000.0f); //arbitrary high limit - pid_set_parameters(&heading_rate_controller, p.headingr_p, p.headingr_i, 0, 0, p.roll_lim); - pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim); - pid_set_parameters(&offtrack_controller, p.xtrack_p, 0, 0, 0, 60.0f * M_DEG_TO_RAD); //TODO: remove hardcoded value + pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, 10000.0f, 0.0f); //arbitrary high limit + pid_set_parameters(&heading_rate_controller, p.headingr_p, p.headingr_i, 0, 0, p.roll_lim, 0.0f); + pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim, 0.0f); + pid_set_parameters(&offtrack_controller, p.xtrack_p, 0, 0, 0, 60.0f * M_DEG_TO_RAD_F, 0.0f); //TODO: remove hardcoded value } /* only run controller if attitude changed */ diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index 1053ac7a31..d7e1a3adcb 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -155,8 +155,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_init(&h); parameters_update(&h, &p); - pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET); - pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET); + pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f, PID_MODE_DERIVATIV_SET); + pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f, PID_MODE_DERIVATIV_SET); initialized = true; } @@ -167,8 +167,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_update(&h, &p); /* apply parameters */ - pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); - pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); + pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f); + pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f); } /* reset integral if on ground */ diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index 4ab92b9557..57aea726aa 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -182,17 +182,17 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, initialized = true; pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, - 1000.0f, PID_MODE_DERIVATIV_CALC); + 1000.0f, 0.2f, PID_MODE_DERIVATIV_CALC); pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, - 1000.0f, PID_MODE_DERIVATIV_CALC); + 1000.0f, 0.2f, PID_MODE_DERIVATIV_CALC); } /* load new parameters with lower rate */ if (motor_skip_counter % 2500 == 0) { /* update parameters from storage */ parameters_update(&h, &p); - pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f); - pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f); + pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f, 0.2f); + pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f, 0.2f); } /* control pitch (forward) output */ diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 08e0ca46fa..d0e67d3ea3 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -43,7 +43,7 @@ #include __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, - float limit, uint8_t mode) + float limit, float diff_filter_factor, uint8_t mode) { pid->kp = kp; pid->ki = ki; @@ -51,6 +51,7 @@ __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, pid->intmax = intmax; pid->limit = limit; pid->mode = mode; + pid->diff_filter_factor = diff_filter_factor; pid->count = 0.0f; pid->saturated = 0.0f; pid->last_output = 0.0f; @@ -60,7 +61,7 @@ __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, pid->control_previous = 0.0f; pid->integral = 0.0f; } -__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit) +__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, float diff_filter_factor) { int ret = 0; @@ -99,6 +100,13 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float ret = 1; } + if (isfinite(diff_filter_factor)) { + pid->limit = diff_filter_factor; + + } else { + ret = 1; + } + return ret; } @@ -137,7 +145,7 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo // Calculated current error value float error = pid->sp - val; - float error_filtered = 0.2f*error + 0.8f*pid->error_previous_filtered; + float error_filtered = pid->diff_filter_factor*error + (1.0f-pid->diff_filter_factor)*pid->error_previous_filtered; // Calculate or measured current error derivative diff --git a/src/modules/systemlib/pid/pid.h b/src/modules/systemlib/pid/pid.h index 5790459c81..f89c36d759 100644 --- a/src/modules/systemlib/pid/pid.h +++ b/src/modules/systemlib/pid/pid.h @@ -64,12 +64,13 @@ typedef struct { float last_output; float limit; uint8_t mode; + float diff_filter_factor; uint8_t count; uint8_t saturated; } PID_t; -__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, uint8_t mode); -__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit); +__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, float diff_filter_factor, uint8_t mode); +__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, float diff_filter_factor); //void pid_set(PID_t *pid, float sp); __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt); From 263b60c200b80af68fea7a491cb17e9db4f65135 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Jun 2013 09:54:57 +0200 Subject: [PATCH 142/486] Hack to make flow controll to compile --- .../flow_position_control/flow_position_control_main.c | 4 +++- .../flow_position_estimator/flow_position_estimator_main.c | 4 ++++ src/examples/flow_speed_control/flow_speed_control_main.c | 2 ++ 3 files changed, 9 insertions(+), 1 deletion(-) diff --git a/src/examples/flow_position_control/flow_position_control_main.c b/src/examples/flow_position_control/flow_position_control_main.c index c177c8fd20..5246ea62b6 100644 --- a/src/examples/flow_position_control/flow_position_control_main.c +++ b/src/examples/flow_position_control/flow_position_control_main.c @@ -268,6 +268,8 @@ flow_position_control_thread_main(int argc, char *argv[]) /* get a local copy of local position */ orb_copy(ORB_ID(vehicle_local_position), vehicle_local_position_sub, &local_pos); +// XXX fix this +#if 0 if (vstatus.state_machine == SYSTEM_STATE_AUTO) { float manual_pitch = manual.pitch / params.rc_scale_pitch; // 0 to 1 @@ -527,7 +529,7 @@ flow_position_control_thread_main(int argc, char *argv[]) if(isfinite(manual.throttle)) speed_sp.thrust_sp = manual.throttle; } - +#endif /* measure in what intervals the controller runs */ perf_count(mc_interval_perf); perf_end(mc_loop_perf); diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c index c0b16d2e78..2389c693d0 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_main.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c @@ -218,6 +218,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) while (!thread_should_exit) { +// XXX fix this +#if 0 if (sensors_ready) { /*This runs at the rate of the sensors */ @@ -273,6 +275,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) * -> accept sonar measurements after reaching calibration distance (values between 0.3m and 1.0m for some time) * -> minimum sonar value 0.3m */ + if (!vehicle_liftoff) { if (vstatus.flag_system_armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f) @@ -437,6 +440,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) } counter++; + #endif } printf("[flow position estimator] exiting.\n"); diff --git a/src/examples/flow_speed_control/flow_speed_control_main.c b/src/examples/flow_speed_control/flow_speed_control_main.c index 9648728c81..0be4b3d5a0 100644 --- a/src/examples/flow_speed_control/flow_speed_control_main.c +++ b/src/examples/flow_speed_control/flow_speed_control_main.c @@ -186,6 +186,7 @@ flow_speed_control_thread_main(int argc, char *argv[]) while (!thread_should_exit) { +#if 0 /* wait for first attitude msg to be sure all data are available */ if (sensors_ready) { @@ -340,6 +341,7 @@ flow_speed_control_thread_main(int argc, char *argv[]) } } } +#endif } printf("[flow speed control] ending now...\n"); From 68fb200f0bc7e995fd604ee9e345f37839d02ced Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Jun 2013 09:55:28 +0200 Subject: [PATCH 143/486] Fixed pid bug, attitude was not controlled --- .../multirotor_attitude_control.c | 10 ++++++---- src/modules/systemlib/pid/pid.c | 5 ++--- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index d7e1a3adcb..4c8f72b8d5 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -155,8 +155,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_init(&h); parameters_update(&h, &p); - pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f, PID_MODE_DERIVATIV_SET); - pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f, PID_MODE_DERIVATIV_SET); + pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f, PID_MODE_DERIVATIV_SET); + pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f, PID_MODE_DERIVATIV_SET); initialized = true; } @@ -167,8 +167,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_update(&h, &p); /* apply parameters */ - pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f); - pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f); + pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f); + pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f); } /* reset integral if on ground */ @@ -188,6 +188,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body , att->roll, att->rollspeed, deltaT); + // printf("rates_sp: %4.4f, att setpoint: %4.4f\n, pitch: %4.4f, pitchspeed: %4.4f, dT: %4.4f", rates_sp->pitch, att_sp->pitch_body, att->pitch, att->pitchspeed, deltaT); + if (control_yaw_position) { /* control yaw rate */ diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index d0e67d3ea3..0188a51c4f 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -101,7 +101,7 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float } if (isfinite(diff_filter_factor)) { - pid->limit = diff_filter_factor; + pid->diff_filter_factor = diff_filter_factor; } else { ret = 1; @@ -179,8 +179,7 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo } // Calculate the output. Limit output magnitude to pid->limit - float output = (pid->error_previous * pid->kp) + (i * pid->ki) + (d * pid->kd); - + float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd); if (output > pid->limit) output = pid->limit; if (output < -pid->limit) output = -pid->limit; From 1ea9ff36402384d66879216a3bc7509651af0be6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 13 Mar 2013 18:00:00 -0700 Subject: [PATCH 144/486] Added possibility to log pid control values Conflicts: apps/multirotor_pos_control/multirotor_pos_control.c src/drivers/ardrone_interface/ardrone_interface.c --- .../fixedwing_att_control/fixedwing_att_control_att.c | 4 ++-- .../fixedwing_att_control/fixedwing_att_control_rate.c | 6 +++--- .../fixedwing_pos_control/fixedwing_pos_control_main.c | 8 ++++---- .../multirotor_att_control/multirotor_attitude_control.c | 4 ++-- .../multirotor_att_control/multirotor_rate_control.c | 4 ++-- src/modules/systemlib/pid/pid.c | 8 ++++++-- src/modules/systemlib/pid/pid.h | 2 +- 7 files changed, 20 insertions(+), 16 deletions(-) diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c index a226757e0c..7009356b78 100644 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c +++ b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c @@ -142,7 +142,7 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att } /* Roll (P) */ - rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0); + rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0, NULL, NULL, NULL); /* Pitch (P) */ @@ -152,7 +152,7 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att /* set pitch plus feedforward roll compensation */ rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body + pitch_sp_rollcompensation, - att->pitch, 0, 0); + att->pitch, 0, 0, NULL, NULL, NULL); /* Yaw (from coordinated turn constraint or lateral force) */ rates_sp->yaw = (att->rollspeed * rates_sp->roll + 9.81f * sinf(att->roll) * cosf(att->pitch) + speed_body[0] * rates_sp->pitch * sinf(att->roll)) diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c index 79194e5151..991d5ec5d1 100644 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c +++ b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c @@ -196,11 +196,11 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* roll rate (PI) */ - actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT); + actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT, NULL, NULL, NULL); /* pitch rate (PI) */ - actuators->control[1] = -pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT); + actuators->control[1] = -pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT, NULL, NULL, NULL); /* yaw rate (PI) */ - actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT); + actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT, NULL, NULL, NULL); counter++; diff --git a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c index 48ec3603ff..9c19c6786e 100644 --- a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c +++ b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c @@ -319,7 +319,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) // XXX what is xtrack_err.past_end? if (distance_res == OK /*&& !xtrack_err.past_end*/) { - float delta_psi_c = pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f); //p.xtrack_p * xtrack_err.distance + float delta_psi_c = pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f, NULL, NULL, NULL); //p.xtrack_p * xtrack_err.distance float psi_c = psi_track + delta_psi_c; float psi_e = psi_c - att.yaw; @@ -336,7 +336,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) } /* calculate roll setpoint, do this artificially around zero */ - float delta_psi_rate_c = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f); + float delta_psi_rate_c = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f, NULL, NULL, NULL); float psi_rate_track = 0; //=V_gr/r_track , this will be needed for implementation of arc following float psi_rate_c = delta_psi_rate_c + psi_rate_track; @@ -359,7 +359,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) float psi_rate_e_scaled = psi_rate_e * ground_speed / 9.81f; //* V_gr / g - attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT); + attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT, NULL, NULL, NULL); if (verbose) { printf("psi_rate_c %.4f ", (double)psi_rate_c); @@ -379,7 +379,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) if (pos_updated) { //TODO: take care of relative vs. ab. altitude - attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f); + attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f, NULL, NULL, NULL); } diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index 4c8f72b8d5..51faaa8c0d 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -182,11 +182,11 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s /* control pitch (forward) output */ rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body , - att->pitch, att->pitchspeed, deltaT); + att->pitch, att->pitchspeed, deltaT, NULL, NULL, NULL); /* control roll (left/right) output */ rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body , - att->roll, att->rollspeed, deltaT); + att->roll, att->rollspeed, deltaT, NULL, NULL, NULL); // printf("rates_sp: %4.4f, att setpoint: %4.4f\n, pitch: %4.4f, pitchspeed: %4.4f, dT: %4.4f", rates_sp->pitch, att_sp->pitch_body, att->pitch, att->pitchspeed, deltaT); diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index 57aea726aa..322c5485ae 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -197,11 +197,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* control pitch (forward) output */ float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch , - rates[1], 0.0f, deltaT); + rates[1], 0.0f, deltaT, NULL, NULL, NULL); /* control roll (left/right) output */ float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll , - rates[0], 0.0f, deltaT); + rates[0], 0.0f, deltaT, NULL, NULL, NULL); /* increase resilience to faulty control inputs */ if (isfinite(pitch_control)) { diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 0188a51c4f..3685b2aebb 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -124,7 +124,7 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float * @param dt * @return */ -__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt) +__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt, float *ctrl_p, float *ctrl_i, float *ctrl_d) { /* error = setpoint - actual_position integral = integral + (error*dt) @@ -152,7 +152,7 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo if (pid->mode == PID_MODE_DERIVATIV_CALC) { // d = (error_filtered - pid->error_previous_filtered) / dt; - d = pid->error_previous_filtered - error_filtered; + d = error_filtered - pid->error_previous_filtered ; pid->error_previous_filtered = error_filtered; } else if (pid->mode == PID_MODE_DERIVATIV_SET) { d = -val_dot; @@ -194,6 +194,10 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo // pid->error_previous = error; // } + *ctrl_p = (error * pid->kp); + *ctrl_i = (i * pid->ki); + *ctrl_d = (d * pid->kd); + return pid->last_output; } diff --git a/src/modules/systemlib/pid/pid.h b/src/modules/systemlib/pid/pid.h index f89c36d759..e3d9038cdd 100644 --- a/src/modules/systemlib/pid/pid.h +++ b/src/modules/systemlib/pid/pid.h @@ -72,7 +72,7 @@ typedef struct { __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, float diff_filter_factor, uint8_t mode); __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, float diff_filter_factor); //void pid_set(PID_t *pid, float sp); -__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt); +__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt, float *ctrl_p, float *ctrl_i, float *ctrl_d); __EXPORT void pid_reset_integral(PID_t *pid); From 562253c508d1a758207d21e116cbbc194d7e0721 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Jun 2013 11:55:08 +0200 Subject: [PATCH 145/486] Fixed bug that I introduced in sdlog2 --- src/modules/sdlog2/sdlog2.c | 34 +++++++++++++++++++++++----------- 1 file changed, 23 insertions(+), 11 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 41c2f67e56..940f30a46b 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -613,7 +613,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ /* number of messages */ - const ssize_t fdsc = 15; + const ssize_t fdsc = 16; /* Sanity check variable and index */ ssize_t fdsc_count = 0; /* file descriptors to wait for */ @@ -622,9 +622,11 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_status_s buf_status; memset(&buf_status, 0, sizeof(buf_status)); + struct actuator_safety_s buf_safety; + memset(&buf_safety, 0, sizeof(buf_safety)); + /* warning! using union here to save memory, elements should be used separately! */ union { - struct actuator_safety_s safety; struct vehicle_command_s cmd; struct sensor_combined_s sensor; struct vehicle_attitude_s att; @@ -645,9 +647,9 @@ int sdlog2_thread_main(int argc, char *argv[]) memset(&buf, 0, sizeof(buf)); struct { - int safety_sub; int cmd_sub; int status_sub; + int safety_sub; int sensor_sub; int att_sub; int att_sp_sub; @@ -693,7 +695,7 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; - /* --- SAFETY STATUS --- */ + /* --- SAFETY --- */ subs.safety_sub = orb_subscribe(ORB_ID(actuator_safety)); fds[fdsc_count].fd = subs.safety_sub; fds[fdsc_count].events = POLLIN; @@ -835,7 +837,6 @@ int sdlog2_thread_main(int argc, char *argv[]) int ifds = 0; int handled_topics = 0; - /* --- VEHICLE COMMAND - LOG MANAGEMENT --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd); @@ -843,22 +844,33 @@ int sdlog2_thread_main(int argc, char *argv[]) handled_topics++; } + /* --- SAFETY- LOG MANAGEMENT --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(actuator_safety), subs.safety_sub, &buf_safety); + + if (log_when_armed) { + handle_status(&buf_safety); + } + + handled_topics++; + } + /* --- VEHICLE STATUS - LOG MANAGEMENT --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf_status); - if (log_when_armed) { - handle_status(&buf.safety); - } + //if (log_when_armed) { + // handle_status(&buf_safety); + //} - handled_topics++; + //handled_topics++; } if (!logging_enabled || !check_data || handled_topics >= poll_ret) { continue; } - ifds = 1; // Begin from fds[1] again + ifds = 2; // Begin from fds[2] again pthread_mutex_lock(&logbuffer_mutex); @@ -880,7 +892,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_STAT.flight_mode = 0; log_msg.body.log_STAT.manual_control_mode = 0; log_msg.body.log_STAT.manual_sas_mode = 0; - log_msg.body.log_STAT.armed = (unsigned char) buf.safety.armed; /* XXX fmu armed correct? */ + log_msg.body.log_STAT.armed = (unsigned char) buf_safety.armed; /* XXX fmu armed correct? */ log_msg.body.log_STAT.battery_voltage = buf_status.voltage_battery; log_msg.body.log_STAT.battery_current = buf_status.current_battery; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; From b52d561b11298abb2982b786676f49eea96259d8 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Jun 2013 12:59:50 +0200 Subject: [PATCH 146/486] Added ctrl debugging values --- .../multirotor_rate_control.c | 16 +++- src/modules/sdlog2/sdlog2.c | 36 +++++++- src/modules/sdlog2/sdlog2_messages.h | 33 ++++++- src/modules/uORB/objects_common.cpp | 3 + .../uORB/topics/vehicle_control_debug.h | 87 +++++++++++++++++++ 5 files changed, 170 insertions(+), 5 deletions(-) create mode 100644 src/modules/uORB/topics/vehicle_control_debug.h diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index 322c5485ae..01bf383e27 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -54,6 +54,9 @@ #include #include #include +#include +#include + PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.0f); /* same on Flamewheel */ PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); @@ -175,6 +178,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, static bool initialized = false; + static struct vehicle_control_debug_s control_debug; + static orb_advert_t control_debug_pub; + + + /* initialize the pid controllers when the function is called for the first time */ if (initialized == false) { parameters_init(&h); @@ -185,6 +193,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, 1000.0f, 0.2f, PID_MODE_DERIVATIV_CALC); pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f, 0.2f, PID_MODE_DERIVATIV_CALC); + + control_debug_pub = orb_advertise(ORB_ID(vehicle_control_debug), &control_debug); } /* load new parameters with lower rate */ @@ -197,11 +207,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* control pitch (forward) output */ float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch , - rates[1], 0.0f, deltaT, NULL, NULL, NULL); + rates[1], 0.0f, deltaT, &control_debug.pitch_rate_p, &control_debug.pitch_rate_i, &control_debug.pitch_rate_d); /* control roll (left/right) output */ float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll , - rates[0], 0.0f, deltaT, NULL, NULL, NULL); + rates[0], 0.0f, deltaT, &control_debug.roll_rate_p, &control_debug.roll_rate_i, &control_debug.roll_rate_d); /* increase resilience to faulty control inputs */ if (isfinite(pitch_control)) { @@ -236,4 +246,6 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, actuators->control[3] = rate_sp->thrust; motor_skip_counter++; + + orb_publish(ORB_ID(vehicle_control_debug), control_debug_pub, &control_debug); } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 940f30a46b..844e02268b 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -74,6 +74,7 @@ #include #include #include +#include #include #include #include @@ -613,7 +614,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ /* number of messages */ - const ssize_t fdsc = 16; + const ssize_t fdsc = 17; /* Sanity check variable and index */ ssize_t fdsc_count = 0; /* file descriptors to wait for */ @@ -639,6 +640,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_global_position_s global_pos; struct vehicle_gps_position_s gps_pos; struct vehicle_vicon_position_s vicon_pos; + struct vehicle_control_debug_s control_debug; struct optical_flow_s flow; struct rc_channels_s rc; struct differential_pressure_s diff_pres; @@ -661,6 +663,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int global_pos_sub; int gps_pos_sub; int vicon_pos_sub; + int control_debug_sub; int flow_sub; int rc_sub; } subs; @@ -680,6 +683,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GPS_s log_GPS; struct log_ATTC_s log_ATTC; struct log_STAT_s log_STAT; + struct log_CTRL_s log_CTRL; struct log_RC_s log_RC; struct log_OUT0_s log_OUT0; } body; @@ -773,6 +777,12 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- CONTROL DEBUG --- */ + subs.control_debug_sub = orb_subscribe(ORB_ID(vehicle_control_debug)); + fds[fdsc_count].fd = subs.control_debug_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* --- OPTICAL FLOW --- */ subs.flow_sub = orb_subscribe(ORB_ID(optical_flow)); fds[fdsc_count].fd = subs.flow_sub; @@ -1058,6 +1068,30 @@ int sdlog2_thread_main(int argc, char *argv[]) // TODO not implemented yet } + /* --- CONTROL DEBUG --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_control_debug), subs.control_debug_sub, &buf.control_debug); + + log_msg.body.log_CTRL.roll_p = buf.control_debug.roll_p; + log_msg.body.log_CTRL.roll_i = buf.control_debug.roll_i; + log_msg.body.log_CTRL.roll_d = buf.control_debug.roll_d; + log_msg.body.log_CTRL.roll_rate_p = buf.control_debug.roll_rate_p; + log_msg.body.log_CTRL.roll_rate_i = buf.control_debug.roll_rate_i; + log_msg.body.log_CTRL.roll_rate_d = buf.control_debug.roll_rate_d; + log_msg.body.log_CTRL.pitch_p = buf.control_debug.pitch_p; + log_msg.body.log_CTRL.pitch_i = buf.control_debug.pitch_i; + log_msg.body.log_CTRL.pitch_d = buf.control_debug.pitch_d; + log_msg.body.log_CTRL.pitch_rate_p = buf.control_debug.pitch_rate_p; + log_msg.body.log_CTRL.pitch_rate_i = buf.control_debug.pitch_rate_i; + log_msg.body.log_CTRL.pitch_rate_d = buf.control_debug.pitch_rate_d; + log_msg.body.log_CTRL.yaw_p = buf.control_debug.yaw_p; + log_msg.body.log_CTRL.yaw_i = buf.control_debug.yaw_i; + log_msg.body.log_CTRL.yaw_d = buf.control_debug.yaw_d; + log_msg.body.log_CTRL.yaw_rate_p = buf.control_debug.yaw_rate_p; + log_msg.body.log_CTRL.yaw_rate_i = buf.control_debug.yaw_rate_i; + log_msg.body.log_CTRL.yaw_rate_d = buf.control_debug.yaw_rate_d; + } + /* --- FLOW --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow); diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 40763ee1e0..a80af33eff 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -156,14 +156,42 @@ struct log_STAT_s { unsigned char battery_warning; }; +/* --- CTRL - CONTROL DEBUG --- */ +#define LOG_CTRL_MSG 11 +struct log_CTRL_s { + float roll_p; + float roll_i; + float roll_d; + + float roll_rate_p; + float roll_rate_i; + float roll_rate_d; + + float pitch_p; + float pitch_i; + float pitch_d; + + float pitch_rate_p; + float pitch_rate_i; + float pitch_rate_d; + + float yaw_p; + float yaw_i; + float yaw_d; + + float yaw_rate_p; + float yaw_rate_i; + float yaw_rate_d; +}; + /* --- RC - RC INPUT CHANNELS --- */ -#define LOG_RC_MSG 11 +#define LOG_RC_MSG 12 struct log_RC_s { float channel[8]; }; /* --- OUT0 - ACTUATOR_0 OUTPUT --- */ -#define LOG_OUT0_MSG 12 +#define LOG_OUT0_MSG 13 struct log_OUT0_s { float output[8]; }; @@ -182,6 +210,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT(STAT, "BBBBBfffB", "State,FlightMode,CtlMode,SASMode,Armed,BatV,BatC,BatRem,BatWarn"), + LOG_FORMAT(CTRL, "ffffffffffffffffff", "RollP,RollI,RollD,RollRateP,RollRateI,RollRateD,PitchP,PitchI,PitchD,PitchRateP,PitchRateI,PitchRateD,YawP,YawI,YawD,YawRateP,YawRateI,YawRateD"), LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), }; diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 7dcb9cf93c..f1f07441d5 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -119,6 +119,9 @@ ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s); #include "topics/manual_control_setpoint.h" ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s); +#include "topics/vehicle_control_debug.h" +ORB_DEFINE(vehicle_control_debug, struct vehicle_control_debug_s); + #include "topics/offboard_control_setpoint.h" ORB_DEFINE(offboard_control_setpoint, struct offboard_control_setpoint_s); diff --git a/src/modules/uORB/topics/vehicle_control_debug.h b/src/modules/uORB/topics/vehicle_control_debug.h new file mode 100644 index 0000000000..6184284a43 --- /dev/null +++ b/src/modules/uORB/topics/vehicle_control_debug.h @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier + * + * 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 vehicle_control_debug.h + * For debugging purposes to log PID parts of controller + */ + +#ifndef TOPIC_VEHICLE_CONTROL_DEBUG_H_ +#define TOPIC_VEHICLE_CONTROL_DEBUG_H_ + +#include +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ +struct vehicle_control_debug_s +{ + uint64_t timestamp; /**< in microseconds since system start */ + + float roll_p; /**< roll P control part */ + float roll_i; /**< roll I control part */ + float roll_d; /**< roll D control part */ + + float roll_rate_p; /**< roll rate P control part */ + float roll_rate_i; /**< roll rate I control part */ + float roll_rate_d; /**< roll rate D control part */ + + float pitch_p; /**< pitch P control part */ + float pitch_i; /**< pitch I control part */ + float pitch_d; /**< pitch D control part */ + + float pitch_rate_p; /**< pitch rate P control part */ + float pitch_rate_i; /**< pitch rate I control part */ + float pitch_rate_d; /**< pitch rate D control part */ + + float yaw_p; /**< yaw P control part */ + float yaw_i; /**< yaw I control part */ + float yaw_d; /**< yaw D control part */ + + float yaw_rate_p; /**< yaw rate P control part */ + float yaw_rate_i; /**< yaw rate I control part */ + float yaw_rate_d; /**< yaw rate D control part */ + +}; /**< vehicle_control_debug */ + + /** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_control_debug); + +#endif From bd7f86bb6adf768169fe23b63d627a252586f6b7 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Jun 2013 14:59:00 +0200 Subject: [PATCH 147/486] Tried to add ctrl debug values to sdlog2 (WIP) --- .../multirotor_rate_control.c | 1 + src/modules/sdlog2/sdlog2.c | 24 +++++++++++-------- src/modules/sdlog2/sdlog2_messages.h | 20 ++++++++-------- 3 files changed, 25 insertions(+), 20 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index 01bf383e27..e8dcbacc7c 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -248,4 +248,5 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, motor_skip_counter++; orb_publish(ORB_ID(vehicle_control_debug), control_debug_pub, &control_debug); + printf("Published control debug\n"); } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 844e02268b..a86623304b 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -91,6 +91,7 @@ #define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \ log_msgs_written++; \ + printf("size: %d\n", LOG_PACKET_SIZE(_msg)); \ } else { \ log_msgs_skipped++; \ /*printf("skip\n");*/ \ @@ -1071,25 +1072,28 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- CONTROL DEBUG --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_control_debug), subs.control_debug_sub, &buf.control_debug); - - log_msg.body.log_CTRL.roll_p = buf.control_debug.roll_p; - log_msg.body.log_CTRL.roll_i = buf.control_debug.roll_i; - log_msg.body.log_CTRL.roll_d = buf.control_debug.roll_d; + printf("copied control debug\n"); + + // log_msg.body.log_CTRL.roll_p = buf.control_debug.roll_p; + // log_msg.body.log_CTRL.roll_i = buf.control_debug.roll_i; + // log_msg.body.log_CTRL.roll_d = buf.control_debug.roll_d; log_msg.body.log_CTRL.roll_rate_p = buf.control_debug.roll_rate_p; log_msg.body.log_CTRL.roll_rate_i = buf.control_debug.roll_rate_i; log_msg.body.log_CTRL.roll_rate_d = buf.control_debug.roll_rate_d; - log_msg.body.log_CTRL.pitch_p = buf.control_debug.pitch_p; - log_msg.body.log_CTRL.pitch_i = buf.control_debug.pitch_i; - log_msg.body.log_CTRL.pitch_d = buf.control_debug.pitch_d; + // log_msg.body.log_CTRL.pitch_p = buf.control_debug.pitch_p; + // log_msg.body.log_CTRL.pitch_i = buf.control_debug.pitch_i; + // log_msg.body.log_CTRL.pitch_d = buf.control_debug.pitch_d; log_msg.body.log_CTRL.pitch_rate_p = buf.control_debug.pitch_rate_p; log_msg.body.log_CTRL.pitch_rate_i = buf.control_debug.pitch_rate_i; log_msg.body.log_CTRL.pitch_rate_d = buf.control_debug.pitch_rate_d; - log_msg.body.log_CTRL.yaw_p = buf.control_debug.yaw_p; - log_msg.body.log_CTRL.yaw_i = buf.control_debug.yaw_i; - log_msg.body.log_CTRL.yaw_d = buf.control_debug.yaw_d; + // log_msg.body.log_CTRL.yaw_p = buf.control_debug.yaw_p; + // log_msg.body.log_CTRL.yaw_i = buf.control_debug.yaw_i; + // log_msg.body.log_CTRL.yaw_d = buf.control_debug.yaw_d; log_msg.body.log_CTRL.yaw_rate_p = buf.control_debug.yaw_rate_p; log_msg.body.log_CTRL.yaw_rate_i = buf.control_debug.yaw_rate_i; log_msg.body.log_CTRL.yaw_rate_d = buf.control_debug.yaw_rate_d; + + LOGBUFFER_WRITE_AND_COUNT(CTRL); } /* --- FLOW --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index a80af33eff..8930db33be 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -159,25 +159,25 @@ struct log_STAT_s { /* --- CTRL - CONTROL DEBUG --- */ #define LOG_CTRL_MSG 11 struct log_CTRL_s { - float roll_p; - float roll_i; - float roll_d; + // float roll_p; + // float roll_i; + // float roll_d; float roll_rate_p; float roll_rate_i; float roll_rate_d; - float pitch_p; - float pitch_i; - float pitch_d; + // float pitch_p; + // float pitch_i; + // float pitch_d; float pitch_rate_p; float pitch_rate_i; float pitch_rate_d; - float yaw_p; - float yaw_i; - float yaw_d; + // float yaw_p; + // float yaw_i; + // float yaw_d; float yaw_rate_p; float yaw_rate_i; @@ -210,7 +210,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT(STAT, "BBBBBfffB", "State,FlightMode,CtlMode,SASMode,Armed,BatV,BatC,BatRem,BatWarn"), - LOG_FORMAT(CTRL, "ffffffffffffffffff", "RollP,RollI,RollD,RollRateP,RollRateI,RollRateD,PitchP,PitchI,PitchD,PitchRateP,PitchRateI,PitchRateD,YawP,YawI,YawD,YawRateP,YawRateI,YawRateD"), + LOG_FORMAT(CTRL, "fffffffff", "RollRP,RollRI,RollRD,PitchRP,PitchRI,PitchRD,YawRP,YawRI,YawRD"), LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), }; From 303694f5f7b7a03488c6075ff2bd67014badfacb Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Jun 2013 09:55:28 +0200 Subject: [PATCH 148/486] Fixed pid bug, attitude was not controlled --- .../multirotor_attitude_control.c | 10 ++++++---- src/modules/systemlib/pid/pid.c | 5 ++--- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index d7e1a3adcb..4c8f72b8d5 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -155,8 +155,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_init(&h); parameters_update(&h, &p); - pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f, PID_MODE_DERIVATIV_SET); - pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f, PID_MODE_DERIVATIV_SET); + pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f, PID_MODE_DERIVATIV_SET); + pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f, PID_MODE_DERIVATIV_SET); initialized = true; } @@ -167,8 +167,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_update(&h, &p); /* apply parameters */ - pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f); - pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f); + pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f); + pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f); } /* reset integral if on ground */ @@ -188,6 +188,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body , att->roll, att->rollspeed, deltaT); + // printf("rates_sp: %4.4f, att setpoint: %4.4f\n, pitch: %4.4f, pitchspeed: %4.4f, dT: %4.4f", rates_sp->pitch, att_sp->pitch_body, att->pitch, att->pitchspeed, deltaT); + if (control_yaw_position) { /* control yaw rate */ diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index d0e67d3ea3..0188a51c4f 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -101,7 +101,7 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float } if (isfinite(diff_filter_factor)) { - pid->limit = diff_filter_factor; + pid->diff_filter_factor = diff_filter_factor; } else { ret = 1; @@ -179,8 +179,7 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo } // Calculate the output. Limit output magnitude to pid->limit - float output = (pid->error_previous * pid->kp) + (i * pid->ki) + (d * pid->kd); - + float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd); if (output > pid->limit) output = pid->limit; if (output < -pid->limit) output = -pid->limit; From c189ac1c85a272142f925339879f22563c092725 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 13 Mar 2013 18:00:00 -0700 Subject: [PATCH 149/486] Added possibility to log pid control values Conflicts: apps/multirotor_pos_control/multirotor_pos_control.c src/drivers/ardrone_interface/ardrone_interface.c --- .../fixedwing_att_control/fixedwing_att_control_att.c | 4 ++-- .../fixedwing_att_control/fixedwing_att_control_rate.c | 6 +++--- .../fixedwing_pos_control/fixedwing_pos_control_main.c | 8 ++++---- .../multirotor_att_control/multirotor_attitude_control.c | 4 ++-- .../multirotor_att_control/multirotor_rate_control.c | 4 ++-- src/modules/systemlib/pid/pid.c | 8 ++++++-- src/modules/systemlib/pid/pid.h | 2 +- 7 files changed, 20 insertions(+), 16 deletions(-) diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c index a226757e0c..7009356b78 100644 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c +++ b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c @@ -142,7 +142,7 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att } /* Roll (P) */ - rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0); + rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0, NULL, NULL, NULL); /* Pitch (P) */ @@ -152,7 +152,7 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att /* set pitch plus feedforward roll compensation */ rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body + pitch_sp_rollcompensation, - att->pitch, 0, 0); + att->pitch, 0, 0, NULL, NULL, NULL); /* Yaw (from coordinated turn constraint or lateral force) */ rates_sp->yaw = (att->rollspeed * rates_sp->roll + 9.81f * sinf(att->roll) * cosf(att->pitch) + speed_body[0] * rates_sp->pitch * sinf(att->roll)) diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c index 79194e5151..991d5ec5d1 100644 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c +++ b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c @@ -196,11 +196,11 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* roll rate (PI) */ - actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT); + actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT, NULL, NULL, NULL); /* pitch rate (PI) */ - actuators->control[1] = -pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT); + actuators->control[1] = -pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT, NULL, NULL, NULL); /* yaw rate (PI) */ - actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT); + actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT, NULL, NULL, NULL); counter++; diff --git a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c index 48ec3603ff..9c19c6786e 100644 --- a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c +++ b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c @@ -319,7 +319,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) // XXX what is xtrack_err.past_end? if (distance_res == OK /*&& !xtrack_err.past_end*/) { - float delta_psi_c = pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f); //p.xtrack_p * xtrack_err.distance + float delta_psi_c = pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f, NULL, NULL, NULL); //p.xtrack_p * xtrack_err.distance float psi_c = psi_track + delta_psi_c; float psi_e = psi_c - att.yaw; @@ -336,7 +336,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) } /* calculate roll setpoint, do this artificially around zero */ - float delta_psi_rate_c = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f); + float delta_psi_rate_c = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f, NULL, NULL, NULL); float psi_rate_track = 0; //=V_gr/r_track , this will be needed for implementation of arc following float psi_rate_c = delta_psi_rate_c + psi_rate_track; @@ -359,7 +359,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) float psi_rate_e_scaled = psi_rate_e * ground_speed / 9.81f; //* V_gr / g - attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT); + attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT, NULL, NULL, NULL); if (verbose) { printf("psi_rate_c %.4f ", (double)psi_rate_c); @@ -379,7 +379,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) if (pos_updated) { //TODO: take care of relative vs. ab. altitude - attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f); + attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f, NULL, NULL, NULL); } diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index 4c8f72b8d5..51faaa8c0d 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -182,11 +182,11 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s /* control pitch (forward) output */ rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body , - att->pitch, att->pitchspeed, deltaT); + att->pitch, att->pitchspeed, deltaT, NULL, NULL, NULL); /* control roll (left/right) output */ rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body , - att->roll, att->rollspeed, deltaT); + att->roll, att->rollspeed, deltaT, NULL, NULL, NULL); // printf("rates_sp: %4.4f, att setpoint: %4.4f\n, pitch: %4.4f, pitchspeed: %4.4f, dT: %4.4f", rates_sp->pitch, att_sp->pitch_body, att->pitch, att->pitchspeed, deltaT); diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index 57aea726aa..322c5485ae 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -197,11 +197,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* control pitch (forward) output */ float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch , - rates[1], 0.0f, deltaT); + rates[1], 0.0f, deltaT, NULL, NULL, NULL); /* control roll (left/right) output */ float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll , - rates[0], 0.0f, deltaT); + rates[0], 0.0f, deltaT, NULL, NULL, NULL); /* increase resilience to faulty control inputs */ if (isfinite(pitch_control)) { diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 0188a51c4f..3685b2aebb 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -124,7 +124,7 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float * @param dt * @return */ -__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt) +__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt, float *ctrl_p, float *ctrl_i, float *ctrl_d) { /* error = setpoint - actual_position integral = integral + (error*dt) @@ -152,7 +152,7 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo if (pid->mode == PID_MODE_DERIVATIV_CALC) { // d = (error_filtered - pid->error_previous_filtered) / dt; - d = pid->error_previous_filtered - error_filtered; + d = error_filtered - pid->error_previous_filtered ; pid->error_previous_filtered = error_filtered; } else if (pid->mode == PID_MODE_DERIVATIV_SET) { d = -val_dot; @@ -194,6 +194,10 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo // pid->error_previous = error; // } + *ctrl_p = (error * pid->kp); + *ctrl_i = (i * pid->ki); + *ctrl_d = (d * pid->kd); + return pid->last_output; } diff --git a/src/modules/systemlib/pid/pid.h b/src/modules/systemlib/pid/pid.h index f89c36d759..e3d9038cdd 100644 --- a/src/modules/systemlib/pid/pid.h +++ b/src/modules/systemlib/pid/pid.h @@ -72,7 +72,7 @@ typedef struct { __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, float diff_filter_factor, uint8_t mode); __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, float diff_filter_factor); //void pid_set(PID_t *pid, float sp); -__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt); +__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt, float *ctrl_p, float *ctrl_i, float *ctrl_d); __EXPORT void pid_reset_integral(PID_t *pid); From 2cb928d87c385335de72bb83710583a288d05cc4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Jun 2013 12:59:50 +0200 Subject: [PATCH 150/486] Added ctrl debugging values Conflicts: src/modules/sdlog2/sdlog2.c --- .../multirotor_rate_control.c | 16 +++- src/modules/sdlog2/sdlog2.c | 36 +++++++- src/modules/sdlog2/sdlog2_messages.h | 28 +++++- src/modules/uORB/objects_common.cpp | 3 + .../uORB/topics/vehicle_control_debug.h | 87 +++++++++++++++++++ 5 files changed, 165 insertions(+), 5 deletions(-) create mode 100644 src/modules/uORB/topics/vehicle_control_debug.h diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index 322c5485ae..01bf383e27 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -54,6 +54,9 @@ #include #include #include +#include +#include + PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.0f); /* same on Flamewheel */ PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); @@ -175,6 +178,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, static bool initialized = false; + static struct vehicle_control_debug_s control_debug; + static orb_advert_t control_debug_pub; + + + /* initialize the pid controllers when the function is called for the first time */ if (initialized == false) { parameters_init(&h); @@ -185,6 +193,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, 1000.0f, 0.2f, PID_MODE_DERIVATIV_CALC); pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f, 0.2f, PID_MODE_DERIVATIV_CALC); + + control_debug_pub = orb_advertise(ORB_ID(vehicle_control_debug), &control_debug); } /* load new parameters with lower rate */ @@ -197,11 +207,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* control pitch (forward) output */ float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch , - rates[1], 0.0f, deltaT, NULL, NULL, NULL); + rates[1], 0.0f, deltaT, &control_debug.pitch_rate_p, &control_debug.pitch_rate_i, &control_debug.pitch_rate_d); /* control roll (left/right) output */ float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll , - rates[0], 0.0f, deltaT, NULL, NULL, NULL); + rates[0], 0.0f, deltaT, &control_debug.roll_rate_p, &control_debug.roll_rate_i, &control_debug.roll_rate_d); /* increase resilience to faulty control inputs */ if (isfinite(pitch_control)) { @@ -236,4 +246,6 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, actuators->control[3] = rate_sp->thrust; motor_skip_counter++; + + orb_publish(ORB_ID(vehicle_control_debug), control_debug_pub, &control_debug); } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index a14bd6f80e..347d5dd20f 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -73,6 +73,7 @@ #include #include #include +#include #include #include #include @@ -612,7 +613,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ /* number of messages */ - const ssize_t fdsc = 15; + const ssize_t fdsc = 16; /* Sanity check variable and index */ ssize_t fdsc_count = 0; /* file descriptors to wait for */ @@ -635,6 +636,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_global_position_s global_pos; struct vehicle_gps_position_s gps_pos; struct vehicle_vicon_position_s vicon_pos; + struct vehicle_control_debug_s control_debug; struct optical_flow_s flow; struct rc_channels_s rc; struct differential_pressure_s diff_pres; @@ -656,6 +658,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int global_pos_sub; int gps_pos_sub; int vicon_pos_sub; + int control_debug_sub; int flow_sub; int rc_sub; } subs; @@ -675,6 +678,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GPS_s log_GPS; struct log_ATTC_s log_ATTC; struct log_STAT_s log_STAT; + struct log_CTRL_s log_CTRL; struct log_RC_s log_RC; struct log_OUT0_s log_OUT0; } body; @@ -762,6 +766,12 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- CONTROL DEBUG --- */ + subs.control_debug_sub = orb_subscribe(ORB_ID(vehicle_control_debug)); + fds[fdsc_count].fd = subs.control_debug_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* --- OPTICAL FLOW --- */ subs.flow_sub = orb_subscribe(ORB_ID(optical_flow)); fds[fdsc_count].fd = subs.flow_sub; @@ -1031,6 +1041,30 @@ int sdlog2_thread_main(int argc, char *argv[]) // TODO not implemented yet } + /* --- CONTROL DEBUG --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_control_debug), subs.control_debug_sub, &buf.control_debug); + + log_msg.body.log_CTRL.roll_p = buf.control_debug.roll_p; + log_msg.body.log_CTRL.roll_i = buf.control_debug.roll_i; + log_msg.body.log_CTRL.roll_d = buf.control_debug.roll_d; + log_msg.body.log_CTRL.roll_rate_p = buf.control_debug.roll_rate_p; + log_msg.body.log_CTRL.roll_rate_i = buf.control_debug.roll_rate_i; + log_msg.body.log_CTRL.roll_rate_d = buf.control_debug.roll_rate_d; + log_msg.body.log_CTRL.pitch_p = buf.control_debug.pitch_p; + log_msg.body.log_CTRL.pitch_i = buf.control_debug.pitch_i; + log_msg.body.log_CTRL.pitch_d = buf.control_debug.pitch_d; + log_msg.body.log_CTRL.pitch_rate_p = buf.control_debug.pitch_rate_p; + log_msg.body.log_CTRL.pitch_rate_i = buf.control_debug.pitch_rate_i; + log_msg.body.log_CTRL.pitch_rate_d = buf.control_debug.pitch_rate_d; + log_msg.body.log_CTRL.yaw_p = buf.control_debug.yaw_p; + log_msg.body.log_CTRL.yaw_i = buf.control_debug.yaw_i; + log_msg.body.log_CTRL.yaw_d = buf.control_debug.yaw_d; + log_msg.body.log_CTRL.yaw_rate_p = buf.control_debug.yaw_rate_p; + log_msg.body.log_CTRL.yaw_rate_i = buf.control_debug.yaw_rate_i; + log_msg.body.log_CTRL.yaw_rate_d = buf.control_debug.yaw_rate_d; + } + /* --- FLOW --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow); diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 40763ee1e0..a3d35b5965 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -156,14 +156,37 @@ struct log_STAT_s { unsigned char battery_warning; }; +/* --- CTRL - CONTROL DEBUG --- */ +#define LOG_CTRL_MSG 11 +struct log_CTRL_s { + float roll_p; + float roll_i; + float roll_d; + float roll_rate_p; + float roll_rate_i; + float roll_rate_d; + float pitch_p; + float pitch_i; + float pitch_d; + float pitch_rate_p; + float pitch_rate_i; + float pitch_rate_d; + float yaw_p; + float yaw_i; + float yaw_d; + float yaw_rate_p; + float yaw_rate_i; + float yaw_rate_d; +}; + /* --- RC - RC INPUT CHANNELS --- */ -#define LOG_RC_MSG 11 +#define LOG_RC_MSG 12 struct log_RC_s { float channel[8]; }; /* --- OUT0 - ACTUATOR_0 OUTPUT --- */ -#define LOG_OUT0_MSG 12 +#define LOG_OUT0_MSG 13 struct log_OUT0_s { float output[8]; }; @@ -182,6 +205,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT(STAT, "BBBBBfffB", "State,FlightMode,CtlMode,SASMode,Armed,BatV,BatC,BatRem,BatWarn"), + LOG_FORMAT(CTRL, "ffffffffffffffffff", "RollP,RollI,RollD,RollRP,RollRI,RollRD,PitchP,PitchI,PitchD,PitchRP,PitchRI,PitchRD,YawP,YawI,YawD,YawRP,YawRI,YawRD"), LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), }; diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index e22b58cad6..ddf9272ecb 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -116,6 +116,9 @@ ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s); #include "topics/manual_control_setpoint.h" ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s); +#include "topics/vehicle_control_debug.h" +ORB_DEFINE(vehicle_control_debug, struct vehicle_control_debug_s); + #include "topics/offboard_control_setpoint.h" ORB_DEFINE(offboard_control_setpoint, struct offboard_control_setpoint_s); diff --git a/src/modules/uORB/topics/vehicle_control_debug.h b/src/modules/uORB/topics/vehicle_control_debug.h new file mode 100644 index 0000000000..6184284a43 --- /dev/null +++ b/src/modules/uORB/topics/vehicle_control_debug.h @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier + * + * 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 vehicle_control_debug.h + * For debugging purposes to log PID parts of controller + */ + +#ifndef TOPIC_VEHICLE_CONTROL_DEBUG_H_ +#define TOPIC_VEHICLE_CONTROL_DEBUG_H_ + +#include +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ +struct vehicle_control_debug_s +{ + uint64_t timestamp; /**< in microseconds since system start */ + + float roll_p; /**< roll P control part */ + float roll_i; /**< roll I control part */ + float roll_d; /**< roll D control part */ + + float roll_rate_p; /**< roll rate P control part */ + float roll_rate_i; /**< roll rate I control part */ + float roll_rate_d; /**< roll rate D control part */ + + float pitch_p; /**< pitch P control part */ + float pitch_i; /**< pitch I control part */ + float pitch_d; /**< pitch D control part */ + + float pitch_rate_p; /**< pitch rate P control part */ + float pitch_rate_i; /**< pitch rate I control part */ + float pitch_rate_d; /**< pitch rate D control part */ + + float yaw_p; /**< yaw P control part */ + float yaw_i; /**< yaw I control part */ + float yaw_d; /**< yaw D control part */ + + float yaw_rate_p; /**< yaw rate P control part */ + float yaw_rate_i; /**< yaw rate I control part */ + float yaw_rate_d; /**< yaw rate D control part */ + +}; /**< vehicle_control_debug */ + + /** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_control_debug); + +#endif From 6f108e18d21b61ea6a3bf137f01023c594085494 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Jun 2013 15:32:53 +0200 Subject: [PATCH 151/486] Just include the rate controls for now --- src/modules/sdlog2/sdlog2.c | 9 --------- src/modules/sdlog2/sdlog2_messages.h | 11 +---------- 2 files changed, 1 insertion(+), 19 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 347d5dd20f..aff0fd4a73 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1045,21 +1045,12 @@ int sdlog2_thread_main(int argc, char *argv[]) if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_control_debug), subs.control_debug_sub, &buf.control_debug); - log_msg.body.log_CTRL.roll_p = buf.control_debug.roll_p; - log_msg.body.log_CTRL.roll_i = buf.control_debug.roll_i; - log_msg.body.log_CTRL.roll_d = buf.control_debug.roll_d; log_msg.body.log_CTRL.roll_rate_p = buf.control_debug.roll_rate_p; log_msg.body.log_CTRL.roll_rate_i = buf.control_debug.roll_rate_i; log_msg.body.log_CTRL.roll_rate_d = buf.control_debug.roll_rate_d; - log_msg.body.log_CTRL.pitch_p = buf.control_debug.pitch_p; - log_msg.body.log_CTRL.pitch_i = buf.control_debug.pitch_i; - log_msg.body.log_CTRL.pitch_d = buf.control_debug.pitch_d; log_msg.body.log_CTRL.pitch_rate_p = buf.control_debug.pitch_rate_p; log_msg.body.log_CTRL.pitch_rate_i = buf.control_debug.pitch_rate_i; log_msg.body.log_CTRL.pitch_rate_d = buf.control_debug.pitch_rate_d; - log_msg.body.log_CTRL.yaw_p = buf.control_debug.yaw_p; - log_msg.body.log_CTRL.yaw_i = buf.control_debug.yaw_i; - log_msg.body.log_CTRL.yaw_d = buf.control_debug.yaw_d; log_msg.body.log_CTRL.yaw_rate_p = buf.control_debug.yaw_rate_p; log_msg.body.log_CTRL.yaw_rate_i = buf.control_debug.yaw_rate_i; log_msg.body.log_CTRL.yaw_rate_d = buf.control_debug.yaw_rate_d; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index a3d35b5965..8dd36c74d1 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -159,21 +159,12 @@ struct log_STAT_s { /* --- CTRL - CONTROL DEBUG --- */ #define LOG_CTRL_MSG 11 struct log_CTRL_s { - float roll_p; - float roll_i; - float roll_d; float roll_rate_p; float roll_rate_i; float roll_rate_d; - float pitch_p; - float pitch_i; - float pitch_d; float pitch_rate_p; float pitch_rate_i; float pitch_rate_d; - float yaw_p; - float yaw_i; - float yaw_d; float yaw_rate_p; float yaw_rate_i; float yaw_rate_d; @@ -205,7 +196,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT(STAT, "BBBBBfffB", "State,FlightMode,CtlMode,SASMode,Armed,BatV,BatC,BatRem,BatWarn"), - LOG_FORMAT(CTRL, "ffffffffffffffffff", "RollP,RollI,RollD,RollRP,RollRI,RollRD,PitchP,PitchI,PitchD,PitchRP,PitchRI,PitchRD,YawP,YawI,YawD,YawRP,YawRI,YawRD"), + LOG_FORMAT(CTRL, "fffffffff", "RollRP,RollRI,RollRD,PitchRP,PitchRI,PitchRD,YawRP,YawRI,YawRD"), LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), }; From 38558f0f168ae0e486df4886533ea522c4ab18ed Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Jun 2013 16:00:44 +0200 Subject: [PATCH 152/486] Count and write for control debug loging was missing (still not working) --- src/modules/sdlog2/sdlog2.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index aff0fd4a73..fed9597757 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1054,6 +1054,8 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_CTRL.yaw_rate_p = buf.control_debug.yaw_rate_p; log_msg.body.log_CTRL.yaw_rate_i = buf.control_debug.yaw_rate_i; log_msg.body.log_CTRL.yaw_rate_d = buf.control_debug.yaw_rate_d; + + LOGBUFFER_WRITE_AND_COUNT(CTRL); } /* --- FLOW --- */ From 216617431da72ce7b34911f63fa5958302a531e1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Jun 2013 16:18:40 +0200 Subject: [PATCH 153/486] Logging of ctrl debug values working now --- src/modules/sdlog2/sdlog2.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index fed9597757..1008c149fe 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1044,7 +1044,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- CONTROL DEBUG --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_control_debug), subs.control_debug_sub, &buf.control_debug); - + log_msg.msg_type = LOG_CTRL_MSG; log_msg.body.log_CTRL.roll_rate_p = buf.control_debug.roll_rate_p; log_msg.body.log_CTRL.roll_rate_i = buf.control_debug.roll_rate_i; log_msg.body.log_CTRL.roll_rate_d = buf.control_debug.roll_rate_d; From 650de90a904368eb93689bbb8a3be63888bf244e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Jun 2013 14:40:55 +0400 Subject: [PATCH 154/486] sdlog2: ARSP, GPOS messages added --- src/modules/sdlog2/sdlog2.c | 18 ++++++++++------ src/modules/sdlog2/sdlog2_messages.h | 31 +++++++++++++++++++++------- 2 files changed, 35 insertions(+), 14 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index e93b934c8b..65cfdc1884 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -608,12 +608,9 @@ int sdlog2_thread_main(int argc, char *argv[]) errx(1, "can't allocate log buffer, exiting."); } - /* file descriptors to wait for */ - struct pollfd fds_control[2]; - /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ /* number of messages */ - const ssize_t fdsc = 16; + const ssize_t fdsc = 17; /* Sanity check variable and index */ ssize_t fdsc_count = 0; /* file descriptors to wait for */ @@ -681,8 +678,9 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_STAT_s log_STAT; struct log_RC_s log_RC; struct log_OUT0_s log_OUT0; - struct log_ARSP_s log_ARSP; struct log_AIRS_s log_AIRS; + struct log_ARSP_s log_ARSP; + struct log_GPOS_s log_GPOS; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -1053,7 +1051,15 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- GLOBAL POSITION --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos); - // TODO not implemented yet + log_msg.msg_type = LOG_GPOS_MSG; + log_msg.body.log_GPOS.lat = buf.global_pos.lat; + log_msg.body.log_GPOS.lon = buf.global_pos.lon; + log_msg.body.log_GPOS.alt = buf.global_pos.alt; + log_msg.body.log_GPOS.vel_n = buf.global_pos.vx; + log_msg.body.log_GPOS.vel_e = buf.global_pos.vy; + log_msg.body.log_GPOS.vel_d = buf.global_pos.vz; + log_msg.body.log_GPOS.hdg = buf.global_pos.hdg; + LOGBUFFER_WRITE_AND_COUNT(GPOS); } /* --- VICON POSITION --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 5eeebcd95b..c47c388c4c 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -171,20 +171,33 @@ struct log_OUT0_s { float output[8]; }; -/* --- ARSP - ATTITUDE RATE SET POINT --- */ -#define LOG_ARSP_MSG 13 -struct log_ARSP_s { - float roll_rate_sp; - float pitch_rate_sp; - float yaw_rate_sp; -}; - /* --- AIRS - AIRSPEED --- */ #define LOG_AIRS_MSG 13 struct log_AIRS_s { float indicated_airspeed; float true_airspeed; }; + +/* --- ARSP - ATTITUDE RATE SET POINT --- */ +#define LOG_ARSP_MSG 14 +struct log_ARSP_s { + float roll_rate_sp; + float pitch_rate_sp; + float yaw_rate_sp; +}; + +/* --- GPOS - GLOBAL POSITION --- */ +#define LOG_GPOS_MSG 15 +struct log_GPOS_s { + int32_t lat; + int32_t lon; + float alt; + float vel_n; + float vel_e; + float vel_d; + float hdg; +}; + #pragma pack(pop) /* construct list of all message formats */ @@ -203,6 +216,8 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), + LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), + LOG_FORMAT(GPOS, "LLfffff", "Lat,Lon,Alt,VelN,VelE,VelD"), }; static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); From a83aca753c2a5c8680c8a3b7258a1b2c25fbbce8 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Jun 2013 14:41:35 +0400 Subject: [PATCH 155/486] position_estimator_inav rewrite, publishes vehicle_global_position now --- .../position_estimator_inav/inertial_filter.c | 10 +- .../position_estimator_inav/inertial_filter.h | 2 +- .../position_estimator_inav_main.c | 297 +++++++++--------- 3 files changed, 160 insertions(+), 149 deletions(-) diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c index b70d3504e6..8cdde5e1a5 100644 --- a/src/modules/position_estimator_inav/inertial_filter.c +++ b/src/modules/position_estimator_inav/inertial_filter.c @@ -13,16 +13,16 @@ void inertial_filter_predict(float dt, float x[3]) x[1] += x[2] * dt; } -void inertial_filter_correct(float dt, float x[3], int i, float z, float w) +void inertial_filter_correct(float edt, float x[3], int i, float w) { - float e = z - x[i]; - x[i] += e * w * dt; + float ewdt = w * edt; + x[i] += ewdt; if (i == 0) { - x[1] += e * w * w * dt; + x[1] += w * ewdt; //x[2] += e * w * w * w * dt / 3.0; // ~ 0.0 } else if (i == 1) { - x[2] += e * w * w * dt; + x[2] += w * ewdt; } } diff --git a/src/modules/position_estimator_inav/inertial_filter.h b/src/modules/position_estimator_inav/inertial_filter.h index 18c194abf2..dca6375dce 100644 --- a/src/modules/position_estimator_inav/inertial_filter.h +++ b/src/modules/position_estimator_inav/inertial_filter.h @@ -10,4 +10,4 @@ void inertial_filter_predict(float dt, float x[3]); -void inertial_filter_correct(float dt, float x[3], int i, float z, float w); +void inertial_filter_correct(float edt, float x[3], int i, float w); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 07ea7cd5cc..ac51a70106 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -57,6 +57,7 @@ #include #include #include +#include #include #include #include @@ -159,14 +160,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float y_est[3] = { 0.0f, 0.0f, 0.0f }; float z_est[3] = { 0.0f, 0.0f, 0.0f }; - int baro_loop_cnt = 0; - int baro_loop_end = 70; /* measurement for 1 second */ + int baro_init_cnt = 0; + int baro_init_num = 70; /* measurement for 1 second */ float baro_alt0 = 0.0f; /* to determine while start up */ double lat_current = 0.0; //[°]] --> 47.0 double lon_current = 0.0; //[°]] -->8.5 double alt_current = 0.0; //[m] above MSL + uint32_t accel_counter = 0; + uint32_t baro_counter = 0; + /* declare and safely initialize all structs */ struct vehicle_status_s vehicle_status; memset(&vehicle_status, 0, sizeof(vehicle_status)); @@ -177,8 +181,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) memset(&gps, 0, sizeof(gps)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); - struct vehicle_local_position_s pos; - memset(&pos, 0, sizeof(pos)); + struct vehicle_local_position_s local_pos; + memset(&local_pos, 0, sizeof(local_pos)); + struct vehicle_global_position_s global_pos; + memset(&global_pos, 0, sizeof(global_pos)); /* subscribe */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -188,79 +194,96 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); /* advertise */ - orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &pos); + orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos); + orb_advert_t vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); struct position_estimator_inav_params params; struct position_estimator_inav_param_handles pos_inav_param_handles; /* initialize parameter handles */ parameters_init(&pos_inav_param_handles); - bool local_flag_baroINITdone = false; /* first parameters read at start up */ struct parameter_update_s param_update; orb_copy(ORB_ID(parameter_update), parameter_update_sub, ¶m_update); /* read from param topic to clear updated flag */ /* first parameters update */ parameters_update(&pos_inav_param_handles, ¶ms); - /* wait for GPS fix, only then can we initialize the projection */ - if (params.use_gps) { - struct pollfd fds_init[1] = { - { .fd = vehicle_gps_position_sub, .events = POLLIN } - }; + struct pollfd fds_init[2] = { + { .fd = sensor_combined_sub, .events = POLLIN }, + { .fd = vehicle_gps_position_sub, .events = POLLIN } + }; - while (gps.fix_type < 3) { - if (poll(fds_init, 1, 5000)) { - if (fds_init[0].revents & POLLIN) { - /* Wait for the GPS update to propagate (we have some time) */ - usleep(5000); - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); + /* wait for initial sensors values: baro, GPS fix, only then can we initialize the projection */ + bool wait_gps = params.use_gps; + bool wait_baro = true; + + while (wait_gps || wait_baro) { + if (poll(fds_init, 2, 1000)) { + if (fds_init[0].revents & POLLIN) { + orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); + if (wait_baro && sensor.baro_counter > baro_counter) { + /* mean calculation over several measurements */ + if (baro_init_cnt < baro_init_num) { + baro_alt0 += sensor.baro_alt_meter; + baro_init_cnt++; + + } else { + wait_baro = false; + baro_alt0 /= (float) baro_init_cnt; + warnx("init baro: alt = %.3f", baro_alt0); + mavlink_log_info(mavlink_fd, "[inav] init baro: alt = %.3f", baro_alt0); + local_pos.home_alt = baro_alt0; + local_pos.home_timestamp = hrt_absolute_time(); + } } } + if (fds_init[1].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); + if (wait_gps && gps.fix_type >= 3) { + wait_gps = false; + /* get GPS position for first initialization */ + lat_current = gps.lat * 1e-7; + lon_current = gps.lon * 1e-7; + alt_current = gps.alt * 1e-3; - static int printcounter = 0; + local_pos.home_lat = lat_current * 1e7; + local_pos.home_lon = lon_current * 1e7; + local_pos.home_hdg = 0.0f; + local_pos.home_timestamp = hrt_absolute_time(); - if (printcounter == 100) { - printcounter = 0; - warnx("waiting for GPS fix type 3..."); + /* initialize coordinates */ + map_projection_init(lat_current, lon_current); + warnx("init GPS: lat = %.10f, lon = %.10f", lat_current, lon_current); + mavlink_log_info(mavlink_fd, "[inav] init GPS: lat = %.10f, lon = %.10f", lat_current, lon_current); + } } - - printcounter++; } - - /* get GPS position for first initialization */ - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - lat_current = ((double)(gps.lat)) * 1e-7; - lon_current = ((double)(gps.lon)) * 1e-7; - alt_current = gps.alt * 1e-3; - - pos.home_lat = lat_current * 1e7; - pos.home_lon = lon_current * 1e7; - pos.home_timestamp = hrt_absolute_time(); - - /* initialize coordinates */ - map_projection_init(lat_current, lon_current); - /* publish global position messages only after first GPS message */ } - warnx("initialized projection with: lat = %.10f, lon = %.10f", lat_current, lon_current); - mavlink_log_info(mavlink_fd, "[inav] home: lat = %.10f, lon = %.10f", lat_current, lon_current); hrt_abstime t_prev = 0; - thread_running = true; - uint32_t accel_counter = 0; + + uint16_t accel_updates = 0; hrt_abstime accel_t = 0; - float accel_dt = 0.0f; - uint32_t baro_counter = 0; + uint16_t baro_updates = 0; hrt_abstime baro_t = 0; hrt_abstime gps_t = 0; - uint16_t accel_updates = 0; - uint16_t baro_updates = 0; uint16_t gps_updates = 0; uint16_t attitude_updates = 0; + hrt_abstime updates_counter_start = hrt_absolute_time(); uint32_t updates_counter_len = 1000000; + hrt_abstime pub_last = hrt_absolute_time(); uint32_t pub_interval = 4000; // limit publish rate to 250 Hz + /* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */ + float accel_corr[] = { 0.0f, 0.0f, 0.0f }; // N E D + float baro_corr = 0.0f; // D + float gps_corr[2][2] = { + { 0.0f, 0.0f }, // N (pos, vel) + { 0.0f, 0.0f }, // E (pos, vel) + }; + /* main loop */ struct pollfd fds[5] = { { .fd = parameter_update_sub, .events = POLLIN }, @@ -269,14 +292,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { .fd = sensor_combined_sub, .events = POLLIN }, { .fd = vehicle_gps_position_sub, .events = POLLIN } }; + + thread_running = true; warnx("main loop started."); while (!thread_should_exit) { - bool accelerometer_updated = false; - bool baro_updated = false; - bool gps_updated = false; - float proj_pos_gps[3] = { 0.0f, 0.0f, 0.0f }; - int ret = poll(fds, params.use_gps ? 5 : 4, 10); // wait maximal this 10 ms = 100 Hz minimum rate hrt_abstime t = hrt_absolute_time(); @@ -314,34 +334,31 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); if (sensor.accelerometer_counter > accel_counter) { - accelerometer_updated = true; + if (att.R_valid) { + /* transform acceleration vector from body frame to NED frame */ + float accel_NED[3]; + for (int i = 0; i < 3; i++) { + accel_NED[i] = 0.0f; + for (int j = 0; j < 3; j++) { + accel_NED[i] += att.R[i][j] * sensor.accelerometer_m_s2[j]; + } + } + accel_NED[2] += CONSTANTS_ONE_G; + accel_corr[0] = accel_NED[0] - x_est[2]; + accel_corr[1] = accel_NED[1] - y_est[2]; + accel_corr[2] = accel_NED[2] - z_est[2]; + } else { + memset(accel_corr, 0, sizeof(accel_corr)); + } accel_counter = sensor.accelerometer_counter; accel_updates++; - accel_dt = accel_t > 0 ? (t - accel_t) / 1000000.0f : 0.0f; - accel_t = t; } if (sensor.baro_counter > baro_counter) { - baro_updated = true; + baro_corr = baro_alt0 - sensor.baro_alt_meter - z_est[2]; baro_counter = sensor.baro_counter; baro_updates++; } - - /* barometric pressure estimation at start up */ - if (!local_flag_baroINITdone && baro_updated) { - /* mean calculation over several measurements */ - if (baro_loop_cnt < baro_loop_end) { - baro_alt0 += sensor.baro_alt_meter; - baro_loop_cnt++; - - } else { - baro_alt0 /= (float)(baro_loop_cnt); - local_flag_baroINITdone = true; - warnx("baro_alt0 = %.2f", baro_alt0); - mavlink_log_info(mavlink_fd, "[inav] baro_alt0 = %.2f", baro_alt0); - pos.home_alt = baro_alt0; - } - } } if (params.use_gps) { @@ -349,80 +366,56 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (fds[4].revents & POLLIN) { /* new GPS value */ orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - /* project GPS lat lon (Geographic coordinate system) to plane */ - map_projection_project(((double)(gps.lat)) * 1e-7, - ((double)(gps.lon)) * 1e-7, &(proj_pos_gps[0]), - &(proj_pos_gps[1])); - proj_pos_gps[2] = (float)(gps.alt * 1e-3); - gps_updated = true; - pos.valid = gps.fix_type >= 3; - gps_updates++; + if (gps.fix_type >= 3) { + /* project GPS lat lon to plane */ + float gps_proj[2]; + map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1])); + gps_corr[0][0] = gps_proj[0] - x_est[0]; + gps_corr[1][0] = gps_proj[1] - y_est[0]; + if (gps.vel_ned_valid) { + gps_corr[0][1] = gps.vel_n_m_s; + gps_corr[1][1] = gps.vel_e_m_s; + } else { + gps_corr[0][1] = 0.0f; + gps_corr[1][1] = 0.0f; + } + local_pos.valid = true; + gps_updates++; + } else { + local_pos.valid = false; + } } - } else { - pos.valid = true; + local_pos.valid = true; } } /* end of poll return value check */ - float dt = t_prev > 0 ? (t - t_prev) / 1000000.0 : 0.0f; + float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; t_prev = t; - if (att.R_valid) { - /* transform acceleration vector from UAV frame to NED frame */ - float accel_NED[3]; + /* inertial filter prediction for altitude */ + inertial_filter_predict(dt, z_est); - for (int i = 0; i < 3; i++) { - accel_NED[i] = 0.0f; + /* inertial filter correction for altitude */ + inertial_filter_correct(baro_corr * dt, z_est, 0, params.w_alt_baro); + inertial_filter_correct(accel_corr[2] * dt, z_est, 2, params.w_alt_acc); - for (int j = 0; j < 3; j++) { - accel_NED[i] += att.R[i][j] * sensor.accelerometer_m_s2[j]; - } - } + if (params.use_gps) { + /* inertial filter prediction for position */ + inertial_filter_predict(dt, x_est); + inertial_filter_predict(dt, y_est); - accel_NED[2] += CONSTANTS_ONE_G; + /* inertial filter correction for position */ + inertial_filter_correct(gps_corr[0][0] * dt, x_est, 0, params.w_pos_gps_p); + inertial_filter_correct(gps_corr[0][1] * dt, x_est, 1, params.w_pos_gps_v); + inertial_filter_correct(gps_corr[1][0] * dt, y_est, 0, params.w_pos_gps_p); + inertial_filter_correct(gps_corr[1][1] * dt, y_est, 1, params.w_pos_gps_v); - /* inertial filter prediction for altitude */ - inertial_filter_predict(dt, z_est); - - /* inertial filter correction for altitude */ - if (local_flag_baroINITdone && baro_updated) { - if (baro_t > 0) { - inertial_filter_correct((t - baro_t) / 1000000.0f, z_est, 0, baro_alt0 - sensor.baro_alt_meter, params.w_alt_baro); - } - - baro_t = t; - } - - if (accelerometer_updated) { - inertial_filter_correct(accel_dt, z_est, 2, accel_NED[2], params.w_alt_acc); - } - - if (params.use_gps) { - /* inertial filter prediction for position */ - inertial_filter_predict(dt, x_est); - inertial_filter_predict(dt, y_est); - - /* inertial filter correction for position */ - if (gps_updated) { - if (gps_t > 0) { - float gps_dt = (t - gps_t) / 1000000.0f; - inertial_filter_correct(gps_dt, x_est, 0, proj_pos_gps[0], params.w_pos_gps_p); - inertial_filter_correct(gps_dt, x_est, 1, gps.vel_n_m_s, params.w_pos_gps_v); - inertial_filter_correct(gps_dt, y_est, 0, proj_pos_gps[1], params.w_pos_gps_p); - inertial_filter_correct(gps_dt, y_est, 1, gps.vel_e_m_s, params.w_pos_gps_v); - } - - gps_t = t; - } - - if (accelerometer_updated) { - inertial_filter_correct(accel_dt, x_est, 2, accel_NED[0], params.w_pos_acc); - inertial_filter_correct(accel_dt, y_est, 2, accel_NED[1], params.w_pos_acc); - } - } + inertial_filter_correct(accel_corr[0] * dt, x_est, 2, params.w_pos_acc); + inertial_filter_correct(accel_corr[1] * dt, y_est, 2, params.w_pos_acc); } if (verbose_mode) { @@ -445,20 +438,38 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (t - pub_last > pub_interval) { pub_last = t; - pos.x = x_est[0]; - pos.vx = x_est[1]; - pos.y = y_est[0]; - pos.vy = y_est[1]; - pos.z = z_est[0]; - pos.vz = z_est[1]; - pos.timestamp = hrt_absolute_time(); + local_pos.timestamp = t; + local_pos.x = x_est[0]; + local_pos.vx = x_est[1]; + local_pos.y = y_est[0]; + local_pos.vy = y_est[1]; + local_pos.z = z_est[0]; + local_pos.vz = z_est[1]; + local_pos.absolute_alt = local_pos.home_alt - local_pos.z; + local_pos.hdg = att.yaw; - if ((isfinite(pos.x)) && (isfinite(pos.vx)) - && (isfinite(pos.y)) - && (isfinite(pos.vy)) - && (isfinite(pos.z)) - && (isfinite(pos.vz))) { - orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &pos); + if ((isfinite(local_pos.x)) && (isfinite(local_pos.vx)) + && (isfinite(local_pos.y)) + && (isfinite(local_pos.vy)) + && (isfinite(local_pos.z)) + && (isfinite(local_pos.vz))) { + orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); + + if (params.use_gps) { + global_pos.valid = local_pos.valid; + double est_lat, est_lon; + map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); + global_pos.lat = (int32_t) (est_lat * 1e7); + global_pos.lon = (int32_t) (est_lon * 1e7); + global_pos.alt = -local_pos.z - local_pos.home_alt; + global_pos.relative_alt = -local_pos.z; + global_pos.vx = local_pos.vx; + global_pos.vy = local_pos.vy; + global_pos.vz = local_pos.vz; + global_pos.hdg = local_pos.hdg; + + orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos); + } } } } From 2124c52cff152e696a73888a7b16556d3c882ec1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Jun 2013 22:06:45 +0400 Subject: [PATCH 156/486] position_estimator_inav bugfixes --- src/modules/position_estimator_inav/inertial_filter.c | 2 +- .../position_estimator_inav/position_estimator_inav_main.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c index 8cdde5e1a5..acaf693f17 100644 --- a/src/modules/position_estimator_inav/inertial_filter.c +++ b/src/modules/position_estimator_inav/inertial_filter.c @@ -20,7 +20,7 @@ void inertial_filter_correct(float edt, float x[3], int i, float w) if (i == 0) { x[1] += w * ewdt; - //x[2] += e * w * w * w * dt / 3.0; // ~ 0.0 + x[2] += w * w * ewdt / 3.0; } else if (i == 1) { x[2] += w * ewdt; diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index ac51a70106..a8a66e93d8 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -355,7 +355,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } if (sensor.baro_counter > baro_counter) { - baro_corr = baro_alt0 - sensor.baro_alt_meter - z_est[2]; + baro_corr = baro_alt0 - sensor.baro_alt_meter - z_est[0]; baro_counter = sensor.baro_counter; baro_updates++; } From effce6edfa3b4258a855342d8f7a265f2f18f521 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Jun 2013 22:07:05 +0400 Subject: [PATCH 157/486] sdlog2 GPOS message bug fix --- src/modules/sdlog2/sdlog2_messages.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index b42f11e61d..55bc367174 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -218,7 +218,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), - LOG_FORMAT(GPOS, "LLfffff", "Lat,Lon,Alt,VelN,VelE,VelD"), + LOG_FORMAT(GPOS, "LLfffff", "Lat,Lon,Alt,VelN,VelE,VelD,Heading"), }; static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); From a25d68440d99b4214ae4477d07c23e852458c4d2 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 17 Jun 2013 21:01:25 +0200 Subject: [PATCH 158/486] Merge with att_fix --- .../attitude_estimator_ekf_params.c | 48 +++++++++---------- 1 file changed, 24 insertions(+), 24 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 7d3812abdb..52dac652be 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -44,42 +44,42 @@ /* Extended Kalman Filter covariances */ /* gyro process noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q0, 1e-4f); -PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q1, 0.08f); -PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q2, 0.009f); +PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q0, 1e-4f); +PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q1, 0.08f); +PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q2, 0.009f); /* gyro offsets process noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q3, 0.005f); -PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q4, 0.0f); +PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f); +PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q4, 0.0f); /* gyro measurement noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_V2_R0, 0.0008f); -PARAM_DEFINE_FLOAT(EKF_ATT_V2_R1, 0.8f); -PARAM_DEFINE_FLOAT(EKF_ATT_V2_R2, 1.0f); +PARAM_DEFINE_FLOAT(EKF_ATT_V3_R0, 0.0008f); +PARAM_DEFINE_FLOAT(EKF_ATT_V3_R1, 10000.0f); +PARAM_DEFINE_FLOAT(EKF_ATT_V3_R2, 1.0f); /* accelerometer measurement noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_V2_R3, 0.0f); +PARAM_DEFINE_FLOAT(EKF_ATT_V3_R3, 0.0f); /* offsets in roll, pitch and yaw of sensor plane and body */ -PARAM_DEFINE_FLOAT(ATT_ROLL_OFFS, 0.0f); -PARAM_DEFINE_FLOAT(ATT_PITCH_OFFS, 0.0f); -PARAM_DEFINE_FLOAT(ATT_YAW_OFFS, 0.0f); +PARAM_DEFINE_FLOAT(ATT_ROLL_OFF3, 0.0f); +PARAM_DEFINE_FLOAT(ATT_PITCH_OFF3, 0.0f); +PARAM_DEFINE_FLOAT(ATT_YAW_OFF3, 0.0f); int parameters_init(struct attitude_estimator_ekf_param_handles *h) { /* PID parameters */ - h->q0 = param_find("EKF_ATT_V2_Q0"); - h->q1 = param_find("EKF_ATT_V2_Q1"); - h->q2 = param_find("EKF_ATT_V2_Q2"); - h->q3 = param_find("EKF_ATT_V2_Q3"); - h->q4 = param_find("EKF_ATT_V2_Q4"); + h->q0 = param_find("EKF_ATT_V3_Q0"); + h->q1 = param_find("EKF_ATT_V3_Q1"); + h->q2 = param_find("EKF_ATT_V3_Q2"); + h->q3 = param_find("EKF_ATT_V3_Q3"); + h->q4 = param_find("EKF_ATT_V3_Q4"); - h->r0 = param_find("EKF_ATT_V2_R0"); - h->r1 = param_find("EKF_ATT_V2_R1"); - h->r2 = param_find("EKF_ATT_V2_R2"); - h->r3 = param_find("EKF_ATT_V2_R3"); + h->r0 = param_find("EKF_ATT_V3_R0"); + h->r1 = param_find("EKF_ATT_V3_R1"); + h->r2 = param_find("EKF_ATT_V3_R2"); + h->r3 = param_find("EKF_ATT_V3_R3"); - h->roll_off = param_find("ATT_ROLL_OFFS"); - h->pitch_off = param_find("ATT_PITCH_OFFS"); - h->yaw_off = param_find("ATT_YAW_OFFS"); + h->roll_off = param_find("ATT_ROLL_OFF3"); + h->pitch_off = param_find("ATT_PITCH_OFF3"); + h->yaw_off = param_find("ATT_YAW_OFF3"); return OK; } From 52f8565f0b5326bedb8b8a970c987e6bf10d71f6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 17 Jun 2013 21:02:52 +0200 Subject: [PATCH 159/486] Corrected number of ORB structs in sdlog2 --- src/modules/sdlog2/sdlog2.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 885155e0de..9cf6640bf9 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -600,7 +600,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ /* number of messages */ - const ssize_t fdsc = 17; + const ssize_t fdsc = 19; /* Sanity check variable and index */ ssize_t fdsc_count = 0; From 2daff9ebbf066c0b14b85364ee056c3d8c7a7734 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 17 Jun 2013 21:03:55 +0200 Subject: [PATCH 160/486] Checkpoint: Quad is flying after PID lib changes --- .../multirotor_attitude_control.c | 6 +++--- .../multirotor_att_control/multirotor_rate_control.c | 10 ++++------ 2 files changed, 7 insertions(+), 9 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index 6da808da49..88cc651488 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -183,12 +183,12 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s /* control pitch (forward) output */ rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body , - att->pitch, att->pitchspeed, deltaT, &control_debug->roll_p, &control_debug->roll_i, &control_debug->roll_d); + att->pitch, att->pitchspeed, deltaT, &control_debug->pitch_p, &control_debug->pitch_i, &control_debug->pitch_d); /* control roll (left/right) output */ rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body , - att->roll, att->rollspeed, deltaT, &control_debug->pitch_p, &control_debug->pitch_i, &control_debug->pitch_d); - + att->roll, att->rollspeed, deltaT, &control_debug->roll_p, &control_debug->roll_i, &control_debug->roll_d); + // printf("rates_sp: %4.4f, att setpoint: %4.4f\n, pitch: %4.4f, pitchspeed: %4.4f, dT: %4.4f", rates_sp->pitch, att_sp->pitch_body, att->pitch, att->pitchspeed, deltaT); if (control_yaw_position) { diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index 8cd97d7c16..a39d6f13d5 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -186,10 +186,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, parameters_update(&h, &p); initialized = true; - pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, - 1000.0f, 0.2f, PID_MODE_DERIVATIV_CALC); - pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, - 1000.0f, 0.2f, PID_MODE_DERIVATIV_CALC); + pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_CALC); + pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_CALC); } @@ -197,8 +195,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, if (motor_skip_counter % 2500 == 0) { /* update parameters from storage */ parameters_update(&h, &p); - pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f, 0.2f); - pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f, 0.2f); + pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f); + pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f); } /* control pitch (forward) output */ From c874f681080e0e68d62a31e88c80240350f8a595 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 17 Jun 2013 21:03:55 +0200 Subject: [PATCH 161/486] Checkpoint: Quad is flying after PID lib changes Conflicts: src/modules/multirotor_att_control/multirotor_attitude_control.c --- .../multirotor_attitude_control.c | 2 +- .../multirotor_att_control/multirotor_rate_control.c | 10 ++++------ 2 files changed, 5 insertions(+), 7 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index 51faaa8c0d..0dad103167 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -187,7 +187,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s /* control roll (left/right) output */ rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body , att->roll, att->rollspeed, deltaT, NULL, NULL, NULL); - + // printf("rates_sp: %4.4f, att setpoint: %4.4f\n, pitch: %4.4f, pitchspeed: %4.4f, dT: %4.4f", rates_sp->pitch, att_sp->pitch_body, att->pitch, att->pitchspeed, deltaT); if (control_yaw_position) { diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index 01bf383e27..9135a93514 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -189,10 +189,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, parameters_update(&h, &p); initialized = true; - pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, - 1000.0f, 0.2f, PID_MODE_DERIVATIV_CALC); - pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, - 1000.0f, 0.2f, PID_MODE_DERIVATIV_CALC); + pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_CALC); + pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_CALC); control_debug_pub = orb_advertise(ORB_ID(vehicle_control_debug), &control_debug); } @@ -201,8 +199,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, if (motor_skip_counter % 2500 == 0) { /* update parameters from storage */ parameters_update(&h, &p); - pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f, 0.2f); - pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f, 0.2f); + pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f); + pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f); } /* control pitch (forward) output */ From cc452834c0dabd2689f5f102ce1cbbe714f056dd Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 18 Jun 2013 00:30:10 +0200 Subject: [PATCH 162/486] First try to prevent motors from stopping when armed --- src/drivers/px4io/px4io.cpp | 108 ++++++++++++++++++++++++++ src/modules/px4iofirmware/mixer.cpp | 20 ++++- src/modules/px4iofirmware/protocol.h | 6 ++ src/modules/px4iofirmware/px4io.h | 2 + src/modules/px4iofirmware/registers.c | 58 ++++++++++++++ 5 files changed, 192 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index d728b7b762..6a596a9876 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -125,6 +125,16 @@ public: */ int set_failsafe_values(const uint16_t *vals, unsigned len); + /** + * Set the minimum PWM signals when armed + */ + int set_min_values(const uint16_t *vals, unsigned len); + + /** + * Set the maximum PWM signal when armed + */ + int set_max_values(const uint16_t *vals, unsigned len); + /** * Print the current status of IO */ @@ -711,6 +721,34 @@ PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len) return io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, vals, len); } +int +PX4IO::set_min_values(const uint16_t *vals, unsigned len) +{ + uint16_t regs[_max_actuators]; + + if (len > _max_actuators) + /* fail with error */ + return E2BIG; + + /* copy values to registers in IO */ + return io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, vals, len); +} + +int +PX4IO::set_max_values(const uint16_t *vals, unsigned len) +{ + uint16_t regs[_max_actuators]; + + if (len > _max_actuators) + /* fail with error */ + return E2BIG; + + /* copy values to registers in IO */ + return io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, vals, len); +} + + + int PX4IO::io_set_arming_state() { @@ -1792,6 +1830,76 @@ px4io_main(int argc, char *argv[]) exit(0); } + if (!strcmp(argv[1], "min")) { + + if (argc < 3) { + errx(1, "min command needs at least one channel value (PWM)"); + } + + if (g_dev != nullptr) { + + /* set values for first 8 channels, fill unassigned channels with 900. */ + uint16_t min[8]; + + for (int i = 0; i < sizeof(min) / sizeof(min[0]); i++) + { + /* set channel to commanline argument or to 900 for non-provided channels */ + if (argc > i + 2) { + min[i] = atoi(argv[i+2]); + if (min[i] < 900 || min[i] > 1200) { + errx(1, "value out of range of 900 < value < 1200. Aborting."); + } + } else { + /* a zero value will the default */ + min[i] = 900; + } + } + + int ret = g_dev->set_min_values(min, sizeof(min) / sizeof(min[0])); + + if (ret != OK) + errx(ret, "failed setting min values"); + } else { + errx(1, "not loaded"); + } + exit(0); + } + + if (!strcmp(argv[1], "max")) { + + if (argc < 3) { + errx(1, "max command needs at least one channel value (PWM)"); + } + + if (g_dev != nullptr) { + + /* set values for first 8 channels, fill unassigned channels with 2100. */ + uint16_t max[8]; + + for (int i = 0; i < sizeof(max) / sizeof(max[0]); i++) + { + /* set channel to commanline argument or to 2100 for non-provided channels */ + if (argc > i + 2) { + max[i] = atoi(argv[i+2]); + if (max[i] < 1800 || max[i] > 2100) { + errx(1, "value out of range of 1800 < value < 2100. Aborting."); + } + } else { + /* a zero value will the default */ + max[i] = 2100; + } + } + + int ret = g_dev->set_max_values(max, sizeof(max) / sizeof(max[0])); + + if (ret != OK) + errx(ret, "failed setting max values"); + } else { + errx(1, "not loaded"); + } + exit(0); + } + if (!strcmp(argv[1], "recovery")) { if (g_dev != nullptr) { diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index a2193b526b..e257e7cb85 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -59,6 +59,11 @@ extern "C" { */ #define FMU_INPUT_DROP_LIMIT_US 200000 +/* + * Time that the ESCs need to initialize + */ + #define ESC_INIT_TIME_US 500000 + /* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */ #define ROLL 0 #define PITCH 1 @@ -69,6 +74,8 @@ extern "C" { /* current servo arm/disarm state */ static bool mixer_servos_armed = false; +static uint64_t time_armed; + /* selected control values and count for mixing */ enum mixer_source { MIX_NONE, @@ -176,8 +183,16 @@ mixer_tick(void) /* save actuator values for FMU readback */ r_page_actuators[i] = FLOAT_TO_REG(outputs[i]); - /* scale to servo output */ - r_page_servos[i] = (outputs[i] * 600.0f) + 1500; + /* scale to control range after init time */ + if (mixer_servos_armed && (hrt_elapsed_time(&time_armed) > ESC_INIT_TIME_US)) { + r_page_servos[i] = (outputs[i] + * (r_page_servo_control_max[i] - r_page_servo_control_min[i])/2 + + (r_page_servo_control_max[i] + r_page_servo_control_min[i])/2); + + /* but use init range from 900 to 2100 right after arming */ + } else { + r_page_servos[i] = (outputs[i] * 600 + 1500); + } } for (unsigned i = mixed; i < IO_SERVO_COUNT; i++) @@ -206,6 +221,7 @@ mixer_tick(void) /* need to arm, but not armed */ up_pwm_servo_arm(true); mixer_servos_armed = true; + time_armed = hrt_absolute_time(); } else if (!should_arm && mixer_servos_armed) { /* armed but need to disarm */ diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 674f9dddd6..1c9715451b 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -183,6 +183,12 @@ /* PWM failsafe values - zero disables the output */ #define PX4IO_PAGE_FAILSAFE_PWM 105 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +/* PWM minimum values for certain ESCs */ +#define PX4IO_PAGE_CONTROL_MIN_PWM 106 /* 0..CONFIG_ACTUATOR_COUNT-1 */ + + /* PWM maximum values for certain ESCs */ +#define PX4IO_PAGE_CONTROL_MAX_PWM 107 /* 0..CONFIG_ACTUATOR_COUNT-1 */ + /** * As-needed mixer data upload. * diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 272cdb7bf4..3e4027c9bd 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -77,6 +77,8 @@ extern volatile uint16_t r_page_setup[]; /* PX4IO_PAGE_SETUP */ extern volatile uint16_t r_page_controls[]; /* PX4IO_PAGE_CONTROLS */ extern uint16_t r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */ extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */ +extern uint16_t r_page_servo_control_min[]; /* PX4IO_PAGE_CONTROL_MIN_PWM */ +extern uint16_t r_page_servo_control_max[]; /* PX4IO_PAGE_CONTROL_MAX_PWM */ /* * Register aliases. diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index df7d6dcd35..1fcfb2906e 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -184,6 +184,22 @@ uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE */ uint16_t r_page_servo_failsafe[IO_SERVO_COUNT] = { 0 }; +/** + * PAGE 106 + * + * minimum PWM values when armed + * + */ +uint16_t r_page_servo_control_min[IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 }; + +/** + * PAGE 107 + * + * maximum PWM values when armed + * + */ +uint16_t r_page_servo_control_max[IO_SERVO_COUNT] = { 2100, 2100, 2100, 2100, 2100, 2100, 2100, 2100 }; + void registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { @@ -247,6 +263,42 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num } break; + case PX4IO_PAGE_CONTROL_MIN_PWM: + + /* copy channel data */ + while ((offset < IO_SERVO_COUNT) && (num_values > 0)) { + + if (*values > 1200) + r_page_servo_control_min[offset] = 1200; + else if (*values < 900) + r_page_servo_control_min[offset] = 900; + else + r_page_servo_control_min[offset] = *values; + + offset++; + num_values--; + values++; + } + break; + + case PX4IO_PAGE_CONTROL_MAX_PWM: + + /* copy channel data */ + while ((offset < IO_SERVO_COUNT) && (num_values > 0)) { + + if (*values > 1200) + r_page_servo_control_max[offset] = 1200; + else if (*values < 900) + r_page_servo_control_max[offset] = 900; + else + r_page_servo_control_max[offset] = *values; + + offset++; + num_values--; + values++; + } + break; + /* handle text going to the mixer parser */ case PX4IO_PAGE_MIXERLOAD: mixer_handle_text(values, num_values * sizeof(*values)); @@ -583,6 +635,12 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val case PX4IO_PAGE_FAILSAFE_PWM: SELECT_PAGE(r_page_servo_failsafe); break; + case PX4IO_PAGE_CONTROL_MIN_PWM: + SELECT_PAGE(r_page_servo_control_min); + break; + case PX4IO_PAGE_CONTROL_MAX_PWM: + SELECT_PAGE(r_page_servo_control_max); + break; default: return -1; From b5f4f1ee808c176c5dc0705b76584b438f151650 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 18 Jun 2013 10:00:08 +0200 Subject: [PATCH 163/486] Adressed performance concern and fixed a copy paste bug --- src/drivers/px4io/px4io.cpp | 4 ++-- src/modules/px4iofirmware/mixer.cpp | 11 +++++++++-- src/modules/px4iofirmware/registers.c | 18 +++++++++++++----- 3 files changed, 24 insertions(+), 9 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 6a596a9876..be4dd5d197 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1851,7 +1851,7 @@ px4io_main(int argc, char *argv[]) } } else { /* a zero value will the default */ - min[i] = 900; + min[i] = 0; } } @@ -1886,7 +1886,7 @@ px4io_main(int argc, char *argv[]) } } else { /* a zero value will the default */ - max[i] = 2100; + max[i] = 0; } } diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index e257e7cb85..62a6ebf133 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -62,7 +62,7 @@ extern "C" { /* * Time that the ESCs need to initialize */ - #define ESC_INIT_TIME_US 500000 + #define ESC_INIT_TIME_US 2000000 /* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */ #define ROLL 0 @@ -75,6 +75,7 @@ extern "C" { static bool mixer_servos_armed = false; static uint64_t time_armed; +static bool init_complete = false; /* selected control values and count for mixing */ enum mixer_source { @@ -177,6 +178,10 @@ mixer_tick(void) /* mix */ mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT); + if (!init_complete && mixer_servos_armed && (hrt_elapsed_time(&time_armed) > ESC_INIT_TIME_US)) { + init_complete = true; + } + /* scale to PWM and update the servo outputs as required */ for (unsigned i = 0; i < mixed; i++) { @@ -184,7 +189,7 @@ mixer_tick(void) r_page_actuators[i] = FLOAT_TO_REG(outputs[i]); /* scale to control range after init time */ - if (mixer_servos_armed && (hrt_elapsed_time(&time_armed) > ESC_INIT_TIME_US)) { + if (init_complete) { r_page_servos[i] = (outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i])/2 + (r_page_servo_control_max[i] + r_page_servo_control_min[i])/2); @@ -222,11 +227,13 @@ mixer_tick(void) up_pwm_servo_arm(true); mixer_servos_armed = true; time_armed = hrt_absolute_time(); + init_complete = false; } else if (!should_arm && mixer_servos_armed) { /* armed but need to disarm */ up_pwm_servo_arm(false); mixer_servos_armed = false; + init_complete = false; } if (mixer_servos_armed) { diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 1fcfb2906e..bc1c839016 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -268,7 +268,11 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* copy channel data */ while ((offset < IO_SERVO_COUNT) && (num_values > 0)) { - if (*values > 1200) + if (*values == 0) + /* set to default */ + r_page_servo_control_min[offset] = 900; + + else if (*values > 1200) r_page_servo_control_min[offset] = 1200; else if (*values < 900) r_page_servo_control_min[offset] = 900; @@ -286,10 +290,14 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* copy channel data */ while ((offset < IO_SERVO_COUNT) && (num_values > 0)) { - if (*values > 1200) - r_page_servo_control_max[offset] = 1200; - else if (*values < 900) - r_page_servo_control_max[offset] = 900; + if (*values == 0) + /* set to default */ + r_page_servo_control_max[offset] = 2100; + + else if (*values > 2100) + r_page_servo_control_max[offset] = 2100; + else if (*values < 1800) + r_page_servo_control_max[offset] = 1800; else r_page_servo_control_max[offset] = *values; From f3ce61d7405a7edc1e5bb2aadf2ccec5bed90feb Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 19 Jun 2013 09:35:19 +0200 Subject: [PATCH 164/486] Forgot to remove some debug stuff --- src/modules/px4iofirmware/mixer.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index e3ec2fa76f..6752a8128d 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -179,14 +179,12 @@ mixer_tick(void) if (mixer_servos_armed && !esc_init_done && !esc_init_active) { esc_init_time = hrt_absolute_time(); esc_init_active = true; - isr_debug(1, "start counting now"); } /* after waiting long enough for the ESC initialization, we can disable the ESC initialization phase */ if (!esc_init_done && esc_init_active && mixer_servos_armed && (hrt_elapsed_time(&esc_init_time) > ESC_INIT_TIME_US)) { esc_init_active = false; esc_init_done = true; - isr_debug(1, "time is up"); } /* mix */ @@ -251,9 +249,7 @@ mixer_tick(void) r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED); isr_debug(5, "> disarmed"); - esc_init_active = false; - isr_debug(1, "disarming, and init aborted"); - } + esc_init_active = false; } if (mixer_servos_armed) { /* update the servo outputs. */ From e2ff60b0a6dbcd714d57e781d9fe174b110a6237 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 19 Jun 2013 09:35:59 +0200 Subject: [PATCH 165/486] Use rollacc and pitchacc from the estimator instead of differentiating in the controller --- .../multirotor_att_control_main.c | 17 ++++++++++++----- .../multirotor_rate_control.c | 12 ++++++------ .../multirotor_rate_control.h | 2 +- 3 files changed, 19 insertions(+), 12 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index c3b2f5d2a2..dd38242d32 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -394,7 +394,8 @@ mc_thread_main(int argc, char *argv[]) /* measure in what intervals the controller runs */ perf_count(mc_interval_perf); - float gyro[3]; + float rates[3]; + float rates_acc[3]; /* get current rate setpoint */ bool rates_sp_valid = false; @@ -405,11 +406,17 @@ mc_thread_main(int argc, char *argv[]) } /* apply controller */ - gyro[0] = att.rollspeed; - gyro[1] = att.pitchspeed; - gyro[2] = att.yawspeed; + rates[0] = att.rollspeed; + rates[1] = att.pitchspeed; + rates[2] = att.yawspeed; - multirotor_control_rates(&rates_sp, gyro, &actuators, &control_debug_pub, &control_debug); + rates_acc[0] = att.rollacc; + rates_acc[1] = att.pitchacc; + rates_acc[2] = att.yawacc; + + + + multirotor_control_rates(&rates_sp, rates, rates_acc, &actuators, &control_debug_pub, &control_debug); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); orb_publish(ORB_ID(vehicle_control_debug), control_debug_pub, &control_debug); diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index a39d6f13d5..e37ede3e09 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -151,7 +151,7 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru } void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], struct actuator_controls_s *actuators, + const float rates[], const float rates_acc[], struct actuator_controls_s *actuators, const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug) { static uint64_t last_run = 0; @@ -186,8 +186,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, parameters_update(&h, &p); initialized = true; - pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_CALC); - pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_CALC); + pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_SET); + pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_SET); } @@ -201,11 +201,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* control pitch (forward) output */ float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch , - rates[1], 0.0f, deltaT, &control_debug->pitch_rate_p, &control_debug->pitch_rate_i, &control_debug->pitch_rate_d); + rates[1], rates_acc[1], deltaT, &control_debug->pitch_rate_p, &control_debug->pitch_rate_i, &control_debug->pitch_rate_d); /* control roll (left/right) output */ float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll , - rates[0], 0.0f, deltaT, &control_debug->roll_rate_p, &control_debug->roll_rate_i, &control_debug->roll_rate_d); + rates[0], rates_acc[0], deltaT, &control_debug->roll_rate_p, &control_debug->roll_rate_i, &control_debug->roll_rate_d); /* increase resilience to faulty control inputs */ if (isfinite(pitch_control)) { @@ -225,7 +225,7 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, warnx("rej. NaN ctrl roll"); } - /* control yaw rate */ + /* control yaw rate */ //XXX use library here and use rates_acc[2] float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]); /* increase resilience to faulty control inputs */ diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.h b/src/modules/multirotor_att_control/multirotor_rate_control.h index 465549540c..fc741c3786 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.h +++ b/src/modules/multirotor_att_control/multirotor_rate_control.h @@ -52,7 +52,7 @@ #include void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], struct actuator_controls_s *actuators, + const float rates[], const float rates_acc[], struct actuator_controls_s *actuators, const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug); #endif /* MULTIROTOR_RATE_CONTROL_H_ */ From 8d6cc86b4f37773c9c4db77b9666fa2a075c1871 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 19 Jun 2013 10:01:16 +0200 Subject: [PATCH 166/486] Cherry-picked commit e2ff60b0a6dbcd714d57e781d9fe174b110a6237: use rateacc values --- .../multirotor_att_control_main.c | 26 +++++++++++++++---- .../multirotor_rate_control.c | 22 ++++++---------- .../multirotor_rate_control.h | 4 ++- 3 files changed, 32 insertions(+), 20 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index d94c0a69c9..38775ed1f5 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -63,6 +63,7 @@ #include #include #include +#include #include #include #include @@ -83,6 +84,7 @@ static int mc_task; static bool motor_test_mode = false; static orb_advert_t actuator_pub; +static orb_advert_t control_debug_pub; static struct vehicle_status_s state; @@ -107,6 +109,9 @@ mc_thread_main(int argc, char *argv[]) struct actuator_controls_s actuators; memset(&actuators, 0, sizeof(actuators)); + struct vehicle_control_debug_s control_debug; + memset(&control_debug, 0, sizeof(control_debug)); + /* subscribe to attitude, motor setpoints and system state */ int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int param_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -133,6 +138,8 @@ mc_thread_main(int argc, char *argv[]) for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { actuators.control[i] = 0.0f; } + + control_debug_pub = orb_advertise(ORB_ID(vehicle_control_debug), &control_debug); actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); @@ -372,7 +379,8 @@ mc_thread_main(int argc, char *argv[]) /* measure in what intervals the controller runs */ perf_count(mc_interval_perf); - float gyro[3]; + float rates[3]; + float rates_acc[3]; /* get current rate setpoint */ bool rates_sp_valid = false; @@ -383,13 +391,21 @@ mc_thread_main(int argc, char *argv[]) } /* apply controller */ - gyro[0] = att.rollspeed; - gyro[1] = att.pitchspeed; - gyro[2] = att.yawspeed; + rates[0] = att.rollspeed; + rates[1] = att.pitchspeed; + rates[2] = att.yawspeed; - multirotor_control_rates(&rates_sp, gyro, &actuators); + rates_acc[0] = att.rollacc; + rates_acc[1] = att.pitchacc; + rates_acc[2] = att.yawacc; + + + + multirotor_control_rates(&rates_sp, rates, rates_acc, &actuators, &control_debug_pub, &control_debug); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + orb_publish(ORB_ID(vehicle_control_debug), control_debug_pub, &control_debug); + /* update state */ flag_control_attitude_enabled = state.flag_control_attitude_enabled; flag_control_manual_enabled = state.flag_control_manual_enabled; diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index 9135a93514..e37ede3e09 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -55,7 +55,7 @@ #include #include #include -#include + PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.0f); /* same on Flamewheel */ @@ -151,7 +151,8 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru } void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], struct actuator_controls_s *actuators) + const float rates[], const float rates_acc[], struct actuator_controls_s *actuators, + const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug) { static uint64_t last_run = 0; const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; @@ -178,10 +179,6 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, static bool initialized = false; - static struct vehicle_control_debug_s control_debug; - static orb_advert_t control_debug_pub; - - /* initialize the pid controllers when the function is called for the first time */ if (initialized == false) { @@ -189,10 +186,9 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, parameters_update(&h, &p); initialized = true; - pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_CALC); - pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_CALC); + pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_SET); + pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_SET); - control_debug_pub = orb_advertise(ORB_ID(vehicle_control_debug), &control_debug); } /* load new parameters with lower rate */ @@ -205,11 +201,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* control pitch (forward) output */ float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch , - rates[1], 0.0f, deltaT, &control_debug.pitch_rate_p, &control_debug.pitch_rate_i, &control_debug.pitch_rate_d); + rates[1], rates_acc[1], deltaT, &control_debug->pitch_rate_p, &control_debug->pitch_rate_i, &control_debug->pitch_rate_d); /* control roll (left/right) output */ float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll , - rates[0], 0.0f, deltaT, &control_debug.roll_rate_p, &control_debug.roll_rate_i, &control_debug.roll_rate_d); + rates[0], rates_acc[0], deltaT, &control_debug->roll_rate_p, &control_debug->roll_rate_i, &control_debug->roll_rate_d); /* increase resilience to faulty control inputs */ if (isfinite(pitch_control)) { @@ -229,7 +225,7 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, warnx("rej. NaN ctrl roll"); } - /* control yaw rate */ + /* control yaw rate */ //XXX use library here and use rates_acc[2] float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]); /* increase resilience to faulty control inputs */ @@ -244,6 +240,4 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, actuators->control[3] = rate_sp->thrust; motor_skip_counter++; - - orb_publish(ORB_ID(vehicle_control_debug), control_debug_pub, &control_debug); } diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.h b/src/modules/multirotor_att_control/multirotor_rate_control.h index 03dec317a3..fc741c3786 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.h +++ b/src/modules/multirotor_att_control/multirotor_rate_control.h @@ -49,8 +49,10 @@ #include #include #include +#include void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], struct actuator_controls_s *actuators); + const float rates[], const float rates_acc[], struct actuator_controls_s *actuators, + const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug); #endif /* MULTIROTOR_RATE_CONTROL_H_ */ From eb38ea17896e9970e9bdf532557ebcd87f81e34a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 19 Jun 2013 12:17:10 +0400 Subject: [PATCH 167/486] position_estimator_inav: better handling of lost GPS, minor fixes --- .../position_estimator_inav_main.c | 60 +++++++++---------- 1 file changed, 30 insertions(+), 30 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index a8a66e93d8..278a319b51 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -254,7 +254,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* initialize coordinates */ map_projection_init(lat_current, lon_current); warnx("init GPS: lat = %.10f, lon = %.10f", lat_current, lon_current); - mavlink_log_info(mavlink_fd, "[inav] init GPS: lat = %.10f, lon = %.10f", lat_current, lon_current); + mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat_current, lon_current); } } } @@ -361,34 +361,27 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } - if (params.use_gps) { + if (params.use_gps && fds[4].revents & POLLIN) { /* vehicle GPS position */ - if (fds[4].revents & POLLIN) { - /* new GPS value */ - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - if (gps.fix_type >= 3) { - /* project GPS lat lon to plane */ - float gps_proj[2]; - map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1])); - gps_corr[0][0] = gps_proj[0] - x_est[0]; - gps_corr[1][0] = gps_proj[1] - y_est[0]; - if (gps.vel_ned_valid) { - gps_corr[0][1] = gps.vel_n_m_s; - gps_corr[1][1] = gps.vel_e_m_s; - } else { - gps_corr[0][1] = 0.0f; - gps_corr[1][1] = 0.0f; - } - local_pos.valid = true; - gps_updates++; + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); + if (gps.fix_type >= 3) { + /* project GPS lat lon to plane */ + float gps_proj[2]; + map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1])); + gps_corr[0][0] = gps_proj[0] - x_est[0]; + gps_corr[1][0] = gps_proj[1] - y_est[0]; + if (gps.vel_ned_valid) { + gps_corr[0][1] = gps.vel_n_m_s; + gps_corr[1][1] = gps.vel_e_m_s; } else { - local_pos.valid = false; + gps_corr[0][1] = 0.0f; + gps_corr[1][1] = 0.0f; } + gps_updates++; + } else { + memset(gps_corr, 0, sizeof(gps_corr)); } - } else { - local_pos.valid = true; } - } /* end of poll return value check */ @@ -403,19 +396,25 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(baro_corr * dt, z_est, 0, params.w_alt_baro); inertial_filter_correct(accel_corr[2] * dt, z_est, 2, params.w_alt_acc); - if (params.use_gps) { + /* dont't try to estimate position when no any position source available */ + bool can_estimate_pos = params.use_gps && gps.fix_type >= 3; + if (can_estimate_pos) { /* inertial filter prediction for position */ inertial_filter_predict(dt, x_est); inertial_filter_predict(dt, y_est); /* inertial filter correction for position */ - inertial_filter_correct(gps_corr[0][0] * dt, x_est, 0, params.w_pos_gps_p); - inertial_filter_correct(gps_corr[0][1] * dt, x_est, 1, params.w_pos_gps_v); - inertial_filter_correct(gps_corr[1][0] * dt, y_est, 0, params.w_pos_gps_p); - inertial_filter_correct(gps_corr[1][1] * dt, y_est, 1, params.w_pos_gps_v); - inertial_filter_correct(accel_corr[0] * dt, x_est, 2, params.w_pos_acc); inertial_filter_correct(accel_corr[1] * dt, y_est, 2, params.w_pos_acc); + + if (params.use_gps && gps.fix_type >= 3) { + inertial_filter_correct(gps_corr[0][0] * dt, x_est, 0, params.w_pos_gps_p); + inertial_filter_correct(gps_corr[1][0] * dt, y_est, 0, params.w_pos_gps_p); + if (gps.vel_ned_valid) { + inertial_filter_correct(gps_corr[0][1] * dt, x_est, 1, params.w_pos_gps_v); + inertial_filter_correct(gps_corr[1][1] * dt, y_est, 1, params.w_pos_gps_v); + } + } } if (verbose_mode) { @@ -439,6 +438,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (t - pub_last > pub_interval) { pub_last = t; local_pos.timestamp = t; + local_pos.valid = can_estimate_pos; local_pos.x = x_est[0]; local_pos.vx = x_est[1]; local_pos.y = y_est[0]; From 53eb76f4d558d345cc85b8f30e232b9bca2f0e51 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 19 Jun 2013 10:34:41 +0200 Subject: [PATCH 168/486] Fixed numeration that was introduced through merge, I should add new log messages to the end --- src/modules/sdlog2/sdlog2_messages.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index a83a725148..b6537b2cdd 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -190,14 +190,14 @@ struct log_OUT0_s { }; /* --- AIRS - AIRSPEED --- */ -#define LOG_AIRS_MSG 13 +#define LOG_AIRS_MSG 14 struct log_AIRS_s { float indicated_airspeed; float true_airspeed; }; /* --- ARSP - ATTITUDE RATE SET POINT --- */ -#define LOG_ARSP_MSG 14 +#define LOG_ARSP_MSG 15 struct log_ARSP_s { float roll_rate_sp; float pitch_rate_sp; From d6c15b16792f3f087a0d934677615949c9e12688 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 19 Jun 2013 10:34:41 +0200 Subject: [PATCH 169/486] Fixed numeration that was introduced through merge, I should add new log messages to the end --- src/modules/sdlog2/sdlog2_messages.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index bd69445b54..2b61378ed7 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -187,14 +187,14 @@ struct log_OUT0_s { }; /* --- AIRS - AIRSPEED --- */ -#define LOG_AIRS_MSG 13 +#define LOG_AIRS_MSG 14 struct log_AIRS_s { float indicated_airspeed; float true_airspeed; }; /* --- ARSP - ATTITUDE RATE SET POINT --- */ -#define LOG_ARSP_MSG 14 +#define LOG_ARSP_MSG 15 struct log_ARSP_s { float roll_rate_sp; float pitch_rate_sp; From c1049483a82857bebb012607578add9492446321 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 19 Jun 2013 12:01:22 +0200 Subject: [PATCH 170/486] Added integral reset for rate controller --- .../multirotor_att_control/multirotor_rate_control.c | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index e37ede3e09..a0266e1b3f 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -199,6 +199,12 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f); } + /* reset integral if on ground */ + if (rate_sp->thrust < 0.01f) { + pid_reset_integral(&pitch_rate_controller); + pid_reset_integral(&roll_rate_controller); + } + /* control pitch (forward) output */ float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch , rates[1], rates_acc[1], deltaT, &control_debug->pitch_rate_p, &control_debug->pitch_rate_i, &control_debug->pitch_rate_d); From ece93ab62834be7f46501b1d31733cf58b5b1188 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 19 Jun 2013 12:01:22 +0200 Subject: [PATCH 171/486] Added integral reset for rate controller --- .../multirotor_att_control/multirotor_rate_control.c | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index e37ede3e09..a0266e1b3f 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -199,6 +199,12 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f); } + /* reset integral if on ground */ + if (rate_sp->thrust < 0.01f) { + pid_reset_integral(&pitch_rate_controller); + pid_reset_integral(&roll_rate_controller); + } + /* control pitch (forward) output */ float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch , rates[1], rates_acc[1], deltaT, &control_debug->pitch_rate_p, &control_debug->pitch_rate_i, &control_debug->pitch_rate_d); From 23858a6726f151cc6d67ecda0d42c7374839d80f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 19 Jun 2013 22:59:40 +0200 Subject: [PATCH 172/486] Added functionality to enable PWM output for stupid ESCs even when safety is not off, arming button functionality remains as is --- src/drivers/px4io/px4io.cpp | 71 +++++++++++++-- src/modules/px4iofirmware/mixer.cpp | 125 ++++++++++++++++++-------- src/modules/px4iofirmware/protocol.h | 6 +- src/modules/px4iofirmware/px4io.h | 1 + src/modules/px4iofirmware/registers.c | 39 +++++++- 5 files changed, 197 insertions(+), 45 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index bce193bcac..0ea90beb4b 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -136,6 +136,11 @@ public: */ int set_max_values(const uint16_t *vals, unsigned len); + /** + * Set an idle PWM signal that is active right after startup, even when SAFETY_SAFE + */ + int set_idle_values(const uint16_t *vals, unsigned len); + /** * Print the current status of IO */ @@ -567,7 +572,8 @@ PX4IO::init() io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED | PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | - PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK, 0); + PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | + PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE, 0); /* publish RC config to IO */ ret = io_set_rc_config(); @@ -793,6 +799,20 @@ PX4IO::set_max_values(const uint16_t *vals, unsigned len) return io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, vals, len); } +int +PX4IO::set_idle_values(const uint16_t *vals, unsigned len) +{ + uint16_t regs[_max_actuators]; + + if (len > _max_actuators) + /* fail with error */ + return E2BIG; + + printf("Sending IDLE values\n"); + + /* copy values to registers in IO */ + return io_reg_set(PX4IO_PAGE_IDLE_PWM, 0, vals, len); +} int @@ -1441,12 +1461,14 @@ PX4IO::print_status() /* setup and state */ printf("features 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES)); uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING); - printf("arming 0x%04x%s%s%s%s\n", + printf("arming 0x%04x%s%s%s%s%s%s\n", arming, ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : ""), ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : ""), ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""), - ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : "")); + ((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ? " FAILSAFE_CUSTOM" : ""), + ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : "")); printf("rates 0x%04x default %u alt %u relays 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES), io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE), @@ -1473,8 +1495,10 @@ PX4IO::print_status() } printf("failsafe"); for (unsigned i = 0; i < _max_actuators; i++) - printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); - printf("\n"); + printf(" %u\n", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); + printf("idle values"); + for (unsigned i = 0; i < _max_actuators; i++) + printf(" %u\n", io_reg_get(PX4IO_PAGE_IDLE_PWM, i)); } int @@ -1973,6 +1997,41 @@ px4io_main(int argc, char *argv[]) exit(0); } + if (!strcmp(argv[1], "idle")) { + + if (argc < 3) { + errx(1, "max command needs at least one channel value (PWM)"); + } + + if (g_dev != nullptr) { + + /* set values for first 8 channels, fill unassigned channels with 0. */ + uint16_t idle[8]; + + for (int i = 0; i < sizeof(idle) / sizeof(idle[0]); i++) + { + /* set channel to commanline argument or to 0 for non-provided channels */ + if (argc > i + 2) { + idle[i] = atoi(argv[i+2]); + if (idle[i] < 900 || idle[i] > 2100) { + errx(1, "value out of range of 900 < value < 2100. Aborting."); + } + } else { + /* a zero value will the default */ + idle[i] = 0; + } + } + + int ret = g_dev->set_idle_values(idle, sizeof(idle) / sizeof(idle[0])); + + if (ret != OK) + errx(ret, "failed setting idle values"); + } else { + errx(1, "not loaded"); + } + exit(0); + } + if (!strcmp(argv[1], "recovery")) { if (g_dev != nullptr) { @@ -2100,5 +2159,5 @@ px4io_main(int argc, char *argv[]) monitor(); out: - errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current', 'failsafe' or 'update'"); + errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current', 'failsafe', 'min, 'max', 'idle' or 'update'"); } diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 6752a8128d..1f118ea2f0 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -63,6 +63,7 @@ extern "C" { * Time that the ESCs need to initialize */ #define ESC_INIT_TIME_US 1000000 + #define ESC_RAMP_TIME_US 2000000 /* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */ #define ROLL 0 @@ -73,10 +74,17 @@ extern "C" { /* current servo arm/disarm state */ static bool mixer_servos_armed = false; - +static bool should_arm = false; +static bool should_always_enable_pwm = false; static uint64_t esc_init_time; -static bool esc_init_active = false; -static bool esc_init_done = false; + +enum esc_state_e { + ESC_OFF, + ESC_INIT, + ESC_RAMP, + ESC_ON +}; +static esc_state_e esc_state; /* selected control values and count for mixing */ enum mixer_source { @@ -175,18 +183,48 @@ mixer_tick(void) float outputs[IO_SERVO_COUNT]; unsigned mixed; - /* after arming, some ESCs need an initalization period, count the time from here */ - if (mixer_servos_armed && !esc_init_done && !esc_init_active) { - esc_init_time = hrt_absolute_time(); - esc_init_active = true; + uint16_t ramp_promille; + + /* update esc init state, but only if we are truely armed and not just PWM enabled */ + if (mixer_servos_armed && should_arm) { + + switch (esc_state) { + + /* after arming, some ESCs need an initalization period, count the time from here */ + case ESC_OFF: + esc_init_time = hrt_absolute_time(); + esc_state = ESC_INIT; + break; + + /* after waiting long enough for the ESC initialization, we can start with the ramp to start the ESCs */ + case ESC_INIT: + if (hrt_elapsed_time(&esc_init_time) > ESC_INIT_TIME_US) { + esc_state = ESC_RAMP; + } + break; + + /* then ramp until the min speed is reached */ + case ESC_RAMP: + if (hrt_elapsed_time(&esc_init_time) > (ESC_INIT_TIME_US + ESC_RAMP_TIME_US)) { + esc_state = ESC_ON; + } + break; + + case ESC_ON: + default: + + break; + } + } else { + esc_state = ESC_OFF; } - /* after waiting long enough for the ESC initialization, we can disable the ESC initialization phase */ - if (!esc_init_done && esc_init_active && mixer_servos_armed && (hrt_elapsed_time(&esc_init_time) > ESC_INIT_TIME_US)) { - esc_init_active = false; - esc_init_done = true; + /* do the calculations during the ramp for all at once */ + if(esc_state == ESC_RAMP) { + ramp_promille = (1000*(hrt_elapsed_time(&esc_init_time)-ESC_INIT_TIME_US))/ESC_RAMP_TIME_US; } + /* mix */ mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT); @@ -196,21 +234,27 @@ mixer_tick(void) /* save actuator values for FMU readback */ r_page_actuators[i] = FLOAT_TO_REG(outputs[i]); - /* XXX maybe this check for an armed FMU could be achieved a little less messy */ - if (source == MIX_FMU && !(r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)) { - r_page_servos[i] = r_page_servo_failsafe[i]; - } - /* during ESC initialization, use low PWM */ - else if (esc_init_active) { - r_page_servos[i] = (outputs[i] * 600 + 1500); - - /* afterwards use min and max values */ - } else { - r_page_servos[i] = (outputs[i] - * (r_page_servo_control_max[i] - r_page_servo_control_min[i])/2 - + (r_page_servo_control_max[i] + r_page_servo_control_min[i])/2); - } + switch (esc_state) { + case ESC_INIT: + r_page_servos[i] = (outputs[i] * 600 + 1500); + break; + case ESC_RAMP: + r_page_servos[i] = (outputs[i] + * (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 - ramp_promille*r_page_servo_control_min[i] - (1000-ramp_promille)*900)/2/1000 + + (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 + ramp_promille*r_page_servo_control_min[i] + (1000-ramp_promille)*900)/2/1000); + break; + + case ESC_ON: + r_page_servos[i] = (outputs[i] + * (r_page_servo_control_max[i] - r_page_servo_control_min[i])/2 + + (r_page_servo_control_max[i] + r_page_servo_control_min[i])/2); + break; + + case ESC_OFF: + default: + break; + } } for (unsigned i = mixed; i < IO_SERVO_COUNT; i++) r_page_servos[i] = 0; @@ -225,36 +269,43 @@ mixer_tick(void) * XXX correct behaviour for failsafe may require an additional case * here. */ - bool should_arm = ( + should_arm = ( /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) && - /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && - /* and either FMU is armed */ ( ( (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && - /* and there is valid input via direct PWM or mixer */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK))) || - - /* or failsafe was set manually */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ) + /* and IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && + /* and FMU is armed */ ( (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && + /* and there is valid input via direct PWM or mixer */ ((r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) || + /* or failsafe was set manually */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ) ) ); + should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) + && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) + && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK); - if (should_arm && !mixer_servos_armed) { + if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) { /* need to arm, but not armed */ up_pwm_servo_arm(true); mixer_servos_armed = true; r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED; - isr_debug(5, "> armed"); + isr_debug(5, "> PWM enabled"); - } else if (!should_arm && mixer_servos_armed) { + } else if ((!should_arm && !should_always_enable_pwm) && mixer_servos_armed) { /* armed but need to disarm */ up_pwm_servo_arm(false); mixer_servos_armed = false; r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED); - isr_debug(5, "> disarmed"); + isr_debug(5, "> PWM disabled"); - esc_init_active = false; } + } - if (mixer_servos_armed) { + if (mixer_servos_armed && should_arm) { /* update the servo outputs. */ for (unsigned i = 0; i < IO_SERVO_COUNT; i++) up_pwm_servo_set(i, r_page_servos[i]); + + } else if (mixer_servos_armed && should_always_enable_pwm) { + /* set the idle servo outputs. */ + for (unsigned i = 0; i < IO_SERVO_COUNT; i++) + up_pwm_servo_set(i, r_page_servo_idle[i]); } } diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index b430541973..0e477a200d 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -155,6 +155,7 @@ #define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK (1 << 2) /* OK to switch to manual override via override RC channel */ #define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM (1 << 3) /* use custom failsafe values, not 0 values of mixer */ #define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 4) /* OK to try in-air restart */ +#define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE (1 << 5) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */ #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 */ @@ -190,9 +191,12 @@ /* PWM minimum values for certain ESCs */ #define PX4IO_PAGE_CONTROL_MIN_PWM 106 /* 0..CONFIG_ACTUATOR_COUNT-1 */ - /* PWM maximum values for certain ESCs */ +/* PWM maximum values for certain ESCs */ #define PX4IO_PAGE_CONTROL_MAX_PWM 107 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +/* PWM idle values that are active, even when SAFETY_SAFE */ +#define PX4IO_PAGE_IDLE_PWM 108 /* 0..CONFIG_ACTUATOR_COUNT-1 */ + /** * As-needed mixer data upload. * diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 3e4027c9bd..042e7fe66f 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -79,6 +79,7 @@ extern uint16_t r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */ extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */ extern uint16_t r_page_servo_control_min[]; /* PX4IO_PAGE_CONTROL_MIN_PWM */ extern uint16_t r_page_servo_control_max[]; /* PX4IO_PAGE_CONTROL_MAX_PWM */ +extern uint16_t r_page_servo_idle[]; /* PX4IO_PAGE_IDLE_PWM */ /* * Register aliases. diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 379cf09e3e..bd13f3b7d8 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -147,7 +147,8 @@ volatile uint16_t r_page_setup[] = PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \ PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \ PX4IO_P_SETUP_ARMING_IO_ARM_OK) | \ - PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM + PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \ + PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE #define PX4IO_P_SETUP_RATES_VALID ((1 << IO_SERVO_COUNT) - 1) #define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) @@ -201,6 +202,14 @@ uint16_t r_page_servo_control_min[IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, */ uint16_t r_page_servo_control_max[IO_SERVO_COUNT] = { 2100, 2100, 2100, 2100, 2100, 2100, 2100, 2100 }; +/** + * PAGE 108 + * + * idle PWM values for difficult ESCs + * + */ +uint16_t r_page_servo_idle[IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 }; + void registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { @@ -308,6 +317,31 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num } break; + case PX4IO_PAGE_IDLE_PWM: + + /* copy channel data */ + while ((offset < IO_SERVO_COUNT) && (num_values > 0)) { + + if (*values == 0) + /* set to default */ + r_page_servo_idle[offset] = 0; + + else if (*values < 900) + r_page_servo_idle[offset] = 900; + else if (*values > 2100) + r_page_servo_idle[offset] = 2100; + else + r_page_servo_idle[offset] = *values; + + /* flag the failsafe values as custom */ + r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE; + + offset++; + num_values--; + values++; + } + break; + /* handle text going to the mixer parser */ case PX4IO_PAGE_MIXERLOAD: mixer_handle_text(values, num_values * sizeof(*values)); @@ -651,6 +685,9 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val case PX4IO_PAGE_CONTROL_MAX_PWM: SELECT_PAGE(r_page_servo_control_max); break; + case PX4IO_PAGE_IDLE_PWM: + SELECT_PAGE(r_page_servo_idle); + break; default: return -1; From 9b6c9358ed072459ac61feed271a209c8c5dea23 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 20 Jun 2013 01:13:49 +0200 Subject: [PATCH 173/486] First try for an ESC calibration tool --- makefiles/config_px4fmu_default.mk | 1 + src/drivers/px4io/px4io.cpp | 5 +- src/modules/px4iofirmware/mixer.cpp | 13 +- src/systemcmds/esc_calib/esc_calib.c | 250 +++++++++++++++++++++++++++ src/systemcmds/esc_calib/module.mk | 41 +++++ 5 files changed, 303 insertions(+), 7 deletions(-) create mode 100644 src/systemcmds/esc_calib/esc_calib.c create mode 100644 src/systemcmds/esc_calib/module.mk diff --git a/makefiles/config_px4fmu_default.mk b/makefiles/config_px4fmu_default.mk index 43288537cb..e8104b3511 100644 --- a/makefiles/config_px4fmu_default.mk +++ b/makefiles/config_px4fmu_default.mk @@ -46,6 +46,7 @@ MODULES += systemcmds/param MODULES += systemcmds/perf MODULES += systemcmds/preflight_check MODULES += systemcmds/pwm +MODULES += systemcmds/esc_calib MODULES += systemcmds/reboot MODULES += systemcmds/top MODULES += systemcmds/tests diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 0ea90beb4b..6d3c32ed95 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1496,9 +1496,10 @@ PX4IO::print_status() printf("failsafe"); for (unsigned i = 0; i < _max_actuators; i++) printf(" %u\n", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); - printf("idle values"); + printf("\nidle values"); for (unsigned i = 0; i < _max_actuators; i++) - printf(" %u\n", io_reg_get(PX4IO_PAGE_IDLE_PWM, i)); + printf(" %u", io_reg_get(PX4IO_PAGE_IDLE_PWM, i)); + printf("\n"); } int diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 1f118ea2f0..fe91667790 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -270,11 +270,14 @@ mixer_tick(void) * here. */ should_arm = ( - /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) && - /* and IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && - /* and FMU is armed */ ( (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && - /* and there is valid input via direct PWM or mixer */ ((r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) || - /* or failsafe was set manually */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ) ) + /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) + /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + /* and FMU is armed */ && ( + ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) + /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ) + /* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) + /* or failsafe was set manually */ || (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) + ) ); should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c new file mode 100644 index 0000000000..188fa4e371 --- /dev/null +++ b/src/systemcmds/esc_calib/esc_calib.c @@ -0,0 +1,250 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file esc_calib.c + * + * Tool for ESC calibration + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include "systemlib/systemlib.h" +#include "systemlib/err.h" +#include "drivers/drv_pwm_output.h" + +static void usage(const char *reason); +__EXPORT int esc_calib_main(int argc, char *argv[]); + +#define MAX_CHANNELS 8 + +static void +usage(const char *reason) +{ + if (reason != NULL) + warnx("%s", reason); + errx(1, + "usage:\n" + "esc_calib [-d ] \n" + " PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n" + " Provide channels (e.g.: 1 2 3 4)\n" + ); + +} + +int +esc_calib_main(int argc, char *argv[]) +{ + const char *dev = PWM_OUTPUT_DEVICE_PATH; + char *ep; + bool channels_selected[MAX_CHANNELS] = {false}; + int ch; + int ret; + char c; + + if (argc < 2) + usage(NULL); + + while ((ch = getopt(argc, argv, "d:")) != EOF) { + switch (ch) { + + case 'd': + dev = optarg; + argc--; + break; + + default: + usage(NULL); + } + } + + if(argc < 1) { + usage("no channels provided"); + } + + while (--argc) { + const char *arg = argv[argc]; + unsigned channel_number = strtol(arg, &ep, 0); + + if (*ep == '\0') { + if (channel_number > MAX_CHANNELS || channel_number <= 0) { + err(1, "invalid channel number: %d", channel_number); + } else { + channels_selected[channel_number-1] = true; + + } + } + } + + /* Wait for confirmation */ + int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY); + if (!console) + err(1, "failed opening console"); + + warnx("ATTENTION, please remove or fix props before starting calibration!\n" + "\n" + "Also press the arming switch first for safety off\n" + "\n" + "Do you really want to start calibration: y or n?\n"); + + /* wait for user input */ + while (1) { + + if (read(console, &c, 1) == 1) { + if (c == 'y' || c == 'Y') { + + break; + } else if (c == 0x03 || c == 0x63 || c == 'q') { + warnx("ESC calibration exited"); + close(console); + exit(0); + } else if (c == 'n' || c == 'N') { + warnx("ESC calibration aborted"); + close(console); + exit(0); + } else { + warnx("Unknown input, ESC calibration aborted"); + close(console); + exit(0); + } + } + /* rate limit to ~ 20 Hz */ + usleep(50000); + } + + /* open for ioctl only */ + int fd = open(dev, 0); + if (fd < 0) + err(1, "can't open %s", dev); + + // XXX arming not necessaire at the moment + // /* Then arm */ + // ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); + // if (ret != OK) + // err(1, "PWM_SERVO_SET_ARM_OK"); + + // ret = ioctl(fd, PWM_SERVO_ARM, 0); + // if (ret != OK) + // err(1, "PWM_SERVO_ARM"); + + + + + /* Wait for user confirmation */ + warnx("Set high PWM\n" + "Connect battery now and hit ENTER after the ESCs confirm the first calibration step"); + + while (1) { + + /* First set high PWM */ + for (unsigned i = 0; i Date: Thu, 20 Jun 2013 11:30:10 +0400 Subject: [PATCH 174/486] Att rate PID fix --- .../multirotor_att_control/multirotor_rate_control.c | 9 +++++---- src/modules/systemlib/pid/pid.c | 12 ++++++++---- src/modules/systemlib/pid/pid.h | 5 ++++- 3 files changed, 17 insertions(+), 9 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index a0266e1b3f..8f26a20141 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -179,6 +179,7 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, static bool initialized = false; + float diff_filter_factor = 1.0f; /* initialize the pid controllers when the function is called for the first time */ if (initialized == false) { @@ -186,8 +187,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, parameters_update(&h, &p); initialized = true; - pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_SET); - pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_SET); + pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, diff_filter_factor, PID_MODE_DERIVATIV_CALC_NO_SP); + pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, diff_filter_factor, PID_MODE_DERIVATIV_CALC_NO_SP); } @@ -195,8 +196,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, if (motor_skip_counter % 2500 == 0) { /* update parameters from storage */ parameters_update(&h, &p); - pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f); - pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f); + pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, diff_filter_factor); + pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, diff_filter_factor); } /* reset integral if on ground */ diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 3685b2aebb..ada364e372 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -144,15 +144,19 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo // Calculated current error value float error = pid->sp - val; - - float error_filtered = pid->diff_filter_factor*error + (1.0f-pid->diff_filter_factor)*pid->error_previous_filtered; + float error_filtered; // Calculate or measured current error derivative if (pid->mode == PID_MODE_DERIVATIV_CALC) { -// d = (error_filtered - pid->error_previous_filtered) / dt; - d = error_filtered - pid->error_previous_filtered ; + error_filtered = pid->error_previous_filtered + (error - pid->error_previous_filtered) * pid->diff_filter_factor; + d = (error_filtered - pid->error_previous_filtered) / fmaxf(dt, 0.003f); // fail-safe for too low dt + pid->error_previous_filtered = error_filtered; + } else if (pid->mode == PID_MODE_DERIVATIV_CALC_NO_SP) { + + error_filtered = pid->error_previous_filtered + (val - pid->error_previous_filtered) * pid->diff_filter_factor; + d = (error_filtered - pid->error_previous_filtered) / fmaxf(dt, 0.003f); // fail-safe for too low dt pid->error_previous_filtered = error_filtered; } else if (pid->mode == PID_MODE_DERIVATIV_SET) { d = -val_dot; diff --git a/src/modules/systemlib/pid/pid.h b/src/modules/systemlib/pid/pid.h index e3d9038cdd..eb9df96cc9 100644 --- a/src/modules/systemlib/pid/pid.h +++ b/src/modules/systemlib/pid/pid.h @@ -47,8 +47,11 @@ /* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error * val_dot in pid_calculate() will be ignored */ #define PID_MODE_DERIVATIV_CALC 0 +/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, setpoint derivative is ignored + * val_dot in pid_calculate() will be ignored */ +#define PID_MODE_DERIVATIV_CALC_NO_SP 1 /* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */ -#define PID_MODE_DERIVATIV_SET 1 +#define PID_MODE_DERIVATIV_SET 2 // Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID) #define PID_MODE_DERIVATIV_NONE 9 From 462a9c84540e9258a5ab4270b258e5809cb5f88b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 20 Jun 2013 11:32:25 +0400 Subject: [PATCH 175/486] sdlog2: add angular accelerations to ATT message --- src/modules/sdlog2/sdlog2.c | 3 +++ src/modules/sdlog2/sdlog2_messages.h | 5 ++++- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index f89b49acf9..3c3f1a0743 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -986,6 +986,9 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_ATT.roll_rate = buf.att.rollspeed; log_msg.body.log_ATT.pitch_rate = buf.att.pitchspeed; log_msg.body.log_ATT.yaw_rate = buf.att.yawspeed; + log_msg.body.log_ATT.roll_acc = buf.att.rollacc; + log_msg.body.log_ATT.pitch_acc = buf.att.pitchacc; + log_msg.body.log_ATT.yaw_acc = buf.att.yawacc; LOGBUFFER_WRITE_AND_COUNT(ATT); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 2b61378ed7..43645c302b 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -63,6 +63,9 @@ struct log_ATT_s { float roll_rate; float pitch_rate; float yaw_rate; + float roll_acc; + float pitch_acc; + float yaw_acc; }; /* --- ATSP - ATTITUDE SET POINT --- */ @@ -206,7 +209,7 @@ struct log_ARSP_s { static const struct log_format_s log_formats[] = { LOG_FORMAT(TIME, "Q", "StartTime"), - LOG_FORMAT(ATT, "ffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate"), + LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollR,PitchR,YawR,RollA,PitchA,YawA"), LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"), From dec1fdbde0c7bb6f3eacae97ab9656f77294cbfc Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 20 Jun 2013 11:52:05 +0400 Subject: [PATCH 176/486] Cleanup: remove useless angular rates from attitude rate controller --- .../multirotor_att_control/multirotor_att_control_main.c | 8 +------- .../multirotor_att_control/multirotor_rate_control.c | 6 +++--- .../multirotor_att_control/multirotor_rate_control.h | 2 +- 3 files changed, 5 insertions(+), 11 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 38775ed1f5..8ba3241ad5 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -395,13 +395,7 @@ mc_thread_main(int argc, char *argv[]) rates[1] = att.pitchspeed; rates[2] = att.yawspeed; - rates_acc[0] = att.rollacc; - rates_acc[1] = att.pitchacc; - rates_acc[2] = att.yawacc; - - - - multirotor_control_rates(&rates_sp, rates, rates_acc, &actuators, &control_debug_pub, &control_debug); + multirotor_control_rates(&rates_sp, rates, &actuators, &control_debug_pub, &control_debug); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); orb_publish(ORB_ID(vehicle_control_debug), control_debug_pub, &control_debug); diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index 8f26a20141..641d833f08 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -151,7 +151,7 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru } void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], const float rates_acc[], struct actuator_controls_s *actuators, + const float rates[], struct actuator_controls_s *actuators, const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug) { static uint64_t last_run = 0; @@ -208,11 +208,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* control pitch (forward) output */ float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch , - rates[1], rates_acc[1], deltaT, &control_debug->pitch_rate_p, &control_debug->pitch_rate_i, &control_debug->pitch_rate_d); + rates[1], 0.0f, deltaT, &control_debug->pitch_rate_p, &control_debug->pitch_rate_i, &control_debug->pitch_rate_d); /* control roll (left/right) output */ float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll , - rates[0], rates_acc[0], deltaT, &control_debug->roll_rate_p, &control_debug->roll_rate_i, &control_debug->roll_rate_d); + rates[0], 0.0f, deltaT, &control_debug->roll_rate_p, &control_debug->roll_rate_i, &control_debug->roll_rate_d); /* increase resilience to faulty control inputs */ if (isfinite(pitch_control)) { diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.h b/src/modules/multirotor_att_control/multirotor_rate_control.h index fc741c3786..465549540c 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.h +++ b/src/modules/multirotor_att_control/multirotor_rate_control.h @@ -52,7 +52,7 @@ #include void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], const float rates_acc[], struct actuator_controls_s *actuators, + const float rates[], struct actuator_controls_s *actuators, const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug); #endif /* MULTIROTOR_RATE_CONTROL_H_ */ From bf0de775329acfc8c450b2958222a83f2a32f977 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 20 Jun 2013 12:33:35 +0400 Subject: [PATCH 177/486] Critical bugfix in PID --- src/modules/systemlib/pid/pid.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index ada364e372..2aafe06366 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -155,7 +155,7 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo pid->error_previous_filtered = error_filtered; } else if (pid->mode == PID_MODE_DERIVATIV_CALC_NO_SP) { - error_filtered = pid->error_previous_filtered + (val - pid->error_previous_filtered) * pid->diff_filter_factor; + error_filtered = pid->error_previous_filtered + (- val - pid->error_previous_filtered) * pid->diff_filter_factor; d = (error_filtered - pid->error_previous_filtered) / fmaxf(dt, 0.003f); // fail-safe for too low dt pid->error_previous_filtered = error_filtered; } else if (pid->mode == PID_MODE_DERIVATIV_SET) { From 7d37c2a8b36309f27d7001ba035d30c72620ba05 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 20 Jun 2013 17:32:29 +0400 Subject: [PATCH 178/486] multirotor_pos_control: bugs fixed --- .../multirotor_pos_control.c | 14 +++++++------- .../multirotor_pos_control_params.c | 2 +- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index c94972e7df..508879ae2e 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -302,14 +302,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE) { /* move position setpoint with manual controls */ - float pos_x_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz); - float pos_y_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz); - if (pos_x_sp_ctl != 0.0f || pos_y_sp_ctl != 0.0f) { + float pos_pitch_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz); + float pos_roll_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz); + if (pos_pitch_sp_ctl != 0.0f || pos_roll_sp_ctl != 0.0f) { /* calculate direction and increment of control in NED frame */ - float dir_sp_ctl = att.yaw + atan2f(pos_y_sp_ctl, pos_x_sp_ctl); - float d_sp_ctl = norm(pos_x_sp_ctl, pos_y_sp_ctl) * params.pos_rate_max; - pos_sp_speed_x = cosf(dir_sp_ctl) * d_sp_ctl; - pos_sp_speed_x = sinf(dir_sp_ctl) * d_sp_ctl; + float pos_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl); + float pos_sp_ctl_speed = norm(pos_pitch_sp_ctl, pos_roll_sp_ctl) * params.pos_rate_max; + pos_sp_speed_x = cosf(pos_sp_ctl_dir) * pos_sp_ctl_speed; + pos_sp_speed_y = sinf(pos_sp_ctl_dir) * pos_sp_ctl_speed; local_pos_sp.x += pos_sp_speed_x * dt; local_pos_sp.y += pos_sp_speed_y * dt; /* limit maximum setpoint from position offset and preserve direction */ diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c index 7c3296cae3..d284de433c 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -68,7 +68,7 @@ int parameters_init(struct multirotor_position_control_param_handles *h) h->pos_d = param_find("MPC_POS_D"); h->pos_rate_max = param_find("MPC_POS_RATE_MAX"); h->slope_max = param_find("MPC_SLOPE_MAX"); - h->slope_max = param_find("MPC_HARD"); + h->hard = param_find("MPC_HARD"); h->rc_scale_pitch = param_find("RC_SCALE_PITCH"); h->rc_scale_roll = param_find("RC_SCALE_ROLL"); From 53dec130c49f69f10527ab55d69de46ee26c58f6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 24 Jun 2013 09:49:46 +0200 Subject: [PATCH 179/486] Adapted flow estimator, position and velocity control to new state machine --- .../flow_position_control_main.c | 21 +++++------ .../flow_position_estimator_main.c | 36 +++++++++++-------- .../flow_speed_control_main.c | 22 +++++++----- .../multirotor_att_control_main.c | 2 +- .../multirotor_attitude_control.c | 8 ++--- 5 files changed, 50 insertions(+), 39 deletions(-) diff --git a/src/examples/flow_position_control/flow_position_control_main.c b/src/examples/flow_position_control/flow_position_control_main.c index 5246ea62b6..ecc133f63f 100644 --- a/src/examples/flow_position_control/flow_position_control_main.c +++ b/src/examples/flow_position_control/flow_position_control_main.c @@ -55,7 +55,8 @@ #include #include #include -#include +#include +#include #include #include #include @@ -158,7 +159,8 @@ flow_position_control_thread_main(int argc, char *argv[]) const float time_scale = powf(10.0f,-6.0f); /* structures */ - struct vehicle_status_s vstatus; + struct actuator_safety_s safety; + struct vehicle_control_mode_s control_mode; struct vehicle_attitude_s att; struct manual_control_setpoint_s manual; struct filtered_bottom_flow_s filtered_flow; @@ -169,7 +171,8 @@ flow_position_control_thread_main(int argc, char *argv[]) /* subscribe to attitude, motor setpoints and system state */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow)); int vehicle_local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); @@ -258,7 +261,7 @@ flow_position_control_thread_main(int argc, char *argv[]) perf_begin(mc_loop_perf); /* get a local copy of the vehicle state */ - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus); + orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); /* get a local copy of manual setpoint */ orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual); /* get a local copy of attitude */ @@ -268,9 +271,7 @@ flow_position_control_thread_main(int argc, char *argv[]) /* get a local copy of local position */ orb_copy(ORB_ID(vehicle_local_position), vehicle_local_position_sub, &local_pos); -// XXX fix this -#if 0 - if (vstatus.state_machine == SYSTEM_STATE_AUTO) + if (control_mode.flag_control_velocity_enabled) { float manual_pitch = manual.pitch / params.rc_scale_pitch; // 0 to 1 float manual_roll = manual.roll / params.rc_scale_roll; // 0 to 1 @@ -492,7 +493,7 @@ flow_position_control_thread_main(int argc, char *argv[]) /* store actual height for speed estimation */ last_local_pos_z = local_pos.z; - speed_sp.thrust_sp = thrust_control; + speed_sp.thrust_sp = thrust_control; //manual.throttle; speed_sp.timestamp = hrt_absolute_time(); /* publish new speed setpoint */ @@ -529,7 +530,6 @@ flow_position_control_thread_main(int argc, char *argv[]) if(isfinite(manual.throttle)) speed_sp.thrust_sp = manual.throttle; } -#endif /* measure in what intervals the controller runs */ perf_count(mc_interval_perf); perf_end(mc_loop_perf); @@ -578,7 +578,8 @@ flow_position_control_thread_main(int argc, char *argv[]) close(parameter_update_sub); close(vehicle_attitude_sub); close(vehicle_local_position_sub); - close(vehicle_status_sub); + close(safety_sub); + close(control_mode_sub); close(manual_control_setpoint_sub); close(speed_sp_pub); diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c index 2389c693d0..8f84307a7a 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_main.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c @@ -56,7 +56,8 @@ #include #include #include -#include +#include +#include #include #include #include @@ -143,8 +144,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) printf("[flow position estimator] starting\n"); /* rotation matrix for transformation of optical flow speed vectors */ - static const int8_t rotM_flow_sensor[3][3] = {{ 0, 1, 0 }, - { -1, 0, 0 }, + static const int8_t rotM_flow_sensor[3][3] = {{ 0, -1, 0 }, + { 1, 0, 0 }, { 0, 0, 1 }}; // 90deg rotated const float time_scale = powf(10.0f,-6.0f); static float speed[3] = {0.0f, 0.0f, 0.0f}; @@ -158,8 +159,10 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) static float sonar_lp = 0.0f; /* subscribe to vehicle status, attitude, sensors and flow*/ - struct vehicle_status_s vstatus; - memset(&vstatus, 0, sizeof(vstatus)); + struct actuator_safety_s safety; + memset(&safety, 0, sizeof(safety)); + struct vehicle_control_mode_s control_mode; + memset(&control_mode, 0, sizeof(control_mode)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_attitude_setpoint_s att_sp; @@ -170,8 +173,11 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) /* subscribe to parameter changes */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); - /* subscribe to vehicle status */ - int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + /* subscribe to safety topic */ + int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + + /* subscribe to safety topic */ + int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); /* subscribe to attitude */ int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); @@ -218,8 +224,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) while (!thread_should_exit) { -// XXX fix this -#if 0 + if (sensors_ready) { /*This runs at the rate of the sensors */ @@ -265,7 +270,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) /* got flow, updating attitude and status as well */ orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); orb_copy(ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub, &att_sp); - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus); + orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); /* vehicle state estimation */ float sonar_new = flow.ground_distance_m; @@ -278,12 +284,12 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) if (!vehicle_liftoff) { - if (vstatus.flag_system_armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f) + if (safety.armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f) vehicle_liftoff = true; } else { - if (!vstatus.flag_system_armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f)) + if (!safety.armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f)) vehicle_liftoff = false; } @@ -350,7 +356,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) } /* filtering ground distance */ - if (!vehicle_liftoff || !vstatus.flag_system_armed) + if (!vehicle_liftoff || !safety.armed) { /* not possible to fly */ sonar_valid = false; @@ -440,7 +446,6 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) } counter++; - #endif } printf("[flow position estimator] exiting.\n"); @@ -448,7 +453,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) close(vehicle_attitude_setpoint_sub); close(vehicle_attitude_sub); - close(vehicle_status_sub); + close(safety_sub); + close(control_mode_sub); close(parameter_update_sub); close(optical_flow_sub); diff --git a/src/examples/flow_speed_control/flow_speed_control_main.c b/src/examples/flow_speed_control/flow_speed_control_main.c index 0be4b3d5a0..4f60e34f2c 100644 --- a/src/examples/flow_speed_control/flow_speed_control_main.c +++ b/src/examples/flow_speed_control/flow_speed_control_main.c @@ -55,7 +55,8 @@ #include #include #include -#include +#include +#include #include #include #include @@ -155,7 +156,8 @@ flow_speed_control_thread_main(int argc, char *argv[]) uint32_t counter = 0; /* structures */ - struct vehicle_status_s vstatus; + struct actuator_safety_s safety; + struct vehicle_control_mode_s control_mode; struct filtered_bottom_flow_s filtered_flow; struct vehicle_bodyframe_speed_setpoint_s speed_sp; @@ -164,7 +166,8 @@ flow_speed_control_thread_main(int argc, char *argv[]) /* subscribe to attitude, motor setpoints and system state */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow)); int vehicle_bodyframe_speed_setpoint_sub = orb_subscribe(ORB_ID(vehicle_bodyframe_speed_setpoint)); @@ -186,7 +189,6 @@ flow_speed_control_thread_main(int argc, char *argv[]) while (!thread_should_exit) { -#if 0 /* wait for first attitude msg to be sure all data are available */ if (sensors_ready) { @@ -227,14 +229,16 @@ flow_speed_control_thread_main(int argc, char *argv[]) { perf_begin(mc_loop_perf); - /* get a local copy of the vehicle state */ - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus); + /* get a local copy of the safety topic */ + orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + /* get a local copy of the control mode */ + orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); /* get a local copy of filtered bottom flow */ orb_copy(ORB_ID(filtered_bottom_flow), filtered_bottom_flow_sub, &filtered_flow); /* get a local copy of bodyframe speed setpoint */ orb_copy(ORB_ID(vehicle_bodyframe_speed_setpoint), vehicle_bodyframe_speed_setpoint_sub, &speed_sp); - if (vstatus.state_machine == SYSTEM_STATE_AUTO) + if (control_mode.flag_control_velocity_enabled) { /* calc new roll/pitch */ float pitch_body = -(speed_sp.vx - filtered_flow.vx) * params.speed_p; @@ -341,7 +345,6 @@ flow_speed_control_thread_main(int argc, char *argv[]) } } } -#endif } printf("[flow speed control] ending now...\n"); @@ -352,7 +355,8 @@ flow_speed_control_thread_main(int argc, char *argv[]) close(vehicle_attitude_sub); close(vehicle_bodyframe_speed_setpoint_sub); close(filtered_bottom_flow_sub); - close(vehicle_status_sub); + close(safety_sub); + close(control_mode_sub); close(att_sp_pub); perf_print_counter(mc_loop_perf); diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 41a9f1df5c..4467d2d821 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -331,7 +331,7 @@ mc_thread_main(int argc, char *argv[]) // */ // /* only move setpoint if manual input is != 0 */ - if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { + if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.1f) { rates_sp.yaw = manual.yaw; control_yaw_position = false; first_time_after_yaw_speed_control = true; diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index 752f292e31..4a1129e1f3 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -156,8 +156,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_init(&h); parameters_update(&h, &p); - pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f, PID_MODE_DERIVATIV_SET); - pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f, PID_MODE_DERIVATIV_SET); + pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 1.0f, PID_MODE_DERIVATIV_SET); + pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 1.0f, PID_MODE_DERIVATIV_SET); initialized = true; } @@ -168,8 +168,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_update(&h, &p); /* apply parameters */ - pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f); - pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f); + pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 1.0f); + pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 1.0f); } /* reset integral if on ground */ From 60e9ea977d90a05826c86eea14f4a2807851d49c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 24 Jun 2013 09:50:55 +0200 Subject: [PATCH 180/486] Conditions where set wrongly in the first 2s after boot --- src/modules/commander/commander.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index edcdf7e546..9d3adaa1d4 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -1658,7 +1658,7 @@ int commander_thread_main(int argc, char *argv[]) /* check for global or local position updates, set a timeout of 2s */ - if (hrt_absolute_time() - last_global_position_time < 2000000) { + if (hrt_absolute_time() - last_global_position_time < 2000000 && hrt_absolute_time() > 2000000) { current_status.condition_global_position_valid = true; // XXX check for controller status and home position as well @@ -1666,7 +1666,7 @@ int commander_thread_main(int argc, char *argv[]) current_status.condition_global_position_valid = false; } - if (hrt_absolute_time() - last_local_position_time < 2000000) { + if (hrt_absolute_time() - last_local_position_time < 2000000 && hrt_absolute_time() > 2000000) { current_status.condition_local_position_valid = true; // XXX check for controller status and home position as well @@ -1675,7 +1675,7 @@ int commander_thread_main(int argc, char *argv[]) } /* Check for valid airspeed/differential pressure measurements */ - if (hrt_absolute_time() - last_diff_pres_time < 2000000) { + if (hrt_absolute_time() - last_diff_pres_time < 2000000 && hrt_absolute_time() > 2000000) { current_status.condition_airspeed_valid = true; } else { From d563960267ab1145d5100f9dcfad6035205e4021 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 24 Jun 2013 17:24:04 +0200 Subject: [PATCH 181/486] Added Flow messages to sdlog2 --- src/modules/sdlog2/sdlog2.c | 10 ++++++++-- src/modules/sdlog2/sdlog2_messages.h | 10 ++++++++++ 2 files changed, 18 insertions(+), 2 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index edcba4e7d0..33b3c657b3 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -616,7 +616,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ /* number of messages */ - const ssize_t fdsc = 19; + const ssize_t fdsc = 20; /* Sanity check variable and index */ ssize_t fdsc_count = 0; @@ -686,6 +686,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_SENS_s log_SENS; struct log_LPOS_s log_LPOS; struct log_LPSP_s log_LPSP; + struct log_FLOW_s log_FLOW; struct log_GPS_s log_GPS; struct log_ATTC_s log_ATTC; struct log_STAT_s log_STAT; @@ -1129,7 +1130,12 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- FLOW --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow); - // TODO not implemented yet + log_msg.msg_type = LOG_FLOW_MSG; + log_msg.body.log_FLOW.flow_comp_x_m = buf.flow.flow_comp_x_m; + log_msg.body.log_FLOW.flow_comp_y_m = buf.flow.flow_comp_y_m; + log_msg.body.log_FLOW.ground_distance_m = buf.flow.ground_distance_m; + log_msg.body.log_FLOW.quality = buf.flow.quality; + LOGBUFFER_WRITE_AND_COUNT(FLOW); } /* --- RC CHANNELS --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 04eae40b04..755d21d55d 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -206,6 +206,15 @@ struct log_ARSP_s { float pitch_rate_sp; float yaw_rate_sp; }; + +/* --- FLOW - FLOW DATA --- */ +#define LOG_FLOW_MSG 16 +struct log_FLOW_s { + float flow_comp_x_m; + float flow_comp_y_m; + float ground_distance_m; + uint8_t quality; +}; #pragma pack(pop) /* construct list of all message formats */ @@ -226,6 +235,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), + LOG_FORMAT(FLOW, "fffB", "FlowX,FlowY,SonAlt,FQual") }; static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); From 794a2248f02e014bc81e977da5a4eee7d71902b5 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 24 Jun 2013 17:24:49 +0200 Subject: [PATCH 182/486] Only some small cleanup to att control --- .../multirotor_att_control_main.c | 98 +++++-------------- 1 file changed, 22 insertions(+), 76 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 4467d2d821..25447aaba2 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -256,7 +256,7 @@ mc_thread_main(int argc, char *argv[]) } - } else if (control_mode.flag_control_manual_enabled) { + } else if (control_mode.flag_control_manual_enabled) { if (control_mode.flag_control_attitude_enabled) { /* initialize to current yaw if switching to manual or att control */ if (control_mode.flag_control_attitude_enabled != flag_control_attitude_enabled || @@ -268,90 +268,34 @@ mc_thread_main(int argc, char *argv[]) static bool rc_loss_first_time = true; /* if the RC signal is lost, try to stay level and go slowly back down to ground */ -/* XXX fix this */ -#if 0 - if (state.rc_signal_lost) { - /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ - param_get(failsafe_throttle_handle, &failsafe_throttle); - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - /* - * Only go to failsafe throttle if last known throttle was - * high enough to create some lift to make hovering state likely. - * - * This is to prevent that someone landing, but not disarming his - * multicopter (throttle = 0) does not make it jump up in the air - * if shutting down his remote. - */ - if (isfinite(manual.throttle) && manual.throttle > 0.2f) { - att_sp.thrust = failsafe_throttle; + rc_loss_first_time = true; - } else { - att_sp.thrust = 0.0f; - } + att_sp.roll_body = manual.roll; + att_sp.pitch_body = manual.pitch; - /* keep current yaw, do not attempt to go to north orientation, - * since if the pilot regains RC control, he will be lost regarding - * the current orientation. - */ - if (rc_loss_first_time) - att_sp.yaw_body = att.yaw; + /* set attitude if arming */ + if (!flag_control_attitude_enabled && safety.armed) { + att_sp.yaw_body = att.yaw; + } - rc_loss_first_time = false; + /* only move setpoint if manual input is != 0 */ + if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.1f) { + rates_sp.yaw = manual.yaw; + control_yaw_position = false; + first_time_after_yaw_speed_control = true; } else { -#endif - rc_loss_first_time = true; - - att_sp.roll_body = manual.roll; - att_sp.pitch_body = manual.pitch; - - /* set attitude if arming */ - if (!flag_control_attitude_enabled && safety.armed) { + if (first_time_after_yaw_speed_control) { att_sp.yaw_body = att.yaw; + first_time_after_yaw_speed_control = false; } - /* act if stabilization is active or if the (nonsense) direct pass through mode is set */ -#warning fix this -// if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS || -// state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { -// -// if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) { -// rates_sp.yaw = manual.yaw; -// control_yaw_position = false; -// -// } else { -// /* -// * This mode SHOULD be the default mode, which is: -// * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS -// * -// * However, we fall back to this setting for all other (nonsense) -// * settings as well. -// */ -// - /* only move setpoint if manual input is != 0 */ - if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.1f) { - rates_sp.yaw = manual.yaw; - control_yaw_position = false; - first_time_after_yaw_speed_control = true; - - } else { - if (first_time_after_yaw_speed_control) { - att_sp.yaw_body = att.yaw; - first_time_after_yaw_speed_control = false; - } - - control_yaw_position = true; - } -// } -// } - - att_sp.thrust = manual.throttle; - att_sp.timestamp = hrt_absolute_time(); -#if 0 + control_yaw_position = true; } -#endif + + att_sp.thrust = manual.throttle; + att_sp.timestamp = hrt_absolute_time(); /* STEP 2: publish the controller output */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); @@ -369,7 +313,9 @@ mc_thread_main(int argc, char *argv[]) } } else { -#warning fix this + + //XXX TODO add acro mode here + /* manual rate inputs, from RC control or joystick */ // if (state.flag_control_rates_enabled && // state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) { From 2096da2d4da2d4482dcdb328d35255101fc722bd Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 25 Jun 2013 13:11:15 +0200 Subject: [PATCH 183/486] Don't interrupt a playing tune unless its a repeated one --- src/drivers/stm32/tone_alarm/tone_alarm.cpp | 29 ++++++++++++++++++--- 1 file changed, 25 insertions(+), 4 deletions(-) diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index ac5511e60a..e8c6fd5426 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -241,6 +241,8 @@ private: static const unsigned _default_ntunes; static const uint8_t _note_tab[]; + unsigned _default_tune_number; // number of currently playing default tune (0 for none) + const char *_user_tune; const char *_tune; // current tune string @@ -456,6 +458,11 @@ const char * const ToneAlarm::_default_tunes[] = { "O1B8O2G+8E8B8G+8O3E8O2B8O3E8O2B8O3G+8E8B8" "O3G+8O4E4P8E16E16E8E8E8E8E4P8E16E4P8O2E16" "O2E2P64", + "MNT75L1O2G", //arming warning + "MBNT100a8", //battery warning slow + "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8" + "a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8" + "a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8", //battery warning fast // XXX why is there a break before a repetition }; const unsigned ToneAlarm::_default_ntunes = sizeof(_default_tunes) / sizeof(_default_tunes[0]); @@ -471,6 +478,7 @@ extern "C" __EXPORT int tone_alarm_main(int argc, char *argv[]); ToneAlarm::ToneAlarm() : CDev("tone_alarm", "/dev/tone_alarm"), + _default_tune_number(0), _user_tune(nullptr), _tune(nullptr), _next(nullptr) @@ -803,8 +811,12 @@ tune_error: // stop (and potentially restart) the tune tune_end: stop_note(); - if (_repeat) + if (_repeat) { start_tune(_tune); + } else { + _tune = nullptr; + _default_tune_number = 0; + } return; } @@ -873,8 +885,17 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg) _tune = nullptr; _next = nullptr; } else { - // play the selected tune - start_tune(_default_tunes[arg - 1]); + /* don't interrupt alarms unless they are repeated */ + if (_tune != nullptr && !_repeat) { + /* abort and let the current tune finish */ + result = -EBUSY; + } else if (_repeat && _default_tune_number == arg) { + /* requested repeating tune already playing */ + } else { + // play the selected tune + _default_tune_number = arg; + start_tune(_default_tunes[arg - 1]); + } } } else { result = -EINVAL; From a6ba7e448586c556009132887503e6830c20029e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 25 Jun 2013 13:15:38 +0200 Subject: [PATCH 184/486] Dropped superseded safety topic, added warning tones before arming --- src/drivers/px4io/px4io.cpp | 15 +++--- src/modules/commander/commander.c | 64 +++++++++++++++-------- src/modules/uORB/objects_common.cpp | 3 -- src/modules/uORB/topics/actuator_safety.h | 2 +- src/modules/uORB/topics/safety.h | 60 --------------------- 5 files changed, 53 insertions(+), 91 deletions(-) delete mode 100644 src/modules/uORB/topics/safety.h diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 6d3c32ed95..df2f18270d 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -81,7 +81,6 @@ #include #include #include -#include #include #include @@ -990,20 +989,24 @@ PX4IO::io_handle_status(uint16_t status) /** * Get and handle the safety status */ - struct safety_s safety; + struct actuator_safety_s safety; safety.timestamp = hrt_absolute_time(); + orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &safety); + if (status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { - safety.status = SAFETY_STATUS_UNLOCKED; + safety.safety_off = true; + safety.safety_switch_available = true; } else { - safety.status = SAFETY_STATUS_SAFE; + safety.safety_off = false; + safety.safety_switch_available = true; } /* lazily publish the safety status */ if (_to_safety > 0) { - orb_publish(ORB_ID(safety), _to_safety, &safety); + orb_publish(ORB_ID(actuator_safety), _to_safety, &safety); } else { - _to_safety = orb_advertise(ORB_ID(safety), &safety); + _to_safety = orb_advertise(ORB_ID(actuator_safety), &safety); } return ret; diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index 9d3adaa1d4..4c884aed36 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. * Author: Petri Tanskanen * Lorenz Meier * Thomas Gubler @@ -79,7 +79,6 @@ #include #include #include -#include #include #include @@ -1159,6 +1158,9 @@ int commander_thread_main(int argc, char *argv[]) commander_initialized = false; bool home_position_set = false; + bool battery_tune_played = false; + bool arm_tune_played = false; + /* set parameters */ failsafe_lowlevel_timeout_ms = 0; param_get(param_find("SYS_FAILSAVE_LL"), &failsafe_lowlevel_timeout_ms); @@ -1248,6 +1250,9 @@ int commander_thread_main(int argc, char *argv[]) safety_pub = orb_advertise(ORB_ID(actuator_safety), &safety); + /* but also subscribe to it */ + int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); /* home position */ @@ -1477,6 +1482,13 @@ int commander_thread_main(int argc, char *argv[]) } } + /* update safety topic */ + orb_check(safety_sub, &new_data); + + if (new_data) { + orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + } + /* update global position estimate */ orb_check(global_position_sub, &new_data); @@ -1573,25 +1585,6 @@ int commander_thread_main(int argc, char *argv[]) last_idle_time = system_load.tasks[0].total_runtime; } - // // XXX Export patterns and threshold to parameters - /* Trigger audio event for low battery */ - if (bat_remain < 0.1f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 4) == 0)) { - /* For less than 10%, start be really annoying at 5 Hz */ - ioctl(buzzer, TONE_SET_ALARM, 0); - ioctl(buzzer, TONE_SET_ALARM, 3); - - } else if (bat_remain < 0.1f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 4) == 2)) { - ioctl(buzzer, TONE_SET_ALARM, 0); - - } else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 0)) { - /* For less than 20%, start be slightly annoying at 1 Hz */ - ioctl(buzzer, TONE_SET_ALARM, 0); - tune_positive(); - - } else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 2)) { - ioctl(buzzer, TONE_SET_ALARM, 0); - } - /* Check battery voltage */ /* write to sys_status */ if (battery_voltage_valid) { @@ -2194,6 +2187,8 @@ int commander_thread_main(int argc, char *argv[]) } + + current_status.counter++; current_status.timestamp = hrt_absolute_time(); @@ -2219,6 +2214,33 @@ int commander_thread_main(int argc, char *argv[]) /* Store old modes to detect and act on state transitions */ voltage_previous = current_status.voltage_battery; + + + /* play tone according to evaluation result */ + /* check if we recently armed */ + if (!arm_tune_played && safety.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available))) { + ioctl(buzzer, TONE_SET_ALARM, 12); + arm_tune_played = true; + + // // XXX Export patterns and threshold to parameters + /* Trigger audio event for low battery */ + } else if (bat_remain < 0.1f && battery_voltage_valid) { + ioctl(buzzer, TONE_SET_ALARM, 14); + battery_tune_played = true; + } else if (bat_remain < 0.2f && battery_voltage_valid) { + ioctl(buzzer, TONE_SET_ALARM, 13); + battery_tune_played = true; + } else if(battery_tune_played) { + ioctl(buzzer, TONE_SET_ALARM, 0); + battery_tune_played = false; + } + + /* reset arm_tune_played when disarmed */ + if (!(safety.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available)))) { + arm_tune_played = false; + } + + /* XXX use this voltage_previous */ fflush(stdout); counter++; diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index b5dafd0caa..b602b17149 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -169,6 +169,3 @@ ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s); #include "topics/debug_key_value.h" ORB_DEFINE(debug_key_value, struct debug_key_value_s); -/* status of the system safety device */ -#include "topics/safety.h" -ORB_DEFINE(safety, struct safety_s); diff --git a/src/modules/uORB/topics/actuator_safety.h b/src/modules/uORB/topics/actuator_safety.h index 3a107d41aa..c431217ab1 100644 --- a/src/modules/uORB/topics/actuator_safety.h +++ b/src/modules/uORB/topics/actuator_safety.h @@ -53,7 +53,7 @@ struct actuator_safety_s { uint64_t timestamp; - + bool safety_switch_available; /**< Set to true if a safety switch is connected */ bool safety_off; /**< Set to true if safety is off */ bool armed; /**< Set to true if system is armed */ bool ready_to_arm; /**< Set to true if system is ready to be armed */ diff --git a/src/modules/uORB/topics/safety.h b/src/modules/uORB/topics/safety.h deleted file mode 100644 index 19e8e8d459..0000000000 --- a/src/modules/uORB/topics/safety.h +++ /dev/null @@ -1,60 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file safety.h - * - * Status of an attached safety device - */ - -#ifndef TOPIC_SAFETY_H -#define TOPIC_SAFETY_H - -#include -#include "../uORB.h" - -enum SAFETY_STATUS { - SAFETY_STATUS_NOT_PRESENT, - SAFETY_STATUS_SAFE, - SAFETY_STATUS_UNLOCKED -}; - -struct safety_s { - uint64_t timestamp; /**< output timestamp in us since system boot */ - enum SAFETY_STATUS status; -}; - -/* actuator output sets; this list can be expanded as more drivers emerge */ -ORB_DECLARE(safety); - -#endif /* TOPIC_SAFETY_H */ From 9ce2b62eb57b519348c4b2fcd58af09999e504e7 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 25 Jun 2013 14:45:27 +0200 Subject: [PATCH 185/486] Beep when arming or disarming with RC --- src/modules/commander/commander.c | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index 4c884aed36..4a5eb23add 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -2033,9 +2033,11 @@ int commander_thread_main(int argc, char *argv[]) (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) ) { arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd); + tune_positive(); } else { mavlink_log_critical(mavlink_fd, "STICK DISARM not allowed"); + tune_negative(); } stick_off_counter = 0; @@ -2050,6 +2052,7 @@ int commander_thread_main(int argc, char *argv[]) if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, safety_pub, &safety, mavlink_fd); stick_on_counter = 0; + tune_positive(); } else { stick_on_counter++; @@ -2219,17 +2222,16 @@ int commander_thread_main(int argc, char *argv[]) /* play tone according to evaluation result */ /* check if we recently armed */ if (!arm_tune_played && safety.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available))) { - ioctl(buzzer, TONE_SET_ALARM, 12); - arm_tune_played = true; + if (ioctl(buzzer, TONE_SET_ALARM, 12) == OK) + arm_tune_played = true; - // // XXX Export patterns and threshold to parameters /* Trigger audio event for low battery */ } else if (bat_remain < 0.1f && battery_voltage_valid) { - ioctl(buzzer, TONE_SET_ALARM, 14); - battery_tune_played = true; + if (ioctl(buzzer, TONE_SET_ALARM, 14) == OK) + battery_tune_played = true; } else if (bat_remain < 0.2f && battery_voltage_valid) { - ioctl(buzzer, TONE_SET_ALARM, 13); - battery_tune_played = true; + if (ioctl(buzzer, TONE_SET_ALARM, 13) == OK) + battery_tune_played = true; } else if(battery_tune_played) { ioctl(buzzer, TONE_SET_ALARM, 0); battery_tune_played = false; From 0ecc9c4bf4f2bf9fe1d99b5cbdf398718d2dccdd Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 25 Jun 2013 16:30:35 +0200 Subject: [PATCH 186/486] Shrinking the main commander file a bit --- .../commander/accelerometer_calibration.c | 14 +- .../commander/accelerometer_calibration.h | 1 - src/modules/commander/airspeed_calibration.c | 111 ++++ src/modules/commander/airspeed_calibration.h | 46 ++ src/modules/commander/baro_calibration.c | 54 ++ src/modules/commander/baro_calibration.h | 46 ++ src/modules/commander/calibration_routines.c | 1 + src/modules/commander/commander.c | 513 +----------------- src/modules/commander/commander.h | 4 - src/modules/commander/commander_helper.c | 129 +++++ src/modules/commander/commander_helper.h | 66 +++ src/modules/commander/gyro_calibration.c | 151 ++++++ src/modules/commander/gyro_calibration.h | 46 ++ src/modules/commander/mag_calibration.c | 278 ++++++++++ src/modules/commander/mag_calibration.h | 46 ++ src/modules/commander/module.mk | 8 +- src/modules/commander/rc_calibration.c | 83 +++ src/modules/commander/rc_calibration.h | 46 ++ src/modules/commander/state_machine_helper.c | 68 +-- src/modules/commander/state_machine_helper.h | 12 +- 20 files changed, 1159 insertions(+), 564 deletions(-) create mode 100644 src/modules/commander/airspeed_calibration.c create mode 100644 src/modules/commander/airspeed_calibration.h create mode 100644 src/modules/commander/baro_calibration.c create mode 100644 src/modules/commander/baro_calibration.h create mode 100644 src/modules/commander/commander_helper.c create mode 100644 src/modules/commander/commander_helper.h create mode 100644 src/modules/commander/gyro_calibration.c create mode 100644 src/modules/commander/gyro_calibration.h create mode 100644 src/modules/commander/mag_calibration.c create mode 100644 src/modules/commander/mag_calibration.h create mode 100644 src/modules/commander/rc_calibration.c create mode 100644 src/modules/commander/rc_calibration.h diff --git a/src/modules/commander/accelerometer_calibration.c b/src/modules/commander/accelerometer_calibration.c index 231739962c..7bae8ad405 100644 --- a/src/modules/commander/accelerometer_calibration.c +++ b/src/modules/commander/accelerometer_calibration.c @@ -70,15 +70,25 @@ */ #include "accelerometer_calibration.h" +#include "commander_helper.h" +#include +#include #include +#include +#include +#include +#include + + #include #include #include #include +#include +#include #include -void do_accel_calibration(int mavlink_fd); int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]); int detect_orientation(int mavlink_fd, int sub_sensor_combined); int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num); @@ -355,7 +365,7 @@ int mat_invert3(float src[3][3], float dst[3][3]) { float det = src[0][0] * (src[1][1] * src[2][2] - src[1][2] * src[2][1]) - src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) + src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]); - if (det == 0.0) + if (det == 0.0f) return ERROR; // Singular matrix dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det; diff --git a/src/modules/commander/accelerometer_calibration.h b/src/modules/commander/accelerometer_calibration.h index 6a920c4ef1..4175a25f4f 100644 --- a/src/modules/commander/accelerometer_calibration.h +++ b/src/modules/commander/accelerometer_calibration.h @@ -9,7 +9,6 @@ #define ACCELEROMETER_CALIBRATION_H_ #include -#include void do_accel_calibration(int mavlink_fd); diff --git a/src/modules/commander/airspeed_calibration.c b/src/modules/commander/airspeed_calibration.c new file mode 100644 index 0000000000..feaf11aee7 --- /dev/null +++ b/src/modules/commander/airspeed_calibration.c @@ -0,0 +1,111 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file airspeed_calibration.c + * Airspeed sensor calibration routine + */ + +#include "airspeed_calibration.h" +#include "commander_helper.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +void do_airspeed_calibration(int mavlink_fd) +{ + /* give directions */ + mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still"); + + const int calibration_count = 2500; + + int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); + struct differential_pressure_s diff_pres; + + int calibration_counter = 0; + float diff_pres_offset = 0.0f; + + while (calibration_counter < calibration_count) { + + /* wait blocking for new data */ + struct pollfd fds[1] = { { .fd = diff_pres_sub, .events = POLLIN } }; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); + diff_pres_offset += diff_pres.differential_pressure_pa; + calibration_counter++; + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_info(mavlink_fd, "airspeed calibration aborted"); + return; + } + } + + diff_pres_offset = diff_pres_offset / calibration_count; + + if (isfinite(diff_pres_offset)) { + + if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { + mavlink_log_critical(mavlink_fd, "Setting offs failed!"); + } + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warn("WARNING: auto-save of params to storage failed"); + } + + //char buf[50]; + //sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]); + //mavlink_log_info(mavlink_fd, buf); + mavlink_log_info(mavlink_fd, "airspeed calibration done"); + + tune_positive(); + + } else { + mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); + } + + close(diff_pres_sub); +} diff --git a/src/modules/commander/airspeed_calibration.h b/src/modules/commander/airspeed_calibration.h new file mode 100644 index 0000000000..92f5651ec1 --- /dev/null +++ b/src/modules/commander/airspeed_calibration.h @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file gyro_calibration.h + * Airspeed sensor calibration routine + */ + +#ifndef AIRSPEED_CALIBRATION_H_ +#define AIRSPEED_CALIBRATION_H_ + +#include + +void do_airspeed_calibration(int mavlink_fd); + +#endif /* AIRSPEED_CALIBRATION_H_ */ \ No newline at end of file diff --git a/src/modules/commander/baro_calibration.c b/src/modules/commander/baro_calibration.c new file mode 100644 index 0000000000..a705947948 --- /dev/null +++ b/src/modules/commander/baro_calibration.c @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file baro_calibration.c + * Barometer calibration routine + */ + +#include "baro_calibration.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +void do_baro_calibration(int mavlink_fd) +{ + // TODO implement this + return; +} diff --git a/src/modules/commander/baro_calibration.h b/src/modules/commander/baro_calibration.h new file mode 100644 index 0000000000..ac0f4be368 --- /dev/null +++ b/src/modules/commander/baro_calibration.h @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mag_calibration.h + * Barometer calibration routine + */ + +#ifndef BARO_CALIBRATION_H_ +#define BARO_CALIBRATION_H_ + +#include + +void do_baro_calibration(int mavlink_fd); + +#endif /* BARO_CALIBRATION_H_ */ \ No newline at end of file diff --git a/src/modules/commander/calibration_routines.c b/src/modules/commander/calibration_routines.c index a269386378..799cd42697 100644 --- a/src/modules/commander/calibration_routines.c +++ b/src/modules/commander/calibration_routines.c @@ -217,3 +217,4 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[], return 0; } + diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index 4a5eb23add..41baa34d5d 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -91,15 +91,15 @@ #include #include +#include "commander_helper.h" #include "state_machine_helper.h" - -#include -#include -#include -#include - #include "calibration_routines.h" #include "accelerometer_calibration.h" +#include "gyro_calibration.h" +#include "mag_calibration.h" +#include "baro_calibration.h" +#include "rc_calibration.h" +#include "airspeed_calibration.h" PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */ //PARAM_DEFINE_INT32(SYS_FAILSAVE_HL, 0); /**< Go into high-level failsafe after 0 ms */ @@ -110,6 +110,9 @@ PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); extern struct system_load_s system_load; +#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f +#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f + /* Decouple update interval and hysteris counters, all depends on intervals */ #define COMMANDER_MONITORING_INTERVAL 50000 #define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f)) @@ -130,7 +133,6 @@ extern struct system_load_s system_load; /* File descriptors */ static int leds; -static int buzzer; static int mavlink_fd; /* flags */ @@ -167,27 +169,17 @@ __EXPORT int commander_main(int argc, char *argv[]); */ int commander_thread_main(int argc, char *argv[]); -int buzzer_init(void); -void buzzer_deinit(void); int led_init(void); void led_deinit(void); int led_toggle(int led); int led_on(int led); int led_off(int led); -void tune_error(void); -void tune_positive(void); -void tune_neutral(void); -void tune_negative(void); void do_reboot(void); -void do_gyro_calibration(void); -void do_mag_calibration(void); -void do_rc_calibration(void); -void do_airspeed_calibration(void); void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd, int safety_pub, struct actuator_safety_s *safety); -int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state); +// int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state); @@ -206,23 +198,6 @@ void usage(const char *reason); */ // static void cal_bsort(float a[], int n); -int buzzer_init() -{ - buzzer = open("/dev/tone_alarm", O_WRONLY); - - if (buzzer < 0) { - warnx("Buzzer: open fail\n"); - return ERROR; - } - - return 0; -} - -void buzzer_deinit() -{ - close(buzzer); -} - int led_init() { @@ -268,27 +243,6 @@ int led_off(int led) return ioctl(leds, LED_OFF, led); } - -void tune_error() -{ - ioctl(buzzer, TONE_SET_ALARM, 2); -} - -void tune_positive() -{ - ioctl(buzzer, TONE_SET_ALARM, 3); -} - -void tune_neutral() -{ - ioctl(buzzer, TONE_SET_ALARM, 4); -} - -void tune_negative() -{ - ioctl(buzzer, TONE_SET_ALARM, 5); -} - void do_reboot() { mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); @@ -298,424 +252,6 @@ void do_reboot() } -void do_rc_calibration() -{ - mavlink_log_info(mavlink_fd, "trim calibration starting"); - - /* XXX fix this */ - // if (current_status.offboard_control_signal_lost) { - // mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal."); - // return; - // } - - int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint)); - struct manual_control_setpoint_s sp; - orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp); - - /* set parameters */ - float p = sp.roll; - param_set(param_find("TRIM_ROLL"), &p); - p = sp.pitch; - param_set(param_find("TRIM_PITCH"), &p); - p = sp.yaw; - param_set(param_find("TRIM_YAW"), &p); - - /* store to permanent storage */ - /* auto-save */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed"); - } - - tune_positive(); - - mavlink_log_info(mavlink_fd, "trim calibration done"); -} - -void do_mag_calibration() -{ - mavlink_log_info(mavlink_fd, "mag calibration starting, hold still"); - - int sub_mag = orb_subscribe(ORB_ID(sensor_mag)); - struct mag_report mag; - - /* 45 seconds */ - uint64_t calibration_interval = 45 * 1000 * 1000; - - /* maximum 2000 values */ - const unsigned int calibration_maxcount = 500; - unsigned int calibration_counter = 0; - - /* limit update rate to get equally spaced measurements over time (in ms) */ - orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); - - int fd = open(MAG_DEVICE_PATH, O_RDONLY); - - /* erase old calibration */ - struct mag_scale mscale_null = { - 0.0f, - 1.0f, - 0.0f, - 1.0f, - 0.0f, - 1.0f, - }; - - if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { - warn("WARNING: failed to set scale / offsets for mag"); - mavlink_log_info(mavlink_fd, "failed to set scale / offsets for mag"); - } - - /* calibrate range */ - if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) { - warnx("failed to calibrate scale"); - } - - close(fd); - - /* calibrate offsets */ - - // uint64_t calibration_start = hrt_absolute_time(); - - uint64_t axis_deadline = hrt_absolute_time(); - uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; - - const char axislabels[3] = { 'X', 'Y', 'Z'}; - int axis_index = -1; - - float *x = (float *)malloc(sizeof(float) * calibration_maxcount); - float *y = (float *)malloc(sizeof(float) * calibration_maxcount); - float *z = (float *)malloc(sizeof(float) * calibration_maxcount); - - if (x == NULL || y == NULL || z == NULL) { - warnx("mag cal failed: out of memory"); - mavlink_log_info(mavlink_fd, "mag cal failed: out of memory"); - warnx("x:%p y:%p z:%p\n", x, y, z); - return; - } - - while (hrt_absolute_time() < calibration_deadline && - calibration_counter < calibration_maxcount) { - - /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = sub_mag, .events = POLLIN } }; - - /* user guidance */ - if (hrt_absolute_time() >= axis_deadline && - axis_index < 3) { - - axis_index++; - - char buf[50]; - sprintf(buf, "please rotate around %c", axislabels[axis_index]); - mavlink_log_info(mavlink_fd, buf); - tune_neutral(); - - axis_deadline += calibration_interval / 3; - } - - if (!(axis_index < 3)) { - break; - } - - // int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time(); - - // if ((axis_left / 1000) == 0 && axis_left > 0) { - // char buf[50]; - // sprintf(buf, "[cmd] %d seconds left for axis %c", axis_left, axislabels[axis_index]); - // mavlink_log_info(mavlink_fd, buf); - // } - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret) { - orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); - - x[calibration_counter] = mag.x; - y[calibration_counter] = mag.y; - z[calibration_counter] = mag.z; - - /* get min/max values */ - - // if (mag.x < mag_min[0]) { - // mag_min[0] = mag.x; - // } - // else if (mag.x > mag_max[0]) { - // mag_max[0] = mag.x; - // } - - // if (raw.magnetometer_ga[1] < mag_min[1]) { - // mag_min[1] = raw.magnetometer_ga[1]; - // } - // else if (raw.magnetometer_ga[1] > mag_max[1]) { - // mag_max[1] = raw.magnetometer_ga[1]; - // } - - // if (raw.magnetometer_ga[2] < mag_min[2]) { - // mag_min[2] = raw.magnetometer_ga[2]; - // } - // else if (raw.magnetometer_ga[2] > mag_max[2]) { - // mag_max[2] = raw.magnetometer_ga[2]; - // } - - calibration_counter++; - - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "mag cal canceled (timed out)"); - break; - } - } - - float sphere_x; - float sphere_y; - float sphere_z; - float sphere_radius; - - sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); - - free(x); - free(y); - free(z); - - if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) { - - fd = open(MAG_DEVICE_PATH, 0); - - struct mag_scale mscale; - - if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale)) - warn("WARNING: failed to get scale / offsets for mag"); - - mscale.x_offset = sphere_x; - mscale.y_offset = sphere_y; - mscale.z_offset = sphere_z; - - if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) - warn("WARNING: failed to set scale / offsets for mag"); - - close(fd); - - /* announce and set new offset */ - - if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) { - warnx("Setting X mag offset failed!\n"); - } - - if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) { - warnx("Setting Y mag offset failed!\n"); - } - - if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) { - warnx("Setting Z mag offset failed!\n"); - } - - if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) { - warnx("Setting X mag scale failed!\n"); - } - - if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) { - warnx("Setting Y mag scale failed!\n"); - } - - if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) { - warnx("Setting Z mag scale failed!\n"); - } - - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); - mavlink_log_info(mavlink_fd, "FAILED storing calibration"); - } - - warnx("\tscale: %.6f %.6f %.6f\n \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n", - (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale, - (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset, (double)sphere_radius); - - char buf[52]; - sprintf(buf, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset, - (double)mscale.y_offset, (double)mscale.z_offset); - mavlink_log_info(mavlink_fd, buf); - - sprintf(buf, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale, - (double)mscale.y_scale, (double)mscale.z_scale); - mavlink_log_info(mavlink_fd, buf); - - mavlink_log_info(mavlink_fd, "mag calibration done"); - - tune_positive(); - /* third beep by cal end routine */ - - } else { - mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)"); - } - - close(sub_mag); -} - -void do_gyro_calibration() -{ - mavlink_log_info(mavlink_fd, "gyro calibration starting, hold still"); - - const int calibration_count = 5000; - - int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); - struct sensor_combined_s raw; - - int calibration_counter = 0; - float gyro_offset[3] = {0.0f, 0.0f, 0.0f}; - - /* set offsets to zero */ - int fd = open(GYRO_DEVICE_PATH, 0); - struct gyro_scale gscale_null = { - 0.0f, - 1.0f, - 0.0f, - 1.0f, - 0.0f, - 1.0f, - }; - - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null)) - warn("WARNING: failed to set scale / offsets for gyro"); - - close(fd); - - while (calibration_counter < calibration_count) { - - /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret) { - orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); - gyro_offset[0] += raw.gyro_rad_s[0]; - gyro_offset[1] += raw.gyro_rad_s[1]; - gyro_offset[2] += raw.gyro_rad_s[2]; - calibration_counter++; - - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); - return; - } - } - - gyro_offset[0] = gyro_offset[0] / calibration_count; - gyro_offset[1] = gyro_offset[1] / calibration_count; - gyro_offset[2] = gyro_offset[2] / calibration_count; - - if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) { - - if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0])) - || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1])) - || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) { - mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!"); - } - - /* set offsets to actual value */ - fd = open(GYRO_DEVICE_PATH, 0); - struct gyro_scale gscale = { - gyro_offset[0], - 1.0f, - gyro_offset[1], - 1.0f, - gyro_offset[2], - 1.0f, - }; - - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) - warn("WARNING: failed to set scale / offsets for gyro"); - - close(fd); - - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); - } - - // char buf[50]; - // sprintf(buf, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]); - // mavlink_log_info(mavlink_fd, buf); - mavlink_log_info(mavlink_fd, "gyro calibration done"); - - tune_positive(); - /* third beep by cal end routine */ - - } else { - mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)"); - } - - close(sub_sensor_combined); -} - - -void do_airspeed_calibration() -{ - /* give directions */ - mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still"); - - const int calibration_count = 2500; - - int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); - struct differential_pressure_s diff_pres; - - int calibration_counter = 0; - float diff_pres_offset = 0.0f; - - while (calibration_counter < calibration_count) { - - /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = diff_pres_sub, .events = POLLIN } }; - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret) { - orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - diff_pres_offset += diff_pres.differential_pressure_pa; - calibration_counter++; - - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "airspeed calibration aborted"); - return; - } - } - - diff_pres_offset = diff_pres_offset / calibration_count; - - if (isfinite(diff_pres_offset)) { - - if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { - mavlink_log_critical(mavlink_fd, "Setting offs failed!"); - } - - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); - } - - //char buf[50]; - //sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]); - //mavlink_log_info(mavlink_fd, buf); - mavlink_log_info(mavlink_fd, "airspeed calibration done"); - - tune_positive(); - - } else { - mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); - } - - close(diff_pres_sub); -} void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd, int safety_pub, struct actuator_safety_s *safety) { @@ -1182,7 +718,7 @@ int commander_thread_main(int argc, char *argv[]) warnx("ERROR: Failed to initialize leds"); } - if (buzzer_init() != 0) { + if (buzzer_init() != OK) { warnx("ERROR: Failed to initialize buzzer"); } @@ -1246,7 +782,12 @@ int commander_thread_main(int argc, char *argv[]) /* advertise to ORB */ status_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); /* publish current state machine */ - state_machine_publish(status_pub, ¤t_status, mavlink_fd); + + /* publish the new state */ + current_status.counter++; + current_status.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_status), status_pub, ¤t_status); + safety_pub = orb_advertise(ORB_ID(actuator_safety), &safety); @@ -1715,7 +1256,7 @@ int commander_thread_main(int argc, char *argv[]) // state_changed = true; // } - orb_check(ORB_ID(vehicle_gps_position), &new_data); + orb_check(gps_sub, &new_data); if (new_data) { @@ -2222,18 +1763,18 @@ int commander_thread_main(int argc, char *argv[]) /* play tone according to evaluation result */ /* check if we recently armed */ if (!arm_tune_played && safety.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available))) { - if (ioctl(buzzer, TONE_SET_ALARM, 12) == OK) + if (tune_arm() == OK) arm_tune_played = true; /* Trigger audio event for low battery */ } else if (bat_remain < 0.1f && battery_voltage_valid) { - if (ioctl(buzzer, TONE_SET_ALARM, 14) == OK) + if (tune_critical_bat() == OK) battery_tune_played = true; } else if (bat_remain < 0.2f && battery_voltage_valid) { - if (ioctl(buzzer, TONE_SET_ALARM, 13) == OK) + if (tune_low_bat() == OK) battery_tune_played = true; } else if(battery_tune_played) { - ioctl(buzzer, TONE_SET_ALARM, 0); + tune_stop(); battery_tune_played = false; } @@ -2305,25 +1846,25 @@ void *commander_low_prio_loop(void *arg) case LOW_PRIO_TASK_GYRO_CALIBRATION: - do_gyro_calibration(); + do_gyro_calibration(mavlink_fd); low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_MAG_CALIBRATION: - do_mag_calibration(); + do_mag_calibration(mavlink_fd); low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_ALTITUDE_CALIBRATION: - // do_baro_calibration(); + // do_baro_calibration(mavlink_fd); case LOW_PRIO_TASK_RC_CALIBRATION: - // do_rc_calibration(); + // do_rc_calibration(mavlink_fd); low_prio_task = LOW_PRIO_TASK_NONE; break; @@ -2337,7 +1878,7 @@ void *commander_low_prio_loop(void *arg) case LOW_PRIO_TASK_AIRSPEED_CALIBRATION: - do_airspeed_calibration(); + do_airspeed_calibration(mavlink_fd); low_prio_task = LOW_PRIO_TASK_NONE; break; diff --git a/src/modules/commander/commander.h b/src/modules/commander/commander.h index 04b4e72ab3..6e57c0ba5f 100644 --- a/src/modules/commander/commander.h +++ b/src/modules/commander/commander.h @@ -49,10 +49,6 @@ #ifndef COMMANDER_H_ #define COMMANDER_H_ -#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f -#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f -void tune_confirm(void); -void tune_error(void); #endif /* COMMANDER_H_ */ diff --git a/src/modules/commander/commander_helper.c b/src/modules/commander/commander_helper.c new file mode 100644 index 0000000000..a75b5dec3c --- /dev/null +++ b/src/modules/commander/commander_helper.c @@ -0,0 +1,129 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler + * Julian Oes + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file commander_helper.c + * Commander helper functions implementations + */ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "commander_helper.h" + +bool is_multirotor(const struct vehicle_status_s *current_status) +{ + return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status->system_type == VEHICLE_TYPE_OCTOROTOR) || + (current_status->system_type == VEHICLE_TYPE_TRICOPTER)); +} + +bool is_rotary_wing(const struct vehicle_status_s *current_status) +{ + return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER) + || (current_status->system_type == VEHICLE_TYPE_COAXIAL); +} + +static int buzzer; + +int buzzer_init() +{ + buzzer = open("/dev/tone_alarm", O_WRONLY); + + if (buzzer < 0) { + warnx("Buzzer: open fail\n"); + return ERROR; + } + + return OK; +} + +void buzzer_deinit() +{ + close(buzzer); +} + +void tune_error() +{ + ioctl(buzzer, TONE_SET_ALARM, 2); +} + +void tune_positive() +{ + ioctl(buzzer, TONE_SET_ALARM, 3); +} + +void tune_neutral() +{ + ioctl(buzzer, TONE_SET_ALARM, 4); +} + +void tune_negative() +{ + ioctl(buzzer, TONE_SET_ALARM, 5); +} + +int tune_arm() +{ + return ioctl(buzzer, TONE_SET_ALARM, 12); +} + +int tune_critical_bat() +{ + return ioctl(buzzer, TONE_SET_ALARM, 14); +} + +int tune_low_bat() +{ + return ioctl(buzzer, TONE_SET_ALARM, 13); +} + +void tune_stop() +{ + ioctl(buzzer, TONE_SET_ALARM, 0); +} + diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h new file mode 100644 index 0000000000..ea96d72a6a --- /dev/null +++ b/src/modules/commander/commander_helper.h @@ -0,0 +1,66 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler + * Julian Oes + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file commander_helper.h + * Commander helper functions definitions + */ + +#ifndef COMMANDER_HELPER_H_ +#define COMMANDER_HELPER_H_ + +#include +#include +#include +#include + + +bool is_multirotor(const struct vehicle_status_s *current_status); +bool is_rotary_wing(const struct vehicle_status_s *current_status); + +int buzzer_init(void); +void buzzer_deinit(void); + +void tune_error(void); +void tune_positive(void); +void tune_neutral(void); +void tune_negative(void); +int tune_arm(void); +int tune_critical_bat(void); +int tune_low_bat(void); + +void tune_stop(void); + +#endif /* COMMANDER_HELPER_H_ */ diff --git a/src/modules/commander/gyro_calibration.c b/src/modules/commander/gyro_calibration.c new file mode 100644 index 0000000000..f452910c9a --- /dev/null +++ b/src/modules/commander/gyro_calibration.c @@ -0,0 +1,151 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file gyro_calibration.c + * Gyroscope calibration routine + */ + +#include "gyro_calibration.h" +#include "commander_helper.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +void do_gyro_calibration(int mavlink_fd) +{ + mavlink_log_info(mavlink_fd, "gyro calibration starting, hold still"); + + const int calibration_count = 5000; + + int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); + struct sensor_combined_s raw; + + int calibration_counter = 0; + float gyro_offset[3] = {0.0f, 0.0f, 0.0f}; + + /* set offsets to zero */ + int fd = open(GYRO_DEVICE_PATH, 0); + struct gyro_scale gscale_null = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null)) + warn("WARNING: failed to set scale / offsets for gyro"); + + close(fd); + + while (calibration_counter < calibration_count) { + + /* wait blocking for new data */ + struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); + gyro_offset[0] += raw.gyro_rad_s[0]; + gyro_offset[1] += raw.gyro_rad_s[1]; + gyro_offset[2] += raw.gyro_rad_s[2]; + calibration_counter++; + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); + return; + } + } + + gyro_offset[0] = gyro_offset[0] / calibration_count; + gyro_offset[1] = gyro_offset[1] / calibration_count; + gyro_offset[2] = gyro_offset[2] / calibration_count; + + if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) { + + if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0])) + || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1])) + || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) { + mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!"); + } + + /* set offsets to actual value */ + fd = open(GYRO_DEVICE_PATH, 0); + struct gyro_scale gscale = { + gyro_offset[0], + 1.0f, + gyro_offset[1], + 1.0f, + gyro_offset[2], + 1.0f, + }; + + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) + warn("WARNING: failed to set scale / offsets for gyro"); + + close(fd); + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warn("WARNING: auto-save of params to storage failed"); + } + + // char buf[50]; + // sprintf(buf, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]); + // mavlink_log_info(mavlink_fd, buf); + mavlink_log_info(mavlink_fd, "gyro calibration done"); + + tune_positive(); + /* third beep by cal end routine */ + + } else { + mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)"); + } + + close(sub_sensor_combined); +} diff --git a/src/modules/commander/gyro_calibration.h b/src/modules/commander/gyro_calibration.h new file mode 100644 index 0000000000..cd262507db --- /dev/null +++ b/src/modules/commander/gyro_calibration.h @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file gyro_calibration.h + * Gyroscope calibration routine + */ + +#ifndef GYRO_CALIBRATION_H_ +#define GYRO_CALIBRATION_H_ + +#include + +void do_gyro_calibration(int mavlink_fd); + +#endif /* GYRO_CALIBRATION_H_ */ \ No newline at end of file diff --git a/src/modules/commander/mag_calibration.c b/src/modules/commander/mag_calibration.c new file mode 100644 index 0000000000..dbd31a7e75 --- /dev/null +++ b/src/modules/commander/mag_calibration.c @@ -0,0 +1,278 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mag_calibration.c + * Magnetometer calibration routine + */ + +#include "mag_calibration.h" +#include "commander_helper.h" +#include "calibration_routines.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +void do_mag_calibration(int mavlink_fd) +{ + mavlink_log_info(mavlink_fd, "mag calibration starting, hold still"); + + int sub_mag = orb_subscribe(ORB_ID(sensor_mag)); + struct mag_report mag; + + /* 45 seconds */ + uint64_t calibration_interval = 45 * 1000 * 1000; + + /* maximum 2000 values */ + const unsigned int calibration_maxcount = 500; + unsigned int calibration_counter = 0; + + /* limit update rate to get equally spaced measurements over time (in ms) */ + orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); + + int fd = open(MAG_DEVICE_PATH, O_RDONLY); + + /* erase old calibration */ + struct mag_scale mscale_null = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + + if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { + warn("WARNING: failed to set scale / offsets for mag"); + mavlink_log_info(mavlink_fd, "failed to set scale / offsets for mag"); + } + + /* calibrate range */ + if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) { + warnx("failed to calibrate scale"); + } + + close(fd); + + /* calibrate offsets */ + + // uint64_t calibration_start = hrt_absolute_time(); + + uint64_t axis_deadline = hrt_absolute_time(); + uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; + + const char axislabels[3] = { 'X', 'Y', 'Z'}; + int axis_index = -1; + + float *x = (float *)malloc(sizeof(float) * calibration_maxcount); + float *y = (float *)malloc(sizeof(float) * calibration_maxcount); + float *z = (float *)malloc(sizeof(float) * calibration_maxcount); + + if (x == NULL || y == NULL || z == NULL) { + warnx("mag cal failed: out of memory"); + mavlink_log_info(mavlink_fd, "mag cal failed: out of memory"); + warnx("x:%p y:%p z:%p\n", x, y, z); + return; + } + + while (hrt_absolute_time() < calibration_deadline && + calibration_counter < calibration_maxcount) { + + /* wait blocking for new data */ + struct pollfd fds[1] = { { .fd = sub_mag, .events = POLLIN } }; + + /* user guidance */ + if (hrt_absolute_time() >= axis_deadline && + axis_index < 3) { + + axis_index++; + + char buf[50]; + sprintf(buf, "please rotate around %c", axislabels[axis_index]); + mavlink_log_info(mavlink_fd, buf); + tune_neutral(); + + axis_deadline += calibration_interval / 3; + } + + if (!(axis_index < 3)) { + break; + } + + // int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time(); + + // if ((axis_left / 1000) == 0 && axis_left > 0) { + // char buf[50]; + // sprintf(buf, "[cmd] %d seconds left for axis %c", axis_left, axislabels[axis_index]); + // mavlink_log_info(mavlink_fd, buf); + // } + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); + + x[calibration_counter] = mag.x; + y[calibration_counter] = mag.y; + z[calibration_counter] = mag.z; + + /* get min/max values */ + + // if (mag.x < mag_min[0]) { + // mag_min[0] = mag.x; + // } + // else if (mag.x > mag_max[0]) { + // mag_max[0] = mag.x; + // } + + // if (raw.magnetometer_ga[1] < mag_min[1]) { + // mag_min[1] = raw.magnetometer_ga[1]; + // } + // else if (raw.magnetometer_ga[1] > mag_max[1]) { + // mag_max[1] = raw.magnetometer_ga[1]; + // } + + // if (raw.magnetometer_ga[2] < mag_min[2]) { + // mag_min[2] = raw.magnetometer_ga[2]; + // } + // else if (raw.magnetometer_ga[2] > mag_max[2]) { + // mag_max[2] = raw.magnetometer_ga[2]; + // } + + calibration_counter++; + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_info(mavlink_fd, "mag cal canceled (timed out)"); + break; + } + } + + float sphere_x; + float sphere_y; + float sphere_z; + float sphere_radius; + + sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); + + free(x); + free(y); + free(z); + + if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) { + + fd = open(MAG_DEVICE_PATH, 0); + + struct mag_scale mscale; + + if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale)) + warn("WARNING: failed to get scale / offsets for mag"); + + mscale.x_offset = sphere_x; + mscale.y_offset = sphere_y; + mscale.z_offset = sphere_z; + + if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) + warn("WARNING: failed to set scale / offsets for mag"); + + close(fd); + + /* announce and set new offset */ + + if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) { + warnx("Setting X mag offset failed!\n"); + } + + if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) { + warnx("Setting Y mag offset failed!\n"); + } + + if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) { + warnx("Setting Z mag offset failed!\n"); + } + + if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) { + warnx("Setting X mag scale failed!\n"); + } + + if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) { + warnx("Setting Y mag scale failed!\n"); + } + + if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) { + warnx("Setting Z mag scale failed!\n"); + } + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warn("WARNING: auto-save of params to storage failed"); + mavlink_log_info(mavlink_fd, "FAILED storing calibration"); + } + + warnx("\tscale: %.6f %.6f %.6f\n \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n", + (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale, + (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset, (double)sphere_radius); + + char buf[52]; + sprintf(buf, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset, + (double)mscale.y_offset, (double)mscale.z_offset); + mavlink_log_info(mavlink_fd, buf); + + sprintf(buf, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale, + (double)mscale.y_scale, (double)mscale.z_scale); + mavlink_log_info(mavlink_fd, buf); + + mavlink_log_info(mavlink_fd, "mag calibration done"); + + tune_positive(); + /* third beep by cal end routine */ + + } else { + mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)"); + } + + close(sub_mag); +} \ No newline at end of file diff --git a/src/modules/commander/mag_calibration.h b/src/modules/commander/mag_calibration.h new file mode 100644 index 0000000000..fd2085f14a --- /dev/null +++ b/src/modules/commander/mag_calibration.h @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mag_calibration.h + * Magnetometer calibration routine + */ + +#ifndef MAG_CALIBRATION_H_ +#define MAG_CALIBRATION_H_ + +#include + +void do_mag_calibration(int mavlink_fd); + +#endif /* MAG_CALIBRATION_H_ */ \ No newline at end of file diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index fe44e955ad..fef8e366b2 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -38,6 +38,12 @@ MODULE_COMMAND = commander SRCS = commander.c \ state_machine_helper.c \ + commander_helper.c \ calibration_routines.c \ - accelerometer_calibration.c + accelerometer_calibration.c \ + gyro_calibration.c \ + mag_calibration.c \ + baro_calibration.c \ + rc_calibration.c \ + airspeed_calibration.c diff --git a/src/modules/commander/rc_calibration.c b/src/modules/commander/rc_calibration.c new file mode 100644 index 0000000000..9aa6b86fe1 --- /dev/null +++ b/src/modules/commander/rc_calibration.c @@ -0,0 +1,83 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file rc_calibration.c + * Remote Control calibration routine + */ + +#include "rc_calibration.h" +#include "commander_helper.h" + +#include +#include +#include +#include +#include +#include + + +void do_rc_calibration(int mavlink_fd) +{ + mavlink_log_info(mavlink_fd, "trim calibration starting"); + + /* XXX fix this */ + // if (current_status.offboard_control_signal_lost) { + // mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal."); + // return; + // } + + int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint)); + struct manual_control_setpoint_s sp; + orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp); + + /* set parameters */ + float p = sp.roll; + param_set(param_find("TRIM_ROLL"), &p); + p = sp.pitch; + param_set(param_find("TRIM_PITCH"), &p); + p = sp.yaw; + param_set(param_find("TRIM_YAW"), &p); + + /* store to permanent storage */ + /* auto-save */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed"); + } + + tune_positive(); + + mavlink_log_info(mavlink_fd, "trim calibration done"); +} \ No newline at end of file diff --git a/src/modules/commander/rc_calibration.h b/src/modules/commander/rc_calibration.h new file mode 100644 index 0000000000..6505c73647 --- /dev/null +++ b/src/modules/commander/rc_calibration.h @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file rc_calibration.h + * Remote Control calibration routine + */ + +#ifndef RC_CALIBRATION_H_ +#define RC_CALIBRATION_H_ + +#include + +void do_rc_calibration(int mavlink_fd); + +#endif /* RC_CALIBRATION_H_ */ \ No newline at end of file diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index 87aad6270c..c15fc91a05 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. * Author: Thomas Gubler * Julian Oes * @@ -54,21 +54,8 @@ #include #include "state_machine_helper.h" -#include "commander.h" +#include "commander_helper.h" -bool is_multirotor(const struct vehicle_status_s *current_status) -{ - return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status->system_type == VEHICLE_TYPE_OCTOROTOR) || - (current_status->system_type == VEHICLE_TYPE_TRICOPTER)); -} - -bool is_rotary_wing(const struct vehicle_status_s *current_status) -{ - return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER) - || (current_status->system_type == VEHICLE_TYPE_COAXIAL); -} int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int safety_pub, struct actuator_safety_s *safety, const int mavlink_fd) { @@ -568,7 +555,11 @@ int hil_state_transition(int status_pub, struct vehicle_status_s *current_status if (valid_transition) { current_status->hil_state = new_state; - state_machine_publish(status_pub, current_status, mavlink_fd); + + current_status->counter++; + current_status->timestamp = hrt_absolute_time(); + + orb_publish(ORB_ID(vehicle_status), status_pub, current_status); ret = OK; } else { mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); @@ -579,50 +570,6 @@ int hil_state_transition(int status_pub, struct vehicle_status_s *current_status -void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - /* publish the new state */ - current_status->counter++; - current_status->timestamp = hrt_absolute_time(); - - /* assemble state vector based on flag values */ -// if (current_status->flag_control_rates_enabled) { -// current_status->onboard_control_sensors_present |= 0x400; -// -// } else { -// current_status->onboard_control_sensors_present &= ~0x400; -// } - -// current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; -// current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; -// current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; -// current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; -// -// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_rates_enabled) ? 0x400 : 0; -// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; -// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; -// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; -// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; - - orb_publish(ORB_ID(vehicle_status), status_pub, current_status); -} - -// void publish_armed_status(const struct vehicle_status_s *current_status) -// { -// struct actuator_armed_s armed; -// armed.armed = current_status->flag_system_armed; -// -// /* XXX allow arming by external components on multicopters only if not yet armed by RC */ -// /* XXX allow arming only if core sensors are ok */ -// armed.ready_to_arm = true; -// -// /* lock down actuators if required, only in HIL */ -// armed.lockdown = (current_status->flag_hil_enabled) ? true : false; -// orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); -// orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); -// } - - // /* // * Wrapper functions (to be used in the commander), all functions assume lock on current_status // */ @@ -805,3 +752,4 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat // // return ret; //} + diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index b015c4efe2..b553a4b56a 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. * Author: Thomas Gubler * Julian Oes * @@ -50,15 +50,7 @@ #include -void navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -bool is_multirotor(const struct vehicle_status_s *current_status); - -bool is_rotary_wing(const struct vehicle_status_s *current_status); - -//int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state); - -void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); +int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int safety_pub, struct actuator_safety_s *safety, const int mavlink_fd); int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, int control_mode_pub, struct vehicle_control_mode_s *control_mode, const int mavlink_fd); From 7958e38c427309941e43a17e987bb04504aa57df Mon Sep 17 00:00:00 2001 From: Sam Kelly Date: Tue, 25 Jun 2013 13:44:42 -0700 Subject: [PATCH 187/486] Enabled NSH on USB by default. --- nuttx/configs/px4fmuv2/nsh/defconfig | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/nuttx/configs/px4fmuv2/nsh/defconfig b/nuttx/configs/px4fmuv2/nsh/defconfig index 307c5079cd..8e69f321af 100755 --- a/nuttx/configs/px4fmuv2/nsh/defconfig +++ b/nuttx/configs/px4fmuv2/nsh/defconfig @@ -237,7 +237,7 @@ CONFIG_SERIAL_TERMIOS=y CONFIG_SERIAL_CONSOLE_REINIT=y CONFIG_STANDARD_SERIAL=y -CONFIG_UART8_SERIAL_CONSOLE=y +CONFIG_UART8_SERIAL_CONSOLE=n #Mavlink messages can be bigger than 128 CONFIG_USART1_TXBUFSIZE=512 @@ -555,7 +555,7 @@ CONFIG_START_MONTH=1 CONFIG_START_DAY=1 CONFIG_GREGORIAN_TIME=n CONFIG_JULIAN_TIME=n -CONFIG_DEV_CONSOLE=y +CONFIG_DEV_CONSOLE=n CONFIG_DEV_LOWCONSOLE=n CONFIG_MUTEX_TYPES=n CONFIG_PRIORITY_INHERITANCE=y @@ -943,7 +943,7 @@ CONFIG_USBDEV_TRACE_NRECORDS=512 # Size of the serial receive/transmit buffers. Default 256. # CONFIG_CDCACM=y -CONFIG_CDCACM_CONSOLE=n +CONFIG_CDCACM_CONSOLE=y #CONFIG_CDCACM_EP0MAXPACKET CONFIG_CDCACM_EPINTIN=1 #CONFIG_CDCACM_EPINTIN_FSSIZE From b1f3a5c92bd900ad15d4f13f43658563be5ed8de Mon Sep 17 00:00:00 2001 From: Sam Kelly Date: Tue, 25 Jun 2013 14:01:27 -0700 Subject: [PATCH 188/486] Enabled MS5611 by default on FMUv2. --- makefiles/config_px4fmuv2_default.mk | 2 +- src/drivers/ms5611/ms5611.cpp | 1017 ++++++++++++++++++++++++-- 2 files changed, 938 insertions(+), 81 deletions(-) diff --git a/makefiles/config_px4fmuv2_default.mk b/makefiles/config_px4fmuv2_default.mk index 26c2499013..bad53aa422 100644 --- a/makefiles/config_px4fmuv2_default.mk +++ b/makefiles/config_px4fmuv2_default.mk @@ -21,7 +21,7 @@ MODULES += drivers/lsm303d MODULES += drivers/l3gd20 #MODULES += drivers/mpu6000 MODULES += drivers/hmc5883 -#MODULES += drivers/ms5611 +MODULES += drivers/ms5611 MODULES += drivers/mb12xx MODULES += drivers/gps MODULES += drivers/hil diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 59ab5936ee..76acf7ccd7 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,12 +33,13 @@ /** * @file ms5611.cpp - * Driver for the MS5611 barometric pressure sensor connected via I2C. + * Driver for the MS5611 barometric pressure sensor connected via I2C or SPI. */ #include #include +#include #include #include @@ -76,6 +77,11 @@ static const int ERROR = -1; # error This requires CONFIG_SCHED_WORKQUEUE. #endif +/* SPI protocol address bits */ +#define DIR_READ (1<<7) +#define DIR_WRITE (0<<7) +#define ADDR_INCREMENT (1<<6) + /** * Calibration PROM as reported by the device. */ @@ -100,11 +106,11 @@ union ms5611_prom_u { }; #pragma pack(pop) -class MS5611 : public device::I2C +class MS5611_I2C : public device::I2C { public: - MS5611(int bus); - virtual ~MS5611(); + MS5611_I2C(int bus); + virtual ~MS5611_I2C(); virtual int init(); @@ -118,8 +124,6 @@ public: protected: virtual int probe(); - -private: union ms5611_prom_u _prom; struct work_s _work; @@ -149,16 +153,7 @@ private: perf_counter_t _buffer_overflows; /** - * Test whether the device supported by the driver is present at a - * specific address. - * - * @param address The I2C bus address to probe. - * @return True if the device is present. - */ - int probe_address(uint8_t address); - - /** - * Initialise the automatic measurement state machine and start it. + * Initialize the automatic measurement state machine and start it. * * @note This function is called at open and error time. It might make sense * to make it more aggressive about resetting the bus in case of errors. @@ -198,26 +193,33 @@ private: * * @return OK if the measurement command was successful. */ - int measure(); + virtual int measure(); /** * Collect the result of the most recent measurement. */ - int collect(); + virtual int collect(); /** * Send a reset command to the MS5611. * * This is required after any bus reset. */ - int cmd_reset(); + virtual int cmd_reset(); /** * Read the MS5611 PROM * * @return OK if the PROM reads successfully. */ - int read_prom(); + virtual int read_prom(); + + /** + * Execute the bus-specific ioctl (I2C or SPI) + * + * @return the bus-specific ioctl return value + */ + virtual int bus_ioctl(struct file *filp, int cmd, unsigned long arg); /** * PROM CRC routine ported from MS5611 application note @@ -227,6 +229,159 @@ private: */ bool crc4(uint16_t *n_prom); +private: + + /** + * Test whether the device supported by the driver is present at a + * specific address. + * + * @param address The I2C bus address to probe. + * @return True if the device is present. + */ + int probe_address(uint8_t address); + +}; + +class MS5611_SPI : public device::SPI +{ +public: + MS5611_SPI(int bus, const char* path, spi_dev_e device); + virtual ~MS5611_SPI(); + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + virtual int probe(); + union ms5611_prom_u _prom; + + struct work_s _work; + unsigned _measure_ticks; + + unsigned _num_reports; + volatile unsigned _next_report; + volatile unsigned _oldest_report; + struct baro_report *_reports; + + bool _collect_phase; + unsigned _measure_phase; + + /* intermediate temperature values per MS5611 datasheet */ + int32_t _TEMP; + int64_t _OFF; + int64_t _SENS; + + /* altitude conversion calibration */ + unsigned _msl_pressure; /* in kPa */ + + orb_advert_t _baro_topic; + + perf_counter_t _sample_perf; + perf_counter_t _measure_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + + /** + * Initialize the automatic measurement state machine and start it. + * + * @note This function is called at open and error time. It might make sense + * to make it more aggressive about resetting the bus in case of errors. + */ + void start_cycle(); + + /** + * Stop the automatic measurement state machine. + */ + void stop_cycle(); + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + * + * This is the heart of the measurement state machine. This function + * alternately starts a measurement, or collects the data from the + * previous measurement. + * + * When the interval between measurements is greater than the minimum + * measurement interval, a gap is inserted between collection + * and measurement to provide the most recent measurement possible + * at the next interval. + */ + void cycle(); + + /** + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); + + /** + * Issue a measurement command for the current state. + * + * @return OK if the measurement command was successful. + */ + virtual int measure(); + + /** + * Collect the result of the most recent measurement. + */ + virtual int collect(); + + /** + * Send a reset command to the MS5611. + * + * This is required after any bus reset. + */ + virtual int cmd_reset(); + + /** + * Read the MS5611 PROM + * + * @return OK if the PROM reads successfully. + */ + virtual int read_prom(); + + /** + * Execute the bus-specific ioctl (I2C or SPI) + * + * @return the bus-specific ioctl return value + */ + virtual int bus_ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * PROM CRC routine ported from MS5611 application note + * + * @param n_prom Pointer to words read from PROM. + * @return True if the CRC matches. + */ + bool crc4(uint16_t *n_prom); + + // XXX this should really go into the SPI base class, as its common code + uint8_t read_reg(unsigned reg); + uint16_t read_reg16(unsigned reg); + void write_reg(unsigned reg, uint8_t value); + void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); + +private: + + /** + * Test whether the device supported by the driver is present at a + * specific address. + * + * @param address The I2C bus address to probe. + * @return True if the device is present. + */ + int probe_address(uint8_t address); + }; /* helper macro for handling report buffer indices */ @@ -243,8 +398,13 @@ private: #define MS5611_CONVERSION_INTERVAL 10000 /* microseconds */ #define MS5611_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */ -#define MS5611_BUS PX4_I2C_BUS_ONBOARD -#define MS5611_ADDRESS_1 PX4_I2C_OBDEV_MS5611 /* address select pins pulled high (PX4FMU series v1.6+) */ +#ifndef PX4_I2C_BUS_ONBOARD + #define MS5611_BUS 1 +#else + #define MS5611_BUS PX4_I2C_BUS_ONBOARD +#endif + +#define MS5611_ADDRESS_1 0x76 /* address select pins pulled high (PX4FMU series v1.6+) */ #define MS5611_ADDRESS_2 0x77 /* address select pins pulled low (PX4FMU prototypes) */ #define ADDR_RESET_CMD 0x1E /* write to this address to reset chip */ @@ -259,8 +419,7 @@ private: */ extern "C" __EXPORT int ms5611_main(int argc, char *argv[]); - -MS5611::MS5611(int bus) : +MS5611_I2C::MS5611_I2C(int bus) : I2C("MS5611", BARO_DEVICE_PATH, bus, 0, 400000), _measure_ticks(0), _num_reports(0), @@ -279,14 +438,46 @@ MS5611::MS5611(int bus) : _comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows")) { - // enable debug() calls - _debug_enabled = true; - // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); + warnx("MS5611 I2C constructor"); } -MS5611::~MS5611() +MS5611_SPI::MS5611_SPI(int bus, const char* path, spi_dev_e device) : + SPI("MS5611", path, bus, device, SPIDEV_MODE3, 2000000), + _measure_ticks(0), + _num_reports(0), + _next_report(0), + _oldest_report(0), + _reports(nullptr), + _collect_phase(false), + _measure_phase(0), + _TEMP(0), + _OFF(0), + _SENS(0), + _msl_pressure(101325), + _baro_topic(-1), + _sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")), + _measure_perf(perf_alloc(PC_ELAPSED, "ms5611_measure")), + _comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows")) +{ + // work_cancel in the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); + warnx("MS5611 SPI constructor"); +} + +MS5611_I2C::~MS5611_I2C() +{ + /* make sure we are truly inactive */ + stop_cycle(); + + /* free any existing reports */ + if (_reports != nullptr) + delete[] _reports; +} + +MS5611_SPI::~MS5611_SPI() { /* make sure we are truly inactive */ stop_cycle(); @@ -297,7 +488,7 @@ MS5611::~MS5611() } int -MS5611::init() +MS5611_I2C::init() { int ret = ERROR; @@ -327,8 +518,9 @@ out: } int -MS5611::probe() +MS5611_I2C::probe() { +#ifdef PX4_I2C_OBDEV_MS5611 _retries = 10; if ((OK == probe_address(MS5611_ADDRESS_1)) || @@ -340,12 +532,64 @@ MS5611::probe() _retries = 0; return OK; } +#endif return -EIO; } int -MS5611::probe_address(uint8_t address) +MS5611_SPI::init() +{ + int ret = ERROR; + + /* do SPI init (and probe) first */ + warnx("MS5611 SPI init function"); + if (SPI::init() != OK) { + warnx("SPI init failed"); + goto out; + } else { + warnx("SPI init ok"); + } + + /* allocate basic report buffers */ + _num_reports = 2; + _reports = new struct baro_report[_num_reports]; + + if (_reports == nullptr) + goto out; + + _oldest_report = _next_report = 0; + + /* get a publish handle on the baro topic */ + memset(&_reports[0], 0, sizeof(_reports[0])); + _baro_topic = orb_advertise(ORB_ID(sensor_baro), &_reports[0]); + + if (_baro_topic < 0) + debug("failed to create sensor_baro object"); + + ret = OK; +out: + return ret; +} + +int +MS5611_SPI::probe() +{ + + warnx("SPI probe"); + /* send reset command */ + if (OK != cmd_reset()) + return -EIO; + + /* read PROM */ + if (OK != read_prom()) + return -EIO; + + return OK; +} + +int +MS5611_I2C::probe_address(uint8_t address) { /* select the address we are going to try */ set_address(address); @@ -362,7 +606,78 @@ MS5611::probe_address(uint8_t address) } ssize_t -MS5611::read(struct file *filp, char *buffer, size_t buflen) +MS5611_I2C::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct baro_report); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + /* if automatic measurement is enabled */ + if (_measure_ticks > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the workq thread while we are doing this; + * we are careful to avoid racing with them. + */ + while (count--) { + if (_oldest_report != _next_report) { + memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); + ret += sizeof(_reports[0]); + INCREMENT(_oldest_report, _num_reports); + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement - run one conversion */ + /* XXX really it'd be nice to lock against other readers here */ + do { + _measure_phase = 0; + _oldest_report = _next_report = 0; + + /* do temperature first */ + if (OK != measure()) { + ret = -EIO; + break; + } + + usleep(MS5611_CONVERSION_INTERVAL); + + if (OK != collect()) { + ret = -EIO; + break; + } + + /* now do a pressure measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } + + usleep(MS5611_CONVERSION_INTERVAL); + + if (OK != collect()) { + ret = -EIO; + break; + } + + /* state machine will have generated a report, copy it out */ + memcpy(buffer, _reports, sizeof(*_reports)); + ret = sizeof(*_reports); + + } while (0); + + return ret; +} + +ssize_t +MS5611_SPI::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct baro_report); int ret = 0; @@ -433,7 +748,21 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) } int -MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) +MS5611_SPI::bus_ioctl(struct file *filp, int cmd, unsigned long arg) +{ + /* give it to the superclass */ + return SPI::ioctl(filp, cmd, arg); +} + +int +MS5611_I2C::bus_ioctl(struct file *filp, int cmd, unsigned long arg) +{ + /* give it to the superclass */ + return I2C::ioctl(filp, cmd, arg); +} + +int +MS5611_I2C::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { @@ -546,12 +875,132 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) break; } - /* give it to the superclass */ + /* give it to the bus-specific superclass */ + // return bus_ioctl(filp, cmd, arg); return I2C::ioctl(filp, cmd, arg); } +int +MS5611_SPI::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop_cycle(); + _measure_ticks = 0; + return OK; + + /* external signalling not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(MS5611_CONVERSION_INTERVAL); + + /* if we need to start the poll state machine, do it */ + if (want_start) + start_cycle(); + + return OK; + } + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); + + /* check against maximum rate */ + if (ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL)) + return -EINVAL; + + /* update interval for next measurement */ + _measure_ticks = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) + start_cycle(); + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_measure_ticks == 0) + return SENSOR_POLLRATE_MANUAL; + + return (1000 / _measure_ticks); + + case SENSORIOCSQUEUEDEPTH: { + /* add one to account for the sentinel in the ring */ + arg++; + + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 2) || (arg > 100)) + return -EINVAL; + + /* allocate new buffer */ + struct baro_report *buf = new struct baro_report[arg]; + + if (nullptr == buf) + return -ENOMEM; + + /* reset the measurement state machine with the new buffer, free the old */ + stop_cycle(); + delete[] _reports; + _num_reports = arg; + _reports = buf; + start_cycle(); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _num_reports - 1; + + case SENSORIOCRESET: + /* XXX implement this */ + return -EINVAL; + + case BAROIOCSMSLPRESSURE: + + /* range-check for sanity */ + if ((arg < 80000) || (arg > 120000)) + return -EINVAL; + + _msl_pressure = arg; + return OK; + + case BAROIOCGMSLPRESSURE: + return _msl_pressure; + + default: + break; + } + + /* give it to the bus-specific superclass */ + // return bus_ioctl(filp, cmd, arg); + return SPI::ioctl(filp, cmd, arg); +} + void -MS5611::start_cycle() +MS5611_I2C::start_cycle() { /* reset the report ring and state machine */ @@ -560,25 +1009,25 @@ MS5611::start_cycle() _oldest_report = _next_report = 0; /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, 1); + work_queue(HPWORK, &_work, (worker_t)&MS5611_I2C::cycle_trampoline, this, 1); } void -MS5611::stop_cycle() +MS5611_I2C::stop_cycle() { work_cancel(HPWORK, &_work); } void -MS5611::cycle_trampoline(void *arg) +MS5611_I2C::cycle_trampoline(void *arg) { - MS5611 *dev = (MS5611 *)arg; + MS5611_I2C *dev = (MS5611_I2C *)arg; dev->cycle(); } void -MS5611::cycle() +MS5611_I2C::cycle() { int ret; @@ -616,7 +1065,7 @@ MS5611::cycle() /* schedule a fresh cycle call when we are ready to measure again */ work_queue(HPWORK, &_work, - (worker_t)&MS5611::cycle_trampoline, + (worker_t)&MS5611_I2C::cycle_trampoline, this, _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL)); @@ -639,13 +1088,107 @@ MS5611::cycle() /* schedule a fresh cycle call when the measurement is done */ work_queue(HPWORK, &_work, - (worker_t)&MS5611::cycle_trampoline, + (worker_t)&MS5611_I2C::cycle_trampoline, + this, + USEC2TICK(MS5611_CONVERSION_INTERVAL)); +} + +void +MS5611_SPI::start_cycle() +{ + + /* reset the report ring and state machine */ + _collect_phase = false; + _measure_phase = 0; + _oldest_report = _next_report = 0; + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&MS5611_SPI::cycle_trampoline, this, 1); +} + +void +MS5611_SPI::stop_cycle() +{ + work_cancel(HPWORK, &_work); +} + +void +MS5611_SPI::cycle_trampoline(void *arg) +{ + MS5611_SPI *dev = (MS5611_SPI *)arg; + + dev->cycle(); +} + +void +MS5611_SPI::cycle() +{ + int ret; + + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + ret = collect(); + if (ret != OK) { + if (ret == -6) { + /* + * The ms5611 seems to regularly fail to respond to + * its address; this happens often enough that we'd rather not + * spam the console with the message. + */ + } else { + //log("collection error %d", ret); + } + /* reset the collection state machine and try again */ + start_cycle(); + return; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + * Don't inject one after temperature measurements, so we can keep + * doing pressure measurements at something close to the desired rate. + */ + if ((_measure_phase != 0) && + (_measure_ticks > USEC2TICK(MS5611_CONVERSION_INTERVAL))) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&MS5611_SPI::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL)); + + return; + } + } + + /* measurement phase */ + ret = measure(); + if (ret != OK) { + //log("measure error %d", ret); + /* reset the collection state machine and try again */ + start_cycle(); + return; + } + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&MS5611_SPI::cycle_trampoline, this, USEC2TICK(MS5611_CONVERSION_INTERVAL)); } int -MS5611::measure() +MS5611_I2C::measure() { int ret; @@ -674,7 +1217,7 @@ MS5611::measure() } int -MS5611::collect() +MS5611_I2C::collect() { int ret; uint8_t cmd; @@ -761,30 +1304,7 @@ MS5611::collect() * double precision: ms5611_read: 992 events, 258641us elapsed, min 202us max 305us * single precision: ms5611_read: 963 events, 208066us elapsed, min 202us max 241us */ -#if 0/* USE_FLOAT */ - /* tropospheric properties (0-11km) for standard atmosphere */ - const float T1 = 15.0f + 273.15f; /* temperature at base height in Kelvin */ - const float a = -6.5f / 1000f; /* temperature gradient in degrees per metre */ - const float g = 9.80665f; /* gravity constant in m/s/s */ - const float R = 287.05f; /* ideal gas constant in J/kg/K */ - /* current pressure at MSL in kPa */ - float p1 = _msl_pressure / 1000.0f; - - /* measured pressure in kPa */ - float p = P / 1000.0f; - - /* - * Solve: - * - * / -(aR / g) \ - * | (p / p1) . T1 | - T1 - * \ / - * h = ------------------------------- + h1 - * a - */ - _reports[_next_report].altitude = (((powf((p / p1), (-(a * R) / g))) * T1) - T1) / a; -#else /* tropospheric properties (0-11km) for standard atmosphere */ const double T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */ const double a = -6.5 / 1000; /* temperature gradient in degrees per metre */ @@ -807,7 +1327,7 @@ MS5611::collect() * a */ _reports[_next_report].altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; -#endif + /* publish it */ orb_publish(ORB_ID(sensor_baro), _baro_topic, &_reports[_next_report]); @@ -833,7 +1353,7 @@ MS5611::collect() } int -MS5611::cmd_reset() +MS5611_I2C::cmd_reset() { unsigned old_retrycount = _retries; uint8_t cmd = ADDR_RESET_CMD; @@ -848,7 +1368,7 @@ MS5611::cmd_reset() } int -MS5611::read_prom() +MS5611_I2C::read_prom() { uint8_t prom_buf[2]; union { @@ -879,8 +1399,264 @@ MS5611::read_prom() return crc4(&_prom.c[0]) ? OK : -EIO; } +uint8_t +MS5611_SPI::read_reg(unsigned reg) +{ + uint8_t cmd[2]; + + cmd[0] = reg | DIR_READ; + + transfer(cmd, cmd, sizeof(cmd)); + + return cmd[1]; +} + +uint16_t +MS5611_SPI::read_reg16(unsigned reg) +{ + uint8_t cmd[3]; + + cmd[0] = reg | DIR_READ; + + transfer(cmd, cmd, sizeof(cmd)); + + return (uint16_t)(cmd[1] << 8) | cmd[2]; +} + +void +MS5611_SPI::write_reg(unsigned reg, uint8_t value) +{ + uint8_t cmd[2]; + + cmd[0] = reg | DIR_WRITE; + cmd[1] = value; + + transfer(cmd, nullptr, sizeof(cmd)); +} + +void +MS5611_SPI::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) +{ + uint8_t val; + + val = read_reg(reg); + val &= ~clearbits; + val |= setbits; + write_reg(reg, val); +} + +int +MS5611_SPI::measure() +{ + int ret; + + perf_begin(_measure_perf); + + /* + * In phase zero, request temperature; in other phases, request pressure. + */ + uint8_t cmd_data = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1; + cmd_data |= DIR_WRITE; + + /* + * Send the command to begin measuring. + * + * Disable retries on this command; we can't know whether failure + * means the device did or did not see the write. + */ + ret = transfer(&cmd_data, nullptr, 1); + + if (OK != ret) + perf_count(_comms_errors); + + perf_end(_measure_perf); + + return ret; +} + +int +MS5611_SPI::collect() +{ + int ret; + + uint8_t data[4]; + union { + uint8_t b[4]; + uint32_t w; + } cvt; + + /* read the most recent measurement */ + data[0] = 0 | DIR_WRITE; + + perf_begin(_sample_perf); + + /* this should be fairly close to the end of the conversion, so the best approximation of the time */ + _reports[_next_report].timestamp = hrt_absolute_time(); + + ret = transfer(&data[0], &data[0], sizeof(data)); + if (ret != OK) { + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; + } + + /* fetch the raw value */ + cvt.b[0] = data[3]; + cvt.b[1] = data[2]; + cvt.b[2] = data[1]; + cvt.b[3] = 0; + uint32_t raw = cvt.w; + + /* handle a measurement */ + if (_measure_phase == 0) { + + /* temperature offset (in ADC units) */ + int32_t dT = (int32_t)raw - ((int32_t)_prom.s.c5_reference_temp << 8); + + /* absolute temperature in centidegrees - note intermediate value is outside 32-bit range */ + _TEMP = 2000 + (int32_t)(((int64_t)dT * _prom.s.c6_temp_coeff_temp) >> 23); + + /* base sensor scale/offset values */ + _SENS = ((int64_t)_prom.s.c1_pressure_sens << 15) + (((int64_t)_prom.s.c3_temp_coeff_pres_sens * dT) >> 8); + _OFF = ((int64_t)_prom.s.c2_pressure_offset << 16) + (((int64_t)_prom.s.c4_temp_coeff_pres_offset * dT) >> 7); + + /* temperature compensation */ + if (_TEMP < 2000) { + + int32_t T2 = POW2(dT) >> 31; + + int64_t f = POW2((int64_t)_TEMP - 2000); + int64_t OFF2 = 5 * f >> 1; + int64_t SENS2 = 5 * f >> 2; + + if (_TEMP < -1500) { + int64_t f2 = POW2(_TEMP + 1500); + OFF2 += 7 * f2; + SENS2 += 11 * f2 >> 1; + } + + _TEMP -= T2; + _OFF -= OFF2; + _SENS -= SENS2; + } + + } else { + + /* pressure calculation, result in Pa */ + int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15; + + /* generate a new report */ + _reports[_next_report].temperature = _TEMP / 100.0f; + _reports[_next_report].pressure = P / 100.0f; /* convert to millibar */ + + /* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */ + + /* + * PERFORMANCE HINT: + * + * The single precision calculation is 50 microseconds faster than the double + * precision variant. It is however not obvious if double precision is required. + * Pending more inspection and tests, we'll leave the double precision variant active. + * + * Measurements: + * double precision: ms5611_read: 992 events, 258641us elapsed, min 202us max 305us + * single precision: ms5611_read: 963 events, 208066us elapsed, min 202us max 241us + */ + + /* tropospheric properties (0-11km) for standard atmosphere */ + const double T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */ + const double a = -6.5 / 1000; /* temperature gradient in degrees per metre */ + const double g = 9.80665; /* gravity constant in m/s/s */ + const double R = 287.05; /* ideal gas constant in J/kg/K */ + + /* current pressure at MSL in kPa */ + double p1 = _msl_pressure / 1000.0; + + /* measured pressure in kPa */ + double p = P / 1000.0; + + /* + * Solve: + * + * / -(aR / g) \ + * | (p / p1) . T1 | - T1 + * \ / + * h = ------------------------------- + h1 + * a + */ + _reports[_next_report].altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; + + /* publish it */ + orb_publish(ORB_ID(sensor_baro), _baro_topic, &_reports[_next_report]); + + /* post a report to the ring - note, not locked */ + INCREMENT(_next_report, _num_reports); + + /* if we are running up against the oldest report, toss it */ + if (_next_report == _oldest_report) { + perf_count(_buffer_overflows); + INCREMENT(_oldest_report, _num_reports); + } + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + } + + /* update the measurement state machine */ + INCREMENT(_measure_phase, MS5611_MEASUREMENT_RATIO + 1); + + perf_end(_sample_perf); + + return OK; +} + +int +MS5611_SPI::cmd_reset() +{ + uint8_t cmd = ADDR_RESET_CMD | DIR_WRITE; + int result; + + result = transfer(&cmd, nullptr, 1); + warnx("transferred reset, result: %d", result); + + return result; +} + +int +MS5611_SPI::read_prom() +{ + uint8_t prom_buf[3]; + union { + uint8_t b[2]; + uint16_t w; + } cvt; + + /* + * Wait for PROM contents to be in the device (2.8 ms) in the case we are + * called immediately after reset. + */ + usleep(3000); + + /* read and convert PROM words */ + for (int i = 0; i < 8; i++) { + uint8_t cmd = (ADDR_PROM_SETUP + (i * 2)); + _prom.c[i] = read_reg16(cmd); + } + + warnx("going for CRC"); + + /* calculate CRC and return success/failure accordingly */ + int ret = crc4(&_prom.c[0]) ? OK : -EIO; + if (ret == OK) { + warnx("CRC OK"); + } else { + warnx("CRC FAIL"); + } + return ret; +} + bool -MS5611::crc4(uint16_t *n_prom) +MS5611_I2C::crc4(uint16_t *n_prom) { int16_t cnt; uint16_t n_rem; @@ -923,7 +1699,74 @@ MS5611::crc4(uint16_t *n_prom) } void -MS5611::print_info() +MS5611_I2C::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_buffer_overflows); + printf("poll interval: %u ticks\n", _measure_ticks); + printf("report queue: %u (%u/%u @ %p)\n", + _num_reports, _oldest_report, _next_report, _reports); + printf("TEMP: %d\n", _TEMP); + printf("SENS: %lld\n", _SENS); + printf("OFF: %lld\n", _OFF); + printf("MSL pressure: %10.4f\n", (double)(_msl_pressure / 100.f)); + + printf("factory_setup %u\n", _prom.s.factory_setup); + printf("c1_pressure_sens %u\n", _prom.s.c1_pressure_sens); + printf("c2_pressure_offset %u\n", _prom.s.c2_pressure_offset); + printf("c3_temp_coeff_pres_sens %u\n", _prom.s.c3_temp_coeff_pres_sens); + printf("c4_temp_coeff_pres_offset %u\n", _prom.s.c4_temp_coeff_pres_offset); + printf("c5_reference_temp %u\n", _prom.s.c5_reference_temp); + printf("c6_temp_coeff_temp %u\n", _prom.s.c6_temp_coeff_temp); + printf("serial_and_crc %u\n", _prom.s.serial_and_crc); +} + +bool +MS5611_SPI::crc4(uint16_t *n_prom) +{ + int16_t cnt; + uint16_t n_rem; + uint16_t crc_read; + uint8_t n_bit; + + n_rem = 0x00; + + /* save the read crc */ + crc_read = n_prom[7]; + + /* remove CRC byte */ + n_prom[7] = (0xFF00 & (n_prom[7])); + + for (cnt = 0; cnt < 16; cnt++) { + /* uneven bytes */ + if (cnt & 1) { + n_rem ^= (uint8_t)((n_prom[cnt >> 1]) & 0x00FF); + + } else { + n_rem ^= (uint8_t)(n_prom[cnt >> 1] >> 8); + } + + for (n_bit = 8; n_bit > 0; n_bit--) { + if (n_rem & 0x8000) { + n_rem = (n_rem << 1) ^ 0x3000; + + } else { + n_rem = (n_rem << 1); + } + } + } + + /* final 4 bit remainder is CRC value */ + n_rem = (0x000F & (n_rem >> 12)); + n_prom[7] = crc_read; + + /* return true if CRCs match */ + return (0x000F & crc_read) == (n_rem ^ 0x00); +} + +void +MS5611_SPI::print_info() { perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); @@ -952,7 +1795,7 @@ MS5611::print_info() namespace ms5611 { -MS5611 *g_dev; +device::CDev *g_dev; void start(); void test(); @@ -971,8 +1814,21 @@ start() if (g_dev != nullptr) errx(1, "already started"); - /* create the driver */ - g_dev = new MS5611(MS5611_BUS); + /* create the driver, try SPI first, fall back to I2C if required */ + #ifdef PX4_SPIDEV_BARO + { + warnx("Attempting SPI start"); + g_dev = new MS5611_SPI(1 /* XXX magic number, SPI1 */, BARO_DEVICE_PATH,(spi_dev_e)PX4_SPIDEV_BARO); + } + #endif + /* try I2C if configuration exists and SPI failed*/ + #ifdef MS5611_BUS + if (g_dev == nullptr) { + warnx("Attempting I2C start"); + g_dev = new MS5611_I2C(MS5611_BUS); + } + + #endif if (g_dev == nullptr) goto fail; @@ -1096,7 +1952,8 @@ info() errx(1, "driver not running"); printf("state @ %p\n", g_dev); - g_dev->print_info(); + MS5611_SPI* spi = (MS5611_SPI*)g_dev; + spi->print_info(); exit(0); } @@ -1154,11 +2011,11 @@ calibrate(unsigned altitude) const float g = 9.80665f; /* gravity constant in m/s/s */ const float R = 287.05f; /* ideal gas constant in J/kg/K */ - warnx("averaged pressure %10.4fkPa at %um", pressure, altitude); + warnx("averaged pressure %10.4fkPa at %um", (double)pressure, altitude); p1 = pressure * (powf(((T1 + (a * (float)altitude)) / T1), (g / (a * R)))); - warnx("calculated MSL pressure %10.4fkPa", p1); + warnx("calculated MSL pressure %10.4fkPa", (double)p1); /* save as integer Pa */ p1 *= 1000.0f; @@ -1211,4 +2068,4 @@ ms5611_main(int argc, char *argv[]) } errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'"); -} +} \ No newline at end of file From 90c458cb618754905ab6d373f22d76e3309adf4c Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 25 Jun 2013 23:08:34 -0700 Subject: [PATCH 189/486] Checkpoint: interface abstraction for px4io driver --- makefiles/config_px4fmuv2_default.mk | 1 + src/drivers/px4io/interface.h | 78 ++++++ src/drivers/px4io/interface_i2c.cpp | 179 ++++++++++++ src/drivers/px4io/interface_serial.cpp | 361 +++++++++++++++++++++++++ src/drivers/px4io/module.mk | 7 +- src/drivers/px4io/px4io.cpp | 91 +++---- src/modules/px4iofirmware/protocol.h | 44 +-- src/modules/systemlib/hx_stream.c | 219 +++++++++------ src/modules/systemlib/hx_stream.h | 60 ++-- 9 files changed, 847 insertions(+), 193 deletions(-) create mode 100644 src/drivers/px4io/interface.h create mode 100644 src/drivers/px4io/interface_i2c.cpp create mode 100644 src/drivers/px4io/interface_serial.cpp diff --git a/makefiles/config_px4fmuv2_default.mk b/makefiles/config_px4fmuv2_default.mk index 26c2499013..0463ccd84a 100644 --- a/makefiles/config_px4fmuv2_default.mk +++ b/makefiles/config_px4fmuv2_default.mk @@ -15,6 +15,7 @@ MODULES += drivers/stm32 MODULES += drivers/stm32/adc MODULES += drivers/stm32/tone_alarm MODULES += drivers/px4fmu +MODULES += drivers/px4io MODULES += drivers/boards/px4fmuv2 MODULES += drivers/rgbled MODULES += drivers/lsm303d diff --git a/src/drivers/px4io/interface.h b/src/drivers/px4io/interface.h new file mode 100644 index 0000000000..834cb9e077 --- /dev/null +++ b/src/drivers/px4io/interface.h @@ -0,0 +1,78 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file interface.h + * + * PX4IO interface classes. + */ + +#include + +#include + +class PX4IO_interface +{ +public: + /** + * Check that the interface initialised OK. + * + * Does not check that communication has been established. + */ + virtual bool ok() = 0; + + /** + * Set PX4IO registers. + * + * @param page The register page to write + * @param offset Offset of the first register to write + * @param values Pointer to values to write + * @param num_values The number of values to write + * @return Zero on success. + */ + virtual int set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) = 0; + + /** + * Get PX4IO registers. + * + * @param page The register page to read + * @param offset Offset of the first register to read + * @param values Pointer to store values read + * @param num_values The number of values to read + * @return Zero on success. + */ + virtual int get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) = 0; +}; + +extern PX4IO_interface *io_i2c_interface(int bus, uint8_t address); +extern PX4IO_interface *io_serial_interface(int port); diff --git a/src/drivers/px4io/interface_i2c.cpp b/src/drivers/px4io/interface_i2c.cpp new file mode 100644 index 0000000000..6895a7e23b --- /dev/null +++ b/src/drivers/px4io/interface_i2c.cpp @@ -0,0 +1,179 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + /** + * @file interface_i2c.cpp + * + * I2C interface for PX4IO + */ + +/* XXX trim includes */ +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include +#include "uploader.h" +#include + +#include "interface.h" + +class PX4IO_I2C : public PX4IO_interface +{ +public: + PX4IO_I2C(int bus, uint8_t address); + virtual ~PX4IO_I2C(); + + virtual int set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values); + virtual int get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values); + + virtual bool ok(); + +private: + static const unsigned _retries = 2; + + struct i2c_dev_s *_dev; + uint8_t _address; +}; + +PX4IO_interface *io_i2c_interface(int bus, uint8_t address) +{ + return new PX4IO_I2C(bus, address); +} + +PX4IO_I2C::PX4IO_I2C(int bus, uint8_t address) : + _dev(nullptr), + _address(address) +{ + _dev = up_i2cinitialize(bus); + if (_dev) + I2C_SETFREQUENCY(_dev, 400000); +} + +PX4IO_I2C::~PX4IO_I2C() +{ + if (_dev) + up_i2cuninitialize(_dev); +} + +bool +PX4IO_I2C::ok() +{ + if (!_dev) + return false; + + /* check any other status here */ + + return true; +} + +int +PX4IO_I2C::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) +{ + int ret; + + /* set up the transfer */ + uint8_t addr[2] = { + page, + offset + }; + i2c_msg_s msgv[2]; + + msgv[0].addr = _address; + msgv[0].flags = 0; + msgv[0].buffer = addr; + msgv[0].length = 2; + + msgv[1].addr = _address; + msgv[1].flags = I2C_M_NORESTART; + msgv[1].buffer = (uint8_t *)values; + msgv[1].length = num_values * sizeof(*values); + + unsigned tries = 0; + do { + + /* perform the transfer */ + ret = I2C_TRANSFER(_dev, msgv, 2); + + if (ret == OK) + break; + + } while (tries++ < _retries); + + return ret; +} + +int +PX4IO_I2C::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) +{ + int ret; + + /* set up the transfer */ + uint8_t addr[2] = { + page, + offset + }; + i2c_msg_s msgv[2]; + + msgv[0].addr = _address; + msgv[0].flags = 0; + msgv[0].buffer = addr; + msgv[0].length = 2; + + msgv[1].addr = _address; + msgv[1].flags = I2C_M_READ; + msgv[1].buffer = (uint8_t *)values; + msgv[1].length = num_values * sizeof(*values); + + unsigned tries = 0; + do { + /* perform the transfer */ + ret = I2C_TRANSFER(_dev, msgv, 2); + + if (ret == OK) + break; + + } while (tries++ < _retries); + + return ret; +} diff --git a/src/drivers/px4io/interface_serial.cpp b/src/drivers/px4io/interface_serial.cpp new file mode 100644 index 0000000000..f91284c723 --- /dev/null +++ b/src/drivers/px4io/interface_serial.cpp @@ -0,0 +1,361 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + /** + * @file interface_serial.cpp + * + * Serial interface for PX4IO + */ + +/* XXX trim includes */ +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +/* XXX might be able to prune these */ +#include +#include +#include +#include + +#include + +#include + +#include "interface.h" + +class PX4IO_serial : public PX4IO_interface +{ +public: + PX4IO_serial(int port); + virtual ~PX4IO_serial(); + + virtual int set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values); + virtual int get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values); + + virtual bool ok(); + +private: + volatile uint32_t *_serial_base; + int _vector; + + uint8_t *_tx_buf; + unsigned _tx_size; + + const uint8_t *_rx_buf; + unsigned _rx_size; + + hx_stream_t _stream; + + sem_t _bus_semaphore; + sem_t _completion_semaphore; + + /** + * Send _tx_size bytes from the buffer, then + * if _rx_size is greater than zero wait for a packet + * to come back. + */ + int _wait_complete(); + + /** + * Interrupt handler. + */ + static int _interrupt(int irq, void *context); + void _do_interrupt(); + + /** + * Stream transmit callback + */ + static void _tx(void *arg, uint8_t data); + void _do_tx(uint8_t data); + + /** + * Stream receive callback + */ + static void _rx(void *arg, const void *data, size_t length); + void _do_rx(const uint8_t *data, size_t length); + + /** + * Serial register accessors. + */ + volatile uint32_t &_sreg(unsigned offset) + { + return *(_serial_base + (offset / sizeof(uint32_t))); + } + volatile uint32_t &_SR() { return _sreg(STM32_USART_SR_OFFSET); } + volatile uint32_t &_DR() { return _sreg(STM32_USART_DR_OFFSET); } + volatile uint32_t &_BRR() { return _sreg(STM32_USART_BRR_OFFSET); } + volatile uint32_t &_CR1() { return _sreg(STM32_USART_CR1_OFFSET); } + volatile uint32_t &_CR2() { return _sreg(STM32_USART_CR2_OFFSET); } + volatile uint32_t &_CR3() { return _sreg(STM32_USART_CR3_OFFSET); } + volatile uint32_t &_GTPR() { return _sreg(STM32_USART_GTPR_OFFSET); } +}; + +/* XXX hack to avoid expensive IRQ lookup */ +static PX4IO_serial *io_serial; + +PX4IO_interface *io_serial_interface(int port) +{ + return new PX4IO_serial(port); +} + +PX4IO_serial::PX4IO_serial(int port) : + _serial_base(0), + _vector(0), + _tx_buf(nullptr), + _tx_size(0), + _rx_size(0), + _stream(0) +{ + /* only allow one instance */ + if (io_serial != nullptr) + return; + + switch (port) { + case 5: + _serial_base = (volatile uint32_t *)STM32_UART5_BASE; + _vector = STM32_IRQ_UART5; + break; + default: + /* not a supported port */ + return; + } + + /* need space for worst-case escapes + hx protocol overhead */ + /* XXX this is kinda gross, but hx transmits a byte at a time */ + _tx_buf = new uint8_t[HX_STREAM_MAX_FRAME]; + + irq_attach(_vector, &_interrupt); + + _stream = hx_stream_init(-1, _rx, this); + + sem_init(&_completion_semaphore, 0, 0); + sem_init(&_bus_semaphore, 0, 1); +} + +PX4IO_serial::~PX4IO_serial() +{ + + if (_tx_buf != nullptr) + delete[] _tx_buf; + + if (_vector) + irq_detach(_vector); + + if (io_serial == this) + io_serial = nullptr; + + if (_stream) + hx_stream_free(_stream); + + sem_destroy(&_completion_semaphore); + sem_destroy(&_bus_semaphore); +} + +bool +PX4IO_serial::ok() +{ + if (_serial_base == 0) + return false; + if (_vector == 0) + return false; + if (_tx_buf == nullptr) + return false; + if (!_stream) + return false; + + return true; +} + +int +PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) +{ + + unsigned count = num_values * sizeof(*values); + if (count > (HX_STREAM_MAX_FRAME - 2)) + return -EINVAL; + + sem_wait(&_bus_semaphore); + + _tx_buf[0] = page; + _tx_buf[1] = offset; + memcpy(&_tx_buf[2], (void *)values, count); + + _tx_size = count + 2; + _rx_size = 0; + + /* start the transaction and wait for it to complete */ + int result = _wait_complete(); + + sem_post(&_bus_semaphore); + return result; +} + +int +PX4IO_serial::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) +{ + + unsigned count = num_values * sizeof(*values); + if (count > HX_STREAM_MAX_FRAME) + return -EINVAL; + + sem_wait(&_bus_semaphore); + + _tx_buf[0] = page; + _tx_buf[1] = offset; + _tx_buf[2] = num_values; + + _tx_size = 3; /* this tells IO that this is a read request */ + _rx_size = count; + + /* start the transaction and wait for it to complete */ + int result = _wait_complete(); + if (result != OK) + goto out; + + /* compare the received count with the expected count */ + if (_rx_size != count) { + return -EIO; + } else { + /* copy back the result */ + memcpy(values, &_tx_buf[0], count); + } +out: + sem_post(&_bus_semaphore); + return OK; +} + +int +PX4IO_serial::_wait_complete() +{ + /* prepare the stream for transmission */ + hx_stream_reset(_stream); + hx_stream_start(_stream, _tx_buf, _tx_size); + + /* enable UART */ + _CR1() |= USART_CR1_RE | + USART_CR1_TE | + USART_CR1_TXEIE | + USART_CR1_RXNEIE | + USART_CR1_UE; + + /* compute the deadline for a 5ms timeout */ + struct timespec abstime; + clock_gettime(CLOCK_REALTIME, &abstime); + abstime.tv_nsec += 5000000; /* 5ms timeout */ + while (abstime.tv_nsec > 1000000000) { + abstime.tv_sec++; + abstime.tv_nsec -= 1000000000; + } + + /* wait for the transaction to complete */ + int ret = sem_timedwait(&_completion_semaphore, &abstime); + + /* disable the UART */ + _CR1() &= ~(USART_CR1_RE | + USART_CR1_TE | + USART_CR1_TXEIE | + USART_CR1_RXNEIE | + USART_CR1_UE); + + return ret; +} + +int +PX4IO_serial::_interrupt(int irq, void *context) +{ + /* ... because NuttX doesn't give us a handle per vector */ + io_serial->_do_interrupt(); + return 0; +} + +void +PX4IO_serial::_do_interrupt() +{ + uint32_t sr = _SR(); + + /* handle transmit completion */ + if (sr & USART_SR_TXE) { + int c = hx_stream_send_next(_stream); + if (c == -1) { + /* transmit (nearly) done, not interested in TX-ready interrupts now */ + _CR1() &= ~USART_CR1_TXEIE; + + /* was this a tx-only operation? */ + if (_rx_size == 0) { + /* wake up waiting sender */ + sem_post(&_completion_semaphore); + } + } else { + _DR() = c; + } + } + + if (sr & USART_SR_RXNE) { + uint8_t c = _DR(); + + hx_stream_rx(_stream, c); + } +} + +void +PX4IO_serial::_rx(void *arg, const void *data, size_t length) +{ + PX4IO_serial *pserial = reinterpret_cast(arg); + + pserial->_do_rx((const uint8_t *)data, length); +} + +void +PX4IO_serial::_do_rx(const uint8_t *data, size_t length) +{ + _rx_buf = data; + + if (length < _rx_size) + _rx_size = length; + + /* notify waiting receiver */ + sem_post(&_completion_semaphore); +} + + + diff --git a/src/drivers/px4io/module.mk b/src/drivers/px4io/module.mk index 328e5a6843..d5bab6599f 100644 --- a/src/drivers/px4io/module.mk +++ b/src/drivers/px4io/module.mk @@ -38,4 +38,9 @@ MODULE_COMMAND = px4io SRCS = px4io.cpp \ - uploader.cpp + uploader.cpp \ + interface_serial.cpp \ + interface_i2c.cpp + +# XXX prune to just get UART registers +INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 19163cebe3..ecf50c859f 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -58,7 +58,6 @@ #include #include -#include #include #include #include @@ -83,16 +82,18 @@ #include #include -#include "uploader.h" #include +#include "uploader.h" +#include "interface.h" + #define PX4IO_SET_DEBUG _IOC(0xff00, 0) #define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1) -class PX4IO : public device::I2C +class PX4IO : public device::CDev { public: - PX4IO(); + PX4IO(PX4IO_interface *interface); virtual ~PX4IO(); virtual int init(); @@ -130,6 +131,8 @@ public: void print_status(); private: + PX4IO_interface *_interface; + // XXX unsigned _max_actuators; unsigned _max_controls; @@ -312,8 +315,9 @@ PX4IO *g_dev; } -PX4IO::PX4IO() : - I2C("px4io", "/dev/px4io", PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000), +PX4IO::PX4IO(PX4IO_interface *interface) : + CDev("px4io", "/dev/px4io"), + _interface(interface), _max_actuators(0), _max_controls(0), _max_rc_input(0), @@ -364,6 +368,9 @@ PX4IO::~PX4IO() if (_task != -1) task_delete(_task); + if (_interface != nullptr) + delete _interface; + g_dev = nullptr; } @@ -375,18 +382,10 @@ PX4IO::init() ASSERT(_task == -1); /* do regular cdev init */ - ret = I2C::init(); + ret = CDev::init(); if (ret != OK) return ret; - /* - * Enable a couple of retries for operations to IO. - * - * Register read/write operations are intentionally idempotent - * so this is safe as designed. - */ - _retries = 2; - /* get some parameters */ _max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT); _max_controls = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT); @@ -395,7 +394,7 @@ PX4IO::init() _max_rc_input = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT); if ((_max_actuators < 1) || (_max_actuators > 255) || (_max_relays < 1) || (_max_relays > 255) || - (_max_transfer < 16) || (_max_transfer > 255) || + (_max_transfer < 16) || (_max_transfer > 255) || (_max_rc_input < 1) || (_max_rc_input > 255)) { log("failed getting parameters from PX4IO"); @@ -700,8 +699,6 @@ PX4IO::io_set_control_state() int PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len) { - uint16_t regs[_max_actuators]; - if (len > _max_actuators) /* fail with error */ return E2BIG; @@ -1114,22 +1111,7 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned return -EINVAL; } - /* set up the transfer */ - uint8_t addr[2] = { - page, - offset - }; - i2c_msg_s msgv[2]; - - msgv[0].flags = 0; - msgv[0].buffer = addr; - msgv[0].length = 2; - msgv[1].flags = I2C_M_NORESTART; - msgv[1].buffer = (uint8_t *)values; - msgv[1].length = num_values * sizeof(*values); - - /* perform the transfer */ - int ret = transfer(msgv, 2); + int ret = _interface->set_reg(page, offset, values, num_values); if (ret != OK) debug("io_reg_set: error %d", ret); return ret; @@ -1144,22 +1126,13 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, uint16_t value) int PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) { - /* set up the transfer */ - uint8_t addr[2] = { - page, - offset - }; - i2c_msg_s msgv[2]; + /* range check the transfer */ + if (num_values > ((_max_transfer) / sizeof(*values))) { + debug("io_reg_get: too many registers (%u, max %u)", num_values, _max_transfer / 2); + return -EINVAL; + } - msgv[0].flags = 0; - msgv[0].buffer = addr; - msgv[0].length = 2; - msgv[1].flags = I2C_M_READ; - msgv[1].buffer = (uint8_t *)values; - msgv[1].length = num_values * sizeof(*values); - - /* perform the transfer */ - int ret = transfer(msgv, 2); + int ret = _interface->get_reg(page, offset, values, num_values); if (ret != OK) debug("io_reg_get: data error %d", ret); return ret; @@ -1603,8 +1576,26 @@ start(int argc, char *argv[]) if (g_dev != nullptr) errx(1, "already loaded"); + PX4IO_interface *interface; + +#if defined(CONFIG_ARCH_BOARD_PX4FMUV2) + interface = io_serial_interface(5); /* XXX wrong port! */ +#elif defined(CONFIG_ARCH_BOARD_PX4FMU) + interface = io_i2c_interface(PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO); +#else +# error Unknown board - cannot select interface. +#endif + + if (interface == nullptr) + errx(1, "cannot alloc interface"); + + if (!interface->ok()) { + delete interface; + errx(1, "interface init failed"); + } + /* create the driver - it will set g_dev */ - (void)new PX4IO(); + (void)new PX4IO(interface); if (g_dev == nullptr) errx(1, "driver alloc failed"); diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 90d63ea1a2..0e40bff691 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -36,8 +36,7 @@ /** * @file protocol.h * - * PX4IO interface protocol - * ======================== + * PX4IO interface protocol. * * Communication is performed via writes to and reads from 16-bit virtual * registers organised into pages of 255 registers each. @@ -46,7 +45,7 @@ * respectively. Subsequent reads and writes increment the offset within * the page. * - * Most pages are readable or writable but not both. + * Some pages are read- or write-only. * * Note that some pages may permit offset values greater than 255, which * can only be achieved by long writes. The offset does not wrap. @@ -63,29 +62,6 @@ * Note that the implementation of readable pages prefers registers within * readable pages to be densely packed. Page numbers do not need to be * packed. - * - * PX4IO I2C interface notes - * ------------------------- - * - * Register read/write operations are mapped directly to PX4IO register - * read/write operations. - * - * PX4IO Serial interface notes - * ---------------------------- - * - * The MSB of the page number is used to distinguish between read and - * write operations. If set, the operation is a write and additional - * data is expected to follow in the packet as for I2C writes. - * - * If clear, the packet is expected to contain a single byte giving the - * number of registers to be read. PX4IO will respond with a packet containing - * the same header (page, offset) and the requested data. - * - * If a read is requested when PX4IO does not have buffer space to store - * the reply, the request will be dropped. PX4IO is always configured with - * enough space to receive one full-sized write and one read request, and - * to send one full-sized read response. - * */ #define PX4IO_CONTROL_CHANNELS 8 @@ -99,14 +75,12 @@ #define REG_TO_FLOAT(_reg) ((float)REG_TO_SIGNED(_reg) / 10000.0f) #define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)((_float) * 10000.0f)) -#define PX4IO_PAGE_WRITE (1<<7) - /* static configuration page */ #define PX4IO_PAGE_CONFIG 0 #define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* magic numbers TBD */ #define PX4IO_P_CONFIG_SOFTWARE_VERSION 1 /* magic numbers TBD */ #define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */ -#define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum packet transfer size */ +#define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum I2C transfer size */ #define PX4IO_P_CONFIG_CONTROL_COUNT 4 /* hardcoded max control count supported */ #define PX4IO_P_CONFIG_ACTUATOR_COUNT 5 /* hardcoded max actuator output count */ #define PX4IO_P_CONFIG_RC_INPUT_COUNT 6 /* hardcoded max R/C input count supported */ @@ -168,7 +142,7 @@ #define PX4IO_RATE_MAP_BASE 0 /* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */ /* setup page */ -#define PX4IO_PAGE_SETUP 64 +#define PX4IO_PAGE_SETUP 100 #define PX4IO_P_SETUP_FEATURES 0 #define PX4IO_P_SETUP_ARMING 1 /* arming controls */ @@ -186,13 +160,13 @@ #define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ /* autopilot control values, -10000..10000 */ -#define PX4IO_PAGE_CONTROLS 65 /* 0..CONFIG_CONTROL_COUNT */ +#define PX4IO_PAGE_CONTROLS 101 /* 0..CONFIG_CONTROL_COUNT */ /* raw text load to the mixer parser - ignores offset */ -#define PX4IO_PAGE_MIXERLOAD 66 /* see px4io_mixdata structure below */ +#define PX4IO_PAGE_MIXERLOAD 102 /* R/C channel config */ -#define PX4IO_PAGE_RC_CONFIG 67 /* R/C input configuration */ +#define PX4IO_PAGE_RC_CONFIG 103 /* R/C input configuration */ #define PX4IO_P_RC_CONFIG_MIN 0 /* lowest input value */ #define PX4IO_P_RC_CONFIG_CENTER 1 /* center input value */ #define PX4IO_P_RC_CONFIG_MAX 2 /* highest input value */ @@ -204,10 +178,10 @@ #define PX4IO_P_RC_CONFIG_STRIDE 6 /* spacing between channel config data */ /* PWM output - overrides mixer */ -#define PX4IO_PAGE_DIRECT_PWM 68 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +#define PX4IO_PAGE_DIRECT_PWM 104 /* 0..CONFIG_ACTUATOR_COUNT-1 */ /* PWM failsafe values - zero disables the output */ -#define PX4IO_PAGE_FAILSAFE_PWM 69 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +#define PX4IO_PAGE_FAILSAFE_PWM 105 /* 0..CONFIG_ACTUATOR_COUNT-1 */ /** * As-needed mixer data upload. diff --git a/src/modules/systemlib/hx_stream.c b/src/modules/systemlib/hx_stream.c index 88f7f762ca..fdc3edac75 100644 --- a/src/modules/systemlib/hx_stream.c +++ b/src/modules/systemlib/hx_stream.c @@ -53,14 +53,26 @@ struct hx_stream { - uint8_t buf[HX_STREAM_MAX_FRAME + 4]; - unsigned frame_bytes; - bool escaped; - bool txerror; + /* RX state */ + uint8_t rx_buf[HX_STREAM_MAX_FRAME + 4]; + unsigned rx_frame_bytes; + bool rx_escaped; + hx_stream_rx_callback rx_callback; + void *rx_callback_arg; + /* TX state */ int fd; - hx_stream_rx_callback callback; - void *callback_arg; + bool tx_error; + uint8_t *tx_buf; + unsigned tx_resid; + uint32_t tx_crc; + enum { + TX_IDLE = 0, + TX_SEND_START, + TX_SEND_DATA, + TX_SENT_ESCAPE, + TX_SEND_END + } tx_state; perf_counter_t pc_tx_frames; perf_counter_t pc_rx_frames; @@ -76,13 +88,12 @@ struct hx_stream { static void hx_tx_raw(hx_stream_t stream, uint8_t c); static void hx_tx_raw(hx_stream_t stream, uint8_t c); static int hx_rx_frame(hx_stream_t stream); -static bool hx_rx_char(hx_stream_t stream, uint8_t c); static void hx_tx_raw(hx_stream_t stream, uint8_t c) { if (write(stream->fd, &c, 1) != 1) - stream->txerror = true; + stream->tx_error = true; } static void @@ -106,11 +117,11 @@ hx_rx_frame(hx_stream_t stream) uint8_t b[4]; uint32_t w; } u; - unsigned length = stream->frame_bytes; + unsigned length = stream->rx_frame_bytes; /* reset the stream */ - stream->frame_bytes = 0; - stream->escaped = false; + stream->rx_frame_bytes = 0; + stream->rx_escaped = false; /* not a real frame - too short */ if (length < 4) { @@ -123,11 +134,11 @@ hx_rx_frame(hx_stream_t stream) length -= 4; /* compute expected CRC */ - u.w = crc32(&stream->buf[0], length); + u.w = crc32(&stream->rx_buf[0], length); /* compare computed and actual CRC */ for (unsigned i = 0; i < 4; i++) { - if (u.b[i] != stream->buf[length + i]) { + if (u.b[i] != stream->rx_buf[length + i]) { perf_count(stream->pc_rx_errors); return 0; } @@ -135,7 +146,7 @@ hx_rx_frame(hx_stream_t stream) /* frame is good */ perf_count(stream->pc_rx_frames); - stream->callback(stream->callback_arg, &stream->buf[0], length); + stream->rx_callback(stream->rx_callback_arg, &stream->rx_buf[0], length); return 1; } @@ -151,8 +162,8 @@ hx_stream_init(int fd, if (stream != NULL) { memset(stream, 0, sizeof(struct hx_stream)); stream->fd = fd; - stream->callback = callback; - stream->callback_arg = arg; + stream->rx_callback = callback; + stream->rx_callback_arg = arg; } return stream; @@ -180,105 +191,135 @@ hx_stream_set_counters(hx_stream_t stream, stream->pc_rx_errors = rx_errors; } +void +hx_stream_reset(hx_stream_t stream) +{ + stream->rx_frame_bytes = 0; + stream->rx_escaped = false; + + stream->tx_buf = NULL; + stream->tx_resid = 0; + stream->tx_state = TX_IDLE; +} + +int +hx_stream_start(hx_stream_t stream, + const void *data, + size_t count) +{ + if (count > HX_STREAM_MAX_FRAME) + return -EINVAL; + + stream->tx_buf = data; + stream->tx_resid = count; + stream->tx_state = TX_SEND_START; + stream->tx_crc = crc32(data, count); + return OK; +} + +int +hx_stream_send_next(hx_stream_t stream) +{ + int c; + + /* sort out what we're going to send */ + switch (stream->tx_state) { + + case TX_SEND_START: + stream->tx_state = TX_SEND_DATA; + return FBO; + + case TX_SEND_DATA: + c = *stream->tx_buf; + + switch (c) { + case FBO: + case CEO: + stream->tx_state = TX_SENT_ESCAPE; + return CEO; + } + break; + + case TX_SENT_ESCAPE: + c = *stream->tx_buf ^ 0x20; + stream->tx_state = TX_SEND_DATA; + break; + + case TX_SEND_END: + stream->tx_state = TX_IDLE; + return FBO; + + case TX_IDLE: + default: + return -1; + } + + /* if we are here, we have consumed a byte from the buffer */ + stream->tx_resid--; + stream->tx_buf++; + + /* buffer exhausted */ + if (stream->tx_resid == 0) { + uint8_t *pcrc = (uint8_t *)&stream->tx_crc; + + /* was the buffer the frame CRC? */ + if (stream->tx_buf == (pcrc + sizeof(stream->tx_crc))) { + stream->tx_state = TX_SEND_END; + } else { + /* no, it was the payload - switch to sending the CRC */ + stream->tx_buf = pcrc; + stream->tx_resid = sizeof(stream->tx_crc); + } + } + return c; +} + int hx_stream_send(hx_stream_t stream, const void *data, size_t count) { - union { - uint8_t b[4]; - uint32_t w; - } u; - const uint8_t *p = (const uint8_t *)data; - unsigned resid = count; + int result; - if (resid > HX_STREAM_MAX_FRAME) - return -EINVAL; + result = hx_start(stream, data, count); + if (result != OK) + return result; - /* start the frame */ - hx_tx_raw(stream, FBO); - - /* transmit the data */ - while (resid--) - hx_tx_byte(stream, *p++); - - /* compute the CRC */ - u.w = crc32(data, count); - - /* send the CRC */ - p = &u.b[0]; - resid = 4; - - while (resid--) - hx_tx_byte(stream, *p++); - - /* and the trailing frame separator */ - hx_tx_raw(stream, FBO); + int c; + while ((c = hx_send_next(stream)) >= 0) + hx_tx_raw(stream, c); /* check for transmit error */ - if (stream->txerror) { - stream->txerror = false; + if (stream->tx_error) { + stream->tx_error = false; return -EIO; } perf_count(stream->pc_tx_frames); - return 0; + return OK; } -static bool -hx_rx_char(hx_stream_t stream, uint8_t c) +void +hx_stream_rx(hx_stream_t stream, uint8_t c) { /* frame end? */ if (c == FBO) { hx_rx_frame(stream); - return true; + return; } /* escaped? */ - if (stream->escaped) { - stream->escaped = false; + if (stream->rx_escaped) { + stream->rx_escaped = false; c ^= 0x20; } else if (c == CEO) { - /* now escaped, ignore the byte */ - stream->escaped = true; - return false; + /* now rx_escaped, ignore the byte */ + stream->rx_escaped = true; + return; } /* save for later */ - if (stream->frame_bytes < sizeof(stream->buf)) - stream->buf[stream->frame_bytes++] = c; - - return false; -} - -void -hx_stream_rx_char(hx_stream_t stream, uint8_t c) -{ - hx_rx_char(stream, c); -} - -int -hx_stream_rx(hx_stream_t stream) -{ - uint16_t buf[16]; - ssize_t len; - - /* read bytes */ - len = read(stream->fd, buf, sizeof(buf)); - if (len <= 0) { - - /* nonblocking stream and no data */ - if (errno == EWOULDBLOCK) - return 0; - - /* error or EOF */ - return -errno; - } - - /* process received characters */ - for (int i = 0; i < len; i++) - hx_rx_char(stream, buf[i]); - - return 0; + if (stream->rx_frame_bytes < sizeof(stream->rx_buf)) + stream->rx_buf[stream->rx_frame_bytes++] = c; } diff --git a/src/modules/systemlib/hx_stream.h b/src/modules/systemlib/hx_stream.h index be4850f745..1f3927222a 100644 --- a/src/modules/systemlib/hx_stream.h +++ b/src/modules/systemlib/hx_stream.h @@ -58,7 +58,8 @@ __BEGIN_DECLS * Allocate a new hx_stream object. * * @param fd The file handle over which the protocol will - * communicate. + * communicate, or -1 if the protocol will use + * hx_stream_start/hx_stream_send_next. * @param callback Called when a frame is received. * @param callback_arg Passed to the callback. * @return A handle to the stream, or NULL if memory could @@ -80,6 +81,7 @@ __EXPORT extern void hx_stream_free(hx_stream_t stream); * * Any counter may be set to NULL to disable counting that datum. * + * @param stream A handle returned from hx_stream_init. * @param tx_frames Counter for transmitted frames. * @param rx_frames Counter for received frames. * @param rx_errors Counter for short and corrupt received frames. @@ -89,6 +91,44 @@ __EXPORT extern void hx_stream_set_counters(hx_stream_t stream, perf_counter_t rx_frames, perf_counter_t rx_errors); +/** + * Reset a stream. + * + * Forces the local stream state to idle. + * + * @param stream A handle returned from hx_stream_init. + */ +__EXPORT extern void hx_stream_reset(hx_stream_t stream); + +/** + * Prepare to send a frame. + * + * Use this in conjunction with hx_stream_send_next to + * set the frame to be transmitted. + * + * Use hx_stream_send() to write to the stream fd directly. + * + * @param stream A handle returned from hx_stream_init. + * @param data Pointer to the data to send. + * @param count The number of bytes to send. + * @return Zero on success, -errno on error. + */ +__EXPORT extern int hx_stream_start(hx_stream_t stream, + const void *data, + size_t count); + +/** + * Get the next byte to send for a stream. + * + * This requires that the stream be prepared for sending by + * calling hx_stream_start first. + * + * @param stream A handle returned from hx_stream_init. + * @return The byte to send, or -1 if there is + * nothing left to send. + */ +__EXPORT extern int hx_stream_send_next(hx_stream_t stream); + /** * Send a frame. * @@ -114,25 +154,9 @@ __EXPORT extern int hx_stream_send(hx_stream_t stream, * @param stream A handle returned from hx_stream_init. * @param c The character to process. */ -__EXPORT extern void hx_stream_rx_char(hx_stream_t stream, +__EXPORT extern void hx_stream_rx(hx_stream_t stream, uint8_t c); -/** - * Handle received bytes from the stream. - * - * Note that this interface should only be used with blocking streams - * when it is OK for the call to block until a frame is received. - * - * When used with a non-blocking stream, it will typically return - * immediately, or after handling a received frame. - * - * @param stream A handle returned from hx_stream_init. - * @return -errno on error, nonzero if a frame - * has been received, or if not enough - * bytes are available to complete a frame. - */ -__EXPORT extern int hx_stream_rx(hx_stream_t stream); - __END_DECLS #endif From 76346bfe19c816491a6982abfa10f48cd9d258f6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 27 Jun 2013 20:20:27 +0200 Subject: [PATCH 190/486] Corrected merge mistake --- src/modules/sdlog2/sdlog2.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index bdb98a24e7..8a07855c05 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -686,7 +686,6 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_SENS_s log_SENS; struct log_LPOS_s log_LPOS; struct log_LPSP_s log_LPSP; - struct log_FLOW_s log_FLOW; struct log_GPS_s log_GPS; struct log_ATTC_s log_ATTC; struct log_STAT_s log_STAT; From 1759f30e3aa4b2da25dcd0c836480f069d917a88 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 28 Jun 2013 11:10:46 +0400 Subject: [PATCH 191/486] Sonar added to position_estimator_inav --- .../position_estimator_inav/inertial_filter.c | 7 +- .../position_estimator_inav/inertial_filter.h | 2 +- .../position_estimator_inav_main.c | 79 +++++++++++++++---- .../position_estimator_inav_params.c | 15 ++++ .../position_estimator_inav_params.h | 10 +++ 5 files changed, 93 insertions(+), 20 deletions(-) diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c index acaf693f17..13328edb4b 100644 --- a/src/modules/position_estimator_inav/inertial_filter.c +++ b/src/modules/position_estimator_inav/inertial_filter.c @@ -13,9 +13,12 @@ void inertial_filter_predict(float dt, float x[3]) x[1] += x[2] * dt; } -void inertial_filter_correct(float edt, float x[3], int i, float w) +void inertial_filter_correct(float e, float dt, float x[3], int i, float w) { - float ewdt = w * edt; + float ewdt = w * dt; + if (ewdt > 1.0f) + ewdt = 1.0f; // prevent over-correcting + ewdt *= e; x[i] += ewdt; if (i == 0) { diff --git a/src/modules/position_estimator_inav/inertial_filter.h b/src/modules/position_estimator_inav/inertial_filter.h index dca6375dce..761c17097d 100644 --- a/src/modules/position_estimator_inav/inertial_filter.h +++ b/src/modules/position_estimator_inav/inertial_filter.h @@ -10,4 +10,4 @@ void inertial_filter_predict(float dt, float x[3]); -void inertial_filter_correct(float edt, float x[3], int i, float w); +void inertial_filter_correct(float e, float dt, float x[3], int i, float w); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 278a319b51..fb09948ec4 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -59,6 +59,7 @@ #include #include #include +#include #include #include #include @@ -183,6 +184,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) memset(&att, 0, sizeof(att)); struct vehicle_local_position_s local_pos; memset(&local_pos, 0, sizeof(local_pos)); + struct optical_flow_s flow; + memset(&flow, 0, sizeof(flow)); struct vehicle_global_position_s global_pos; memset(&global_pos, 0, sizeof(global_pos)); @@ -191,6 +194,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow)); int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); /* advertise */ @@ -263,12 +267,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) hrt_abstime t_prev = 0; uint16_t accel_updates = 0; - hrt_abstime accel_t = 0; uint16_t baro_updates = 0; - hrt_abstime baro_t = 0; - hrt_abstime gps_t = 0; uint16_t gps_updates = 0; uint16_t attitude_updates = 0; + uint16_t flow_updates = 0; hrt_abstime updates_counter_start = hrt_absolute_time(); uint32_t updates_counter_len = 1000000; @@ -283,6 +285,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { 0.0f, 0.0f }, // N (pos, vel) { 0.0f, 0.0f }, // E (pos, vel) }; + float sonar_corr = 0.0f; + float sonar_corr_filtered = 0.0f; + float flow_corr[] = { 0.0f, 0.0f }; // X, Y + + float sonar_prev = 0.0f; + hrt_abstime sonar_time = 0; /* main loop */ struct pollfd fds[5] = { @@ -290,6 +298,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { .fd = vehicle_status_sub, .events = POLLIN }, { .fd = vehicle_attitude_sub, .events = POLLIN }, { .fd = sensor_combined_sub, .events = POLLIN }, + { .fd = optical_flow_sub, .events = POLLIN }, { .fd = vehicle_gps_position_sub, .events = POLLIN } }; @@ -297,7 +306,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) warnx("main loop started."); while (!thread_should_exit) { - int ret = poll(fds, params.use_gps ? 5 : 4, 10); // wait maximal this 10 ms = 100 Hz minimum rate + int ret = poll(fds, params.use_gps ? 6 : 5, 10); // wait maximal this 10 ms = 100 Hz minimum rate hrt_abstime t = hrt_absolute_time(); if (ret < 0) { @@ -355,13 +364,45 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } if (sensor.baro_counter > baro_counter) { - baro_corr = baro_alt0 - sensor.baro_alt_meter - z_est[0]; + baro_corr = - sensor.baro_alt_meter - z_est[0]; baro_counter = sensor.baro_counter; baro_updates++; } } - if (params.use_gps && fds[4].revents & POLLIN) { + /* optical flow */ + if (fds[4].revents & POLLIN) { + orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); + if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && (flow.ground_distance_m != sonar_prev || t - sonar_time < 150000)) { + if (flow.ground_distance_m != sonar_prev) { + sonar_time = t; + sonar_prev = flow.ground_distance_m; + sonar_corr = -flow.ground_distance_m - z_est[0]; + sonar_corr_filtered += (sonar_corr - sonar_corr_filtered) * params.sonar_filt; + if (fabsf(sonar_corr) > params.sonar_err) { + // correction is too large: spike or new ground level? + if (fabsf(sonar_corr - sonar_corr_filtered) > params.sonar_err) { + // spike detected, ignore + sonar_corr = 0.0f; + } else { + // new ground level + baro_alt0 += sonar_corr; + warnx("new home: alt = %.3f", baro_alt0); + mavlink_log_info(mavlink_fd, "[inav] new home: alt = %.3f", baro_alt0); + local_pos.home_alt = baro_alt0; + local_pos.home_timestamp = hrt_absolute_time(); + sonar_corr = 0.0f; + sonar_corr_filtered = 0.0; + } + } + } + } else { + sonar_corr = 0.0f; + } + flow_updates++; + } + + if (params.use_gps && fds[5].revents & POLLIN) { /* vehicle GPS position */ orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); if (gps.fix_type >= 3) { @@ -393,8 +434,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_predict(dt, z_est); /* inertial filter correction for altitude */ - inertial_filter_correct(baro_corr * dt, z_est, 0, params.w_alt_baro); - inertial_filter_correct(accel_corr[2] * dt, z_est, 2, params.w_alt_acc); + baro_alt0 += sonar_corr * params.w_alt_sonar * dt; + inertial_filter_correct(baro_corr + baro_alt0, dt, z_est, 0, params.w_alt_baro); + inertial_filter_correct(sonar_corr, dt, z_est, 0, params.w_alt_sonar); + inertial_filter_correct(accel_corr[2], dt, z_est, 2, params.w_alt_acc); /* dont't try to estimate position when no any position source available */ bool can_estimate_pos = params.use_gps && gps.fix_type >= 3; @@ -404,15 +447,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_predict(dt, y_est); /* inertial filter correction for position */ - inertial_filter_correct(accel_corr[0] * dt, x_est, 2, params.w_pos_acc); - inertial_filter_correct(accel_corr[1] * dt, y_est, 2, params.w_pos_acc); + inertial_filter_correct(accel_corr[0], dt, x_est, 2, params.w_pos_acc); + inertial_filter_correct(accel_corr[1], dt, y_est, 2, params.w_pos_acc); if (params.use_gps && gps.fix_type >= 3) { - inertial_filter_correct(gps_corr[0][0] * dt, x_est, 0, params.w_pos_gps_p); - inertial_filter_correct(gps_corr[1][0] * dt, y_est, 0, params.w_pos_gps_p); + inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p); + inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p); if (gps.vel_ned_valid) { - inertial_filter_correct(gps_corr[0][1] * dt, x_est, 1, params.w_pos_gps_v); - inertial_filter_correct(gps_corr[1][1] * dt, y_est, 1, params.w_pos_gps_v); + inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v); + inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v); } } } @@ -421,17 +464,19 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* print updates rate */ if (t - updates_counter_start > updates_counter_len) { float updates_dt = (t - updates_counter_start) * 0.000001f; - printf( - "[inav] updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s\n", + warnx( + "updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s", accel_updates / updates_dt, baro_updates / updates_dt, gps_updates / updates_dt, - attitude_updates / updates_dt); + attitude_updates / updates_dt, + flow_updates / updates_dt); updates_counter_start = t; accel_updates = 0; baro_updates = 0; gps_updates = 0; attitude_updates = 0; + flow_updates = 0; } } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 49dc7f51ff..eac2fc1ce9 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -43,18 +43,28 @@ PARAM_DEFINE_INT32(INAV_USE_GPS, 1); PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 1.0f); PARAM_DEFINE_FLOAT(INAV_W_ALT_ACC, 50.0f); +PARAM_DEFINE_FLOAT(INAV_W_ALT_SONAR, 3.0f); PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_P, 4.0f); PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 0.0f); PARAM_DEFINE_FLOAT(INAV_W_POS_ACC, 10.0f); +PARAM_DEFINE_FLOAT(INAV_W_POS_FLOW, 10.0f); +PARAM_DEFINE_FLOAT(INAV_FLOW_K, 1.0f); +PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.5f); +PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); int parameters_init(struct position_estimator_inav_param_handles *h) { h->use_gps = param_find("INAV_USE_GPS"); h->w_alt_baro = param_find("INAV_W_ALT_BARO"); h->w_alt_acc = param_find("INAV_W_ALT_ACC"); + h->w_alt_sonar = param_find("INAV_W_ALT_SONAR"); h->w_pos_gps_p = param_find("INAV_W_POS_GPS_P"); h->w_pos_gps_v = param_find("INAV_W_POS_GPS_V"); h->w_pos_acc = param_find("INAV_W_POS_ACC"); + h->w_pos_flow = param_find("INAV_W_POS_FLOW"); + h->flow_k = param_find("INAV_FLOW_K"); + h->sonar_filt = param_find("INAV_SONAR_FILT"); + h->sonar_err = param_find("INAV_SONAR_ERR"); return OK; } @@ -64,9 +74,14 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->use_gps, &(p->use_gps)); param_get(h->w_alt_baro, &(p->w_alt_baro)); param_get(h->w_alt_acc, &(p->w_alt_acc)); + param_get(h->w_alt_sonar, &(p->w_alt_sonar)); param_get(h->w_pos_gps_p, &(p->w_pos_gps_p)); param_get(h->w_pos_gps_v, &(p->w_pos_gps_v)); param_get(h->w_pos_acc, &(p->w_pos_acc)); + param_get(h->w_pos_flow, &(p->w_pos_flow)); + param_get(h->flow_k, &(p->flow_k)); + param_get(h->sonar_filt, &(p->sonar_filt)); + param_get(h->sonar_err, &(p->sonar_err)); return OK; } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index 870227fefc..cca172b5d6 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -44,18 +44,28 @@ struct position_estimator_inav_params { int use_gps; float w_alt_baro; float w_alt_acc; + float w_alt_sonar; float w_pos_gps_p; float w_pos_gps_v; float w_pos_acc; + float w_pos_flow; + float flow_k; + float sonar_filt; + float sonar_err; }; struct position_estimator_inav_param_handles { param_t use_gps; param_t w_alt_baro; param_t w_alt_acc; + param_t w_alt_sonar; param_t w_pos_gps_p; param_t w_pos_gps_v; param_t w_pos_acc; + param_t w_pos_flow; + param_t flow_k; + param_t sonar_filt; + param_t sonar_err; }; /** From 8e732fc52763a05739e4f13caf0dff84817839cd Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 28 Jun 2013 12:58:12 +0400 Subject: [PATCH 192/486] position_estimator_inav default parameters changed, some fixes --- .../multirotor_pos_control/multirotor_pos_control.c | 10 ++++++++++ .../position_estimator_inav_main.c | 1 + .../position_estimator_inav_params.c | 2 +- 3 files changed, 12 insertions(+), 1 deletion(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 508879ae2e..3a14d516a6 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -209,6 +209,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { float pos_y_integral = 0.0f; const float alt_ctl_dz = 0.2f; const float pos_ctl_dz = 0.05f; + float home_alt = 0.0f; + hrt_abstime home_alt_t = 0; thread_running = true; @@ -288,6 +290,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { float pos_sp_speed_z = 0.0f; if (status.flag_control_manual_enabled) { + if (local_pos.home_timestamp != home_alt_t) { + if (home_alt_t != 0) { + /* home alt changed, don't follow large ground level changes in manual flight */ + local_pos_sp.z -= local_pos.home_alt - home_alt; + } + home_alt_t = local_pos.home_timestamp; + home_alt = local_pos.home_alt; + } /* move altitude setpoint with manual controls */ float alt_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz); if (alt_sp_ctl != 0.0f) { diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index fb09948ec4..428269e4aa 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -391,6 +391,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[inav] new home: alt = %.3f", baro_alt0); local_pos.home_alt = baro_alt0; local_pos.home_timestamp = hrt_absolute_time(); + z_est[0] += sonar_corr; sonar_corr = 0.0f; sonar_corr_filtered = 0.0; } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index eac2fc1ce9..c90c611a77 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -49,7 +49,7 @@ PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 0.0f); PARAM_DEFINE_FLOAT(INAV_W_POS_ACC, 10.0f); PARAM_DEFINE_FLOAT(INAV_W_POS_FLOW, 10.0f); PARAM_DEFINE_FLOAT(INAV_FLOW_K, 1.0f); -PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.5f); +PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.02f); PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); int parameters_init(struct position_estimator_inav_param_handles *h) From 7dfaed3ee7718c15731070f3e3bd54d725f72029 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 28 Jun 2013 23:35:48 +0400 Subject: [PATCH 193/486] multirotor_pos_control: new ground level -> altitude setpoint correction fixed --- src/modules/multirotor_pos_control/multirotor_pos_control.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 3a14d516a6..cb6afa1cb4 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -293,7 +293,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { if (local_pos.home_timestamp != home_alt_t) { if (home_alt_t != 0) { /* home alt changed, don't follow large ground level changes in manual flight */ - local_pos_sp.z -= local_pos.home_alt - home_alt; + local_pos_sp.z += local_pos.home_alt - home_alt; } home_alt_t = local_pos.home_timestamp; home_alt = local_pos.home_alt; From d1562f926f487d1ed05751d45a2516be8c192564 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 28 Jun 2013 23:39:35 -0700 Subject: [PATCH 194/486] More implementation for the serial side on IO; fix a couple of bugs on the FMU side. Still needs serial init and some more testing/config on the FMU side, but closer to being ready to test. --- src/drivers/boards/px4iov2/px4iov2_internal.h | 6 + src/drivers/px4io/interface_serial.cpp | 23 +--- src/modules/px4iofirmware/px4io.h | 7 +- src/modules/px4iofirmware/registers.c | 9 ++ src/modules/px4iofirmware/serial.c | 127 +++++++++++------- src/modules/systemlib/hx_stream.c | 18 +-- 6 files changed, 109 insertions(+), 81 deletions(-) diff --git a/src/drivers/boards/px4iov2/px4iov2_internal.h b/src/drivers/boards/px4iov2/px4iov2_internal.h index c4c592fe44..282ed75487 100755 --- a/src/drivers/boards/px4iov2/px4iov2_internal.h +++ b/src/drivers/boards/px4iov2/px4iov2_internal.h @@ -56,6 +56,12 @@ ******************************************************************************/ /* Configuration **************************************************************/ +/****************************************************************************** + * Serial + ******************************************************************************/ +#define SERIAL_BASE STM32_USART1_BASE +#define SERIAL_VECTOR STM32_IRQ_USART1 + /****************************************************************************** * GPIOS ******************************************************************************/ diff --git a/src/drivers/px4io/interface_serial.cpp b/src/drivers/px4io/interface_serial.cpp index f91284c723..d0af2912a3 100644 --- a/src/drivers/px4io/interface_serial.cpp +++ b/src/drivers/px4io/interface_serial.cpp @@ -160,6 +160,8 @@ PX4IO_serial::PX4IO_serial(int port) : return; } + /* XXX need to configure the port here */ + /* need space for worst-case escapes + hx protocol overhead */ /* XXX this is kinda gross, but hx transmits a byte at a time */ _tx_buf = new uint8_t[HX_STREAM_MAX_FRAME]; @@ -257,7 +259,7 @@ PX4IO_serial::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned n return -EIO; } else { /* copy back the result */ - memcpy(values, &_tx_buf[0], count); + memcpy(values, &_rx_buf[0], count); } out: sem_post(&_bus_semaphore); @@ -267,16 +269,12 @@ out: int PX4IO_serial::_wait_complete() { - /* prepare the stream for transmission */ + /* prepare the stream for transmission (also discards any received noise) */ hx_stream_reset(_stream); hx_stream_start(_stream, _tx_buf, _tx_size); - /* enable UART */ - _CR1() |= USART_CR1_RE | - USART_CR1_TE | - USART_CR1_TXEIE | - USART_CR1_RXNEIE | - USART_CR1_UE; + /* enable transmit-ready interrupt, which will start transmission */ + _CR1() |= USART_CR1_TXEIE; /* compute the deadline for a 5ms timeout */ struct timespec abstime; @@ -290,13 +288,6 @@ PX4IO_serial::_wait_complete() /* wait for the transaction to complete */ int ret = sem_timedwait(&_completion_semaphore, &abstime); - /* disable the UART */ - _CR1() &= ~(USART_CR1_RE | - USART_CR1_TE | - USART_CR1_TXEIE | - USART_CR1_RXNEIE | - USART_CR1_UE); - return ret; } @@ -317,7 +308,7 @@ PX4IO_serial::_do_interrupt() if (sr & USART_SR_TXE) { int c = hx_stream_send_next(_stream); if (c == -1) { - /* transmit (nearly) done, not interested in TX-ready interrupts now */ + /* no more bytes to send, not interested in interrupts now */ _CR1() &= ~USART_CR1_TXEIE; /* was this a tx-only operation? */ diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 2077e6244d..47bcb8ddfc 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -42,7 +42,12 @@ #include #include -#include +#ifdef CONFIG_ARCH_BOARD_PX4IO +# include +#endif +#ifdef CONFIG_ARCH_BOARD_PX4IOV2 +# include +#endif #include "protocol.h" diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index df7d6dcd35..42554456c3 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -44,6 +44,7 @@ #include #include +#include #include "px4io.h" #include "protocol.h" @@ -349,10 +350,18 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_P_SETUP_RELAYS: value &= PX4IO_P_SETUP_RELAYS_VALID; r_setup_relays = value; +#ifdef POWER_RELAY1 POWER_RELAY1(value & (1 << 0) ? 1 : 0); +#endif +#ifdef POWER_RELAY2 POWER_RELAY2(value & (1 << 1) ? 1 : 0); +#endif +#ifdef POWER_ACC1 POWER_ACC1(value & (1 << 2) ? 1 : 0); +#endif +#ifdef POWER_ACC2 POWER_ACC2(value & (1 << 3) ? 1 : 0); +#endif break; case PX4IO_P_SETUP_SET_DEBUG: diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index bf9456e942..f691969c4b 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -46,36 +46,40 @@ #include #include +/* XXX might be able to prune these */ +#include +#include +#include +#include #include //#define DEBUG #include "px4io.h" -static uint8_t tx_buf[66]; /* XXX hardcoded magic number */ - static hx_stream_t if_stream; +static volatile bool sending = false; +static int serial_interrupt(int vector, void *context); static void serial_callback(void *arg, const void *data, unsigned length); +/* serial register accessors */ +#define REG(_x) (*(volatile uint32_t *)(SERIAL_BASE + _x)) +#define rSR REG(STM32_USART_SR_OFFSET) +#define rDR REG(STM32_USART_DR_OFFSET) +#define rBRR REG(STM32_USART_BRR_OFFSET) +#define rCR1 REG(STM32_USART_CR1_OFFSET) +#define rCR2 REG(STM32_USART_CR2_OFFSET) +#define rCR3 REG(STM32_USART_CR3_OFFSET) +#define rGTPR REG(STM32_USART_GTPR_OFFSET) + void interface_init(void) { - int fd = open("/dev/ttyS1", O_RDWR, O_NONBLOCK); - if (fd < 0) { - debug("serial fail"); - return; - } + /* XXX do serial port init here */ - /* configure serial port - XXX increase port speed? */ - struct termios t; - tcgetattr(fd, &t); - cfsetspeed(&t, 115200); - t.c_cflag &= ~(CSTOPB | PARENB); - tcsetattr(fd, TCSANOW, &t); - - /* allocate the HX stream we'll use for communication */ - if_stream = hx_stream_init(fd, serial_callback, NULL); + irq_attach(SERIAL_VECTOR, serial_interrupt); + if_stream = hx_stream_init(-1, serial_callback, NULL); /* XXX add stream stats counters? */ @@ -85,8 +89,31 @@ interface_init(void) void interface_tick() { - /* process incoming bytes */ - hx_stream_rx(if_stream); + /* XXX nothing interesting to do here */ +} + +static int +serial_interrupt(int vector, void *context) +{ + uint32_t sr = rSR; + + if (sr & USART_SR_TXE) { + int c = hx_stream_send_next(if_stream); + if (c == -1) { + /* no more bytes to send, not interested in interrupts now */ + rCR1 &= ~USART_CR1_TXEIE; + sending = false; + } else { + rDR = c; + } + } + + if (sr & USART_SR_RXNE) { + uint8_t c = rDR; + + hx_stream_rx(if_stream, c); + } + return 0; } static void @@ -98,36 +125,40 @@ serial_callback(void *arg, const void *data, unsigned length) if (length < 2) return; - /* it's a write operation, pass it to the register API */ - if (message[0] & PX4IO_PAGE_WRITE) { - registers_set(message[0] & ~PX4IO_PAGE_WRITE, message[1], - (const uint16_t *)&message[2], - (length - 2) / 2); - + /* we got a new request while we were still sending the last reply - not supported */ + if (sending) return; + + /* reads are page / offset / length */ + if (length == 3) { + uint16_t *registers; + unsigned count; + + /* get registers for response, send an empty reply on error */ + if (registers_get(message[0], message[1], ®isters, &count) < 0) + count = 0; + + /* constrain count to requested size or message limit */ + if (count > message[2]) + count = message[2]; + if (count > HX_STREAM_MAX_FRAME) + count = HX_STREAM_MAX_FRAME; + + /* start sending the reply */ + sending = true; + hx_stream_reset(if_stream); + hx_stream_start(if_stream, registers, count * 2 + 2); + + /* enable the TX-ready interrupt */ + rCR1 |= USART_CR1_TXEIE; + return; + + } else { + + /* it's a write operation, pass it to the register API */ + registers_set(message[0], + message[1], + (const uint16_t *)&message[2], + (length - 2) / 2); } - - /* it's a read - must contain length byte */ - if (length != 3) - return; - uint16_t *registers; - unsigned count; - - tx_buf[0] = message[0]; - tx_buf[1] = message[1]; - - /* get registers for response, send an empty reply on error */ - if (registers_get(message[0], message[1], ®isters, &count) < 0) - count = 0; - - /* fill buffer with message, limited by length */ -#define TX_MAX ((sizeof(tx_buf) - 2) / 2) - if (count > TX_MAX) - count = TX_MAX; - if (count > message[2]) - count = message[2]; - memcpy(&tx_buf[2], registers, count * 2); - - /* try to send the message */ - hx_stream_send(if_stream, tx_buf, count * 2 + 2); } \ No newline at end of file diff --git a/src/modules/systemlib/hx_stream.c b/src/modules/systemlib/hx_stream.c index fdc3edac75..8e9c2bfcf6 100644 --- a/src/modules/systemlib/hx_stream.c +++ b/src/modules/systemlib/hx_stream.c @@ -96,20 +96,6 @@ hx_tx_raw(hx_stream_t stream, uint8_t c) stream->tx_error = true; } -static void -hx_tx_byte(hx_stream_t stream, uint8_t c) -{ - switch (c) { - case FBO: - case CEO: - hx_tx_raw(stream, CEO); - c ^= 0x20; - break; - } - - hx_tx_raw(stream, c); -} - static int hx_rx_frame(hx_stream_t stream) { @@ -281,12 +267,12 @@ hx_stream_send(hx_stream_t stream, { int result; - result = hx_start(stream, data, count); + result = hx_stream_start(stream, data, count); if (result != OK) return result; int c; - while ((c = hx_send_next(stream)) >= 0) + while ((c = hx_stream_send_next(stream)) >= 0) hx_tx_raw(stream, c); /* check for transmit error */ From 843fb2d37179b82601a51e2d210052318f3301ab Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 1 Jul 2013 09:29:06 +0400 Subject: [PATCH 195/486] position_estimator_inav init bugs fixed --- .../position_estimator_inav_main.c | 91 ++++++++++++++----- 1 file changed, 66 insertions(+), 25 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 428269e4aa..fb5a779bc0 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -62,6 +62,7 @@ #include #include #include +#include #include #include #include @@ -162,7 +163,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float z_est[3] = { 0.0f, 0.0f, 0.0f }; int baro_init_cnt = 0; - int baro_init_num = 70; /* measurement for 1 second */ + int baro_init_num = 200; float baro_alt0 = 0.0f; /* to determine while start up */ double lat_current = 0.0; //[°]] --> 47.0 @@ -220,12 +221,25 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* wait for initial sensors values: baro, GPS fix, only then can we initialize the projection */ bool wait_gps = params.use_gps; bool wait_baro = true; + hrt_abstime wait_gps_start = 0; + const hrt_abstime wait_gps_delay = 5000000; // wait for 5s after 3D fix - while (wait_gps || wait_baro) { - if (poll(fds_init, 2, 1000)) { + thread_running = true; + + while ((wait_gps || wait_baro) && !thread_should_exit) { + int ret = poll(fds_init, params.use_gps ? 2 : 1, 1000); + + if (ret < 0) { + /* poll error */ + errx(1, "subscriptions poll error on init."); + + } else if (ret > 0) { if (fds_init[0].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); + if (wait_baro && sensor.baro_counter > baro_counter) { + baro_counter = sensor.baro_counter; + /* mean calculation over several measurements */ if (baro_init_cnt < baro_init_num) { baro_alt0 += sensor.baro_alt_meter; @@ -241,24 +255,33 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } } - if (fds_init[1].revents & POLLIN) { + + if (params.use_gps && (fds_init[1].revents & POLLIN)) { orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); + if (wait_gps && gps.fix_type >= 3) { - wait_gps = false; - /* get GPS position for first initialization */ - lat_current = gps.lat * 1e-7; - lon_current = gps.lon * 1e-7; - alt_current = gps.alt * 1e-3; + hrt_abstime t = hrt_absolute_time(); - local_pos.home_lat = lat_current * 1e7; - local_pos.home_lon = lon_current * 1e7; - local_pos.home_hdg = 0.0f; - local_pos.home_timestamp = hrt_absolute_time(); + if (wait_gps_start == 0) { + wait_gps_start = t; - /* initialize coordinates */ - map_projection_init(lat_current, lon_current); - warnx("init GPS: lat = %.10f, lon = %.10f", lat_current, lon_current); - mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat_current, lon_current); + } else if (t - wait_gps_start > wait_gps_delay) { + wait_gps = false; + /* get GPS position for first initialization */ + lat_current = gps.lat * 1e-7; + lon_current = gps.lon * 1e-7; + alt_current = gps.alt * 1e-3; + + local_pos.home_lat = lat_current * 1e7; + local_pos.home_lon = lon_current * 1e7; + local_pos.home_hdg = 0.0f; + local_pos.home_timestamp = t; + + /* initialize coordinates */ + map_projection_init(lat_current, lon_current); + warnx("init GPS: lat = %.10f, lon = %.10f", lat_current, lon_current); + mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat_current, lon_current); + } } } } @@ -282,8 +305,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float accel_corr[] = { 0.0f, 0.0f, 0.0f }; // N E D float baro_corr = 0.0f; // D float gps_corr[2][2] = { - { 0.0f, 0.0f }, // N (pos, vel) - { 0.0f, 0.0f }, // E (pos, vel) + { 0.0f, 0.0f }, // N (pos, vel) + { 0.0f, 0.0f }, // E (pos, vel) }; float sonar_corr = 0.0f; float sonar_corr_filtered = 0.0f; @@ -293,7 +316,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) hrt_abstime sonar_time = 0; /* main loop */ - struct pollfd fds[5] = { + struct pollfd fds[6] = { { .fd = parameter_update_sub, .events = POLLIN }, { .fd = vehicle_status_sub, .events = POLLIN }, { .fd = vehicle_attitude_sub, .events = POLLIN }, @@ -302,8 +325,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { .fd = vehicle_gps_position_sub, .events = POLLIN } }; - thread_running = true; - warnx("main loop started."); + if (!thread_should_exit) { + warnx("main loop started."); + } while (!thread_should_exit) { int ret = poll(fds, params.use_gps ? 6 : 5, 10); // wait maximal this 10 ms = 100 Hz minimum rate @@ -346,19 +370,24 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (att.R_valid) { /* transform acceleration vector from body frame to NED frame */ float accel_NED[3]; + for (int i = 0; i < 3; i++) { accel_NED[i] = 0.0f; + for (int j = 0; j < 3; j++) { accel_NED[i] += att.R[i][j] * sensor.accelerometer_m_s2[j]; } } + accel_NED[2] += CONSTANTS_ONE_G; accel_corr[0] = accel_NED[0] - x_est[2]; accel_corr[1] = accel_NED[1] - y_est[2]; accel_corr[2] = accel_NED[2] - z_est[2]; + } else { memset(accel_corr, 0, sizeof(accel_corr)); } + accel_counter = sensor.accelerometer_counter; accel_updates++; } @@ -373,17 +402,20 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* optical flow */ if (fds[4].revents & POLLIN) { orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); + if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && (flow.ground_distance_m != sonar_prev || t - sonar_time < 150000)) { if (flow.ground_distance_m != sonar_prev) { sonar_time = t; sonar_prev = flow.ground_distance_m; sonar_corr = -flow.ground_distance_m - z_est[0]; sonar_corr_filtered += (sonar_corr - sonar_corr_filtered) * params.sonar_filt; + if (fabsf(sonar_corr) > params.sonar_err) { // correction is too large: spike or new ground level? if (fabsf(sonar_corr - sonar_corr_filtered) > params.sonar_err) { // spike detected, ignore sonar_corr = 0.0f; + } else { // new ground level baro_alt0 += sonar_corr; @@ -397,29 +429,36 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } } + } else { sonar_corr = 0.0f; } + flow_updates++; } - if (params.use_gps && fds[5].revents & POLLIN) { + if (params.use_gps && (fds[5].revents & POLLIN)) { /* vehicle GPS position */ orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); + if (gps.fix_type >= 3) { /* project GPS lat lon to plane */ float gps_proj[2]; map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1])); gps_corr[0][0] = gps_proj[0] - x_est[0]; gps_corr[1][0] = gps_proj[1] - y_est[0]; + if (gps.vel_ned_valid) { gps_corr[0][1] = gps.vel_n_m_s; gps_corr[1][1] = gps.vel_e_m_s; + } else { gps_corr[0][1] = 0.0f; gps_corr[1][1] = 0.0f; } + gps_updates++; + } else { memset(gps_corr, 0, sizeof(gps_corr)); } @@ -442,6 +481,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* dont't try to estimate position when no any position source available */ bool can_estimate_pos = params.use_gps && gps.fix_type >= 3; + if (can_estimate_pos) { /* inertial filter prediction for position */ inertial_filter_predict(dt, x_est); @@ -454,6 +494,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (params.use_gps && gps.fix_type >= 3) { inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p); inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p); + if (gps.vel_ned_valid) { inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v); inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v); @@ -505,8 +546,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) global_pos.valid = local_pos.valid; double est_lat, est_lon; map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); - global_pos.lat = (int32_t) (est_lat * 1e7); - global_pos.lon = (int32_t) (est_lon * 1e7); + global_pos.lat = (int32_t)(est_lat * 1e7); + global_pos.lon = (int32_t)(est_lon * 1e7); global_pos.alt = -local_pos.z - local_pos.home_alt; global_pos.relative_alt = -local_pos.z; global_pos.vx = local_pos.vx; From be6ad7af3b65841d2b460e3064c166dc9167401f Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 3 Jul 2013 00:08:12 -0700 Subject: [PATCH 196/486] Rework the FMU<->IO connection to use a simple fixed-size DMA packet; this should let us reduce overall latency and bump the bitrate up. Will still require some tuning. --- src/drivers/boards/px4fmuv2/px4fmu_internal.h | 11 + src/drivers/boards/px4iov2/px4iov2_internal.h | 9 +- src/drivers/px4io/interface_serial.cpp | 343 ++++++++++-------- src/modules/px4iofirmware/serial.c | 171 +++++---- 4 files changed, 310 insertions(+), 224 deletions(-) diff --git a/src/drivers/boards/px4fmuv2/px4fmu_internal.h b/src/drivers/boards/px4fmuv2/px4fmu_internal.h index 78f6a2e65d..1698336b4b 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu_internal.h +++ b/src/drivers/boards/px4fmuv2/px4fmu_internal.h @@ -58,6 +58,17 @@ __BEGIN_DECLS ****************************************************************************************************/ /* Configuration ************************************************************************************/ +/* PX4IO connection configuration */ +#define PX4IO_SERIAL_TX_GPIO GPIO_USART6_TX +#define PX4IO_SERIAL_RX_GPIO GPIO_USART6_RX +#define PX4IO_SERIAL_BASE STM32_USART6_BASE /* hardwired on the board */ +#define PX4IO_SERIAL_VECTOR STM32_IRQ_USART6 +#define PX4IO_SERIAL_TX_DMAMAP DMAMAP_USART6_TX_2 +#define PX4IO_SERIAL_RX_DMAMAP DMAMAP_USART6_RX_2 +#define PX4IO_SERIAL_CLOCK STM32_PCLK2_FREQUENCY +#define PX4IO_SERIAL_BITRATE 1500000 /* 1.5Mbps -> max rate for IO */ + + /* PX4FMU GPIOs ***********************************************************************************/ /* LEDs */ diff --git a/src/drivers/boards/px4iov2/px4iov2_internal.h b/src/drivers/boards/px4iov2/px4iov2_internal.h index 282ed75487..b8aa6d053e 100755 --- a/src/drivers/boards/px4iov2/px4iov2_internal.h +++ b/src/drivers/boards/px4iov2/px4iov2_internal.h @@ -59,8 +59,13 @@ /****************************************************************************** * Serial ******************************************************************************/ -#define SERIAL_BASE STM32_USART1_BASE -#define SERIAL_VECTOR STM32_IRQ_USART1 +#define PX4FMU_SERIAL_BASE STM32_USART2_BASE +#define PX4FMU_SERIAL_TX_GPIO GPIO_USART2_TX +#define PX4FMU_SERIAL_RX_GPIO GPIO_USART2_RX +#define PX4FMU_SERIAL_TX_DMA DMACHAN_USART2_TX +#define PX4FMU_SERIAL_RX_DMA DMACHAN_USART2_RX +#define PX4FMU_SERIAL_CLOCK STM32_PCLK1_FREQUENCY +#define PX4FMU_SERIAL_BITRATE 1500000 /****************************************************************************** * GPIOS diff --git a/src/drivers/px4io/interface_serial.cpp b/src/drivers/px4io/interface_serial.cpp index d0af2912a3..9471cecddf 100644 --- a/src/drivers/px4io/interface_serial.cpp +++ b/src/drivers/px4io/interface_serial.cpp @@ -59,14 +59,28 @@ #include -#include +#include +#include /* XXX should really not be hardcoding v2 here */ #include "interface.h" +const unsigned max_rw_regs = 32; // by agreement w/IO + +#pragma pack(push, 1) +struct IOPacket { + uint8_t count; +#define PKT_CTRL_WRITE (1<<7) + uint8_t spare; + uint8_t page; + uint8_t offset; + uint16_t regs[max_rw_regs]; +}; +#pragma pack(pop) + class PX4IO_serial : public PX4IO_interface { public: - PX4IO_serial(int port); + PX4IO_serial(); virtual ~PX4IO_serial(); virtual int set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values); @@ -75,119 +89,133 @@ public: virtual bool ok(); private: - volatile uint32_t *_serial_base; - int _vector; + /* + * XXX tune this value + * + * At 1.5Mbps each register takes 13.3µs, and we always transfer a full packet. + * Packet overhead is 26µs for the four-byte header. + * + * 32 registers = 451µs + * + * Maybe we can just send smaller packets (e.g. 8 regs) and loop for larger (less common) + * transfers? Could cause issues with any regs expecting to be written atomically... + */ + static IOPacket _dma_buffer; // XXX static to ensure DMA-able memory - uint8_t *_tx_buf; - unsigned _tx_size; + static const unsigned _serial_tx_dma = PX4IO_SERIAL_RX_DMAMAP; + DMA_HANDLE _tx_dma; - const uint8_t *_rx_buf; - unsigned _rx_size; + static const unsigned _serial_rx_dma = PX4IO_SERIAL_TX_DMAMAP; + DMA_HANDLE _rx_dma; - hx_stream_t _stream; + /** set if we have started a transaction that expects a reply */ + bool _expect_reply; + /** saved DMA status (in case we care) */ + uint8_t _dma_status; + + /** bus-ownership lock */ sem_t _bus_semaphore; + + /** client-waiting lock/signal */ sem_t _completion_semaphore; /** - * Send _tx_size bytes from the buffer, then - * if _rx_size is greater than zero wait for a packet - * to come back. + * Start the transaction with IO and wait for it to complete. + * + * @param expect_reply If true, expect a reply from IO. */ - int _wait_complete(); + int _wait_complete(bool expect_reply); /** - * Interrupt handler. + * DMA completion handler. */ - static int _interrupt(int irq, void *context); - void _do_interrupt(); + static void _dma_callback(DMA_HANDLE handle, uint8_t status, void *arg); + void _do_dma_callback(DMA_HANDLE handle, uint8_t status); /** - * Stream transmit callback + * (re)configure the DMA */ - static void _tx(void *arg, uint8_t data); - void _do_tx(uint8_t data); - - /** - * Stream receive callback - */ - static void _rx(void *arg, const void *data, size_t length); - void _do_rx(const uint8_t *data, size_t length); + void _reset_dma(); /** * Serial register accessors. */ volatile uint32_t &_sreg(unsigned offset) { - return *(_serial_base + (offset / sizeof(uint32_t))); + return *(volatile uint32_t *)(PX4IO_SERIAL_BASE + offset); } - volatile uint32_t &_SR() { return _sreg(STM32_USART_SR_OFFSET); } - volatile uint32_t &_DR() { return _sreg(STM32_USART_DR_OFFSET); } - volatile uint32_t &_BRR() { return _sreg(STM32_USART_BRR_OFFSET); } - volatile uint32_t &_CR1() { return _sreg(STM32_USART_CR1_OFFSET); } - volatile uint32_t &_CR2() { return _sreg(STM32_USART_CR2_OFFSET); } - volatile uint32_t &_CR3() { return _sreg(STM32_USART_CR3_OFFSET); } - volatile uint32_t &_GTPR() { return _sreg(STM32_USART_GTPR_OFFSET); } + uint32_t _SR() { return _sreg(STM32_USART_SR_OFFSET); } + void _SR(uint32_t val) { _sreg(STM32_USART_SR_OFFSET) = val; } + uint32_t _DR() { return _sreg(STM32_USART_DR_OFFSET); } + void _DR(uint32_t val) { _sreg(STM32_USART_DR_OFFSET) = val; } + uint32_t _BRR() { return _sreg(STM32_USART_BRR_OFFSET); } + void _BRR(uint32_t val) { _sreg(STM32_USART_BRR_OFFSET) = val; } + uint32_t _CR1() { return _sreg(STM32_USART_CR1_OFFSET); } + void _CR1(uint32_t val) { _sreg(STM32_USART_CR1_OFFSET) = val; } + uint32_t _CR2() { return _sreg(STM32_USART_CR2_OFFSET); } + void _CR2(uint32_t val) { _sreg(STM32_USART_CR2_OFFSET) = val; } + uint32_t _CR3() { return _sreg(STM32_USART_CR3_OFFSET); } + void _CR3(uint32_t val) { _sreg(STM32_USART_CR3_OFFSET) = val; } + uint32_t _GTPR() { return _sreg(STM32_USART_GTPR_OFFSET); } + void _GTPR(uint32_t val) { _sreg(STM32_USART_GTPR_OFFSET) = val; } + }; -/* XXX hack to avoid expensive IRQ lookup */ -static PX4IO_serial *io_serial; +IOPacket PX4IO_serial::_dma_buffer; PX4IO_interface *io_serial_interface(int port) { - return new PX4IO_serial(port); + return new PX4IO_serial(); } -PX4IO_serial::PX4IO_serial(int port) : - _serial_base(0), - _vector(0), - _tx_buf(nullptr), - _tx_size(0), - _rx_size(0), - _stream(0) +PX4IO_serial::PX4IO_serial() : + _tx_dma(nullptr), + _rx_dma(nullptr), + _expect_reply(false) { - /* only allow one instance */ - if (io_serial != nullptr) + /* allocate DMA */ + _tx_dma = stm32_dmachannel(_serial_tx_dma); + _rx_dma = stm32_dmachannel(_serial_rx_dma); + if ((_tx_dma == nullptr) || (_rx_dma == nullptr)) return; - switch (port) { - case 5: - _serial_base = (volatile uint32_t *)STM32_UART5_BASE; - _vector = STM32_IRQ_UART5; - break; - default: - /* not a supported port */ - return; - } + /* configure pins for serial use */ + stm32_configgpio(PX4IO_SERIAL_TX_GPIO); + stm32_configgpio(PX4IO_SERIAL_RX_GPIO); - /* XXX need to configure the port here */ + /* reset & configure the UART */ + _CR1(0); + _CR2(0); + _CR3(0); - /* need space for worst-case escapes + hx protocol overhead */ - /* XXX this is kinda gross, but hx transmits a byte at a time */ - _tx_buf = new uint8_t[HX_STREAM_MAX_FRAME]; + /* configure line speed */ + uint32_t usartdiv32 = PX4IO_SERIAL_CLOCK / (PX4IO_SERIAL_BITRATE / 2); + uint32_t mantissa = usartdiv32 >> 5; + uint32_t fraction = (usartdiv32 - (mantissa << 5) + 1) >> 1; + _BRR((mantissa << USART_BRR_MANT_SHIFT) | (fraction << USART_BRR_FRAC_SHIFT)); - irq_attach(_vector, &_interrupt); + /* enable UART and DMA but no interrupts */ + _CR3(USART_CR3_DMAR | USART_CR3_DMAT); + _CR1(USART_CR1_RE | USART_CR1_TE | USART_CR1_UE); - _stream = hx_stream_init(-1, _rx, this); + /* configure DMA */ + _reset_dma(); + /* create semaphores */ sem_init(&_completion_semaphore, 0, 0); sem_init(&_bus_semaphore, 0, 1); } PX4IO_serial::~PX4IO_serial() { + if (_tx_dma != nullptr) + stm32_dmafree(_tx_dma); + if (_rx_dma != nullptr) + stm32_dmafree(_rx_dma); - if (_tx_buf != nullptr) - delete[] _tx_buf; - - if (_vector) - irq_detach(_vector); - - if (io_serial == this) - io_serial = nullptr; - - if (_stream) - hx_stream_free(_stream); + stm32_unconfiggpio(PX4IO_SERIAL_TX_GPIO); + stm32_unconfiggpio(PX4IO_SERIAL_RX_GPIO); sem_destroy(&_completion_semaphore); sem_destroy(&_bus_semaphore); @@ -196,13 +224,9 @@ PX4IO_serial::~PX4IO_serial() bool PX4IO_serial::ok() { - if (_serial_base == 0) + if (_tx_dma == nullptr) return false; - if (_vector == 0) - return false; - if (_tx_buf == nullptr) - return false; - if (!_stream) + if (_rx_dma == nullptr) return false; return true; @@ -211,22 +235,20 @@ PX4IO_serial::ok() int PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { - - unsigned count = num_values * sizeof(*values); - if (count > (HX_STREAM_MAX_FRAME - 2)) + if (num_values > max_rw_regs) return -EINVAL; sem_wait(&_bus_semaphore); - _tx_buf[0] = page; - _tx_buf[1] = offset; - memcpy(&_tx_buf[2], (void *)values, count); - - _tx_size = count + 2; - _rx_size = 0; + _dma_buffer.count = num_values | PKT_CTRL_WRITE; + _dma_buffer.spare = 0; + _dma_buffer.page = page; + _dma_buffer.offset = offset; + memcpy((void *)&_dma_buffer.regs[0], (void *)values, (2 * num_values)); + /* XXX implement check byte */ /* start the transaction and wait for it to complete */ - int result = _wait_complete(); + int result = _wait_complete(false); sem_post(&_bus_semaphore); return result; @@ -235,31 +257,28 @@ PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsi int PX4IO_serial::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) { - - unsigned count = num_values * sizeof(*values); - if (count > HX_STREAM_MAX_FRAME) + if (num_values > max_rw_regs) return -EINVAL; sem_wait(&_bus_semaphore); - _tx_buf[0] = page; - _tx_buf[1] = offset; - _tx_buf[2] = num_values; - - _tx_size = 3; /* this tells IO that this is a read request */ - _rx_size = count; + _dma_buffer.count = num_values; + _dma_buffer.spare = 0; + _dma_buffer.page = page; + _dma_buffer.offset = offset; /* start the transaction and wait for it to complete */ - int result = _wait_complete(); + int result = _wait_complete(true); if (result != OK) goto out; /* compare the received count with the expected count */ - if (_rx_size != count) { + if (_dma_buffer.count != num_values) { return -EIO; } else { + /* XXX implement check byte */ /* copy back the result */ - memcpy(values, &_rx_buf[0], count); + memcpy(values, &_dma_buffer.regs[0], (2 * num_values)); } out: sem_post(&_bus_semaphore); @@ -267,14 +286,18 @@ out: } int -PX4IO_serial::_wait_complete() +PX4IO_serial::_wait_complete(bool expect_reply) { - /* prepare the stream for transmission (also discards any received noise) */ - hx_stream_reset(_stream); - hx_stream_start(_stream, _tx_buf, _tx_size); - /* enable transmit-ready interrupt, which will start transmission */ - _CR1() |= USART_CR1_TXEIE; + /* save for callback use */ + _expect_reply = expect_reply; + + /* start RX DMA */ + if (expect_reply) + stm32_dmastart(_rx_dma, _dma_callback, this, false); + + /* start TX DMA - no callback if we also expect a reply */ + stm32_dmastart(_tx_dma, _dma_callback, expect_reply ? nullptr : this, false); /* compute the deadline for a 5ms timeout */ struct timespec abstime; @@ -285,68 +308,82 @@ PX4IO_serial::_wait_complete() abstime.tv_nsec -= 1000000000; } - /* wait for the transaction to complete */ - int ret = sem_timedwait(&_completion_semaphore, &abstime); + /* wait for the transaction to complete - 64 bytes @ 1.5Mbps ~426µs */ + int ret; + for (;;) { + ret = sem_timedwait(&_completion_semaphore, &abstime); + + if (ret == OK) + break; + + if (ret == ETIMEDOUT) { + /* something has broken - clear out any partial DMA state and reconfigure */ + _reset_dma(); + break; + } + } return ret; } -int -PX4IO_serial::_interrupt(int irq, void *context) +void +PX4IO_serial::_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) { - /* ... because NuttX doesn't give us a handle per vector */ - io_serial->_do_interrupt(); - return 0; + if (arg != nullptr) { + PX4IO_serial *ps = reinterpret_cast(arg); + + ps->_do_dma_callback(handle, status); + } } void -PX4IO_serial::_do_interrupt() +PX4IO_serial::_do_dma_callback(DMA_HANDLE handle, uint8_t status) { - uint32_t sr = _SR(); + /* on completion of a no-reply transmit, wake the sender */ + if (handle == _tx_dma) { + if ((status & DMA_STATUS_TCIF) && !_expect_reply) { + _dma_status = status; + sem_post(&_completion_semaphore); + } + /* XXX if we think we're going to see DMA errors, we should handle them here */ + } - /* handle transmit completion */ - if (sr & USART_SR_TXE) { - int c = hx_stream_send_next(_stream); - if (c == -1) { - /* no more bytes to send, not interested in interrupts now */ - _CR1() &= ~USART_CR1_TXEIE; - - /* was this a tx-only operation? */ - if (_rx_size == 0) { - /* wake up waiting sender */ - sem_post(&_completion_semaphore); - } - } else { - _DR() = c; + /* on completion of a reply, wake the waiter */ + if (handle == _rx_dma) { + if (status & DMA_STATUS_TCIF) { + /* XXX if we are worried about overrun/synch, check UART status here */ + _dma_status = status; + sem_post(&_completion_semaphore); } } - - if (sr & USART_SR_RXNE) { - uint8_t c = _DR(); - - hx_stream_rx(_stream, c); - } } void -PX4IO_serial::_rx(void *arg, const void *data, size_t length) +PX4IO_serial::_reset_dma() { - PX4IO_serial *pserial = reinterpret_cast(arg); - - pserial->_do_rx((const uint8_t *)data, length); -} - -void -PX4IO_serial::_do_rx(const uint8_t *data, size_t length) -{ - _rx_buf = data; - - if (length < _rx_size) - _rx_size = length; - - /* notify waiting receiver */ - sem_post(&_completion_semaphore); -} - - + stm32_dmastop(_tx_dma); + stm32_dmastop(_rx_dma); + stm32_dmasetup( + _tx_dma, + PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET, + reinterpret_cast(&_dma_buffer), + sizeof(_dma_buffer), + DMA_SCR_DIR_M2P | + DMA_SCR_MINC | + DMA_SCR_PSIZE_8BITS | + DMA_SCR_MSIZE_8BITS | + DMA_SCR_PBURST_SINGLE | + DMA_SCR_MBURST_SINGLE); + stm32_dmasetup( + _rx_dma, + PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET, + reinterpret_cast(&_dma_buffer), + sizeof(_dma_buffer), + DMA_SCR_DIR_P2M | + DMA_SCR_MINC | + DMA_SCR_PSIZE_8BITS | + DMA_SCR_MSIZE_8BITS | + DMA_SCR_PBURST_SINGLE | + DMA_SCR_MBURST_SINGLE); +} \ No newline at end of file diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index f691969c4b..3fedfeb2c2 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -56,14 +56,29 @@ //#define DEBUG #include "px4io.h" -static hx_stream_t if_stream; static volatile bool sending = false; -static int serial_interrupt(int vector, void *context); -static void serial_callback(void *arg, const void *data, unsigned length); +static void dma_callback(DMA_HANDLE handle, uint8_t status, void *arg); +static DMA_HANDLE tx_dma; +static DMA_HANDLE rx_dma; + +#define MAX_RW_REGS 32 // by agreement w/FMU + +#pragma pack(push, 1) +struct IOPacket { + uint8_t count; +#define PKT_CTRL_WRITE (1<<7) + uint8_t spare; + uint8_t page; + uint8_t offset; + uint16_t regs[MAX_RW_REGS]; +}; +#pragma pack(pop) + +static struct IOPacket dma_packet; /* serial register accessors */ -#define REG(_x) (*(volatile uint32_t *)(SERIAL_BASE + _x)) +#define REG(_x) (*(volatile uint32_t *)(PX4FMU_SERIAL_BASE + _x)) #define rSR REG(STM32_USART_SR_OFFSET) #define rDR REG(STM32_USART_DR_OFFSET) #define rBRR REG(STM32_USART_BRR_OFFSET) @@ -75,13 +90,51 @@ static void serial_callback(void *arg, const void *data, unsigned length); void interface_init(void) { + /* allocate DMA */ + tx_dma = stm32_dmachannel(PX4FMU_SERIAL_TX_DMA); + rx_dma = stm32_dmachannel(PX4FMU_SERIAL_RX_DMA); - /* XXX do serial port init here */ + /* configure pins for serial use */ + stm32_configgpio(PX4FMU_SERIAL_TX_GPIO); + stm32_configgpio(PX4FMU_SERIAL_RX_GPIO); - irq_attach(SERIAL_VECTOR, serial_interrupt); - if_stream = hx_stream_init(-1, serial_callback, NULL); + /* reset and configure the UART */ + rCR1 = 0; + rCR2 = 0; + rCR3 = 0; - /* XXX add stream stats counters? */ + /* configure line speed */ + uint32_t usartdiv32 = PX4FMU_SERIAL_CLOCK / (PX4FMU_SERIAL_BITRATE / 2); + uint32_t mantissa = usartdiv32 >> 5; + uint32_t fraction = (usartdiv32 - (mantissa << 5) + 1) >> 1; + rBRR = (mantissa << USART_BRR_MANT_SHIFT) | (fraction << USART_BRR_FRAC_SHIFT); + + /* enable UART and DMA */ + rCR3 = USART_CR3_DMAR | USART_CR3_DMAT; + rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE; + + /* configure DMA */ + stm32_dmasetup( + tx_dma, + (uint32_t)&rDR, + (uint32_t)&dma_packet, + sizeof(dma_packet), + DMA_CCR_DIR | + DMA_CCR_MINC | + DMA_CCR_PSIZE_8BITS | + DMA_CCR_MSIZE_8BITS); + + stm32_dmasetup( + rx_dma, + (uint32_t)&rDR, + (uint32_t)&dma_packet, + sizeof(dma_packet), + DMA_CCR_MINC | + DMA_CCR_PSIZE_8BITS | + DMA_CCR_MSIZE_8BITS); + + /* start receive DMA ready for the first packet */ + stm32_dmastart(rx_dma, dma_callback, NULL, false); debug("serial init"); } @@ -89,76 +142,56 @@ interface_init(void) void interface_tick() { - /* XXX nothing interesting to do here */ -} - -static int -serial_interrupt(int vector, void *context) -{ - uint32_t sr = rSR; - - if (sr & USART_SR_TXE) { - int c = hx_stream_send_next(if_stream); - if (c == -1) { - /* no more bytes to send, not interested in interrupts now */ - rCR1 &= ~USART_CR1_TXEIE; - sending = false; - } else { - rDR = c; - } - } - - if (sr & USART_SR_RXNE) { - uint8_t c = rDR; - - hx_stream_rx(if_stream, c); - } - return 0; + /* XXX look for stuck/damaged DMA and reset? */ } static void -serial_callback(void *arg, const void *data, unsigned length) +dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) { - const uint8_t *message = (const uint8_t *)data; - - /* malformed frame, ignore it */ - if (length < 2) + if (!(status & DMA_STATUS_TCIF)) { + /* XXX what do we do here? it's fatal! */ return; + } - /* we got a new request while we were still sending the last reply - not supported */ - if (sending) + /* if this is transmit-complete, make a note */ + if (handle == tx_dma) { + sending = false; return; + } - /* reads are page / offset / length */ - if (length == 3) { - uint16_t *registers; - unsigned count; - - /* get registers for response, send an empty reply on error */ - if (registers_get(message[0], message[1], ®isters, &count) < 0) - count = 0; - - /* constrain count to requested size or message limit */ - if (count > message[2]) - count = message[2]; - if (count > HX_STREAM_MAX_FRAME) - count = HX_STREAM_MAX_FRAME; - - /* start sending the reply */ - sending = true; - hx_stream_reset(if_stream); - hx_stream_start(if_stream, registers, count * 2 + 2); - - /* enable the TX-ready interrupt */ - rCR1 |= USART_CR1_TXEIE; - return; + /* we just received a request; sort out what to do */ + /* XXX implement check byte */ + /* XXX if we care about overruns, check the UART received-data-ready bit */ + bool send_reply = false; + if (dma_packet.count & PKT_CTRL_WRITE) { + /* it's a blind write - pass it on */ + registers_set(dma_packet.page, dma_packet.offset, &dma_packet.regs[0], dma_packet.count); } else { - /* it's a write operation, pass it to the register API */ - registers_set(message[0], - message[1], - (const uint16_t *)&message[2], - (length - 2) / 2); + /* it's a read - get register pointer for reply */ + int result; + unsigned count; + uint16_t *registers; + + result = registers_get(dma_packet.page, dma_packet.offset, ®isters, &count); + if (result < 0) + count = 0; + + /* constrain reply to packet size */ + if (count > MAX_RW_REGS) + count = MAX_RW_REGS; + + /* copy reply registers into DMA buffer */ + send_reply = true; + memcpy((void *)&dma_packet.regs[0], registers, count); + dma_packet.count = count; } -} \ No newline at end of file + + /* re-set DMA for reception first, so we are ready to receive before we start sending */ + stm32_dmastart(rx_dma, dma_callback, NULL, false); + + /* if we have a reply to send, start that now */ + if (send_reply) + stm32_dmastart(tx_dma, dma_callback, NULL, false); +} From ea1f61e09388c0fd348dd54a46332bad939ffe00 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 3 Jul 2013 22:22:10 -0700 Subject: [PATCH 197/486] USB console isn't working. Go back to UART8 which is. --- nuttx/configs/px4fmuv2/nsh/defconfig | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/nuttx/configs/px4fmuv2/nsh/defconfig b/nuttx/configs/px4fmuv2/nsh/defconfig index 8e69f321af..16be7cd411 100755 --- a/nuttx/configs/px4fmuv2/nsh/defconfig +++ b/nuttx/configs/px4fmuv2/nsh/defconfig @@ -234,10 +234,10 @@ STM32_FLASH_PREFETCH=y # CONFIG_USARTn_2STOP - Two stop bits # CONFIG_SERIAL_TERMIOS=y -CONFIG_SERIAL_CONSOLE_REINIT=y +CONFIG_SERIAL_CONSOLE_REINIT=n CONFIG_STANDARD_SERIAL=y -CONFIG_UART8_SERIAL_CONSOLE=n +CONFIG_UART8_SERIAL_CONSOLE=y #Mavlink messages can be bigger than 128 CONFIG_USART1_TXBUFSIZE=512 @@ -555,8 +555,8 @@ CONFIG_START_MONTH=1 CONFIG_START_DAY=1 CONFIG_GREGORIAN_TIME=n CONFIG_JULIAN_TIME=n -CONFIG_DEV_CONSOLE=n -CONFIG_DEV_LOWCONSOLE=n +CONFIG_DEV_CONSOLE=y +CONFIG_DEV_LOWCONSOLE=y CONFIG_MUTEX_TYPES=n CONFIG_PRIORITY_INHERITANCE=y CONFIG_SEM_PREALLOCHOLDERS=0 @@ -943,7 +943,7 @@ CONFIG_USBDEV_TRACE_NRECORDS=512 # Size of the serial receive/transmit buffers. Default 256. # CONFIG_CDCACM=y -CONFIG_CDCACM_CONSOLE=y +CONFIG_CDCACM_CONSOLE=n #CONFIG_CDCACM_EP0MAXPACKET CONFIG_CDCACM_EPINTIN=1 #CONFIG_CDCACM_EPINTIN_FSSIZE @@ -1014,7 +1014,7 @@ CONFIG_NSH_ROMFSETC=y CONFIG_NSH_ARCHROMFS=y CONFIG_NSH_CONSOLE=y CONFIG_NSH_USBCONSOLE=n -CONFIG_NSH_USBCONDEV="/dev/ttyACM0" +#CONFIG_NSH_USBCONDEV="/dev/ttyACM0" CONFIG_NSH_TELNET=n CONFIG_NSH_ARCHINIT=y CONFIG_NSH_IOBUFFER_SIZE=512 From 03a15bfdc5d3903240f040e5de5baac12bb3080f Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 3 Jul 2013 22:23:01 -0700 Subject: [PATCH 198/486] Fix argument parsing in the rgbled command. --- src/drivers/rgbled/rgbled.cpp | 54 +++++++++++++++-------------------- 1 file changed, 23 insertions(+), 31 deletions(-) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index dc1e469c0a..dae41d934f 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -248,7 +248,7 @@ RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg) void RGBLED::led_trampoline(void *arg) { - RGBLED *rgbl = (RGBLED *)arg; + RGBLED *rgbl = reinterpret_cast(arg); rgbl->led(); } @@ -413,35 +413,34 @@ void rgbled_usage(); void rgbled_usage() { warnx("missing command: try 'start', 'systemstate', 'test', 'info', 'off'"); warnx("options:"); - warnx("\t-b --bus i2cbus (3)"); - warnx("\t-a --ddr addr (9)"); + warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED); + errx(0, " -a addr (0x%x)", ADDR); } int rgbled_main(int argc, char *argv[]) { - int i2cdevice = PX4_I2C_BUS_LED; int rgbledadr = ADDR; /* 7bit */ - int x; - - for (x = 1; x < argc; x++) { - if (strcmp(argv[x], "-b") == 0 || strcmp(argv[x], "--bus") == 0) { - if (argc > x + 1) { - i2cdevice = atoi(argv[x + 1]); - } + int ch; + while ((ch = getopt(argc, argv, "a:b:")) != EOF) { + switch (ch) { + case 'a': + rgbledadr = strtol(optarg, NULL, 0); + break; + case 'b': + i2cdevice = strtol(optarg, NULL, 0); + break; + default: + rgbled_usage(); } - - if (strcmp(argv[x], "-a") == 0 || strcmp(argv[x], "--addr") == 0) { - if (argc > x + 1) { - rgbledadr = atoi(argv[x + 1]); - } - } - } + argc -= optind; + argv += optind; + const char *verb = argv[0]; - if (!strcmp(argv[1], "start")) { + if (!strcmp(verb, "start")) { if (g_rgbled != nullptr) errx(1, "already started"); @@ -459,39 +458,32 @@ rgbled_main(int argc, char *argv[]) exit(0); } - + /* need the driver past this point */ if (g_rgbled == nullptr) { fprintf(stderr, "not started\n"); rgbled_usage(); exit(0); } - if (!strcmp(argv[1], "test")) { + if (!strcmp(verb, "test")) { g_rgbled->setMode(LED_MODE_TEST); exit(0); } - if (!strcmp(argv[1], "systemstate")) { + if (!strcmp(verb, "systemstate")) { g_rgbled->setMode(LED_MODE_SYSTEMSTATE); exit(0); } - if (!strcmp(argv[1], "info")) { + if (!strcmp(verb, "info")) { g_rgbled->info(); exit(0); } - if (!strcmp(argv[1], "off")) { + if (!strcmp(verb, "off")) { g_rgbled->setMode(LED_MODE_OFF); exit(0); } - - /* things that require access to the device */ - int fd = open(RGBLED_DEVICE_PATH, 0); - if (fd < 0) - err(1, "can't open RGBLED device"); - rgbled_usage(); - exit(0); } From 7255c02c20ac11289a059503c4562be7bc23179b Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 3 Jul 2013 23:41:40 -0700 Subject: [PATCH 199/486] Add a test hook for the PX4IO interface. Wire up some simple tests for the serial interface. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signal quality looks good at 1.5Mbps. Transmit timing is ~450µs per packet, ~20µs packet-to-packet delay spinning in a loop transmitting. --- src/drivers/px4io/interface.h | 9 +- src/drivers/px4io/interface_i2c.cpp | 8 + src/drivers/px4io/interface_serial.cpp | 69 +++++- src/drivers/px4io/px4io.cpp | 282 +++++++++++++------------ 4 files changed, 225 insertions(+), 143 deletions(-) diff --git a/src/drivers/px4io/interface.h b/src/drivers/px4io/interface.h index 834cb9e077..cb7da1081d 100644 --- a/src/drivers/px4io/interface.h +++ b/src/drivers/px4io/interface.h @@ -72,7 +72,14 @@ public: * @return Zero on success. */ virtual int get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) = 0; + + /** + * Perform an interface test. + * + * @param mode Optional test mode. + */ + virtual int test(unsigned mode) = 0; }; extern PX4IO_interface *io_i2c_interface(int bus, uint8_t address); -extern PX4IO_interface *io_serial_interface(int port); +extern PX4IO_interface *io_serial_interface(); diff --git a/src/drivers/px4io/interface_i2c.cpp b/src/drivers/px4io/interface_i2c.cpp index 6895a7e23b..45a707883e 100644 --- a/src/drivers/px4io/interface_i2c.cpp +++ b/src/drivers/px4io/interface_i2c.cpp @@ -69,6 +69,8 @@ public: virtual bool ok(); + virtual int test(unsigned mode); + private: static const unsigned _retries = 2; @@ -107,6 +109,12 @@ PX4IO_I2C::ok() return true; } +int +PX4IO_I2C::test(unsigned mode) +{ + return 0; +} + int PX4IO_I2C::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { diff --git a/src/drivers/px4io/interface_serial.cpp b/src/drivers/px4io/interface_serial.cpp index 9471cecddf..817bcba8e8 100644 --- a/src/drivers/px4io/interface_serial.cpp +++ b/src/drivers/px4io/interface_serial.cpp @@ -87,6 +87,7 @@ public: virtual int get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values); virtual bool ok(); + virtual int test(unsigned mode); private: /* @@ -102,10 +103,7 @@ private: */ static IOPacket _dma_buffer; // XXX static to ensure DMA-able memory - static const unsigned _serial_tx_dma = PX4IO_SERIAL_RX_DMAMAP; DMA_HANDLE _tx_dma; - - static const unsigned _serial_rx_dma = PX4IO_SERIAL_TX_DMAMAP; DMA_HANDLE _rx_dma; /** set if we have started a transaction that expects a reply */ @@ -164,7 +162,7 @@ private: IOPacket PX4IO_serial::_dma_buffer; -PX4IO_interface *io_serial_interface(int port) +PX4IO_interface *io_serial_interface() { return new PX4IO_serial(); } @@ -175,8 +173,8 @@ PX4IO_serial::PX4IO_serial() : _expect_reply(false) { /* allocate DMA */ - _tx_dma = stm32_dmachannel(_serial_tx_dma); - _rx_dma = stm32_dmachannel(_serial_rx_dma); + _tx_dma = stm32_dmachannel(PX4IO_SERIAL_TX_DMAMAP); + _rx_dma = stm32_dmachannel(PX4IO_SERIAL_RX_DMAMAP); if ((_tx_dma == nullptr) || (_rx_dma == nullptr)) return; @@ -209,14 +207,25 @@ PX4IO_serial::PX4IO_serial() : PX4IO_serial::~PX4IO_serial() { - if (_tx_dma != nullptr) + if (_tx_dma != nullptr) { + stm32_dmastop(_tx_dma); stm32_dmafree(_tx_dma); - if (_rx_dma != nullptr) + } + if (_rx_dma != nullptr) { + stm32_dmastop(_rx_dma); stm32_dmafree(_rx_dma); + } + /* reset the UART */ + _CR1(0); + _CR2(0); + _CR3(0); + + /* restore the GPIOs */ stm32_unconfiggpio(PX4IO_SERIAL_TX_GPIO); stm32_unconfiggpio(PX4IO_SERIAL_RX_GPIO); + /* and kill our semaphores */ sem_destroy(&_completion_semaphore); sem_destroy(&_bus_semaphore); } @@ -232,6 +241,40 @@ PX4IO_serial::ok() return true; } +int +PX4IO_serial::test(unsigned mode) +{ + + switch (mode) { + case 0: + lowsyslog("test 0\n"); + + /* kill DMA, this is a PIO test */ + stm32_dmastop(_tx_dma); + stm32_dmastop(_rx_dma); + _CR3(_CR3() & ~(USART_CR3_DMAR | USART_CR3_DMAT)); + + for (;;) { + while (!(_SR() & USART_SR_TXE)) + ; + _DR(0x55); + } + return 0; + + case 1: + lowsyslog("test 1\n"); + { + uint16_t value = 0x5555; + for (;;) { + if (set_reg(0x55, 0x55, &value, 1) != OK) + return -2; + } + return 0; + } + } + return -1; +} + int PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { @@ -297,16 +340,20 @@ PX4IO_serial::_wait_complete(bool expect_reply) stm32_dmastart(_rx_dma, _dma_callback, this, false); /* start TX DMA - no callback if we also expect a reply */ - stm32_dmastart(_tx_dma, _dma_callback, expect_reply ? nullptr : this, false); + stm32_dmastart(_tx_dma, /*expect_reply ? nullptr :*/ _dma_callback, this, false); /* compute the deadline for a 5ms timeout */ struct timespec abstime; clock_gettime(CLOCK_REALTIME, &abstime); +#if 1 + abstime.tv_sec++; +#else abstime.tv_nsec += 5000000; /* 5ms timeout */ while (abstime.tv_nsec > 1000000000) { abstime.tv_sec++; abstime.tv_nsec -= 1000000000; } +#endif /* wait for the transaction to complete - 64 bytes @ 1.5Mbps ~426µs */ int ret; @@ -316,11 +363,13 @@ PX4IO_serial::_wait_complete(bool expect_reply) if (ret == OK) break; - if (ret == ETIMEDOUT) { + if (errno == ETIMEDOUT) { + lowsyslog("timeout waiting for PX4IO link (%d/%d)\n", stm32_dmaresidual(_tx_dma), stm32_dmaresidual(_rx_dma)); /* something has broken - clear out any partial DMA state and reconfigure */ _reset_dma(); break; } + lowsyslog("unexpected ret %d/%d\n", ret, errno); } return ret; diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index ecf50c859f..663609aeda 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -104,8 +104,8 @@ public: /** * Set the update rate for actuator outputs from FMU to IO. * - * @param rate The rate in Hz actuator outpus are sent to IO. - * Min 10 Hz, max 400 Hz + * @param rate The rate in Hz actuator outpus are sent to IO. + * Min 10 Hz, max 400 Hz */ int set_update_rate(int rate); @@ -120,14 +120,14 @@ public: /** * Push failsafe values to IO. * - * @param vals Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full) - * @param len Number of channels, could up to 8 + * @param vals Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full) + * @param len Number of channels, could up to 8 */ int set_failsafe_values(const uint16_t *vals, unsigned len); /** - * Print the current status of IO - */ + * Print the current status of IO + */ void print_status(); private: @@ -1579,7 +1579,7 @@ start(int argc, char *argv[]) PX4IO_interface *interface; #if defined(CONFIG_ARCH_BOARD_PX4FMUV2) - interface = io_serial_interface(5); /* XXX wrong port! */ + interface = io_serial_interface(); #elif defined(CONFIG_ARCH_BOARD_PX4FMU) interface = io_i2c_interface(PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO); #else @@ -1710,6 +1710,35 @@ monitor(void) } } +void +if_test(unsigned mode) +{ + PX4IO_interface *interface; + +#if defined(CONFIG_ARCH_BOARD_PX4FMUV2) + interface = io_serial_interface(); +#elif defined(CONFIG_ARCH_BOARD_PX4FMU) + interface = io_i2c_interface(PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO); +#else +# error Unknown board - cannot select interface. +#endif + + if (interface == nullptr) + errx(1, "cannot alloc interface"); + + if (!interface->ok()) { + delete interface; + errx(1, "interface init failed"); + } else { + + int result = interface->test(mode); + delete interface; + errx(0, "test returned %d", result); + } + + exit(0); +} + } int @@ -1722,130 +1751,6 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) start(argc - 1, argv + 1); - if (!strcmp(argv[1], "limit")) { - - if (g_dev != nullptr) { - - if ((argc > 2)) { - g_dev->set_update_rate(atoi(argv[2])); - } else { - errx(1, "missing argument (50 - 200 Hz)"); - return 1; - } - } - exit(0); - } - - if (!strcmp(argv[1], "current")) { - if (g_dev != nullptr) { - if ((argc > 3)) { - g_dev->set_battery_current_scaling(atof(argv[2]), atof(argv[3])); - } else { - errx(1, "missing argument (apm_per_volt, amp_offset)"); - return 1; - } - } - exit(0); - } - - if (!strcmp(argv[1], "failsafe")) { - - if (argc < 3) { - errx(1, "failsafe command needs at least one channel value (ppm)"); - } - - if (g_dev != nullptr) { - - /* set values for first 8 channels, fill unassigned channels with 1500. */ - uint16_t failsafe[8]; - - for (int i = 0; i < sizeof(failsafe) / sizeof(failsafe[0]); i++) - { - /* set channel to commanline argument or to 900 for non-provided channels */ - if (argc > i + 2) { - failsafe[i] = atoi(argv[i+2]); - if (failsafe[i] < 800 || failsafe[i] > 2200) { - errx(1, "value out of range of 800 < value < 2200. Aborting."); - } - } else { - /* a zero value will result in stopping to output any pulse */ - failsafe[i] = 0; - } - } - - int ret = g_dev->set_failsafe_values(failsafe, sizeof(failsafe) / sizeof(failsafe[0])); - - if (ret != OK) - errx(ret, "failed setting failsafe values"); - } else { - errx(1, "not loaded"); - } - exit(0); - } - - if (!strcmp(argv[1], "recovery")) { - - if (g_dev != nullptr) { - /* - * Enable in-air restart support. - * We can cheat and call the driver directly, as it - * doesn't reference filp in ioctl() - */ - g_dev->ioctl(NULL, PX4IO_INAIR_RESTART_ENABLE, 1); - } else { - errx(1, "not loaded"); - } - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - - if (g_dev != nullptr) { - /* stop the driver */ - delete g_dev; - } else { - errx(1, "not loaded"); - } - exit(0); - } - - - if (!strcmp(argv[1], "status")) { - - if (g_dev != nullptr) { - printf("[px4io] loaded\n"); - g_dev->print_status(); - } else { - printf("[px4io] not loaded\n"); - } - - exit(0); - } - - if (!strcmp(argv[1], "debug")) { - if (argc <= 2) { - printf("usage: px4io debug LEVEL\n"); - exit(1); - } - if (g_dev == nullptr) { - printf("px4io is not started\n"); - exit(1); - } - uint8_t level = atoi(argv[2]); - /* we can cheat and call the driver directly, as it - * doesn't reference filp in ioctl() - */ - int ret = g_dev->ioctl(NULL, PX4IO_SET_DEBUG, level); - if (ret != 0) { - printf("SET_DEBUG failed - %d\n", ret); - exit(1); - } - printf("SET_DEBUG %u OK\n", (unsigned)level); - exit(0); - } - - /* note, stop not currently implemented */ - if (!strcmp(argv[1], "update")) { if (g_dev != nullptr) { @@ -1896,6 +1801,119 @@ px4io_main(int argc, char *argv[]) return ret; } + if (!strcmp(argv[1], "iftest")) { + if (g_dev != nullptr) + errx(1, "can't iftest when started"); + + if_test((argc > 2) ? strtol(argv[2], NULL, 0) : 0); + } + + /* commands below here require a started driver */ + + if (g_dev == nullptr) + errx(1, "not started"); + + if (!strcmp(argv[1], "limit")) { + + if ((argc > 2)) { + g_dev->set_update_rate(atoi(argv[2])); + } else { + errx(1, "missing argument (50 - 200 Hz)"); + return 1; + } + exit(0); + } + + if (!strcmp(argv[1], "current")) { + if ((argc > 3)) { + g_dev->set_battery_current_scaling(atof(argv[2]), atof(argv[3])); + } else { + errx(1, "missing argument (apm_per_volt, amp_offset)"); + return 1; + } + exit(0); + } + + if (!strcmp(argv[1], "failsafe")) { + + if (argc < 3) { + errx(1, "failsafe command needs at least one channel value (ppm)"); + } + + /* set values for first 8 channels, fill unassigned channels with 1500. */ + uint16_t failsafe[8]; + + for (int i = 0; i < sizeof(failsafe) / sizeof(failsafe[0]); i++) { + + /* set channel to commandline argument or to 900 for non-provided channels */ + if (argc > i + 2) { + failsafe[i] = atoi(argv[i+2]); + if (failsafe[i] < 800 || failsafe[i] > 2200) { + errx(1, "value out of range of 800 < value < 2200. Aborting."); + } + } else { + /* a zero value will result in stopping to output any pulse */ + failsafe[i] = 0; + } + } + + int ret = g_dev->set_failsafe_values(failsafe, sizeof(failsafe) / sizeof(failsafe[0])); + + if (ret != OK) + errx(ret, "failed setting failsafe values"); + exit(0); + } + + if (!strcmp(argv[1], "recovery")) { + + /* + * Enable in-air restart support. + * We can cheat and call the driver directly, as it + * doesn't reference filp in ioctl() + */ + g_dev->ioctl(NULL, PX4IO_INAIR_RESTART_ENABLE, 1); + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + + /* stop the driver */ + delete g_dev; + exit(0); + } + + + if (!strcmp(argv[1], "status")) { + + printf("[px4io] loaded\n"); + g_dev->print_status(); + + exit(0); + } + + if (!strcmp(argv[1], "debug")) { + if (argc <= 2) { + printf("usage: px4io debug LEVEL\n"); + exit(1); + } + if (g_dev == nullptr) { + printf("px4io is not started\n"); + exit(1); + } + uint8_t level = atoi(argv[2]); + /* we can cheat and call the driver directly, as it + * doesn't reference filp in ioctl() + */ + int ret = g_dev->ioctl(NULL, PX4IO_SET_DEBUG, level); + if (ret != 0) { + printf("SET_DEBUG failed - %d\n", ret); + exit(1); + } + printf("SET_DEBUG %u OK\n", (unsigned)level); + exit(0); + } + if (!strcmp(argv[1], "rx_dsm") || !strcmp(argv[1], "rx_dsm_10bit") || !strcmp(argv[1], "rx_dsm_11bit") || @@ -1909,6 +1927,6 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "monitor")) monitor(); - out: +out: errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current', 'failsafe' or 'update'"); } From f2079ae7ff3459f5a72abeef419053fa8b7cfcf5 Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 4 Jul 2013 11:21:25 -0700 Subject: [PATCH 200/486] =?UTF-8?q?Add=20DMA=20error=20handling.=20Add=20s?= =?UTF-8?q?erial=20error=20handling.=20Add=20short-packet-reception=20hand?= =?UTF-8?q?ling=20(this=20may=20allow=20us=20to=20send/receive=20shorter?= =?UTF-8?q?=20packets=E2=80=A6=20needs=20testing).?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/drivers/px4io/interface_serial.cpp | 305 +++++++++++++++++++------ 1 file changed, 235 insertions(+), 70 deletions(-) diff --git a/src/drivers/px4io/interface_serial.cpp b/src/drivers/px4io/interface_serial.cpp index 817bcba8e8..2a05de1741 100644 --- a/src/drivers/px4io/interface_serial.cpp +++ b/src/drivers/px4io/interface_serial.cpp @@ -52,6 +52,7 @@ #include /* XXX might be able to prune these */ +#include #include #include #include @@ -62,6 +63,8 @@ #include #include /* XXX should really not be hardcoding v2 here */ +#include + #include "interface.h" const unsigned max_rw_regs = 32; // by agreement w/IO @@ -77,6 +80,8 @@ struct IOPacket { }; #pragma pack(pop) +#define PKT_SIZE(_n) (4 + (2 * (_n))) + class PX4IO_serial : public PX4IO_interface { public: @@ -105,12 +110,13 @@ private: DMA_HANDLE _tx_dma; DMA_HANDLE _rx_dma; + volatile unsigned _rx_length; - /** set if we have started a transaction that expects a reply */ - bool _expect_reply; - - /** saved DMA status (in case we care) */ - uint8_t _dma_status; + /** saved DMA status */ + static const unsigned _dma_status_inactive = 0x80000000; // low bits overlap DMA_STATUS_* values + static const unsigned _dma_status_waiting = 0x00000000; + volatile unsigned _tx_dma_status; + volatile unsigned _rx_dma_status; /** bus-ownership lock */ sem_t _bus_semaphore; @@ -123,18 +129,25 @@ private: * * @param expect_reply If true, expect a reply from IO. */ - int _wait_complete(bool expect_reply); + int _wait_complete(bool expect_reply, unsigned tx_length); /** * DMA completion handler. */ static void _dma_callback(DMA_HANDLE handle, uint8_t status, void *arg); - void _do_dma_callback(DMA_HANDLE handle, uint8_t status); + void _do_tx_dma_callback(unsigned status); + void _do_rx_dma_callback(unsigned status); /** - * (re)configure the DMA + * Serial interrupt handler. */ - void _reset_dma(); + static int _interrupt(int vector, void *context); + void _do_interrupt(); + + /** + * Cancel any DMA in progress with an error. + */ + void _abort_dma(); /** * Serial register accessors. @@ -158,9 +171,19 @@ private: uint32_t _GTPR() { return _sreg(STM32_USART_GTPR_OFFSET); } void _GTPR(uint32_t val) { _sreg(STM32_USART_GTPR_OFFSET) = val; } + /** + * Performance counters. + */ + perf_counter_t _perf_dmasetup; + perf_counter_t _perf_timeouts; + perf_counter_t _perf_errors; + perf_counter_t _perf_wr; + perf_counter_t _perf_rd; + }; IOPacket PX4IO_serial::_dma_buffer; +static PX4IO_serial *g_interface; PX4IO_interface *io_serial_interface() { @@ -170,7 +193,14 @@ PX4IO_interface *io_serial_interface() PX4IO_serial::PX4IO_serial() : _tx_dma(nullptr), _rx_dma(nullptr), - _expect_reply(false) + _rx_length(0), + _tx_dma_status(_dma_status_inactive), + _rx_dma_status(_dma_status_inactive), + _perf_dmasetup(perf_alloc(PC_ELAPSED, "dmasetup")), + _perf_timeouts(perf_alloc(PC_COUNT, "timeouts")), + _perf_errors(perf_alloc(PC_COUNT, "errors ")), + _perf_wr(perf_alloc(PC_ELAPSED, "writes ")), + _perf_rd(perf_alloc(PC_ELAPSED, "reads ")) { /* allocate DMA */ _tx_dma = stm32_dmachannel(PX4IO_SERIAL_TX_DMAMAP); @@ -193,16 +223,19 @@ PX4IO_serial::PX4IO_serial() : uint32_t fraction = (usartdiv32 - (mantissa << 5) + 1) >> 1; _BRR((mantissa << USART_BRR_MANT_SHIFT) | (fraction << USART_BRR_FRAC_SHIFT)); - /* enable UART and DMA but no interrupts */ - _CR3(USART_CR3_DMAR | USART_CR3_DMAT); - _CR1(USART_CR1_RE | USART_CR1_TE | USART_CR1_UE); + /* enable UART in DMA mode, enable error and line idle interrupts */ + _CR3(USART_CR3_DMAR | USART_CR3_DMAT | USART_CR3_EIE); + _CR1(USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE); - /* configure DMA */ - _reset_dma(); + /* attach serial interrupt handler */ + irq_attach(PX4IO_SERIAL_VECTOR, _interrupt); + up_enable_irq(PX4IO_SERIAL_VECTOR); /* create semaphores */ sem_init(&_completion_semaphore, 0, 0); sem_init(&_bus_semaphore, 0, 1); + + g_interface = this; } PX4IO_serial::~PX4IO_serial() @@ -221,6 +254,10 @@ PX4IO_serial::~PX4IO_serial() _CR2(0); _CR3(0); + /* detach our interrupt handler */ + up_disable_irq(PX4IO_SERIAL_VECTOR); + irq_detach(PX4IO_SERIAL_VECTOR); + /* restore the GPIOs */ stm32_unconfiggpio(PX4IO_SERIAL_TX_GPIO); stm32_unconfiggpio(PX4IO_SERIAL_RX_GPIO); @@ -228,6 +265,9 @@ PX4IO_serial::~PX4IO_serial() /* and kill our semaphores */ sem_destroy(&_completion_semaphore); sem_destroy(&_bus_semaphore); + + if (g_interface == this) + g_interface = nullptr; } bool @@ -265,9 +305,14 @@ PX4IO_serial::test(unsigned mode) lowsyslog("test 1\n"); { uint16_t value = 0x5555; - for (;;) { + for (unsigned count = 0;; count++) { if (set_reg(0x55, 0x55, &value, 1) != OK) return -2; + if (count > 10000) { + perf_print_counter(_perf_dmasetup); + perf_print_counter(_perf_wr); + count = 0; + } } return 0; } @@ -291,7 +336,7 @@ PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsi /* XXX implement check byte */ /* start the transaction and wait for it to complete */ - int result = _wait_complete(false); + int result = _wait_complete(false, PKT_SIZE(num_values)); sem_post(&_bus_semaphore); return result; @@ -311,7 +356,7 @@ PX4IO_serial::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned n _dma_buffer.offset = offset; /* start the transaction and wait for it to complete */ - int result = _wait_complete(true); + int result = _wait_complete(true, PKT_SIZE(0)); if (result != OK) goto out; @@ -329,24 +374,58 @@ out: } int -PX4IO_serial::_wait_complete(bool expect_reply) +PX4IO_serial::_wait_complete(bool expect_reply, unsigned tx_length) { - /* save for callback use */ - _expect_reply = expect_reply; - /* start RX DMA */ - if (expect_reply) + if (expect_reply) { + perf_begin(_perf_rd); + perf_begin(_perf_dmasetup); + + /* DMA setup time ~3µs */ + _rx_dma_status = _dma_status_waiting; + _rx_length = 0; + stm32_dmasetup( + _rx_dma, + PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET, + reinterpret_cast(&_dma_buffer), + sizeof(_dma_buffer), + DMA_SCR_DIR_P2M | + DMA_SCR_MINC | + DMA_SCR_PSIZE_8BITS | + DMA_SCR_MSIZE_8BITS | + DMA_SCR_PBURST_SINGLE | + DMA_SCR_MBURST_SINGLE); stm32_dmastart(_rx_dma, _dma_callback, this, false); + perf_end(_perf_dmasetup); + } else { + perf_begin(_perf_wr); + } + /* start TX DMA - no callback if we also expect a reply */ + /* DMA setup time ~3µs */ + perf_begin(_perf_dmasetup); + _tx_dma_status = _dma_status_waiting; + stm32_dmasetup( + _tx_dma, + PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET, + reinterpret_cast(&_dma_buffer), + sizeof(_dma_buffer), /* XXX should be tx_length */ + DMA_SCR_DIR_M2P | + DMA_SCR_MINC | + DMA_SCR_PSIZE_8BITS | + DMA_SCR_MSIZE_8BITS | + DMA_SCR_PBURST_SINGLE | + DMA_SCR_MBURST_SINGLE); stm32_dmastart(_tx_dma, /*expect_reply ? nullptr :*/ _dma_callback, this, false); + perf_end(_perf_dmasetup); /* compute the deadline for a 5ms timeout */ struct timespec abstime; clock_gettime(CLOCK_REALTIME, &abstime); #if 1 - abstime.tv_sec++; + abstime.tv_sec++; /* long timeout for testing */ #else abstime.tv_nsec += 5000000; /* 5ms timeout */ while (abstime.tv_nsec > 1000000000) { @@ -366,12 +445,32 @@ PX4IO_serial::_wait_complete(bool expect_reply) if (errno == ETIMEDOUT) { lowsyslog("timeout waiting for PX4IO link (%d/%d)\n", stm32_dmaresidual(_tx_dma), stm32_dmaresidual(_rx_dma)); /* something has broken - clear out any partial DMA state and reconfigure */ - _reset_dma(); + _abort_dma(); + perf_count(_perf_timeouts); break; } lowsyslog("unexpected ret %d/%d\n", ret, errno); } + /* check for DMA errors */ + if (ret == OK) { + if (_tx_dma_status & DMA_STATUS_TEIF) { + lowsyslog("DMA transmit error\n"); + ret = -1; + } + if (_rx_dma_status & DMA_STATUS_TEIF) { + lowsyslog("DMA receive error\n"); + ret = -1; + } + perf_count(_perf_errors); + } + + /* reset DMA status */ + _tx_dma_status = _dma_status_inactive; + _rx_dma_status = _dma_status_inactive; + + perf_end(expect_reply ? _perf_rd : _perf_wr); + return ret; } @@ -381,58 +480,124 @@ PX4IO_serial::_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) if (arg != nullptr) { PX4IO_serial *ps = reinterpret_cast(arg); - ps->_do_dma_callback(handle, status); - } -} - -void -PX4IO_serial::_do_dma_callback(DMA_HANDLE handle, uint8_t status) -{ - /* on completion of a no-reply transmit, wake the sender */ - if (handle == _tx_dma) { - if ((status & DMA_STATUS_TCIF) && !_expect_reply) { - _dma_status = status; - sem_post(&_completion_semaphore); - } - /* XXX if we think we're going to see DMA errors, we should handle them here */ - } - - /* on completion of a reply, wake the waiter */ - if (handle == _rx_dma) { - if (status & DMA_STATUS_TCIF) { - /* XXX if we are worried about overrun/synch, check UART status here */ - _dma_status = status; - sem_post(&_completion_semaphore); + if (handle == ps->_tx_dma) { + ps->_do_tx_dma_callback(status); + } else if (handle == ps->_rx_dma) { + ps->_do_rx_dma_callback(status); } } } void -PX4IO_serial::_reset_dma() +PX4IO_serial::_do_tx_dma_callback(unsigned status) +{ + /* on completion of a no-reply transmit, wake the sender */ + if (_tx_dma_status == _dma_status_waiting) { + + /* save TX status */ + _tx_dma_status = status; + + /* if we aren't going on to expect a reply, complete now */ + if (_rx_dma_status != _dma_status_waiting) + sem_post(&_completion_semaphore); + } +} + +void +PX4IO_serial::_do_rx_dma_callback(unsigned status) +{ + ASSERT(_tx_dma_status != _dma_status_waiting); + + /* on completion of a reply, wake the waiter */ + if (_rx_dma_status == _dma_status_waiting) { + + /* check for packet overrun - this will occur after DMA completes */ + if (_SR() & (USART_SR_ORE | USART_SR_RXNE)) { + _DR(); + status = DMA_STATUS_TEIF; + } + + /* save RX status */ + _rx_dma_status = status; + + /* DMA may have stopped short */ + _rx_length = sizeof(IOPacket) - stm32_dmaresidual(_rx_dma); + + /* complete now */ + sem_post(&_completion_semaphore); + } +} + +int +PX4IO_serial::_interrupt(int irq, void *context) +{ + if (g_interface != nullptr) + g_interface->_do_interrupt(); + return 0; +} + +void +PX4IO_serial::_do_interrupt() +{ + uint32_t sr = _SR(); /* get UART status register */ + + /* handle error/exception conditions */ + if (sr & (USART_SR_ORE | USART_SR_NE | USART_SR_FE | USART_SR_IDLE)) { + /* read DR to clear status */ + _DR(); + } else { + ASSERT(0); + } + + if (sr & (USART_SR_ORE | /* overrun error - packet was too big for DMA or DMA was too slow */ + USART_SR_NE | /* noise error - we have lost a byte due to noise */ + USART_SR_FE)) { /* framing error - start/stop bit lost or line break */ + + /* + * If we are in the process of listening for something, these are all fatal; + * abort the DMA with an error. + */ + if (_rx_dma_status == _dma_status_waiting) { + _abort_dma(); + return; + } + + /* XXX we might want to use FE / line break as an out-of-band handshake ... handle it here */ + + /* don't attempt to handle IDLE if it's set - things went bad */ + return; + } + + if (sr & USART_SR_IDLE) { + + /* if there was DMA transmission still going, this is an error */ + if (_tx_dma_status == _dma_status_waiting) { + + /* babble from IO */ + _abort_dma(); + return; + } + + /* if there is DMA reception going on, this is a short packet */ + if (_rx_dma_status == _dma_status_waiting) { + + /* stop the receive DMA */ + stm32_dmastop(_rx_dma); + + /* complete the short reception */ + _do_rx_dma_callback(DMA_STATUS_TCIF); + } + } +} + +void +PX4IO_serial::_abort_dma() { stm32_dmastop(_tx_dma); stm32_dmastop(_rx_dma); + _do_tx_dma_callback(DMA_STATUS_TEIF); + _do_rx_dma_callback(DMA_STATUS_TEIF); - stm32_dmasetup( - _tx_dma, - PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET, - reinterpret_cast(&_dma_buffer), - sizeof(_dma_buffer), - DMA_SCR_DIR_M2P | - DMA_SCR_MINC | - DMA_SCR_PSIZE_8BITS | - DMA_SCR_MSIZE_8BITS | - DMA_SCR_PBURST_SINGLE | - DMA_SCR_MBURST_SINGLE); - stm32_dmasetup( - _rx_dma, - PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET, - reinterpret_cast(&_dma_buffer), - sizeof(_dma_buffer), - DMA_SCR_DIR_P2M | - DMA_SCR_MINC | - DMA_SCR_PSIZE_8BITS | - DMA_SCR_MSIZE_8BITS | - DMA_SCR_PBURST_SINGLE | - DMA_SCR_MBURST_SINGLE); + _tx_dma_status = _dma_status_inactive; + _rx_dma_status = _dma_status_inactive; } \ No newline at end of file From 2e001aad0429c816321bfc76264282935911746e Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 4 Jul 2013 12:50:59 -0700 Subject: [PATCH 201/486] Add PX4IOv2 support to the uploader. --- src/drivers/boards/px4fmu/px4fmu_internal.h | 3 ++ src/drivers/boards/px4fmuv2/px4fmu_internal.h | 1 + src/drivers/px4io/uploader.cpp | 37 +++++++++++++++++-- 3 files changed, 37 insertions(+), 4 deletions(-) diff --git a/src/drivers/boards/px4fmu/px4fmu_internal.h b/src/drivers/boards/px4fmu/px4fmu_internal.h index 671a58f8f6..5a73c10bfa 100644 --- a/src/drivers/boards/px4fmu/px4fmu_internal.h +++ b/src/drivers/boards/px4fmu/px4fmu_internal.h @@ -58,6 +58,9 @@ __BEGIN_DECLS ****************************************************************************************************/ /* Configuration ************************************************************************************/ +/* PX4IO connection configuration */ +#define PX4IO_SERIAL_DEVICE "/dev/ttyS2" + //#ifdef CONFIG_STM32_SPI2 //# error "SPI2 is not supported on this board" //#endif diff --git a/src/drivers/boards/px4fmuv2/px4fmu_internal.h b/src/drivers/boards/px4fmuv2/px4fmu_internal.h index 1698336b4b..dd291b9b7f 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu_internal.h +++ b/src/drivers/boards/px4fmuv2/px4fmu_internal.h @@ -59,6 +59,7 @@ __BEGIN_DECLS /* Configuration ************************************************************************************/ /* PX4IO connection configuration */ +#define PX4IO_SERIAL_DEVICE "/dev/ttyS5" #define PX4IO_SERIAL_TX_GPIO GPIO_USART6_TX #define PX4IO_SERIAL_RX_GPIO GPIO_USART6_RX #define PX4IO_SERIAL_BASE STM32_USART6_BASE /* hardwired on the board */ diff --git a/src/drivers/px4io/uploader.cpp b/src/drivers/px4io/uploader.cpp index 15524c3aeb..67298af322 100644 --- a/src/drivers/px4io/uploader.cpp +++ b/src/drivers/px4io/uploader.cpp @@ -49,10 +49,21 @@ #include #include #include +#include #include #include "uploader.h" +#ifdef CONFIG_ARCH_BOARD_PX4FMUV2 +#include +#endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU +#include +#endif + +// define for comms logging +//#define UDEBUG + static const uint32_t crctab[] = { 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, @@ -114,13 +125,23 @@ PX4IO_Uploader::upload(const char *filenames[]) const char *filename = NULL; size_t fw_size; - _io_fd = open("/dev/ttyS2", O_RDWR); +#ifndef PX4IO_SERIAL_DEVICE +#error Must define PX4IO_SERIAL_DEVICE in board configuration to support firmware upload +#endif + + _io_fd = open(PX4IO_SERIAL_DEVICE, O_RDWR); if (_io_fd < 0) { log("could not open interface"); return -errno; } + /* adjust line speed to match bootloader */ + struct termios t; + tcgetattr(_io_fd, &t); + cfsetspeed(&t, 115200); + tcsetattr(_io_fd, TCSANOW, &t); + /* look for the bootloader */ ret = sync(); @@ -251,12 +272,16 @@ PX4IO_Uploader::recv(uint8_t &c, unsigned timeout) int ret = ::poll(&fds[0], 1, timeout); if (ret < 1) { +#ifdef UDEBUG log("poll timeout %d", ret); +#endif return -ETIMEDOUT; } read(_io_fd, &c, 1); - //log("recv 0x%02x", c); +#ifdef UDEBUG + log("recv 0x%02x", c); +#endif return OK; } @@ -282,16 +307,20 @@ PX4IO_Uploader::drain() do { ret = recv(c, 1000); +#ifdef UDEBUG if (ret == OK) { - //log("discard 0x%02x", c); + log("discard 0x%02x", c); } +#endif } while (ret == OK); } int PX4IO_Uploader::send(uint8_t c) { - //log("send 0x%02x", c); +#ifdef UDEBUG + log("send 0x%02x", c); +#endif if (write(_io_fd, &c, 1) != 1) return -errno; From f7963a8c84792ff6d95fa52d92f308c596e309e4 Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 4 Jul 2013 23:14:13 -0700 Subject: [PATCH 202/486] Fix printing of PC_COUNT perf counters --- Debug/PX4 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Debug/PX4 b/Debug/PX4 index 085cffe434..e99228ee0c 100644 --- a/Debug/PX4 +++ b/Debug/PX4 @@ -27,7 +27,7 @@ define _perf_print # PC_COUNT if $hdr->type == 0 set $count = (struct perf_ctr_count *)$hdr - printf "%llu events,\n", $count->event_count; + printf "%llu events\n", $count->event_count end # PC_ELPASED if $hdr->type == 1 From c21237667bc2802f675c74641d25f538db5f2c0c Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 4 Jul 2013 23:16:13 -0700 Subject: [PATCH 203/486] iov2 pin definition cleanup sweep --- src/drivers/boards/px4iov2/px4iov2_init.c | 19 +++++-------------- src/drivers/boards/px4iov2/px4iov2_internal.h | 19 +++++++------------ src/modules/px4iofirmware/controls.c | 4 ++-- src/modules/px4iofirmware/px4io.c | 2 ++ src/modules/px4iofirmware/px4io.h | 4 +++- 5 files changed, 19 insertions(+), 29 deletions(-) diff --git a/src/drivers/boards/px4iov2/px4iov2_init.c b/src/drivers/boards/px4iov2/px4iov2_init.c index 09469d7e80..5d7b225609 100644 --- a/src/drivers/boards/px4iov2/px4iov2_init.c +++ b/src/drivers/boards/px4iov2/px4iov2_init.c @@ -104,31 +104,25 @@ __EXPORT void stm32_boardinitialize(void) /* configure GPIOs */ - /* turn off - all leds are active low */ - stm32_gpiowrite(GPIO_LED1, true); - stm32_gpiowrite(GPIO_LED2, true); - stm32_gpiowrite(GPIO_LED3, true); + /* LEDS - default to off */ stm32_configgpio(GPIO_LED1); stm32_configgpio(GPIO_LED2); stm32_configgpio(GPIO_LED3); - stm32_configgpio(GPIO_BTN_SAFETY); /* spektrum power enable is active high - disable it by default */ + /* XXX might not want to do this on warm restart? */ stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, false); stm32_configgpio(GPIO_SPEKTRUM_PWR_EN); - /* servo power enable is active low, and has a pull down resistor - * to keep it low during boot (since it may power the whole board.) - */ - stm32_gpiowrite(GPIO_SERVO_PWR_EN, false); - stm32_configgpio(GPIO_SERVO_PWR_EN); - stm32_configgpio(GPIO_SERVO_FAULT_DETECT); + /* RSSI inputs */ stm32_configgpio(GPIO_TIM_RSSI); /* xxx alternate function */ stm32_configgpio(GPIO_ADC_RSSI); + + /* servo rail voltage */ stm32_configgpio(GPIO_ADC_VSERVO); stm32_configgpio(GPIO_SBUS_INPUT); /* xxx alternate function */ @@ -140,7 +134,6 @@ __EXPORT void stm32_boardinitialize(void) stm32_gpiowrite(GPIO_SBUS_OENABLE, true); stm32_configgpio(GPIO_SBUS_OENABLE); - stm32_configgpio(GPIO_PPM); /* xxx alternate function */ stm32_gpiowrite(GPIO_PWM1, false); @@ -166,6 +159,4 @@ __EXPORT void stm32_boardinitialize(void) stm32_gpiowrite(GPIO_PWM8, false); stm32_configgpio(GPIO_PWM8); - -// message("[boot] Successfully initialized px4iov2 gpios\n"); } diff --git a/src/drivers/boards/px4iov2/px4iov2_internal.h b/src/drivers/boards/px4iov2/px4iov2_internal.h index b8aa6d053e..81a75431c3 100755 --- a/src/drivers/boards/px4iov2/px4iov2_internal.h +++ b/src/drivers/boards/px4iov2/px4iov2_internal.h @@ -60,6 +60,7 @@ * Serial ******************************************************************************/ #define PX4FMU_SERIAL_BASE STM32_USART2_BASE +#define PX4FMU_SERIAL_VECTOR STM32_IRQ_USART2 #define PX4FMU_SERIAL_TX_GPIO GPIO_USART2_TX #define PX4FMU_SERIAL_RX_GPIO GPIO_USART2_RX #define PX4FMU_SERIAL_TX_DMA DMACHAN_USART2_TX @@ -73,13 +74,9 @@ /* LEDS **********************************************************************/ -#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14) -#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15) -#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN13) - -#define GPIO_LED_BLUE BOARD_GPIO_LED1 -#define GPIO_LED_AMBER BOARD_GPIO_LED2 -#define GPIO_LED_SAFETY BOARD_GPIO_LED3 +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14) +#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15) +#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13) /* Safety switch button *******************************************************/ @@ -89,17 +86,14 @@ #define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) -#define GPIO_SERVO_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN15) - -#define GPIO_SERVO_FAULT_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN13) - +#define GPIO_SERVO_FAULT_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN15) /* Analog inputs **************************************************************/ #define GPIO_ADC_VSERVO (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN4) + /* the same rssi signal goes to both an adc and a timer input */ #define GPIO_ADC_RSSI (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN5) -/* floating pin */ #define GPIO_TIM_RSSI (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN12) /* PWM pins **************************************************************/ @@ -117,6 +111,7 @@ /* SBUS pins *************************************************************/ +/* XXX these should be UART pins */ #define GPIO_SBUS_INPUT (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN11) #define GPIO_SBUS_OUTPUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) #define GPIO_SBUS_OENABLE (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN4) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 3cf9ca149b..95103964e5 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -59,10 +59,10 @@ static perf_counter_t c_gather_ppm; void controls_init(void) { - /* DSM input */ + /* DSM input (USART1) */ dsm_init("/dev/ttyS0"); - /* S.bus input */ + /* S.bus input (USART3) */ sbus_init("/dev/ttyS2"); /* default to a 1:1 input map, all enabled */ diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 385920d53b..e70b3fe880 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -143,7 +143,9 @@ user_start(int argc, char *argv[]) LED_SAFETY(false); /* turn on servo power (if supported) */ +#ifdef POWER_SERVO POWER_SERVO(true); +#endif /* start the safety switch handler */ safety_init(); diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 47bcb8ddfc..4965d07245 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -128,7 +128,9 @@ extern struct sys_state_s system_state; #define LED_AMBER(_s) stm32_gpiowrite(GPIO_LED2, !(_s)) #define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s)) -#define POWER_SERVO(_s) stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s)) +#ifdef GPIO_SERVO_PWR_EN +# define POWER_SERVO(_s) stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s)) +#endif #ifdef GPIO_ACC1_PWR_EN # define POWER_ACC1(_s) stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s)) #endif From 52096f017f170ac873b782bc4ac260851ad01d89 Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 4 Jul 2013 23:17:16 -0700 Subject: [PATCH 204/486] Switch to the 'normal' way of doing register accessors. Be more aggressive en/disabling DMA in the UART, since ST say you should. --- src/drivers/px4io/interface_serial.cpp | 139 +++++++++++++++---------- 1 file changed, 82 insertions(+), 57 deletions(-) diff --git a/src/drivers/px4io/interface_serial.cpp b/src/drivers/px4io/interface_serial.cpp index 2a05de1741..5d9eac0761 100644 --- a/src/drivers/px4io/interface_serial.cpp +++ b/src/drivers/px4io/interface_serial.cpp @@ -65,6 +65,8 @@ #include +#include + #include "interface.h" const unsigned max_rw_regs = 32; // by agreement w/IO @@ -80,6 +82,16 @@ struct IOPacket { }; #pragma pack(pop) +/* serial register accessors */ +#define REG(_x) (*(volatile uint32_t *)(PX4IO_SERIAL_BASE + _x)) +#define rSR REG(STM32_USART_SR_OFFSET) +#define rDR REG(STM32_USART_DR_OFFSET) +#define rBRR REG(STM32_USART_BRR_OFFSET) +#define rCR1 REG(STM32_USART_CR1_OFFSET) +#define rCR2 REG(STM32_USART_CR2_OFFSET) +#define rCR3 REG(STM32_USART_CR3_OFFSET) +#define rGTPR REG(STM32_USART_GTPR_OFFSET) + #define PKT_SIZE(_n) (4 + (2 * (_n))) class PX4IO_serial : public PX4IO_interface @@ -149,28 +161,6 @@ private: */ void _abort_dma(); - /** - * Serial register accessors. - */ - volatile uint32_t &_sreg(unsigned offset) - { - return *(volatile uint32_t *)(PX4IO_SERIAL_BASE + offset); - } - uint32_t _SR() { return _sreg(STM32_USART_SR_OFFSET); } - void _SR(uint32_t val) { _sreg(STM32_USART_SR_OFFSET) = val; } - uint32_t _DR() { return _sreg(STM32_USART_DR_OFFSET); } - void _DR(uint32_t val) { _sreg(STM32_USART_DR_OFFSET) = val; } - uint32_t _BRR() { return _sreg(STM32_USART_BRR_OFFSET); } - void _BRR(uint32_t val) { _sreg(STM32_USART_BRR_OFFSET) = val; } - uint32_t _CR1() { return _sreg(STM32_USART_CR1_OFFSET); } - void _CR1(uint32_t val) { _sreg(STM32_USART_CR1_OFFSET) = val; } - uint32_t _CR2() { return _sreg(STM32_USART_CR2_OFFSET); } - void _CR2(uint32_t val) { _sreg(STM32_USART_CR2_OFFSET) = val; } - uint32_t _CR3() { return _sreg(STM32_USART_CR3_OFFSET); } - void _CR3(uint32_t val) { _sreg(STM32_USART_CR3_OFFSET) = val; } - uint32_t _GTPR() { return _sreg(STM32_USART_GTPR_OFFSET); } - void _GTPR(uint32_t val) { _sreg(STM32_USART_GTPR_OFFSET) = val; } - /** * Performance counters. */ @@ -213,24 +203,28 @@ PX4IO_serial::PX4IO_serial() : stm32_configgpio(PX4IO_SERIAL_RX_GPIO); /* reset & configure the UART */ - _CR1(0); - _CR2(0); - _CR3(0); + rCR1 = 0; + rCR2 = 0; + rCR3 = 0; + + /* eat any existing interrupt status */ + (void)rSR; + (void)rDR; /* configure line speed */ uint32_t usartdiv32 = PX4IO_SERIAL_CLOCK / (PX4IO_SERIAL_BITRATE / 2); uint32_t mantissa = usartdiv32 >> 5; uint32_t fraction = (usartdiv32 - (mantissa << 5) + 1) >> 1; - _BRR((mantissa << USART_BRR_MANT_SHIFT) | (fraction << USART_BRR_FRAC_SHIFT)); - - /* enable UART in DMA mode, enable error and line idle interrupts */ - _CR3(USART_CR3_DMAR | USART_CR3_DMAT | USART_CR3_EIE); - _CR1(USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE); + rBRR = (mantissa << USART_BRR_MANT_SHIFT) | (fraction << USART_BRR_FRAC_SHIFT); /* attach serial interrupt handler */ irq_attach(PX4IO_SERIAL_VECTOR, _interrupt); up_enable_irq(PX4IO_SERIAL_VECTOR); + /* enable UART in DMA mode, enable error and line idle interrupts */ + /* rCR3 = USART_CR3_EIE;*/ + rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE /*| USART_CR1_IDLEIE*/; + /* create semaphores */ sem_init(&_completion_semaphore, 0, 0); sem_init(&_bus_semaphore, 0, 1); @@ -250,9 +244,9 @@ PX4IO_serial::~PX4IO_serial() } /* reset the UART */ - _CR1(0); - _CR2(0); - _CR3(0); + rCR1 = 0; + rCR2 = 0; + rCR3 = 0; /* detach our interrupt handler */ up_disable_irq(PX4IO_SERIAL_VECTOR); @@ -292,30 +286,35 @@ PX4IO_serial::test(unsigned mode) /* kill DMA, this is a PIO test */ stm32_dmastop(_tx_dma); stm32_dmastop(_rx_dma); - _CR3(_CR3() & ~(USART_CR3_DMAR | USART_CR3_DMAT)); + rCR3 &= ~(USART_CR3_DMAR | USART_CR3_DMAT); for (;;) { - while (!(_SR() & USART_SR_TXE)) + while (!(rSR & USART_SR_TXE)) ; - _DR(0x55); + rDR = 0x55; } return 0; case 1: lowsyslog("test 1\n"); { - uint16_t value = 0x5555; for (unsigned count = 0;; count++) { - if (set_reg(0x55, 0x55, &value, 1) != OK) + uint16_t value = count & 0xffff; + + if (set_reg(PX4IO_PAGE_TEST, PX4IO_P_TEST_LED, &value, 1) != OK) return -2; - if (count > 10000) { + if (count > 100) { perf_print_counter(_perf_dmasetup); perf_print_counter(_perf_wr); count = 0; } + usleep(100000); } return 0; } + case 2: + lowsyslog("test 2\n"); + return 0; } return -1; } @@ -333,6 +332,9 @@ PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsi _dma_buffer.page = page; _dma_buffer.offset = offset; memcpy((void *)&_dma_buffer.regs[0], (void *)values, (2 * num_values)); + for (unsigned i = num_values; i < max_rw_regs; i++) + _dma_buffer.regs[i] = 0x55aa; + /* XXX implement check byte */ /* start the transaction and wait for it to complete */ @@ -377,6 +379,10 @@ int PX4IO_serial::_wait_complete(bool expect_reply, unsigned tx_length) { + /* clear any lingering error status */ + (void)rSR; + (void)rDR; + /* start RX DMA */ if (expect_reply) { perf_begin(_perf_rd); @@ -397,6 +403,7 @@ PX4IO_serial::_wait_complete(bool expect_reply, unsigned tx_length) DMA_SCR_PBURST_SINGLE | DMA_SCR_MBURST_SINGLE); stm32_dmastart(_rx_dma, _dma_callback, this, false); + rCR3 |= USART_CR3_DMAR; perf_end(_perf_dmasetup); } else { @@ -419,6 +426,8 @@ PX4IO_serial::_wait_complete(bool expect_reply, unsigned tx_length) DMA_SCR_PBURST_SINGLE | DMA_SCR_MBURST_SINGLE); stm32_dmastart(_tx_dma, /*expect_reply ? nullptr :*/ _dma_callback, this, false); + rCR3 |= USART_CR3_DMAT; + perf_end(_perf_dmasetup); /* compute the deadline for a 5ms timeout */ @@ -439,8 +448,18 @@ PX4IO_serial::_wait_complete(bool expect_reply, unsigned tx_length) for (;;) { ret = sem_timedwait(&_completion_semaphore, &abstime); - if (ret == OK) + if (ret == OK) { + /* check for DMA errors */ + if (_tx_dma_status & DMA_STATUS_TEIF) { + lowsyslog("DMA transmit error\n"); + ret = -1; + } + if (_rx_dma_status & DMA_STATUS_TEIF) { + lowsyslog("DMA receive error\n"); + ret = -1; + } break; + } if (errno == ETIMEDOUT) { lowsyslog("timeout waiting for PX4IO link (%d/%d)\n", stm32_dmaresidual(_tx_dma), stm32_dmaresidual(_rx_dma)); @@ -452,24 +471,14 @@ PX4IO_serial::_wait_complete(bool expect_reply, unsigned tx_length) lowsyslog("unexpected ret %d/%d\n", ret, errno); } - /* check for DMA errors */ - if (ret == OK) { - if (_tx_dma_status & DMA_STATUS_TEIF) { - lowsyslog("DMA transmit error\n"); - ret = -1; - } - if (_rx_dma_status & DMA_STATUS_TEIF) { - lowsyslog("DMA receive error\n"); - ret = -1; - } - perf_count(_perf_errors); - } - /* reset DMA status */ _tx_dma_status = _dma_status_inactive; _rx_dma_status = _dma_status_inactive; + /* update counters */ perf_end(expect_reply ? _perf_rd : _perf_wr); + if (ret != OK) + perf_count(_perf_errors); return ret; } @@ -497,6 +506,9 @@ PX4IO_serial::_do_tx_dma_callback(unsigned status) /* save TX status */ _tx_dma_status = status; + /* disable UART DMA */ + rCR3 &= ~USART_CR3_DMAT; + /* if we aren't going on to expect a reply, complete now */ if (_rx_dma_status != _dma_status_waiting) sem_post(&_completion_semaphore); @@ -512,14 +524,18 @@ PX4IO_serial::_do_rx_dma_callback(unsigned status) if (_rx_dma_status == _dma_status_waiting) { /* check for packet overrun - this will occur after DMA completes */ - if (_SR() & (USART_SR_ORE | USART_SR_RXNE)) { - _DR(); + uint32_t sr = rSR; + if (sr & (USART_SR_ORE | USART_SR_RXNE)) { + (void)rDR; status = DMA_STATUS_TEIF; } /* save RX status */ _rx_dma_status = status; + /* disable UART DMA */ + rCR3 &= ~USART_CR3_DMAR; + /* DMA may have stopped short */ _rx_length = sizeof(IOPacket) - stm32_dmaresidual(_rx_dma); @@ -539,12 +555,12 @@ PX4IO_serial::_interrupt(int irq, void *context) void PX4IO_serial::_do_interrupt() { - uint32_t sr = _SR(); /* get UART status register */ + uint32_t sr = rSR; /* get UART status register */ /* handle error/exception conditions */ if (sr & (USART_SR_ORE | USART_SR_NE | USART_SR_FE | USART_SR_IDLE)) { /* read DR to clear status */ - _DR(); + (void)rDR; } else { ASSERT(0); } @@ -593,8 +609,17 @@ PX4IO_serial::_do_interrupt() void PX4IO_serial::_abort_dma() { + /* disable UART DMA */ + rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR); + (void)rSR; + (void)rDR; + (void)rDR; + + /* stop DMA */ stm32_dmastop(_tx_dma); stm32_dmastop(_rx_dma); + + /* complete DMA as though in error */ _do_tx_dma_callback(DMA_STATUS_TEIF); _do_rx_dma_callback(DMA_STATUS_TEIF); From 43210413a98dfe32302cd99c86847729100133fa Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 4 Jul 2013 23:17:55 -0700 Subject: [PATCH 205/486] More test work on the px4io side of the serial interface. --- src/modules/px4iofirmware/protocol.h | 4 + src/modules/px4iofirmware/px4io.h | 2 +- src/modules/px4iofirmware/registers.c | 14 +- src/modules/px4iofirmware/serial.c | 193 ++++++++++++++++++++------ 4 files changed, 169 insertions(+), 44 deletions(-) diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 0e40bff691..b19146b061 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -183,6 +183,10 @@ /* PWM failsafe values - zero disables the output */ #define PX4IO_PAGE_FAILSAFE_PWM 105 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +/* Debug and test page - not used in normal operation */ +#define PX4IO_PAGE_TEST 127 +#define PX4IO_P_TEST_LED 0 /* set the amber LED on/off */ + /** * As-needed mixer data upload. * diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 4965d07245..0779ffd8fa 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -172,7 +172,7 @@ extern void interface_tick(void); /** * Register space */ -extern void registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values); +extern int registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values); extern int registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values); /** diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 42554456c3..b977375f47 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -185,7 +185,7 @@ uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE */ uint16_t r_page_servo_failsafe[IO_SERVO_COUNT] = { 0 }; -void +int registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { @@ -261,11 +261,13 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* iterate individual registers, set each in turn */ while (num_values--) { if (registers_set_one(page, offset, *values)) - break; + return -1; offset++; values++; } + break; } + return 0; } static int @@ -451,6 +453,14 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) /* case PX4IO_RC_PAGE_CONFIG */ } + case PX4IO_PAGE_TEST: + switch (offset) { + case PX4IO_P_TEST_LED: + LED_AMBER(value & 1); + break; + } + break; + default: return -1; } diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index 3fedfeb2c2..b51ff87d3e 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -51,17 +51,28 @@ #include #include #include -#include +#include //#define DEBUG #include "px4io.h" static volatile bool sending = false; -static void dma_callback(DMA_HANDLE handle, uint8_t status, void *arg); +static perf_counter_t pc_rx; +static perf_counter_t pc_errors; +static perf_counter_t pc_ore; +static perf_counter_t pc_fe; +static perf_counter_t pc_ne; +static perf_counter_t pc_regerr; + +static void rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg); +static void tx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg); static DMA_HANDLE tx_dma; static DMA_HANDLE rx_dma; +static int serial_interrupt(int irq, void *context); +static void dma_reset(void); + #define MAX_RW_REGS 32 // by agreement w/FMU #pragma pack(push, 1) @@ -90,6 +101,13 @@ static struct IOPacket dma_packet; void interface_init(void) { + pc_rx = perf_alloc(PC_COUNT, "rx count"); + pc_errors = perf_alloc(PC_COUNT, "errors"); + pc_ore = perf_alloc(PC_COUNT, "overrun"); + pc_fe = perf_alloc(PC_COUNT, "framing"); + pc_ne = perf_alloc(PC_COUNT, "noise"); + pc_regerr = perf_alloc(PC_COUNT, "regerr"); + /* allocate DMA */ tx_dma = stm32_dmachannel(PX4FMU_SERIAL_TX_DMA); rx_dma = stm32_dmachannel(PX4FMU_SERIAL_RX_DMA); @@ -103,38 +121,37 @@ interface_init(void) rCR2 = 0; rCR3 = 0; + /* clear status/errors */ + (void)rSR; + (void)rDR; + /* configure line speed */ uint32_t usartdiv32 = PX4FMU_SERIAL_CLOCK / (PX4FMU_SERIAL_BITRATE / 2); uint32_t mantissa = usartdiv32 >> 5; uint32_t fraction = (usartdiv32 - (mantissa << 5) + 1) >> 1; rBRR = (mantissa << USART_BRR_MANT_SHIFT) | (fraction << USART_BRR_FRAC_SHIFT); + /* connect our interrupt */ + irq_attach(PX4FMU_SERIAL_VECTOR, serial_interrupt); + up_enable_irq(PX4FMU_SERIAL_VECTOR); + /* enable UART and DMA */ - rCR3 = USART_CR3_DMAR | USART_CR3_DMAT; - rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE; + /*rCR3 = USART_CR3_EIE; */ + rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE /*| USART_CR1_IDLEIE*/; - /* configure DMA */ - stm32_dmasetup( - tx_dma, - (uint32_t)&rDR, - (uint32_t)&dma_packet, - sizeof(dma_packet), - DMA_CCR_DIR | - DMA_CCR_MINC | - DMA_CCR_PSIZE_8BITS | - DMA_CCR_MSIZE_8BITS); +#if 0 /* keep this for signal integrity testing */ + for (;;) { + while (!(rSR & USART_SR_TXE)) + ; + rDR = 0xfa; + while (!(rSR & USART_SR_TXE)) + ; + rDR = 0xa0; + } +#endif - stm32_dmasetup( - rx_dma, - (uint32_t)&rDR, - (uint32_t)&dma_packet, - sizeof(dma_packet), - DMA_CCR_MINC | - DMA_CCR_PSIZE_8BITS | - DMA_CCR_MSIZE_8BITS); - - /* start receive DMA ready for the first packet */ - stm32_dmastart(rx_dma, dma_callback, NULL, false); + /* configure RX DMA and return to listening state */ + dma_reset(); debug("serial init"); } @@ -146,27 +163,36 @@ interface_tick() } static void -dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) +tx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) { - if (!(status & DMA_STATUS_TCIF)) { - /* XXX what do we do here? it's fatal! */ - return; - } - - /* if this is transmit-complete, make a note */ - if (handle == tx_dma) { - sending = false; - return; - } + sending = false; + rCR3 &= ~USART_CR3_DMAT; +} +static void +rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) +{ /* we just received a request; sort out what to do */ + + rCR3 &= ~USART_CR3_DMAR; + + /* work out how big the packet actually is */ + //unsigned rx_length = sizeof(IOPacket) - stm32_dmaresidual(rx_dma); + /* XXX implement check byte */ - /* XXX if we care about overruns, check the UART received-data-ready bit */ + + perf_count(pc_rx); + + /* default to not sending a reply */ bool send_reply = false; if (dma_packet.count & PKT_CTRL_WRITE) { + dma_packet.count &= ~PKT_CTRL_WRITE; + /* it's a blind write - pass it on */ - registers_set(dma_packet.page, dma_packet.offset, &dma_packet.regs[0], dma_packet.count); + if (registers_set(dma_packet.page, dma_packet.offset, &dma_packet.regs[0], dma_packet.count)) + perf_count(pc_regerr); + } else { /* it's a read - get register pointer for reply */ @@ -189,9 +215,94 @@ dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) } /* re-set DMA for reception first, so we are ready to receive before we start sending */ - stm32_dmastart(rx_dma, dma_callback, NULL, false); + /* XXX latency here could mean loss of back-to-back writes; do we want to always send an ack? */ + /* XXX always sending an ack would simplify the FMU side (always wait for reply) too */ + dma_reset(); /* if we have a reply to send, start that now */ - if (send_reply) - stm32_dmastart(tx_dma, dma_callback, NULL, false); + if (send_reply) { + stm32_dmasetup( + tx_dma, + (uint32_t)&rDR, + (uint32_t)&dma_packet, + sizeof(dma_packet), /* XXX cut back to actual transmit size */ + DMA_CCR_DIR | + DMA_CCR_MINC | + DMA_CCR_PSIZE_8BITS | + DMA_CCR_MSIZE_8BITS); + sending = true; + stm32_dmastart(tx_dma, tx_dma_callback, NULL, false); + rCR3 |= USART_CR3_DMAT; + } } + +static int +serial_interrupt(int irq, void *context) +{ + uint32_t sr = rSR; /* get UART status register */ + + /* handle error/exception conditions */ + if (sr & (USART_SR_ORE | USART_SR_NE | USART_SR_FE | USART_SR_IDLE)) { + /* read DR to clear status */ + (void)rDR; + } else { + ASSERT(0); + } + + if (sr & (USART_SR_ORE | /* overrun error - packet was too big for DMA or DMA was too slow */ + USART_SR_NE | /* noise error - we have lost a byte due to noise */ + USART_SR_FE)) { /* framing error - start/stop bit lost or line break */ + + perf_count(pc_errors); + if (sr & USART_SR_ORE) + perf_count(pc_ore); + if (sr & USART_SR_NE) + perf_count(pc_ne); + if (sr & USART_SR_FE) + perf_count(pc_fe); + + /* reset DMA state machine back to listening-for-packet */ + dma_reset(); + + /* don't attempt to handle IDLE if it's set - things went bad */ + return 0; + } + + if (sr & USART_SR_IDLE) { + + /* XXX if there was DMA transmission still going, this is an error */ + + /* stop the receive DMA */ + stm32_dmastop(rx_dma); + + /* and treat this like DMA completion */ + rx_dma_callback(rx_dma, DMA_STATUS_TCIF, NULL); + } + + return 0; +} + +static void +dma_reset(void) +{ + rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR); + + + /* kill any pending DMA */ + stm32_dmastop(tx_dma); + stm32_dmastop(rx_dma); + + /* reset the RX side */ + stm32_dmasetup( + rx_dma, + (uint32_t)&rDR, + (uint32_t)&dma_packet, + sizeof(dma_packet), + DMA_CCR_MINC | + DMA_CCR_PSIZE_8BITS | + DMA_CCR_MSIZE_8BITS); + + /* start receive DMA ready for the next packet */ + stm32_dmastart(rx_dma, rx_dma_callback, NULL, false); + rCR3 |= USART_CR3_DMAR; +} \ No newline at end of file From 94b638d848a62312a9cd6722779965d211565d3c Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 4 Jul 2013 23:19:24 -0700 Subject: [PATCH 206/486] One more piece of paranoia when resetting DMA --- src/modules/px4iofirmware/serial.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index b51ff87d3e..93cff26c26 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -286,7 +286,9 @@ static void dma_reset(void) { rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR); - + (void)rSR; + (void)rDR; + (void)rDR; /* kill any pending DMA */ stm32_dmastop(tx_dma); From 83213c66df27e41d5941ff21a4249df3f6f5f45e Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 4 Jul 2013 23:22:59 -0700 Subject: [PATCH 207/486] Reset the PX4IO rx DMA if we haven't seen any traffic in a while; this gets us back into sync. --- src/modules/px4iofirmware/serial.c | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index 93cff26c26..4eef99d9f5 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -73,6 +73,9 @@ static DMA_HANDLE rx_dma; static int serial_interrupt(int irq, void *context); static void dma_reset(void); +/* if we spend this many ticks idle, reset the DMA */ +static unsigned idle_ticks; + #define MAX_RW_REGS 32 // by agreement w/FMU #pragma pack(push, 1) @@ -160,6 +163,10 @@ void interface_tick() { /* XXX look for stuck/damaged DMA and reset? */ + if (idle_ticks++ > 100) { + dma_reset(); + idle_ticks = 0; + } } static void @@ -175,6 +182,7 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) /* we just received a request; sort out what to do */ rCR3 &= ~USART_CR3_DMAR; + idle_ticks = 0; /* work out how big the packet actually is */ //unsigned rx_length = sizeof(IOPacket) - stm32_dmaresidual(rx_dma); From d83323d4a28c0beb9686bd28193344b6b3079f00 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 5 Jul 2013 16:18:55 -0700 Subject: [PATCH 208/486] Use the NuttX built-in crc32, it works fine. --- src/drivers/px4io/uploader.cpp | 50 +++------------------------------- 1 file changed, 4 insertions(+), 46 deletions(-) diff --git a/src/drivers/px4io/uploader.cpp b/src/drivers/px4io/uploader.cpp index 67298af322..28c5844380 100644 --- a/src/drivers/px4io/uploader.cpp +++ b/src/drivers/px4io/uploader.cpp @@ -52,6 +52,8 @@ #include #include +#include + #include "uploader.h" #ifdef CONFIG_ARCH_BOARD_PX4FMUV2 @@ -64,50 +66,6 @@ // define for comms logging //#define UDEBUG -static const uint32_t crctab[] = -{ - 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, - 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, - 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, - 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, - 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, - 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, - 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, - 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, - 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, - 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, - 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, - 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, - 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, - 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, - 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, - 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, - 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, - 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, - 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, - 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, - 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, - 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, - 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, - 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, - 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, - 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, - 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, - 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, - 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, - 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, - 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, - 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d -}; - -static uint32_t -crc32(const uint8_t *src, unsigned len, unsigned state) -{ - for (unsigned i = 0; i < len; i++) - state = crctab[(state ^ src[i]) & 0xff] ^ (state >> 8); - return state; -} - PX4IO_Uploader::PX4IO_Uploader() : _io_fd(-1), _fw_fd(-1) @@ -594,14 +552,14 @@ PX4IO_Uploader::verify_rev3(size_t fw_size_local) return -errno; /* calculate crc32 sum */ - sum = crc32((uint8_t *)&file_buf, sizeof(file_buf), sum); + sum = crc32part((uint8_t *)&file_buf, sizeof(file_buf), sum); bytes_read += count; } /* fill the rest with 0xff */ while (bytes_read < fw_size_remote) { - sum = crc32(&fill_blank, sizeof(fill_blank), sum); + sum = crc32part(&fill_blank, sizeof(fill_blank), sum); bytes_read += sizeof(fill_blank); } From e55a37697d56bfbec3bcd1febc9f0e185663f45d Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 5 Jul 2013 16:34:44 -0700 Subject: [PATCH 209/486] Always send and expect a reply for every message. --- src/drivers/px4io/interface_serial.cpp | 56 ++++++++++++-------------- src/modules/px4iofirmware/serial.c | 28 ++++++------- 2 files changed, 37 insertions(+), 47 deletions(-) diff --git a/src/drivers/px4io/interface_serial.cpp b/src/drivers/px4io/interface_serial.cpp index 5d9eac0761..1ce9e9c6d8 100644 --- a/src/drivers/px4io/interface_serial.cpp +++ b/src/drivers/px4io/interface_serial.cpp @@ -167,8 +167,7 @@ private: perf_counter_t _perf_dmasetup; perf_counter_t _perf_timeouts; perf_counter_t _perf_errors; - perf_counter_t _perf_wr; - perf_counter_t _perf_rd; + perf_counter_t _perf_txns; }; @@ -189,8 +188,7 @@ PX4IO_serial::PX4IO_serial() : _perf_dmasetup(perf_alloc(PC_ELAPSED, "dmasetup")), _perf_timeouts(perf_alloc(PC_COUNT, "timeouts")), _perf_errors(perf_alloc(PC_COUNT, "errors ")), - _perf_wr(perf_alloc(PC_ELAPSED, "writes ")), - _perf_rd(perf_alloc(PC_ELAPSED, "reads ")) + _perf_txns(perf_alloc(PC_ELAPSED, "txns ")) { /* allocate DMA */ _tx_dma = stm32_dmachannel(PX4IO_SERIAL_TX_DMAMAP); @@ -305,10 +303,12 @@ PX4IO_serial::test(unsigned mode) return -2; if (count > 100) { perf_print_counter(_perf_dmasetup); - perf_print_counter(_perf_wr); + perf_print_counter(_perf_txns); + perf_print_counter(_perf_timeouts); + perf_print_counter(_perf_errors); count = 0; } - usleep(100000); + usleep(10000); } return 0; } @@ -384,35 +384,29 @@ PX4IO_serial::_wait_complete(bool expect_reply, unsigned tx_length) (void)rDR; /* start RX DMA */ - if (expect_reply) { - perf_begin(_perf_rd); - perf_begin(_perf_dmasetup); + perf_begin(_perf_txns); + perf_begin(_perf_dmasetup); - /* DMA setup time ~3µs */ - _rx_dma_status = _dma_status_waiting; - _rx_length = 0; - stm32_dmasetup( - _rx_dma, - PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET, - reinterpret_cast(&_dma_buffer), - sizeof(_dma_buffer), - DMA_SCR_DIR_P2M | - DMA_SCR_MINC | - DMA_SCR_PSIZE_8BITS | - DMA_SCR_MSIZE_8BITS | - DMA_SCR_PBURST_SINGLE | - DMA_SCR_MBURST_SINGLE); - stm32_dmastart(_rx_dma, _dma_callback, this, false); - rCR3 |= USART_CR3_DMAR; + /* DMA setup time ~3µs */ + _rx_dma_status = _dma_status_waiting; + _rx_length = 0; + stm32_dmasetup( + _rx_dma, + PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET, + reinterpret_cast(&_dma_buffer), + sizeof(_dma_buffer), + DMA_SCR_DIR_P2M | + DMA_SCR_MINC | + DMA_SCR_PSIZE_8BITS | + DMA_SCR_MSIZE_8BITS | + DMA_SCR_PBURST_SINGLE | + DMA_SCR_MBURST_SINGLE); + stm32_dmastart(_rx_dma, _dma_callback, this, false); + rCR3 |= USART_CR3_DMAR; - perf_end(_perf_dmasetup); - } else { - perf_begin(_perf_wr); - } /* start TX DMA - no callback if we also expect a reply */ /* DMA setup time ~3µs */ - perf_begin(_perf_dmasetup); _tx_dma_status = _dma_status_waiting; stm32_dmasetup( _tx_dma, @@ -476,7 +470,7 @@ PX4IO_serial::_wait_complete(bool expect_reply, unsigned tx_length) _rx_dma_status = _dma_status_inactive; /* update counters */ - perf_end(expect_reply ? _perf_rd : _perf_wr); + perf_end(_perf_txns); if (ret != OK) perf_count(_perf_errors); diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index 4eef99d9f5..c109cb57f1 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -192,7 +192,6 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) perf_count(pc_rx); /* default to not sending a reply */ - bool send_reply = false; if (dma_packet.count & PKT_CTRL_WRITE) { dma_packet.count &= ~PKT_CTRL_WRITE; @@ -217,7 +216,6 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) count = MAX_RW_REGS; /* copy reply registers into DMA buffer */ - send_reply = true; memcpy((void *)&dma_packet.regs[0], registers, count); dma_packet.count = count; } @@ -228,20 +226,18 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) dma_reset(); /* if we have a reply to send, start that now */ - if (send_reply) { - stm32_dmasetup( - tx_dma, - (uint32_t)&rDR, - (uint32_t)&dma_packet, - sizeof(dma_packet), /* XXX cut back to actual transmit size */ - DMA_CCR_DIR | - DMA_CCR_MINC | - DMA_CCR_PSIZE_8BITS | - DMA_CCR_MSIZE_8BITS); - sending = true; - stm32_dmastart(tx_dma, tx_dma_callback, NULL, false); - rCR3 |= USART_CR3_DMAT; - } + stm32_dmasetup( + tx_dma, + (uint32_t)&rDR, + (uint32_t)&dma_packet, + sizeof(dma_packet), /* XXX cut back to actual transmit size */ + DMA_CCR_DIR | + DMA_CCR_MINC | + DMA_CCR_PSIZE_8BITS | + DMA_CCR_MSIZE_8BITS); + sending = true; + stm32_dmastart(tx_dma, tx_dma_callback, NULL, false); + rCR3 |= USART_CR3_DMAT; } static int From 313231566c936927eef1fd4a8fc7012122342941 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 5 Jul 2013 16:41:27 -0700 Subject: [PATCH 210/486] Encode the packet type and result in the unused high bits of the word count. --- src/drivers/px4io/interface_serial.cpp | 26 ++++++++++++++++++-------- src/modules/px4iofirmware/serial.c | 26 +++++++++++++++++++------- 2 files changed, 37 insertions(+), 15 deletions(-) diff --git a/src/drivers/px4io/interface_serial.cpp b/src/drivers/px4io/interface_serial.cpp index 1ce9e9c6d8..ffd852cf08 100644 --- a/src/drivers/px4io/interface_serial.cpp +++ b/src/drivers/px4io/interface_serial.cpp @@ -73,8 +73,7 @@ const unsigned max_rw_regs = 32; // by agreement w/IO #pragma pack(push, 1) struct IOPacket { - uint8_t count; -#define PKT_CTRL_WRITE (1<<7) + uint8_t count_code; uint8_t spare; uint8_t page; uint8_t offset; @@ -92,7 +91,18 @@ struct IOPacket { #define rCR3 REG(STM32_USART_CR3_OFFSET) #define rGTPR REG(STM32_USART_GTPR_OFFSET) -#define PKT_SIZE(_n) (4 + (2 * (_n))) +#define PKT_CODE_READ 0x00 /* FMU->IO read transaction */ +#define PKT_CODE_WRITE 0x40 /* FMU->IO write transaction */ +#define PKT_CODE_SUCCESS 0x00 /* IO->FMU success reply */ +#define PKT_CODE_CORRUPT 0x40 /* IO->FMU bad packet reply */ +#define PKT_CODE_ERROR 0x80 /* IO->FMU register op error reply */ + +#define PKT_CODE_MASK 0xc0 +#define PKT_COUNT_MASK 0x3f + +#define PKT_COUNT(_p) ((_p).count_code & PKT_COUNT_MASK) +#define PKT_CODE(_p) ((_p).count_code & PKT_CODE_MASK) +#define PKT_SIZE(_p) ((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p))) class PX4IO_serial : public PX4IO_interface { @@ -327,7 +337,7 @@ PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsi sem_wait(&_bus_semaphore); - _dma_buffer.count = num_values | PKT_CTRL_WRITE; + _dma_buffer.count_code = num_values | PKT_CODE_WRITE; _dma_buffer.spare = 0; _dma_buffer.page = page; _dma_buffer.offset = offset; @@ -338,7 +348,7 @@ PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsi /* XXX implement check byte */ /* start the transaction and wait for it to complete */ - int result = _wait_complete(false, PKT_SIZE(num_values)); + int result = _wait_complete(false, PKT_SIZE(_dma_buffer)); sem_post(&_bus_semaphore); return result; @@ -352,18 +362,18 @@ PX4IO_serial::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned n sem_wait(&_bus_semaphore); - _dma_buffer.count = num_values; + _dma_buffer.count_code = num_values | PKT_CODE_READ; _dma_buffer.spare = 0; _dma_buffer.page = page; _dma_buffer.offset = offset; /* start the transaction and wait for it to complete */ - int result = _wait_complete(true, PKT_SIZE(0)); + int result = _wait_complete(true, PKT_SIZE(_dma_buffer)); if (result != OK) goto out; /* compare the received count with the expected count */ - if (_dma_buffer.count != num_values) { + if (PKT_COUNT(_dma_buffer) != num_values) { return -EIO; } else { /* XXX implement check byte */ diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index c109cb57f1..64ca6195a7 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -80,8 +80,7 @@ static unsigned idle_ticks; #pragma pack(push, 1) struct IOPacket { - uint8_t count; -#define PKT_CTRL_WRITE (1<<7) + uint8_t count_code; uint8_t spare; uint8_t page; uint8_t offset; @@ -89,6 +88,21 @@ struct IOPacket { }; #pragma pack(pop) +#define PKT_CODE_READ 0x00 /* FMU->IO read transaction */ +#define PKT_CODE_WRITE 0x40 /* FMU->IO write transaction */ +#define PKT_CODE_SUCCESS 0x00 /* IO->FMU success reply */ +#define PKT_CODE_CORRUPT 0x40 /* IO->FMU bad packet reply */ +#define PKT_CODE_ERROR 0x80 /* IO->FMU register op error reply */ + +#define PKT_CODE_MASK 0xc0 +#define PKT_COUNT_MASK 0x3f + +#define PKT_COUNT(_p) ((_p).count_code & PKT_COUNT_MASK) +#define PKT_CODE(_p) ((_p).count_code & PKT_CODE_MASK) +#define PKT_SIZE(_p) ((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p))) + +//static uint8_t crc_packet(void); + static struct IOPacket dma_packet; /* serial register accessors */ @@ -192,12 +206,10 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) perf_count(pc_rx); /* default to not sending a reply */ - if (dma_packet.count & PKT_CTRL_WRITE) { - - dma_packet.count &= ~PKT_CTRL_WRITE; + if (PKT_CODE(dma_packet) == PKT_CODE_WRITE) { /* it's a blind write - pass it on */ - if (registers_set(dma_packet.page, dma_packet.offset, &dma_packet.regs[0], dma_packet.count)) + if (registers_set(dma_packet.page, dma_packet.offset, &dma_packet.regs[0], PKT_COUNT(dma_packet))) perf_count(pc_regerr); } else { @@ -217,7 +229,7 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) /* copy reply registers into DMA buffer */ memcpy((void *)&dma_packet.regs[0], registers, count); - dma_packet.count = count; + dma_packet.count_code = count | PKT_CODE_SUCCESS; } /* re-set DMA for reception first, so we are ready to receive before we start sending */ From 5a8f874166e92ccb1d3a108164f403f622787a89 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 5 Jul 2013 16:56:47 -0700 Subject: [PATCH 211/486] Add an 8-bit CRC to each transmitted packet. --- src/drivers/px4io/interface_serial.cpp | 60 ++++++++++++++++++++++++-- src/modules/px4iofirmware/serial.c | 57 ++++++++++++++++++++++-- 2 files changed, 110 insertions(+), 7 deletions(-) diff --git a/src/drivers/px4io/interface_serial.cpp b/src/drivers/px4io/interface_serial.cpp index ffd852cf08..e688d672ce 100644 --- a/src/drivers/px4io/interface_serial.cpp +++ b/src/drivers/px4io/interface_serial.cpp @@ -74,7 +74,7 @@ const unsigned max_rw_regs = 32; // by agreement w/IO #pragma pack(push, 1) struct IOPacket { uint8_t count_code; - uint8_t spare; + uint8_t crc; uint8_t page; uint8_t offset; uint16_t regs[max_rw_regs]; @@ -104,6 +104,9 @@ struct IOPacket { #define PKT_CODE(_p) ((_p).count_code & PKT_CODE_MASK) #define PKT_SIZE(_p) ((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p))) +static uint8_t crc_packet(IOPacket &pkt); + + class PX4IO_serial : public PX4IO_interface { public: @@ -338,7 +341,6 @@ PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsi sem_wait(&_bus_semaphore); _dma_buffer.count_code = num_values | PKT_CODE_WRITE; - _dma_buffer.spare = 0; _dma_buffer.page = page; _dma_buffer.offset = offset; memcpy((void *)&_dma_buffer.regs[0], (void *)values, (2 * num_values)); @@ -363,7 +365,6 @@ PX4IO_serial::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned n sem_wait(&_bus_semaphore); _dma_buffer.count_code = num_values | PKT_CODE_READ; - _dma_buffer.spare = 0; _dma_buffer.page = page; _dma_buffer.offset = offset; @@ -417,6 +418,8 @@ PX4IO_serial::_wait_complete(bool expect_reply, unsigned tx_length) /* start TX DMA - no callback if we also expect a reply */ /* DMA setup time ~3µs */ + _dma_buffer.crc = 0; + _dma_buffer.crc = crc_packet(_dma_buffer); _tx_dma_status = _dma_status_waiting; stm32_dmasetup( _tx_dma, @@ -629,4 +632,53 @@ PX4IO_serial::_abort_dma() _tx_dma_status = _dma_status_inactive; _rx_dma_status = _dma_status_inactive; -} \ No newline at end of file +} + +static const uint8_t crc8_tab[256] = +{ + 0x00, 0x07, 0x0E, 0x09, 0x1C, 0x1B, 0x12, 0x15, + 0x38, 0x3F, 0x36, 0x31, 0x24, 0x23, 0x2A, 0x2D, + 0x70, 0x77, 0x7E, 0x79, 0x6C, 0x6B, 0x62, 0x65, + 0x48, 0x4F, 0x46, 0x41, 0x54, 0x53, 0x5A, 0x5D, + 0xE0, 0xE7, 0xEE, 0xE9, 0xFC, 0xFB, 0xF2, 0xF5, + 0xD8, 0xDF, 0xD6, 0xD1, 0xC4, 0xC3, 0xCA, 0xCD, + 0x90, 0x97, 0x9E, 0x99, 0x8C, 0x8B, 0x82, 0x85, + 0xA8, 0xAF, 0xA6, 0xA1, 0xB4, 0xB3, 0xBA, 0xBD, + 0xC7, 0xC0, 0xC9, 0xCE, 0xDB, 0xDC, 0xD5, 0xD2, + 0xFF, 0xF8, 0xF1, 0xF6, 0xE3, 0xE4, 0xED, 0xEA, + 0xB7, 0xB0, 0xB9, 0xBE, 0xAB, 0xAC, 0xA5, 0xA2, + 0x8F, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9D, 0x9A, + 0x27, 0x20, 0x29, 0x2E, 0x3B, 0x3C, 0x35, 0x32, + 0x1F, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0D, 0x0A, + 0x57, 0x50, 0x59, 0x5E, 0x4B, 0x4C, 0x45, 0x42, + 0x6F, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7D, 0x7A, + 0x89, 0x8E, 0x87, 0x80, 0x95, 0x92, 0x9B, 0x9C, + 0xB1, 0xB6, 0xBF, 0xB8, 0xAD, 0xAA, 0xA3, 0xA4, + 0xF9, 0xFE, 0xF7, 0xF0, 0xE5, 0xE2, 0xEB, 0xEC, + 0xC1, 0xC6, 0xCF, 0xC8, 0xDD, 0xDA, 0xD3, 0xD4, + 0x69, 0x6E, 0x67, 0x60, 0x75, 0x72, 0x7B, 0x7C, + 0x51, 0x56, 0x5F, 0x58, 0x4D, 0x4A, 0x43, 0x44, + 0x19, 0x1E, 0x17, 0x10, 0x05, 0x02, 0x0B, 0x0C, + 0x21, 0x26, 0x2F, 0x28, 0x3D, 0x3A, 0x33, 0x34, + 0x4E, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5C, 0x5B, + 0x76, 0x71, 0x78, 0x7F, 0x6A, 0x6D, 0x64, 0x63, + 0x3E, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2C, 0x2B, + 0x06, 0x01, 0x08, 0x0F, 0x1A, 0x1D, 0x14, 0x13, + 0xAE, 0xA9, 0xA0, 0xA7, 0xB2, 0xB5, 0xBC, 0xBB, + 0x96, 0x91, 0x98, 0x9F, 0x8A, 0x8D, 0x84, 0x83, + 0xDE, 0xD9, 0xD0, 0xD7, 0xC2, 0xC5, 0xCC, 0xCB, + 0xE6, 0xE1, 0xE8, 0xEF, 0xFA, 0xFD, 0xF4, 0xF3 +}; + +static uint8_t +crc_packet(IOPacket &pkt) +{ + uint8_t *end = (uint8_t *)(&pkt.regs[PKT_COUNT(pkt)]); + uint8_t *p = (uint8_t *)&pkt; + uint8_t c = 0; + + while (p < end) + c = crc8_tab[c ^ *(p++)]; + + return c; +} diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index 64ca6195a7..722d058093 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -81,7 +81,7 @@ static unsigned idle_ticks; #pragma pack(push, 1) struct IOPacket { uint8_t count_code; - uint8_t spare; + uint8_t crc; uint8_t page; uint8_t offset; uint16_t regs[MAX_RW_REGS]; @@ -101,7 +101,7 @@ struct IOPacket { #define PKT_CODE(_p) ((_p).count_code & PKT_CODE_MASK) #define PKT_SIZE(_p) ((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p))) -//static uint8_t crc_packet(void); +static uint8_t crc_packet(void); static struct IOPacket dma_packet; @@ -238,6 +238,8 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) dma_reset(); /* if we have a reply to send, start that now */ + dma_packet.crc = 0; + dma_packet.crc = crc_packet(); stm32_dmasetup( tx_dma, (uint32_t)&rDR, @@ -323,4 +325,53 @@ dma_reset(void) /* start receive DMA ready for the next packet */ stm32_dmastart(rx_dma, rx_dma_callback, NULL, false); rCR3 |= USART_CR3_DMAR; -} \ No newline at end of file +} + +static const uint8_t crc8_tab[256] = +{ + 0x00, 0x07, 0x0E, 0x09, 0x1C, 0x1B, 0x12, 0x15, + 0x38, 0x3F, 0x36, 0x31, 0x24, 0x23, 0x2A, 0x2D, + 0x70, 0x77, 0x7E, 0x79, 0x6C, 0x6B, 0x62, 0x65, + 0x48, 0x4F, 0x46, 0x41, 0x54, 0x53, 0x5A, 0x5D, + 0xE0, 0xE7, 0xEE, 0xE9, 0xFC, 0xFB, 0xF2, 0xF5, + 0xD8, 0xDF, 0xD6, 0xD1, 0xC4, 0xC3, 0xCA, 0xCD, + 0x90, 0x97, 0x9E, 0x99, 0x8C, 0x8B, 0x82, 0x85, + 0xA8, 0xAF, 0xA6, 0xA1, 0xB4, 0xB3, 0xBA, 0xBD, + 0xC7, 0xC0, 0xC9, 0xCE, 0xDB, 0xDC, 0xD5, 0xD2, + 0xFF, 0xF8, 0xF1, 0xF6, 0xE3, 0xE4, 0xED, 0xEA, + 0xB7, 0xB0, 0xB9, 0xBE, 0xAB, 0xAC, 0xA5, 0xA2, + 0x8F, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9D, 0x9A, + 0x27, 0x20, 0x29, 0x2E, 0x3B, 0x3C, 0x35, 0x32, + 0x1F, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0D, 0x0A, + 0x57, 0x50, 0x59, 0x5E, 0x4B, 0x4C, 0x45, 0x42, + 0x6F, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7D, 0x7A, + 0x89, 0x8E, 0x87, 0x80, 0x95, 0x92, 0x9B, 0x9C, + 0xB1, 0xB6, 0xBF, 0xB8, 0xAD, 0xAA, 0xA3, 0xA4, + 0xF9, 0xFE, 0xF7, 0xF0, 0xE5, 0xE2, 0xEB, 0xEC, + 0xC1, 0xC6, 0xCF, 0xC8, 0xDD, 0xDA, 0xD3, 0xD4, + 0x69, 0x6E, 0x67, 0x60, 0x75, 0x72, 0x7B, 0x7C, + 0x51, 0x56, 0x5F, 0x58, 0x4D, 0x4A, 0x43, 0x44, + 0x19, 0x1E, 0x17, 0x10, 0x05, 0x02, 0x0B, 0x0C, + 0x21, 0x26, 0x2F, 0x28, 0x3D, 0x3A, 0x33, 0x34, + 0x4E, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5C, 0x5B, + 0x76, 0x71, 0x78, 0x7F, 0x6A, 0x6D, 0x64, 0x63, + 0x3E, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2C, 0x2B, + 0x06, 0x01, 0x08, 0x0F, 0x1A, 0x1D, 0x14, 0x13, + 0xAE, 0xA9, 0xA0, 0xA7, 0xB2, 0xB5, 0xBC, 0xBB, + 0x96, 0x91, 0x98, 0x9F, 0x8A, 0x8D, 0x84, 0x83, + 0xDE, 0xD9, 0xD0, 0xD7, 0xC2, 0xC5, 0xCC, 0xCB, + 0xE6, 0xE1, 0xE8, 0xEF, 0xFA, 0xFD, 0xF4, 0xF3 +}; + +static uint8_t +crc_packet() +{ + uint8_t *end = (uint8_t *)(&dma_packet.regs[PKT_COUNT(dma_packet)]); + uint8_t *p = (uint8_t *)&dma_packet; + uint8_t c = 0; + + while (p < end) + c = crc8_tab[c ^ *(p++)]; + + return c; +} From 50cae347b41b66d236c26ea0dfdeb99b65824ebb Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 5 Jul 2013 17:13:54 -0700 Subject: [PATCH 212/486] Check packet CRCs and count errors; don't reject packets yet. --- src/drivers/px4io/interface_serial.cpp | 13 ++++++++++++- src/modules/px4iofirmware/serial.c | 7 +++++++ 2 files changed, 19 insertions(+), 1 deletion(-) diff --git a/src/drivers/px4io/interface_serial.cpp b/src/drivers/px4io/interface_serial.cpp index e688d672ce..814f66ea48 100644 --- a/src/drivers/px4io/interface_serial.cpp +++ b/src/drivers/px4io/interface_serial.cpp @@ -181,6 +181,7 @@ private: perf_counter_t _perf_timeouts; perf_counter_t _perf_errors; perf_counter_t _perf_txns; + perf_counter_t _perf_crcerrs; }; @@ -201,7 +202,8 @@ PX4IO_serial::PX4IO_serial() : _perf_dmasetup(perf_alloc(PC_ELAPSED, "dmasetup")), _perf_timeouts(perf_alloc(PC_COUNT, "timeouts")), _perf_errors(perf_alloc(PC_COUNT, "errors ")), - _perf_txns(perf_alloc(PC_ELAPSED, "txns ")) + _perf_txns(perf_alloc(PC_ELAPSED, "txns ")), + _perf_crcerrs(perf_alloc(PC_COUNT, "crcerrs ")) { /* allocate DMA */ _tx_dma = stm32_dmachannel(PX4IO_SERIAL_TX_DMAMAP); @@ -319,6 +321,7 @@ PX4IO_serial::test(unsigned mode) perf_print_counter(_perf_txns); perf_print_counter(_perf_timeouts); perf_print_counter(_perf_errors); + perf_print_counter(_perf_crcerrs); count = 0; } usleep(10000); @@ -460,11 +463,19 @@ PX4IO_serial::_wait_complete(bool expect_reply, unsigned tx_length) if (_tx_dma_status & DMA_STATUS_TEIF) { lowsyslog("DMA transmit error\n"); ret = -1; + break; } if (_rx_dma_status & DMA_STATUS_TEIF) { lowsyslog("DMA receive error\n"); ret = -1; + break; } + + /* check packet CRC */ + uint8_t crc = _dma_buffer.crc; + _dma_buffer.crc = 0; + if (crc != crc_packet(_dma_buffer)) + perf_count(_perf_crcerrs); break; } diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index 722d058093..6c6a9a2b18 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -64,6 +64,7 @@ static perf_counter_t pc_ore; static perf_counter_t pc_fe; static perf_counter_t pc_ne; static perf_counter_t pc_regerr; +static perf_counter_t pc_crcerr; static void rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg); static void tx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg); @@ -124,6 +125,7 @@ interface_init(void) pc_fe = perf_alloc(PC_COUNT, "framing"); pc_ne = perf_alloc(PC_COUNT, "noise"); pc_regerr = perf_alloc(PC_COUNT, "regerr"); + pc_crcerr = perf_alloc(PC_COUNT, "crcerr"); /* allocate DMA */ tx_dma = stm32_dmachannel(PX4FMU_SERIAL_TX_DMA); @@ -205,6 +207,11 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) perf_count(pc_rx); + uint8_t crc = dma_packet.crc; + dma_packet.crc = 0; + if (crc != crc_packet()) + perf_count(pc_crcerr); + /* default to not sending a reply */ if (PKT_CODE(dma_packet) == PKT_CODE_WRITE) { From 6c5a15da9b611d52a70af0ffd3d97bf757b974f5 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 5 Jul 2013 17:39:28 -0700 Subject: [PATCH 213/486] Eliminate the TD DMA callback; we don't need to know that it's completed. Fix abort behaviour on timeouts, now we don't wedge after the first one. --- src/drivers/px4io/interface_serial.cpp | 75 +++++++------------------- 1 file changed, 18 insertions(+), 57 deletions(-) diff --git a/src/drivers/px4io/interface_serial.cpp b/src/drivers/px4io/interface_serial.cpp index 814f66ea48..1af64379ba 100644 --- a/src/drivers/px4io/interface_serial.cpp +++ b/src/drivers/px4io/interface_serial.cpp @@ -140,7 +140,6 @@ private: /** saved DMA status */ static const unsigned _dma_status_inactive = 0x80000000; // low bits overlap DMA_STATUS_* values static const unsigned _dma_status_waiting = 0x00000000; - volatile unsigned _tx_dma_status; volatile unsigned _rx_dma_status; /** bus-ownership lock */ @@ -151,16 +150,13 @@ private: /** * Start the transaction with IO and wait for it to complete. - * - * @param expect_reply If true, expect a reply from IO. */ - int _wait_complete(bool expect_reply, unsigned tx_length); + int _wait_complete(); /** * DMA completion handler. */ static void _dma_callback(DMA_HANDLE handle, uint8_t status, void *arg); - void _do_tx_dma_callback(unsigned status); void _do_rx_dma_callback(unsigned status); /** @@ -197,7 +193,6 @@ PX4IO_serial::PX4IO_serial() : _tx_dma(nullptr), _rx_dma(nullptr), _rx_length(0), - _tx_dma_status(_dma_status_inactive), _rx_dma_status(_dma_status_inactive), _perf_dmasetup(perf_alloc(PC_ELAPSED, "dmasetup")), _perf_timeouts(perf_alloc(PC_COUNT, "timeouts")), @@ -314,8 +309,9 @@ PX4IO_serial::test(unsigned mode) for (unsigned count = 0;; count++) { uint16_t value = count & 0xffff; - if (set_reg(PX4IO_PAGE_TEST, PX4IO_P_TEST_LED, &value, 1) != OK) - return -2; + set_reg(PX4IO_PAGE_TEST, PX4IO_P_TEST_LED, &value, 1); + /* ignore errors */ + if (count > 100) { perf_print_counter(_perf_dmasetup); perf_print_counter(_perf_txns); @@ -353,7 +349,7 @@ PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsi /* XXX implement check byte */ /* start the transaction and wait for it to complete */ - int result = _wait_complete(false, PKT_SIZE(_dma_buffer)); + int result = _wait_complete(); sem_post(&_bus_semaphore); return result; @@ -372,13 +368,14 @@ PX4IO_serial::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned n _dma_buffer.offset = offset; /* start the transaction and wait for it to complete */ - int result = _wait_complete(true, PKT_SIZE(_dma_buffer)); + int result = _wait_complete(); if (result != OK) goto out; /* compare the received count with the expected count */ if (PKT_COUNT(_dma_buffer) != num_values) { - return -EIO; + result = -EIO; + goto out; } else { /* XXX implement check byte */ /* copy back the result */ @@ -386,13 +383,12 @@ PX4IO_serial::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned n } out: sem_post(&_bus_semaphore); - return OK; + return result; } int -PX4IO_serial::_wait_complete(bool expect_reply, unsigned tx_length) +PX4IO_serial::_wait_complete() { - /* clear any lingering error status */ (void)rSR; (void)rDR; @@ -418,12 +414,10 @@ PX4IO_serial::_wait_complete(bool expect_reply, unsigned tx_length) stm32_dmastart(_rx_dma, _dma_callback, this, false); rCR3 |= USART_CR3_DMAR; - /* start TX DMA - no callback if we also expect a reply */ /* DMA setup time ~3µs */ _dma_buffer.crc = 0; _dma_buffer.crc = crc_packet(_dma_buffer); - _tx_dma_status = _dma_status_waiting; stm32_dmasetup( _tx_dma, PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET, @@ -435,7 +429,7 @@ PX4IO_serial::_wait_complete(bool expect_reply, unsigned tx_length) DMA_SCR_MSIZE_8BITS | DMA_SCR_PBURST_SINGLE | DMA_SCR_MBURST_SINGLE); - stm32_dmastart(_tx_dma, /*expect_reply ? nullptr :*/ _dma_callback, this, false); + stm32_dmastart(_tx_dma, nullptr, nullptr, false); rCR3 |= USART_CR3_DMAT; perf_end(_perf_dmasetup); @@ -460,11 +454,6 @@ PX4IO_serial::_wait_complete(bool expect_reply, unsigned tx_length) if (ret == OK) { /* check for DMA errors */ - if (_tx_dma_status & DMA_STATUS_TEIF) { - lowsyslog("DMA transmit error\n"); - ret = -1; - break; - } if (_rx_dma_status & DMA_STATUS_TEIF) { lowsyslog("DMA receive error\n"); ret = -1; @@ -490,7 +479,6 @@ PX4IO_serial::_wait_complete(bool expect_reply, unsigned tx_length) } /* reset DMA status */ - _tx_dma_status = _dma_status_inactive; _rx_dma_status = _dma_status_inactive; /* update counters */ @@ -507,37 +495,13 @@ PX4IO_serial::_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) if (arg != nullptr) { PX4IO_serial *ps = reinterpret_cast(arg); - if (handle == ps->_tx_dma) { - ps->_do_tx_dma_callback(status); - } else if (handle == ps->_rx_dma) { - ps->_do_rx_dma_callback(status); - } - } -} - -void -PX4IO_serial::_do_tx_dma_callback(unsigned status) -{ - /* on completion of a no-reply transmit, wake the sender */ - if (_tx_dma_status == _dma_status_waiting) { - - /* save TX status */ - _tx_dma_status = status; - - /* disable UART DMA */ - rCR3 &= ~USART_CR3_DMAT; - - /* if we aren't going on to expect a reply, complete now */ - if (_rx_dma_status != _dma_status_waiting) - sem_post(&_completion_semaphore); + ps->_do_rx_dma_callback(status); } } void PX4IO_serial::_do_rx_dma_callback(unsigned status) { - ASSERT(_tx_dma_status != _dma_status_waiting); - /* on completion of a reply, wake the waiter */ if (_rx_dma_status == _dma_status_waiting) { @@ -552,7 +516,7 @@ PX4IO_serial::_do_rx_dma_callback(unsigned status) _rx_dma_status = status; /* disable UART DMA */ - rCR3 &= ~USART_CR3_DMAR; + rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR); /* DMA may have stopped short */ _rx_length = sizeof(IOPacket) - stm32_dmaresidual(_rx_dma); @@ -593,6 +557,10 @@ PX4IO_serial::_do_interrupt() */ if (_rx_dma_status == _dma_status_waiting) { _abort_dma(); + + /* complete DMA as though in error */ + _do_rx_dma_callback(DMA_STATUS_TEIF); + return; } @@ -605,7 +573,7 @@ PX4IO_serial::_do_interrupt() if (sr & USART_SR_IDLE) { /* if there was DMA transmission still going, this is an error */ - if (_tx_dma_status == _dma_status_waiting) { + if (stm32_dmaresidual(_tx_dma) != 0) { /* babble from IO */ _abort_dma(); @@ -636,13 +604,6 @@ PX4IO_serial::_abort_dma() /* stop DMA */ stm32_dmastop(_tx_dma); stm32_dmastop(_rx_dma); - - /* complete DMA as though in error */ - _do_tx_dma_callback(DMA_STATUS_TEIF); - _do_rx_dma_callback(DMA_STATUS_TEIF); - - _tx_dma_status = _dma_status_inactive; - _rx_dma_status = _dma_status_inactive; } static const uint8_t crc8_tab[256] = From 1f7f7862ceee827c1c3a6087defb8cb52db3f4c9 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 5 Jul 2013 17:53:55 -0700 Subject: [PATCH 214/486] Fix the USART6 default baudrate to match the IO bootloader. --- nuttx/configs/px4fmuv2/nsh/defconfig | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/nuttx/configs/px4fmuv2/nsh/defconfig b/nuttx/configs/px4fmuv2/nsh/defconfig index 16be7cd411..17803c4d72 100755 --- a/nuttx/configs/px4fmuv2/nsh/defconfig +++ b/nuttx/configs/px4fmuv2/nsh/defconfig @@ -188,6 +188,7 @@ CONFIG_STM32_PWR=y CONFIG_STM32_TIM1=y CONFIG_STM32_TIM8=n CONFIG_STM32_USART1=y +# Mostly owned by the px4io driver, but uploader needs this CONFIG_STM32_USART6=y # We use our own driver, but leave this on. CONFIG_STM32_ADC1=y @@ -260,7 +261,7 @@ CONFIG_USART1_BAUD=115200 CONFIG_USART2_BAUD=115200 CONFIG_USART3_BAUD=115200 CONFIG_UART4_BAUD=115200 -CONFIG_USART6_BAUD=9600 +CONFIG_USART6_BAUD=115200 CONFIG_UART7_BAUD=115200 CONFIG_UART8_BAUD=57600 From 46a4a443210b73be01da5d63f9cef955658347ee Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 5 Jul 2013 18:36:00 -0700 Subject: [PATCH 215/486] Be more consistent with the packet format definition. Free perf counters in ~PX4IO_serial --- src/drivers/px4io/interface_serial.cpp | 18 ++++++++++++------ src/modules/px4iofirmware/serial.c | 10 +++++----- 2 files changed, 17 insertions(+), 11 deletions(-) diff --git a/src/drivers/px4io/interface_serial.cpp b/src/drivers/px4io/interface_serial.cpp index 1af64379ba..a603f87d63 100644 --- a/src/drivers/px4io/interface_serial.cpp +++ b/src/drivers/px4io/interface_serial.cpp @@ -69,7 +69,7 @@ #include "interface.h" -const unsigned max_rw_regs = 32; // by agreement w/IO +#define PKT_MAX_REGS 32 // by agreement w/IO #pragma pack(push, 1) struct IOPacket { @@ -77,7 +77,7 @@ struct IOPacket { uint8_t crc; uint8_t page; uint8_t offset; - uint16_t regs[max_rw_regs]; + uint16_t regs[PKT_MAX_REGS]; }; #pragma pack(pop) @@ -268,6 +268,12 @@ PX4IO_serial::~PX4IO_serial() sem_destroy(&_completion_semaphore); sem_destroy(&_bus_semaphore); + perf_free(_perf_dmasetup); + perf_free(_perf_timeouts); + perf_free(_perf_errors); + perf_free(_perf_txns); + perf_free(_perf_crcerrs); + if (g_interface == this) g_interface = nullptr; } @@ -334,7 +340,7 @@ PX4IO_serial::test(unsigned mode) int PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { - if (num_values > max_rw_regs) + if (num_values > PKT_MAX_REGS) return -EINVAL; sem_wait(&_bus_semaphore); @@ -343,7 +349,7 @@ PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsi _dma_buffer.page = page; _dma_buffer.offset = offset; memcpy((void *)&_dma_buffer.regs[0], (void *)values, (2 * num_values)); - for (unsigned i = num_values; i < max_rw_regs; i++) + for (unsigned i = num_values; i < PKT_MAX_REGS; i++) _dma_buffer.regs[i] = 0x55aa; /* XXX implement check byte */ @@ -358,7 +364,7 @@ PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsi int PX4IO_serial::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) { - if (num_values > max_rw_regs) + if (num_values > PKT_MAX_REGS) return -EINVAL; sem_wait(&_bus_semaphore); @@ -422,7 +428,7 @@ PX4IO_serial::_wait_complete() _tx_dma, PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET, reinterpret_cast(&_dma_buffer), - sizeof(_dma_buffer), /* XXX should be tx_length */ + sizeof(_dma_buffer), /* XXX should be PKT_LENGTH() */ DMA_SCR_DIR_M2P | DMA_SCR_MINC | DMA_SCR_PSIZE_8BITS | diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index 6c6a9a2b18..38cfd3ccf5 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -77,7 +77,7 @@ static void dma_reset(void); /* if we spend this many ticks idle, reset the DMA */ static unsigned idle_ticks; -#define MAX_RW_REGS 32 // by agreement w/FMU +#define PKT_MAX_REGS 32 // by agreement w/FMU #pragma pack(push, 1) struct IOPacket { @@ -85,7 +85,7 @@ struct IOPacket { uint8_t crc; uint8_t page; uint8_t offset; - uint16_t regs[MAX_RW_REGS]; + uint16_t regs[PKT_MAX_REGS]; }; #pragma pack(pop) @@ -231,8 +231,8 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) count = 0; /* constrain reply to packet size */ - if (count > MAX_RW_REGS) - count = MAX_RW_REGS; + if (count > PKT_MAX_REGS) + count = PKT_MAX_REGS; /* copy reply registers into DMA buffer */ memcpy((void *)&dma_packet.regs[0], registers, count); @@ -251,7 +251,7 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) tx_dma, (uint32_t)&rDR, (uint32_t)&dma_packet, - sizeof(dma_packet), /* XXX cut back to actual transmit size */ + sizeof(dma_packet), /* XXX should be PKT_LENGTH() */ DMA_CCR_DIR | DMA_CCR_MINC | DMA_CCR_PSIZE_8BITS | From 10e673aa4b16a7b50656962b4ead7fa87fa94d59 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 5 Jul 2013 19:02:42 -0700 Subject: [PATCH 216/486] Send error response if register write fails. --- src/modules/px4iofirmware/serial.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index 38cfd3ccf5..e170d5bdf8 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -216,8 +216,12 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) if (PKT_CODE(dma_packet) == PKT_CODE_WRITE) { /* it's a blind write - pass it on */ - if (registers_set(dma_packet.page, dma_packet.offset, &dma_packet.regs[0], PKT_COUNT(dma_packet))) + if (registers_set(dma_packet.page, dma_packet.offset, &dma_packet.regs[0], PKT_COUNT(dma_packet))) { perf_count(pc_regerr); + dma_packet.count_code = PKT_CODE_ERROR; + } else { + dma_packet.count_code = PKT_CODE_SUCCESS; + } } else { From 87c3d1a8c14e9d97bb98d8255c1ba35e875b6c81 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 5 Jul 2013 19:03:08 -0700 Subject: [PATCH 217/486] More link performance counters. --- src/drivers/px4io/interface_serial.cpp | 48 ++++++++++++++++++-------- 1 file changed, 33 insertions(+), 15 deletions(-) diff --git a/src/drivers/px4io/interface_serial.cpp b/src/drivers/px4io/interface_serial.cpp index a603f87d63..2ba251b5fc 100644 --- a/src/drivers/px4io/interface_serial.cpp +++ b/src/drivers/px4io/interface_serial.cpp @@ -175,9 +175,10 @@ private: */ perf_counter_t _perf_dmasetup; perf_counter_t _perf_timeouts; - perf_counter_t _perf_errors; - perf_counter_t _perf_txns; perf_counter_t _perf_crcerrs; + perf_counter_t _perf_dmaerrs; + perf_counter_t _perf_protoerrs; + perf_counter_t _perf_txns; }; @@ -194,11 +195,12 @@ PX4IO_serial::PX4IO_serial() : _rx_dma(nullptr), _rx_length(0), _rx_dma_status(_dma_status_inactive), - _perf_dmasetup(perf_alloc(PC_ELAPSED, "dmasetup")), - _perf_timeouts(perf_alloc(PC_COUNT, "timeouts")), - _perf_errors(perf_alloc(PC_COUNT, "errors ")), - _perf_txns(perf_alloc(PC_ELAPSED, "txns ")), - _perf_crcerrs(perf_alloc(PC_COUNT, "crcerrs ")) + _perf_dmasetup(perf_alloc(PC_ELAPSED, "dmasetup ")), + _perf_timeouts(perf_alloc(PC_COUNT, "timeouts ")), + _perf_crcerrs(perf_alloc(PC_COUNT, "crcerrs ")), + _perf_dmaerrs(perf_alloc(PC_COUNT, "dmaerrs ")), + _perf_protoerrs(perf_alloc(PC_COUNT, "protoerrs")), + _perf_txns(perf_alloc(PC_ELAPSED, "txns ")) { /* allocate DMA */ _tx_dma = stm32_dmachannel(PX4IO_SERIAL_TX_DMAMAP); @@ -270,9 +272,10 @@ PX4IO_serial::~PX4IO_serial() perf_free(_perf_dmasetup); perf_free(_perf_timeouts); - perf_free(_perf_errors); perf_free(_perf_txns); perf_free(_perf_crcerrs); + perf_free(_perf_dmaerrs); + perf_free(_perf_protoerrs); if (g_interface == this) g_interface = nullptr; @@ -320,10 +323,11 @@ PX4IO_serial::test(unsigned mode) if (count > 100) { perf_print_counter(_perf_dmasetup); - perf_print_counter(_perf_txns); perf_print_counter(_perf_timeouts); - perf_print_counter(_perf_errors); + perf_print_counter(_perf_txns); perf_print_counter(_perf_crcerrs); + perf_print_counter(_perf_dmaerrs); + perf_print_counter(_perf_protoerrs); count = 0; } usleep(10000); @@ -461,26 +465,42 @@ PX4IO_serial::_wait_complete() if (ret == OK) { /* check for DMA errors */ if (_rx_dma_status & DMA_STATUS_TEIF) { - lowsyslog("DMA receive error\n"); + perf_count(_perf_dmaerrs); ret = -1; + errno = EIO; break; } /* check packet CRC */ uint8_t crc = _dma_buffer.crc; _dma_buffer.crc = 0; - if (crc != crc_packet(_dma_buffer)) + if (crc != crc_packet(_dma_buffer)) { perf_count(_perf_crcerrs); + ret = -1; + errno = EIO; + break; + } + + /* check packet response code */ + if (PKT_CODE(_dma_buffer) != PKT_CODE_SUCCESS) { + perf_count(_perf_protoerrs); + ret = -1; + errno = EIO; + break; + } + + /* successful txn */ break; } if (errno == ETIMEDOUT) { - lowsyslog("timeout waiting for PX4IO link (%d/%d)\n", stm32_dmaresidual(_tx_dma), stm32_dmaresidual(_rx_dma)); /* something has broken - clear out any partial DMA state and reconfigure */ _abort_dma(); perf_count(_perf_timeouts); break; } + + /* we might? see this for EINTR */ lowsyslog("unexpected ret %d/%d\n", ret, errno); } @@ -489,8 +509,6 @@ PX4IO_serial::_wait_complete() /* update counters */ perf_end(_perf_txns); - if (ret != OK) - perf_count(_perf_errors); return ret; } From f9a85ac7e64ca816253e94a473e2ef04f2962ab5 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 5 Jul 2013 19:16:25 -0700 Subject: [PATCH 218/486] Remove the TX completion callback on the IO side. Report CRC, read and protocol errors. --- src/modules/px4iofirmware/serial.c | 89 ++++++++++++++++-------------- 1 file changed, 49 insertions(+), 40 deletions(-) diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index e170d5bdf8..665a7622c3 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -56,8 +56,6 @@ //#define DEBUG #include "px4io.h" -static volatile bool sending = false; - static perf_counter_t pc_rx; static perf_counter_t pc_errors; static perf_counter_t pc_ore; @@ -66,8 +64,8 @@ static perf_counter_t pc_ne; static perf_counter_t pc_regerr; static perf_counter_t pc_crcerr; +static void rx_handle_packet(void); static void rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg); -static void tx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg); static DMA_HANDLE tx_dma; static DMA_HANDLE rx_dma; @@ -186,33 +184,24 @@ interface_tick() } static void -tx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) +rx_handle_packet(void) { - sending = false; - rCR3 &= ~USART_CR3_DMAT; -} - -static void -rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) -{ - /* we just received a request; sort out what to do */ - - rCR3 &= ~USART_CR3_DMAR; - idle_ticks = 0; - - /* work out how big the packet actually is */ - //unsigned rx_length = sizeof(IOPacket) - stm32_dmaresidual(rx_dma); - - /* XXX implement check byte */ - perf_count(pc_rx); + /* check packet CRC */ uint8_t crc = dma_packet.crc; dma_packet.crc = 0; - if (crc != crc_packet()) + if (crc != crc_packet()) { perf_count(pc_crcerr); - /* default to not sending a reply */ + /* send a CRC error reply */ + dma_packet.count_code = PKT_CODE_CORRUPT; + dma_packet.page = 0xff; + dma_packet.offset = 0xff; + + return; + } + if (PKT_CODE(dma_packet) == PKT_CODE_WRITE) { /* it's a blind write - pass it on */ @@ -222,33 +211,54 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) } else { dma_packet.count_code = PKT_CODE_SUCCESS; } + return; + } - } else { + if (PKT_CODE(dma_packet) == PKT_CODE_READ) { /* it's a read - get register pointer for reply */ - int result; unsigned count; uint16_t *registers; - result = registers_get(dma_packet.page, dma_packet.offset, ®isters, &count); - if (result < 0) - count = 0; + if (registers_get(dma_packet.page, dma_packet.offset, ®isters, &count) < 0) { + perf_count(pc_regerr); + dma_packet.count_code = PKT_CODE_ERROR; + } else { + /* constrain reply to requested size */ + if (count > PKT_MAX_REGS) + count = PKT_MAX_REGS; + if (count > PKT_COUNT(dma_packet)) + count = PKT_COUNT(dma_packet); - /* constrain reply to packet size */ - if (count > PKT_MAX_REGS) - count = PKT_MAX_REGS; - - /* copy reply registers into DMA buffer */ - memcpy((void *)&dma_packet.regs[0], registers, count); - dma_packet.count_code = count | PKT_CODE_SUCCESS; + /* copy reply registers into DMA buffer */ + memcpy((void *)&dma_packet.regs[0], registers, count); + dma_packet.count_code = count | PKT_CODE_SUCCESS; + } + return; } + /* send a bad-packet error reply */ + dma_packet.count_code = PKT_CODE_CORRUPT; + dma_packet.page = 0xff; + dma_packet.offset = 0xfe; +} + +static void +rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) +{ + /* disable UART DMA */ + rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR); + + /* reset the idle counter */ + idle_ticks = 0; + + /* handle the received packet */ + rx_handle_packet(); + /* re-set DMA for reception first, so we are ready to receive before we start sending */ - /* XXX latency here could mean loss of back-to-back writes; do we want to always send an ack? */ - /* XXX always sending an ack would simplify the FMU side (always wait for reply) too */ dma_reset(); - /* if we have a reply to send, start that now */ + /* send the reply to the previous request */ dma_packet.crc = 0; dma_packet.crc = crc_packet(); stm32_dmasetup( @@ -260,8 +270,7 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) DMA_CCR_MINC | DMA_CCR_PSIZE_8BITS | DMA_CCR_MSIZE_8BITS); - sending = true; - stm32_dmastart(tx_dma, tx_dma_callback, NULL, false); + stm32_dmastart(tx_dma, NULL, NULL, false); rCR3 |= USART_CR3_DMAT; } From bcfb713fe9f123db6d594315465b30dc7210be75 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 5 Jul 2013 20:35:55 -0700 Subject: [PATCH 219/486] Enable handling for short-packet reception on IO using the line-idle interrupt from the UART. --- src/modules/px4iofirmware/serial.c | 37 ++++++++++++++++++------------ 1 file changed, 22 insertions(+), 15 deletions(-) diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index 665a7622c3..21ecde7276 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -56,11 +56,13 @@ //#define DEBUG #include "px4io.h" -static perf_counter_t pc_rx; +static perf_counter_t pc_txns; static perf_counter_t pc_errors; static perf_counter_t pc_ore; static perf_counter_t pc_fe; static perf_counter_t pc_ne; +static perf_counter_t pc_idle; +static perf_counter_t pc_badidle; static perf_counter_t pc_regerr; static perf_counter_t pc_crcerr; @@ -117,11 +119,13 @@ static struct IOPacket dma_packet; void interface_init(void) { - pc_rx = perf_alloc(PC_COUNT, "rx count"); + pc_txns = perf_alloc(PC_ELAPSED, "txns"); pc_errors = perf_alloc(PC_COUNT, "errors"); pc_ore = perf_alloc(PC_COUNT, "overrun"); pc_fe = perf_alloc(PC_COUNT, "framing"); pc_ne = perf_alloc(PC_COUNT, "noise"); + pc_idle = perf_alloc(PC_COUNT, "idle"); + pc_badidle = perf_alloc(PC_COUNT, "badidle"); pc_regerr = perf_alloc(PC_COUNT, "regerr"); pc_crcerr = perf_alloc(PC_COUNT, "crcerr"); @@ -154,7 +158,7 @@ interface_init(void) /* enable UART and DMA */ /*rCR3 = USART_CR3_EIE; */ - rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE /*| USART_CR1_IDLEIE*/; + rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE; #if 0 /* keep this for signal integrity testing */ for (;;) { @@ -186,8 +190,6 @@ interface_tick() static void rx_handle_packet(void) { - perf_count(pc_rx); - /* check packet CRC */ uint8_t crc = dma_packet.crc; dma_packet.crc = 0; @@ -246,6 +248,8 @@ rx_handle_packet(void) static void rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) { + perf_begin(pc_txns); + /* disable UART DMA */ rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR); @@ -272,21 +276,17 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) DMA_CCR_MSIZE_8BITS); stm32_dmastart(tx_dma, NULL, NULL, false); rCR3 |= USART_CR3_DMAT; + + perf_end(pc_txns); } static int serial_interrupt(int irq, void *context) { uint32_t sr = rSR; /* get UART status register */ + (void)rDR; /* required to clear any of the interrupt status that brought us here */ - /* handle error/exception conditions */ - if (sr & (USART_SR_ORE | USART_SR_NE | USART_SR_FE | USART_SR_IDLE)) { - /* read DR to clear status */ - (void)rDR; - } else { - ASSERT(0); - } - +#if 0 if (sr & (USART_SR_ORE | /* overrun error - packet was too big for DMA or DMA was too slow */ USART_SR_NE | /* noise error - we have lost a byte due to noise */ USART_SR_FE)) { /* framing error - start/stop bit lost or line break */ @@ -305,10 +305,17 @@ serial_interrupt(int irq, void *context) /* don't attempt to handle IDLE if it's set - things went bad */ return 0; } - +#endif if (sr & USART_SR_IDLE) { - /* XXX if there was DMA transmission still going, this is an error */ + /* the packet might have been short - go check */ + unsigned length = sizeof(dma_packet) - stm32_dmaresidual(rx_dma); + if ((length < 1) || (length < PKT_SIZE(dma_packet))) { + perf_count(pc_badidle); + return 0; + } + + perf_count(pc_idle); /* stop the receive DMA */ stm32_dmastop(rx_dma); From 3c8c596ac7a2eacc3f4ac047a58bd5dceb36a203 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 5 Jul 2013 20:37:05 -0700 Subject: [PATCH 220/486] Enable handling for short-packet reception on FMU using the line-idle interrupt from the UART. Enable short packets at both ends. --- src/drivers/px4io/interface_serial.cpp | 124 +++++++++++++------------ src/modules/px4iofirmware/serial.c | 2 +- 2 files changed, 67 insertions(+), 59 deletions(-) diff --git a/src/drivers/px4io/interface_serial.cpp b/src/drivers/px4io/interface_serial.cpp index 2ba251b5fc..09362fe155 100644 --- a/src/drivers/px4io/interface_serial.cpp +++ b/src/drivers/px4io/interface_serial.cpp @@ -135,7 +135,6 @@ private: DMA_HANDLE _tx_dma; DMA_HANDLE _rx_dma; - volatile unsigned _rx_length; /** saved DMA status */ static const unsigned _dma_status_inactive = 0x80000000; // low bits overlap DMA_STATUS_* values @@ -173,12 +172,14 @@ private: /** * Performance counters. */ - perf_counter_t _perf_dmasetup; - perf_counter_t _perf_timeouts; - perf_counter_t _perf_crcerrs; - perf_counter_t _perf_dmaerrs; - perf_counter_t _perf_protoerrs; - perf_counter_t _perf_txns; + perf_counter_t _pc_dmasetup; + perf_counter_t _pc_timeouts; + perf_counter_t _pc_crcerrs; + perf_counter_t _pc_dmaerrs; + perf_counter_t _pc_protoerrs; + perf_counter_t _pc_idle; + perf_counter_t _pc_badidle; + perf_counter_t _pc_txns; }; @@ -193,14 +194,15 @@ PX4IO_interface *io_serial_interface() PX4IO_serial::PX4IO_serial() : _tx_dma(nullptr), _rx_dma(nullptr), - _rx_length(0), _rx_dma_status(_dma_status_inactive), - _perf_dmasetup(perf_alloc(PC_ELAPSED, "dmasetup ")), - _perf_timeouts(perf_alloc(PC_COUNT, "timeouts ")), - _perf_crcerrs(perf_alloc(PC_COUNT, "crcerrs ")), - _perf_dmaerrs(perf_alloc(PC_COUNT, "dmaerrs ")), - _perf_protoerrs(perf_alloc(PC_COUNT, "protoerrs")), - _perf_txns(perf_alloc(PC_ELAPSED, "txns ")) + _pc_dmasetup(perf_alloc(PC_ELAPSED, "dmasetup ")), + _pc_timeouts(perf_alloc(PC_COUNT, "timeouts ")), + _pc_crcerrs(perf_alloc(PC_COUNT, "crcerrs ")), + _pc_dmaerrs(perf_alloc(PC_COUNT, "dmaerrs ")), + _pc_protoerrs(perf_alloc(PC_COUNT, "protoerrs")), + _pc_idle(perf_alloc(PC_COUNT, "idle ")), + _pc_badidle(perf_alloc(PC_COUNT, "badidle ")), + _pc_txns(perf_alloc(PC_ELAPSED, "txns ")) { /* allocate DMA */ _tx_dma = stm32_dmachannel(PX4IO_SERIAL_TX_DMAMAP); @@ -233,7 +235,7 @@ PX4IO_serial::PX4IO_serial() : /* enable UART in DMA mode, enable error and line idle interrupts */ /* rCR3 = USART_CR3_EIE;*/ - rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE /*| USART_CR1_IDLEIE*/; + rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE; /* create semaphores */ sem_init(&_completion_semaphore, 0, 0); @@ -270,12 +272,14 @@ PX4IO_serial::~PX4IO_serial() sem_destroy(&_completion_semaphore); sem_destroy(&_bus_semaphore); - perf_free(_perf_dmasetup); - perf_free(_perf_timeouts); - perf_free(_perf_txns); - perf_free(_perf_crcerrs); - perf_free(_perf_dmaerrs); - perf_free(_perf_protoerrs); + perf_free(_pc_dmasetup); + perf_free(_pc_timeouts); + perf_free(_pc_crcerrs); + perf_free(_pc_dmaerrs); + perf_free(_pc_protoerrs); + perf_free(_pc_idle); + perf_free(_pc_badidle); + perf_free(_pc_txns); if (g_interface == this) g_interface = nullptr; @@ -321,13 +325,15 @@ PX4IO_serial::test(unsigned mode) set_reg(PX4IO_PAGE_TEST, PX4IO_P_TEST_LED, &value, 1); /* ignore errors */ - if (count > 100) { - perf_print_counter(_perf_dmasetup); - perf_print_counter(_perf_timeouts); - perf_print_counter(_perf_txns); - perf_print_counter(_perf_crcerrs); - perf_print_counter(_perf_dmaerrs); - perf_print_counter(_perf_protoerrs); + if (count >= 100) { + perf_print_counter(_pc_dmasetup); + perf_print_counter(_pc_timeouts); + perf_print_counter(_pc_crcerrs); + perf_print_counter(_pc_dmaerrs); + perf_print_counter(_pc_protoerrs); + perf_print_counter(_pc_idle); + perf_print_counter(_pc_badidle); + perf_print_counter(_pc_txns); count = 0; } usleep(10000); @@ -404,17 +410,27 @@ PX4IO_serial::_wait_complete() (void)rDR; /* start RX DMA */ - perf_begin(_perf_txns); - perf_begin(_perf_dmasetup); + perf_begin(_pc_txns); + perf_begin(_pc_dmasetup); /* DMA setup time ~3µs */ _rx_dma_status = _dma_status_waiting; - _rx_length = 0; + + /* + * Note that we enable circular buffer mode as a workaround for + * there being no API to disable the DMA FIFO. We need direct mode + * because otherwise when the line idle interrupt fires there + * will be packet bytes still in the DMA FIFO, and we will assume + * that the idle was spurious. + * + * XXX this should be fixed with a NuttX change. + */ stm32_dmasetup( _rx_dma, PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET, reinterpret_cast(&_dma_buffer), sizeof(_dma_buffer), + DMA_SCR_CIRC | /* XXX see note above */ DMA_SCR_DIR_P2M | DMA_SCR_MINC | DMA_SCR_PSIZE_8BITS | @@ -432,7 +448,7 @@ PX4IO_serial::_wait_complete() _tx_dma, PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET, reinterpret_cast(&_dma_buffer), - sizeof(_dma_buffer), /* XXX should be PKT_LENGTH() */ + PKT_SIZE(_dma_buffer), DMA_SCR_DIR_M2P | DMA_SCR_MINC | DMA_SCR_PSIZE_8BITS | @@ -442,7 +458,7 @@ PX4IO_serial::_wait_complete() stm32_dmastart(_tx_dma, nullptr, nullptr, false); rCR3 |= USART_CR3_DMAT; - perf_end(_perf_dmasetup); + perf_end(_pc_dmasetup); /* compute the deadline for a 5ms timeout */ struct timespec abstime; @@ -465,7 +481,7 @@ PX4IO_serial::_wait_complete() if (ret == OK) { /* check for DMA errors */ if (_rx_dma_status & DMA_STATUS_TEIF) { - perf_count(_perf_dmaerrs); + perf_count(_pc_dmaerrs); ret = -1; errno = EIO; break; @@ -475,7 +491,7 @@ PX4IO_serial::_wait_complete() uint8_t crc = _dma_buffer.crc; _dma_buffer.crc = 0; if (crc != crc_packet(_dma_buffer)) { - perf_count(_perf_crcerrs); + perf_count(_pc_crcerrs); ret = -1; errno = EIO; break; @@ -483,7 +499,7 @@ PX4IO_serial::_wait_complete() /* check packet response code */ if (PKT_CODE(_dma_buffer) != PKT_CODE_SUCCESS) { - perf_count(_perf_protoerrs); + perf_count(_pc_protoerrs); ret = -1; errno = EIO; break; @@ -496,7 +512,7 @@ PX4IO_serial::_wait_complete() if (errno == ETIMEDOUT) { /* something has broken - clear out any partial DMA state and reconfigure */ _abort_dma(); - perf_count(_perf_timeouts); + perf_count(_pc_timeouts); break; } @@ -508,7 +524,7 @@ PX4IO_serial::_wait_complete() _rx_dma_status = _dma_status_inactive; /* update counters */ - perf_end(_perf_txns); + perf_end(_pc_txns); return ret; } @@ -542,9 +558,6 @@ PX4IO_serial::_do_rx_dma_callback(unsigned status) /* disable UART DMA */ rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR); - /* DMA may have stopped short */ - _rx_length = sizeof(IOPacket) - stm32_dmaresidual(_rx_dma); - /* complete now */ sem_post(&_completion_semaphore); } @@ -562,15 +575,9 @@ void PX4IO_serial::_do_interrupt() { uint32_t sr = rSR; /* get UART status register */ + (void)rDR; /* read DR to clear status */ - /* handle error/exception conditions */ - if (sr & (USART_SR_ORE | USART_SR_NE | USART_SR_FE | USART_SR_IDLE)) { - /* read DR to clear status */ - (void)rDR; - } else { - ASSERT(0); - } - +#if 0 if (sr & (USART_SR_ORE | /* overrun error - packet was too big for DMA or DMA was too slow */ USART_SR_NE | /* noise error - we have lost a byte due to noise */ USART_SR_FE)) { /* framing error - start/stop bit lost or line break */ @@ -593,20 +600,21 @@ PX4IO_serial::_do_interrupt() /* don't attempt to handle IDLE if it's set - things went bad */ return; } - +#endif if (sr & USART_SR_IDLE) { - /* if there was DMA transmission still going, this is an error */ - if (stm32_dmaresidual(_tx_dma) != 0) { - - /* babble from IO */ - _abort_dma(); - return; - } - /* if there is DMA reception going on, this is a short packet */ if (_rx_dma_status == _dma_status_waiting) { + /* verify that the received packet is complete */ + unsigned length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma); + if ((length < 1) || (length < PKT_SIZE(_dma_buffer))) { + perf_count(_pc_badidle); + return; + } + + perf_count(_pc_idle); + /* stop the receive DMA */ stm32_dmastop(_rx_dma); diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index 21ecde7276..b918193738 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -269,7 +269,7 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) tx_dma, (uint32_t)&rDR, (uint32_t)&dma_packet, - sizeof(dma_packet), /* XXX should be PKT_LENGTH() */ + PKT_SIZE(dma_packet), DMA_CCR_DIR | DMA_CCR_MINC | DMA_CCR_PSIZE_8BITS | From 6871d2909b5be7eb93bf23aea771a86aa1b0ae3f Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 5 Jul 2013 22:53:57 -0700 Subject: [PATCH 221/486] Add a mechanism for cancelling begin/end perf counters. --- src/modules/systemlib/perf_counter.c | 41 +++++++++++++++++++++++----- src/modules/systemlib/perf_counter.h | 14 +++++++++- 2 files changed, 47 insertions(+), 8 deletions(-) diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c index 879f4715af..3c1e10287d 100644 --- a/src/modules/systemlib/perf_counter.c +++ b/src/modules/systemlib/perf_counter.c @@ -201,23 +201,50 @@ perf_end(perf_counter_t handle) switch (handle->type) { case PC_ELAPSED: { struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; - hrt_abstime elapsed = hrt_absolute_time() - pce->time_start; - pce->event_count++; - pce->time_total += elapsed; + if (pce->time_start != 0) { + hrt_abstime elapsed = hrt_absolute_time() - pce->time_start; - if ((pce->time_least > elapsed) || (pce->time_least == 0)) - pce->time_least = elapsed; + pce->event_count++; + pce->time_total += elapsed; - if (pce->time_most < elapsed) - pce->time_most = elapsed; + if ((pce->time_least > elapsed) || (pce->time_least == 0)) + pce->time_least = elapsed; + + if (pce->time_most < elapsed) + pce->time_most = elapsed; + + pce->time_start = 0; + } } + break; default: break; } } +void +perf_cancel(perf_counter_t handle) +{ + if (handle == NULL) + return; + + switch (handle->type) { + case PC_ELAPSED: { + struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; + + pce->time_start = 0; + } + break; + + default: + break; + } +} + + + void perf_reset(perf_counter_t handle) { diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h index 5c2cb15b2f..4cd8b67a17 100644 --- a/src/modules/systemlib/perf_counter.h +++ b/src/modules/systemlib/perf_counter.h @@ -92,13 +92,25 @@ __EXPORT extern void perf_begin(perf_counter_t handle); * End a performance event. * * This call applies to counters that operate over ranges of time; PC_ELAPSED etc. + * If a call is made without a corresopnding perf_begin call, or if perf_cancel + * has been called subsequently, no change is made to the counter. * * @param handle The handle returned from perf_alloc. */ __EXPORT extern void perf_end(perf_counter_t handle); /** - * Reset a performance event. + * Cancel a performance event. + * + * This call applies to counters that operate over ranges of time; PC_ELAPSED etc. + * It reverts the effect of a previous perf_begin. + * + * @param handle The handle returned from perf_alloc. + */ +__EXPORT extern void perf_cancel(perf_counter_t handle); + +/** + * Reset a performance counter. * * This call resets performance counter to initial state * From a4b0e3ecbe2d012eac7545cce14829866bacc813 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 5 Jul 2013 22:54:44 -0700 Subject: [PATCH 222/486] Add retry-on-error for non-protocol errors. Add more performance counters; run test #1 faster. --- src/drivers/px4io/interface_serial.cpp | 150 ++++++++++++++++--------- 1 file changed, 98 insertions(+), 52 deletions(-) diff --git a/src/drivers/px4io/interface_serial.cpp b/src/drivers/px4io/interface_serial.cpp index 09362fe155..7e4a54ea54 100644 --- a/src/drivers/px4io/interface_serial.cpp +++ b/src/drivers/px4io/interface_serial.cpp @@ -172,14 +172,16 @@ private: /** * Performance counters. */ + perf_counter_t _pc_txns; perf_counter_t _pc_dmasetup; + perf_counter_t _pc_retries; perf_counter_t _pc_timeouts; perf_counter_t _pc_crcerrs; perf_counter_t _pc_dmaerrs; perf_counter_t _pc_protoerrs; + perf_counter_t _pc_uerrs; perf_counter_t _pc_idle; perf_counter_t _pc_badidle; - perf_counter_t _pc_txns; }; @@ -195,14 +197,16 @@ PX4IO_serial::PX4IO_serial() : _tx_dma(nullptr), _rx_dma(nullptr), _rx_dma_status(_dma_status_inactive), + _pc_txns(perf_alloc(PC_ELAPSED, "txns ")), _pc_dmasetup(perf_alloc(PC_ELAPSED, "dmasetup ")), + _pc_retries(perf_alloc(PC_COUNT, "retries ")), _pc_timeouts(perf_alloc(PC_COUNT, "timeouts ")), _pc_crcerrs(perf_alloc(PC_COUNT, "crcerrs ")), _pc_dmaerrs(perf_alloc(PC_COUNT, "dmaerrs ")), _pc_protoerrs(perf_alloc(PC_COUNT, "protoerrs")), + _pc_uerrs(perf_alloc(PC_COUNT, "uarterrs ")), _pc_idle(perf_alloc(PC_COUNT, "idle ")), - _pc_badidle(perf_alloc(PC_COUNT, "badidle ")), - _pc_txns(perf_alloc(PC_ELAPSED, "txns ")) + _pc_badidle(perf_alloc(PC_COUNT, "badidle ")) { /* allocate DMA */ _tx_dma = stm32_dmachannel(PX4IO_SERIAL_TX_DMAMAP); @@ -272,14 +276,16 @@ PX4IO_serial::~PX4IO_serial() sem_destroy(&_completion_semaphore); sem_destroy(&_bus_semaphore); + perf_free(_pc_txns); perf_free(_pc_dmasetup); + perf_free(_pc_retries); perf_free(_pc_timeouts); perf_free(_pc_crcerrs); perf_free(_pc_dmaerrs); perf_free(_pc_protoerrs); + perf_free(_pc_uerrs); perf_free(_pc_idle); perf_free(_pc_badidle); - perf_free(_pc_txns); if (g_interface == this) g_interface = nullptr; @@ -317,26 +323,29 @@ PX4IO_serial::test(unsigned mode) return 0; case 1: - lowsyslog("test 1\n"); { + unsigned fails = 0; for (unsigned count = 0;; count++) { uint16_t value = count & 0xffff; - set_reg(PX4IO_PAGE_TEST, PX4IO_P_TEST_LED, &value, 1); - /* ignore errors */ + if (set_reg(PX4IO_PAGE_TEST, PX4IO_P_TEST_LED, &value, 1) != 0) + fails++; - if (count >= 100) { + if (count >= 1000) { + lowsyslog("==== test 1 : %u failures ====\n", fails); + perf_print_counter(_pc_txns); perf_print_counter(_pc_dmasetup); + perf_print_counter(_pc_retries); perf_print_counter(_pc_timeouts); perf_print_counter(_pc_crcerrs); perf_print_counter(_pc_dmaerrs); perf_print_counter(_pc_protoerrs); + perf_print_counter(_pc_uerrs); perf_print_counter(_pc_idle); perf_print_counter(_pc_badidle); - perf_print_counter(_pc_txns); count = 0; } - usleep(10000); + usleep(1000); } return 0; } @@ -350,22 +359,44 @@ PX4IO_serial::test(unsigned mode) int PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { - if (num_values > PKT_MAX_REGS) - return -EINVAL; + if (num_values > PKT_MAX_REGS) { + errno = EINVAL; + return -1; + } sem_wait(&_bus_semaphore); - _dma_buffer.count_code = num_values | PKT_CODE_WRITE; - _dma_buffer.page = page; - _dma_buffer.offset = offset; - memcpy((void *)&_dma_buffer.regs[0], (void *)values, (2 * num_values)); - for (unsigned i = num_values; i < PKT_MAX_REGS; i++) - _dma_buffer.regs[i] = 0x55aa; + int result; + for (unsigned retries = 0; retries < 3; retries++) { - /* XXX implement check byte */ + _dma_buffer.count_code = num_values | PKT_CODE_WRITE; + _dma_buffer.page = page; + _dma_buffer.offset = offset; + memcpy((void *)&_dma_buffer.regs[0], (void *)values, (2 * num_values)); + for (unsigned i = num_values; i < PKT_MAX_REGS; i++) + _dma_buffer.regs[i] = 0x55aa; - /* start the transaction and wait for it to complete */ - int result = _wait_complete(); + /* XXX implement check byte */ + + /* start the transaction and wait for it to complete */ + result = _wait_complete(); + + /* successful transaction? */ + if (result == OK) { + + /* check result in packet */ + if (PKT_CODE(_dma_buffer) == PKT_CODE_ERROR) { + + /* IO didn't like it - no point retrying */ + errno = EINVAL; + result = -1; + perf_count(_pc_protoerrs); + } + + break; + } + perf_count(_pc_retries); + } sem_post(&_bus_semaphore); return result; @@ -379,23 +410,45 @@ PX4IO_serial::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned n sem_wait(&_bus_semaphore); - _dma_buffer.count_code = num_values | PKT_CODE_READ; - _dma_buffer.page = page; - _dma_buffer.offset = offset; + int result; + for (unsigned retries = 0; retries < 3; retries++) { - /* start the transaction and wait for it to complete */ - int result = _wait_complete(); - if (result != OK) - goto out; + _dma_buffer.count_code = num_values | PKT_CODE_READ; + _dma_buffer.page = page; + _dma_buffer.offset = offset; - /* compare the received count with the expected count */ - if (PKT_COUNT(_dma_buffer) != num_values) { - result = -EIO; - goto out; - } else { - /* XXX implement check byte */ - /* copy back the result */ - memcpy(values, &_dma_buffer.regs[0], (2 * num_values)); + /* start the transaction and wait for it to complete */ + result = _wait_complete(); + + /* successful transaction? */ + if (result == OK) { + + /* check result in packet */ + if (PKT_CODE(_dma_buffer) == PKT_CODE_ERROR) { + + /* IO didn't like it - no point retrying */ + errno = EINVAL; + result = -1; + perf_count(_pc_protoerrs); + + /* compare the received count with the expected count */ + } else if (PKT_COUNT(_dma_buffer) != num_values) { + + /* IO returned the wrong number of registers - no point retrying */ + errno = EIO; + result = -1; + perf_count(_pc_protoerrs); + + /* successful read */ + } else { + + /* copy back the result */ + memcpy(values, &_dma_buffer.regs[0], (2 * num_values)); + } + + break; + } + perf_count(_pc_retries); } out: sem_post(&_bus_semaphore); @@ -463,11 +516,11 @@ PX4IO_serial::_wait_complete() /* compute the deadline for a 5ms timeout */ struct timespec abstime; clock_gettime(CLOCK_REALTIME, &abstime); -#if 1 +#if 0 abstime.tv_sec++; /* long timeout for testing */ #else - abstime.tv_nsec += 5000000; /* 5ms timeout */ - while (abstime.tv_nsec > 1000000000) { + abstime.tv_nsec += 10000000; /* 0ms timeout */ + if (abstime.tv_nsec > 1000000000) { abstime.tv_sec++; abstime.tv_nsec -= 1000000000; } @@ -487,25 +540,17 @@ PX4IO_serial::_wait_complete() break; } - /* check packet CRC */ + /* check packet CRC - corrupt packet errors mean IO receive CRC error */ uint8_t crc = _dma_buffer.crc; _dma_buffer.crc = 0; - if (crc != crc_packet(_dma_buffer)) { + if ((crc != crc_packet(_dma_buffer)) | (PKT_CODE(_dma_buffer) == PKT_CODE_CORRUPT)) { perf_count(_pc_crcerrs); ret = -1; errno = EIO; break; } - /* check packet response code */ - if (PKT_CODE(_dma_buffer) != PKT_CODE_SUCCESS) { - perf_count(_pc_protoerrs); - ret = -1; - errno = EIO; - break; - } - - /* successful txn */ + /* successful txn (may still be reporting an error) */ break; } @@ -513,6 +558,7 @@ PX4IO_serial::_wait_complete() /* something has broken - clear out any partial DMA state and reconfigure */ _abort_dma(); perf_count(_pc_timeouts); + perf_cancel(_pc_txns); /* don't count this as a transaction */ break; } @@ -577,7 +623,6 @@ PX4IO_serial::_do_interrupt() uint32_t sr = rSR; /* get UART status register */ (void)rDR; /* read DR to clear status */ -#if 0 if (sr & (USART_SR_ORE | /* overrun error - packet was too big for DMA or DMA was too slow */ USART_SR_NE | /* noise error - we have lost a byte due to noise */ USART_SR_FE)) { /* framing error - start/stop bit lost or line break */ @@ -588,6 +633,7 @@ PX4IO_serial::_do_interrupt() */ if (_rx_dma_status == _dma_status_waiting) { _abort_dma(); + perf_count(_pc_uerrs); /* complete DMA as though in error */ _do_rx_dma_callback(DMA_STATUS_TEIF); @@ -600,7 +646,7 @@ PX4IO_serial::_do_interrupt() /* don't attempt to handle IDLE if it's set - things went bad */ return; } -#endif + if (sr & USART_SR_IDLE) { /* if there is DMA reception going on, this is a short packet */ From 0589346ce6ccbcee03437ee293cc03d900bf76d9 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Jul 2013 00:00:10 -0700 Subject: [PATCH 223/486] Abort the px4io worker task if subscribing to the required ORB topics fails. --- src/drivers/px4io/px4io.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 663609aeda..5895a4eb5c 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -572,6 +572,14 @@ PX4IO::task_main() _t_param = orb_subscribe(ORB_ID(parameter_update)); orb_set_interval(_t_param, 500); /* 2Hz update rate max. */ + if ((_t_actuators < 0) || + (_t_armed < 0) || + (_t_vstatus < 0) || + (_t_param < 0)) { + log("subscription(s) failed"); + goto out; + } + /* poll descriptor */ pollfd fds[4]; fds[0].fd = _t_actuators; @@ -668,6 +676,7 @@ PX4IO::task_main() unlock(); +out: debug("exiting"); /* clean up the alternate device node */ From 19b2e1de8505be6ab23bedab7b105a20ac7af405 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Jul 2013 00:00:44 -0700 Subject: [PATCH 224/486] Copy the correct number of bytes back for register read operations. Basic PX4IO comms are working now. --- src/modules/px4iofirmware/serial.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index b918193738..f19021d8c9 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -233,7 +233,7 @@ rx_handle_packet(void) count = PKT_COUNT(dma_packet); /* copy reply registers into DMA buffer */ - memcpy((void *)&dma_packet.regs[0], registers, count); + memcpy((void *)&dma_packet.regs[0], registers, count * 2); dma_packet.count_code = count | PKT_CODE_SUCCESS; } return; From 87a4f1507a3bca4bcae870b13ff5416669ededf0 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Jul 2013 00:16:37 -0700 Subject: [PATCH 225/486] Move the common definitions for the PX4IO serial protocol into the shared header. --- src/drivers/px4io/interface_serial.cpp | 81 +------------------------- src/modules/px4iofirmware/protocol.h | 78 +++++++++++++++++++++++++ src/modules/px4iofirmware/serial.c | 79 +------------------------ 3 files changed, 82 insertions(+), 156 deletions(-) diff --git a/src/drivers/px4io/interface_serial.cpp b/src/drivers/px4io/interface_serial.cpp index 7e4a54ea54..7e0eb19266 100644 --- a/src/drivers/px4io/interface_serial.cpp +++ b/src/drivers/px4io/interface_serial.cpp @@ -69,18 +69,6 @@ #include "interface.h" -#define PKT_MAX_REGS 32 // by agreement w/IO - -#pragma pack(push, 1) -struct IOPacket { - uint8_t count_code; - uint8_t crc; - uint8_t page; - uint8_t offset; - uint16_t regs[PKT_MAX_REGS]; -}; -#pragma pack(pop) - /* serial register accessors */ #define REG(_x) (*(volatile uint32_t *)(PX4IO_SERIAL_BASE + _x)) #define rSR REG(STM32_USART_SR_OFFSET) @@ -91,22 +79,6 @@ struct IOPacket { #define rCR3 REG(STM32_USART_CR3_OFFSET) #define rGTPR REG(STM32_USART_GTPR_OFFSET) -#define PKT_CODE_READ 0x00 /* FMU->IO read transaction */ -#define PKT_CODE_WRITE 0x40 /* FMU->IO write transaction */ -#define PKT_CODE_SUCCESS 0x00 /* IO->FMU success reply */ -#define PKT_CODE_CORRUPT 0x40 /* IO->FMU bad packet reply */ -#define PKT_CODE_ERROR 0x80 /* IO->FMU register op error reply */ - -#define PKT_CODE_MASK 0xc0 -#define PKT_COUNT_MASK 0x3f - -#define PKT_COUNT(_p) ((_p).count_code & PKT_COUNT_MASK) -#define PKT_CODE(_p) ((_p).count_code & PKT_CODE_MASK) -#define PKT_SIZE(_p) ((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p))) - -static uint8_t crc_packet(IOPacket &pkt); - - class PX4IO_serial : public PX4IO_interface { public: @@ -496,7 +468,7 @@ PX4IO_serial::_wait_complete() /* start TX DMA - no callback if we also expect a reply */ /* DMA setup time ~3µs */ _dma_buffer.crc = 0; - _dma_buffer.crc = crc_packet(_dma_buffer); + _dma_buffer.crc = crc_packet(&_dma_buffer); stm32_dmasetup( _tx_dma, PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET, @@ -543,7 +515,7 @@ PX4IO_serial::_wait_complete() /* check packet CRC - corrupt packet errors mean IO receive CRC error */ uint8_t crc = _dma_buffer.crc; _dma_buffer.crc = 0; - if ((crc != crc_packet(_dma_buffer)) | (PKT_CODE(_dma_buffer) == PKT_CODE_CORRUPT)) { + if ((crc != crc_packet(&_dma_buffer)) | (PKT_CODE(_dma_buffer) == PKT_CODE_CORRUPT)) { perf_count(_pc_crcerrs); ret = -1; errno = EIO; @@ -683,52 +655,3 @@ PX4IO_serial::_abort_dma() stm32_dmastop(_tx_dma); stm32_dmastop(_rx_dma); } - -static const uint8_t crc8_tab[256] = -{ - 0x00, 0x07, 0x0E, 0x09, 0x1C, 0x1B, 0x12, 0x15, - 0x38, 0x3F, 0x36, 0x31, 0x24, 0x23, 0x2A, 0x2D, - 0x70, 0x77, 0x7E, 0x79, 0x6C, 0x6B, 0x62, 0x65, - 0x48, 0x4F, 0x46, 0x41, 0x54, 0x53, 0x5A, 0x5D, - 0xE0, 0xE7, 0xEE, 0xE9, 0xFC, 0xFB, 0xF2, 0xF5, - 0xD8, 0xDF, 0xD6, 0xD1, 0xC4, 0xC3, 0xCA, 0xCD, - 0x90, 0x97, 0x9E, 0x99, 0x8C, 0x8B, 0x82, 0x85, - 0xA8, 0xAF, 0xA6, 0xA1, 0xB4, 0xB3, 0xBA, 0xBD, - 0xC7, 0xC0, 0xC9, 0xCE, 0xDB, 0xDC, 0xD5, 0xD2, - 0xFF, 0xF8, 0xF1, 0xF6, 0xE3, 0xE4, 0xED, 0xEA, - 0xB7, 0xB0, 0xB9, 0xBE, 0xAB, 0xAC, 0xA5, 0xA2, - 0x8F, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9D, 0x9A, - 0x27, 0x20, 0x29, 0x2E, 0x3B, 0x3C, 0x35, 0x32, - 0x1F, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0D, 0x0A, - 0x57, 0x50, 0x59, 0x5E, 0x4B, 0x4C, 0x45, 0x42, - 0x6F, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7D, 0x7A, - 0x89, 0x8E, 0x87, 0x80, 0x95, 0x92, 0x9B, 0x9C, - 0xB1, 0xB6, 0xBF, 0xB8, 0xAD, 0xAA, 0xA3, 0xA4, - 0xF9, 0xFE, 0xF7, 0xF0, 0xE5, 0xE2, 0xEB, 0xEC, - 0xC1, 0xC6, 0xCF, 0xC8, 0xDD, 0xDA, 0xD3, 0xD4, - 0x69, 0x6E, 0x67, 0x60, 0x75, 0x72, 0x7B, 0x7C, - 0x51, 0x56, 0x5F, 0x58, 0x4D, 0x4A, 0x43, 0x44, - 0x19, 0x1E, 0x17, 0x10, 0x05, 0x02, 0x0B, 0x0C, - 0x21, 0x26, 0x2F, 0x28, 0x3D, 0x3A, 0x33, 0x34, - 0x4E, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5C, 0x5B, - 0x76, 0x71, 0x78, 0x7F, 0x6A, 0x6D, 0x64, 0x63, - 0x3E, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2C, 0x2B, - 0x06, 0x01, 0x08, 0x0F, 0x1A, 0x1D, 0x14, 0x13, - 0xAE, 0xA9, 0xA0, 0xA7, 0xB2, 0xB5, 0xBC, 0xBB, - 0x96, 0x91, 0x98, 0x9F, 0x8A, 0x8D, 0x84, 0x83, - 0xDE, 0xD9, 0xD0, 0xD7, 0xC2, 0xC5, 0xCC, 0xCB, - 0xE6, 0xE1, 0xE8, 0xEF, 0xFA, 0xFD, 0xF4, 0xF3 -}; - -static uint8_t -crc_packet(IOPacket &pkt) -{ - uint8_t *end = (uint8_t *)(&pkt.regs[PKT_COUNT(pkt)]); - uint8_t *p = (uint8_t *)&pkt; - uint8_t c = 0; - - while (p < end) - c = crc8_tab[c ^ *(p++)]; - - return c; -} diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index b19146b061..48ad4316f1 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -206,3 +206,81 @@ struct px4io_mixdata { }; #pragma pack(pop) +/** + * Serial protocol encapsulation. + */ + +#define PKT_MAX_REGS 32 // by agreement w/FMU + +#pragma pack(push, 1) +struct IOPacket { + uint8_t count_code; + uint8_t crc; + uint8_t page; + uint8_t offset; + uint16_t regs[PKT_MAX_REGS]; +}; +#pragma pack(pop) + +#define PKT_CODE_READ 0x00 /* FMU->IO read transaction */ +#define PKT_CODE_WRITE 0x40 /* FMU->IO write transaction */ +#define PKT_CODE_SUCCESS 0x00 /* IO->FMU success reply */ +#define PKT_CODE_CORRUPT 0x40 /* IO->FMU bad packet reply */ +#define PKT_CODE_ERROR 0x80 /* IO->FMU register op error reply */ + +#define PKT_CODE_MASK 0xc0 +#define PKT_COUNT_MASK 0x3f + +#define PKT_COUNT(_p) ((_p).count_code & PKT_COUNT_MASK) +#define PKT_CODE(_p) ((_p).count_code & PKT_CODE_MASK) +#define PKT_SIZE(_p) ((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p))) + +static const uint8_t crc8_tab[256] __attribute__((unused)) = +{ + 0x00, 0x07, 0x0E, 0x09, 0x1C, 0x1B, 0x12, 0x15, + 0x38, 0x3F, 0x36, 0x31, 0x24, 0x23, 0x2A, 0x2D, + 0x70, 0x77, 0x7E, 0x79, 0x6C, 0x6B, 0x62, 0x65, + 0x48, 0x4F, 0x46, 0x41, 0x54, 0x53, 0x5A, 0x5D, + 0xE0, 0xE7, 0xEE, 0xE9, 0xFC, 0xFB, 0xF2, 0xF5, + 0xD8, 0xDF, 0xD6, 0xD1, 0xC4, 0xC3, 0xCA, 0xCD, + 0x90, 0x97, 0x9E, 0x99, 0x8C, 0x8B, 0x82, 0x85, + 0xA8, 0xAF, 0xA6, 0xA1, 0xB4, 0xB3, 0xBA, 0xBD, + 0xC7, 0xC0, 0xC9, 0xCE, 0xDB, 0xDC, 0xD5, 0xD2, + 0xFF, 0xF8, 0xF1, 0xF6, 0xE3, 0xE4, 0xED, 0xEA, + 0xB7, 0xB0, 0xB9, 0xBE, 0xAB, 0xAC, 0xA5, 0xA2, + 0x8F, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9D, 0x9A, + 0x27, 0x20, 0x29, 0x2E, 0x3B, 0x3C, 0x35, 0x32, + 0x1F, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0D, 0x0A, + 0x57, 0x50, 0x59, 0x5E, 0x4B, 0x4C, 0x45, 0x42, + 0x6F, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7D, 0x7A, + 0x89, 0x8E, 0x87, 0x80, 0x95, 0x92, 0x9B, 0x9C, + 0xB1, 0xB6, 0xBF, 0xB8, 0xAD, 0xAA, 0xA3, 0xA4, + 0xF9, 0xFE, 0xF7, 0xF0, 0xE5, 0xE2, 0xEB, 0xEC, + 0xC1, 0xC6, 0xCF, 0xC8, 0xDD, 0xDA, 0xD3, 0xD4, + 0x69, 0x6E, 0x67, 0x60, 0x75, 0x72, 0x7B, 0x7C, + 0x51, 0x56, 0x5F, 0x58, 0x4D, 0x4A, 0x43, 0x44, + 0x19, 0x1E, 0x17, 0x10, 0x05, 0x02, 0x0B, 0x0C, + 0x21, 0x26, 0x2F, 0x28, 0x3D, 0x3A, 0x33, 0x34, + 0x4E, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5C, 0x5B, + 0x76, 0x71, 0x78, 0x7F, 0x6A, 0x6D, 0x64, 0x63, + 0x3E, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2C, 0x2B, + 0x06, 0x01, 0x08, 0x0F, 0x1A, 0x1D, 0x14, 0x13, + 0xAE, 0xA9, 0xA0, 0xA7, 0xB2, 0xB5, 0xBC, 0xBB, + 0x96, 0x91, 0x98, 0x9F, 0x8A, 0x8D, 0x84, 0x83, + 0xDE, 0xD9, 0xD0, 0xD7, 0xC2, 0xC5, 0xCC, 0xCB, + 0xE6, 0xE1, 0xE8, 0xEF, 0xFA, 0xFD, 0xF4, 0xF3 +}; + +static uint8_t crc_packet(struct IOPacket *pkt) __attribute__((unused)); +static uint8_t +crc_packet(struct IOPacket *pkt) +{ + uint8_t *end = (uint8_t *)(&pkt->regs[PKT_COUNT(*pkt)]); + uint8_t *p = (uint8_t *)pkt; + uint8_t c = 0; + + while (p < end) + c = crc8_tab[c ^ *(p++)]; + + return c; +} diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index f19021d8c9..8742044c37 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -77,33 +77,6 @@ static void dma_reset(void); /* if we spend this many ticks idle, reset the DMA */ static unsigned idle_ticks; -#define PKT_MAX_REGS 32 // by agreement w/FMU - -#pragma pack(push, 1) -struct IOPacket { - uint8_t count_code; - uint8_t crc; - uint8_t page; - uint8_t offset; - uint16_t regs[PKT_MAX_REGS]; -}; -#pragma pack(pop) - -#define PKT_CODE_READ 0x00 /* FMU->IO read transaction */ -#define PKT_CODE_WRITE 0x40 /* FMU->IO write transaction */ -#define PKT_CODE_SUCCESS 0x00 /* IO->FMU success reply */ -#define PKT_CODE_CORRUPT 0x40 /* IO->FMU bad packet reply */ -#define PKT_CODE_ERROR 0x80 /* IO->FMU register op error reply */ - -#define PKT_CODE_MASK 0xc0 -#define PKT_COUNT_MASK 0x3f - -#define PKT_COUNT(_p) ((_p).count_code & PKT_COUNT_MASK) -#define PKT_CODE(_p) ((_p).count_code & PKT_CODE_MASK) -#define PKT_SIZE(_p) ((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p))) - -static uint8_t crc_packet(void); - static struct IOPacket dma_packet; /* serial register accessors */ @@ -193,7 +166,7 @@ rx_handle_packet(void) /* check packet CRC */ uint8_t crc = dma_packet.crc; dma_packet.crc = 0; - if (crc != crc_packet()) { + if (crc != crc_packet(&dma_packet)) { perf_count(pc_crcerr); /* send a CRC error reply */ @@ -264,7 +237,7 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) /* send the reply to the previous request */ dma_packet.crc = 0; - dma_packet.crc = crc_packet(); + dma_packet.crc = crc_packet(&dma_packet); stm32_dmasetup( tx_dma, (uint32_t)&rDR, @@ -354,51 +327,3 @@ dma_reset(void) rCR3 |= USART_CR3_DMAR; } -static const uint8_t crc8_tab[256] = -{ - 0x00, 0x07, 0x0E, 0x09, 0x1C, 0x1B, 0x12, 0x15, - 0x38, 0x3F, 0x36, 0x31, 0x24, 0x23, 0x2A, 0x2D, - 0x70, 0x77, 0x7E, 0x79, 0x6C, 0x6B, 0x62, 0x65, - 0x48, 0x4F, 0x46, 0x41, 0x54, 0x53, 0x5A, 0x5D, - 0xE0, 0xE7, 0xEE, 0xE9, 0xFC, 0xFB, 0xF2, 0xF5, - 0xD8, 0xDF, 0xD6, 0xD1, 0xC4, 0xC3, 0xCA, 0xCD, - 0x90, 0x97, 0x9E, 0x99, 0x8C, 0x8B, 0x82, 0x85, - 0xA8, 0xAF, 0xA6, 0xA1, 0xB4, 0xB3, 0xBA, 0xBD, - 0xC7, 0xC0, 0xC9, 0xCE, 0xDB, 0xDC, 0xD5, 0xD2, - 0xFF, 0xF8, 0xF1, 0xF6, 0xE3, 0xE4, 0xED, 0xEA, - 0xB7, 0xB0, 0xB9, 0xBE, 0xAB, 0xAC, 0xA5, 0xA2, - 0x8F, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9D, 0x9A, - 0x27, 0x20, 0x29, 0x2E, 0x3B, 0x3C, 0x35, 0x32, - 0x1F, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0D, 0x0A, - 0x57, 0x50, 0x59, 0x5E, 0x4B, 0x4C, 0x45, 0x42, - 0x6F, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7D, 0x7A, - 0x89, 0x8E, 0x87, 0x80, 0x95, 0x92, 0x9B, 0x9C, - 0xB1, 0xB6, 0xBF, 0xB8, 0xAD, 0xAA, 0xA3, 0xA4, - 0xF9, 0xFE, 0xF7, 0xF0, 0xE5, 0xE2, 0xEB, 0xEC, - 0xC1, 0xC6, 0xCF, 0xC8, 0xDD, 0xDA, 0xD3, 0xD4, - 0x69, 0x6E, 0x67, 0x60, 0x75, 0x72, 0x7B, 0x7C, - 0x51, 0x56, 0x5F, 0x58, 0x4D, 0x4A, 0x43, 0x44, - 0x19, 0x1E, 0x17, 0x10, 0x05, 0x02, 0x0B, 0x0C, - 0x21, 0x26, 0x2F, 0x28, 0x3D, 0x3A, 0x33, 0x34, - 0x4E, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5C, 0x5B, - 0x76, 0x71, 0x78, 0x7F, 0x6A, 0x6D, 0x64, 0x63, - 0x3E, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2C, 0x2B, - 0x06, 0x01, 0x08, 0x0F, 0x1A, 0x1D, 0x14, 0x13, - 0xAE, 0xA9, 0xA0, 0xA7, 0xB2, 0xB5, 0xBC, 0xBB, - 0x96, 0x91, 0x98, 0x9F, 0x8A, 0x8D, 0x84, 0x83, - 0xDE, 0xD9, 0xD0, 0xD7, 0xC2, 0xC5, 0xCC, 0xCB, - 0xE6, 0xE1, 0xE8, 0xEF, 0xFA, 0xFD, 0xF4, 0xF3 -}; - -static uint8_t -crc_packet() -{ - uint8_t *end = (uint8_t *)(&dma_packet.regs[PKT_COUNT(dma_packet)]); - uint8_t *p = (uint8_t *)&dma_packet; - uint8_t c = 0; - - while (p < end) - c = crc8_tab[c ^ *(p++)]; - - return c; -} From 4d400aa7e75aa091fb649bbeaf0a2d6a644c177c Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Jul 2013 12:27:37 -0700 Subject: [PATCH 226/486] Enable UART error handling on PX4IO. --- src/modules/px4iofirmware/serial.c | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index 8742044c37..e4bc68f580 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -129,8 +129,8 @@ interface_init(void) irq_attach(PX4FMU_SERIAL_VECTOR, serial_interrupt); up_enable_irq(PX4FMU_SERIAL_VECTOR); - /* enable UART and DMA */ - /*rCR3 = USART_CR3_EIE; */ + /* enable UART and error/idle interrupts */ + rCR3 = USART_CR3_EIE; rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE; #if 0 /* keep this for signal integrity testing */ @@ -259,7 +259,6 @@ serial_interrupt(int irq, void *context) uint32_t sr = rSR; /* get UART status register */ (void)rDR; /* required to clear any of the interrupt status that brought us here */ -#if 0 if (sr & (USART_SR_ORE | /* overrun error - packet was too big for DMA or DMA was too slow */ USART_SR_NE | /* noise error - we have lost a byte due to noise */ USART_SR_FE)) { /* framing error - start/stop bit lost or line break */ @@ -278,7 +277,7 @@ serial_interrupt(int irq, void *context) /* don't attempt to handle IDLE if it's set - things went bad */ return 0; } -#endif + if (sr & USART_SR_IDLE) { /* the packet might have been short - go check */ From 17f9c7d15ccb6301e2be3eaa8cde8b3f710ce085 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Jul 2013 12:28:01 -0700 Subject: [PATCH 227/486] Crank up the test speed for 'px4io iftest 1' --- src/drivers/px4io/interface_serial.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/drivers/px4io/interface_serial.cpp b/src/drivers/px4io/interface_serial.cpp index 7e0eb19266..9727daa717 100644 --- a/src/drivers/px4io/interface_serial.cpp +++ b/src/drivers/px4io/interface_serial.cpp @@ -303,7 +303,7 @@ PX4IO_serial::test(unsigned mode) if (set_reg(PX4IO_PAGE_TEST, PX4IO_P_TEST_LED, &value, 1) != 0) fails++; - if (count >= 1000) { + if (count >= 5000) { lowsyslog("==== test 1 : %u failures ====\n", fails); perf_print_counter(_pc_txns); perf_print_counter(_pc_dmasetup); @@ -317,7 +317,6 @@ PX4IO_serial::test(unsigned mode) perf_print_counter(_pc_badidle); count = 0; } - usleep(1000); } return 0; } From a65a1237f05c885245237e9ffecd79dee9de4dbc Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Jul 2013 12:29:14 -0700 Subject: [PATCH 228/486] Optimise the RC input fetch for <= 9ch transmitters; this eliminates one read per cycle from IO in the common case. --- src/drivers/px4io/px4io.cpp | 50 ++++++++++++++++++------------------- 1 file changed, 25 insertions(+), 25 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 5895a4eb5c..951bfe6952 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -959,38 +959,38 @@ int PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) { uint32_t channel_count; - int ret = OK; + int ret; /* we don't have the status bits, so input_source has to be set elsewhere */ input_rc.input_source = RC_INPUT_SOURCE_UNKNOWN; - /* - * XXX Because the channel count and channel data are fetched - * separately, there is a risk of a race between the two - * that could leave us with channel data and a count that - * are out of sync. - * Fixing this would require a guarantee of atomicity from - * IO, and a single fetch for both count and channels. - * - * XXX Since IO has the input calibration info, we ought to be - * able to get the pre-fixed-up controls directly. - * - * XXX can we do this more cheaply? If we knew we had DMA, it would - * almost certainly be better to just get all the inputs... - */ - channel_count = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); - if (channel_count == _io_reg_get_error) - return -EIO; - if (channel_count > RC_INPUT_MAX_CHANNELS) - channel_count = RC_INPUT_MAX_CHANNELS; - input_rc.channel_count = channel_count; + static const unsigned prolog = (PX4IO_P_RAW_RC_BASE - PX4IO_P_RAW_RC_COUNT); + uint16_t regs[RC_INPUT_MAX_CHANNELS + prolog]; - if (channel_count > 0) { - ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE, input_rc.values, channel_count); - if (ret == OK) - input_rc.timestamp = hrt_absolute_time(); + /* + * Read the channel count and the first 9 channels. + * + * This should be the common case (9 channel R/C control being a reasonable upper bound). + */ + input_rc.timestamp = hrt_absolute_time(); + ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, ®s[0], prolog + 9); + if (ret != OK) + return ret; + + /* + * Get the channel count any any extra channels. This is no more expensive than reading the + * channel count once. + */ + channel_count = regs[0]; + if (channel_count > 9) { + ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, ®s[prolog + 9], channel_count - 9); + if (ret != OK) + return ret; } + input_rc.channel_count = channel_count; + memcpy(input_rc.values, ®s[prolog], channel_count * 2); + return ret; } From 8fa226c909d5d34606e8f28bb0b54aeda8f91010 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Jul 2013 23:59:35 -0700 Subject: [PATCH 229/486] Tweak protocol register assignments and add new registers to accommodate differences in IOv2. --- src/drivers/px4io/px4io.cpp | 53 ++++++--- src/modules/px4iofirmware/controls.c | 12 +- src/modules/px4iofirmware/mixer.cpp | 16 +-- src/modules/px4iofirmware/protocol.h | 52 +++++---- src/modules/px4iofirmware/px4io.h | 58 +++++----- src/modules/px4iofirmware/registers.c | 103 ++++++++++++++---- .../preflight_check/preflight_check.c | 2 +- 7 files changed, 198 insertions(+), 98 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 951bfe6952..15e213afb7 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -134,6 +134,7 @@ private: PX4IO_interface *_interface; // XXX + unsigned _hardware; unsigned _max_actuators; unsigned _max_controls; unsigned _max_rc_input; @@ -318,6 +319,7 @@ PX4IO *g_dev; PX4IO::PX4IO(PX4IO_interface *interface) : CDev("px4io", "/dev/px4io"), _interface(interface), + _hardware(0), _max_actuators(0), _max_controls(0), _max_rc_input(0), @@ -387,18 +389,25 @@ PX4IO::init() return ret; /* get some parameters */ + unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); + if (protocol != PX4IO_PROTOCOL_VERSION) { + log("protocol/firmware mismatch"); + mavlink_log_emergency(_mavlink_fd, "[IO] protocol/firmware mismatch, abort."); + return -1; + } + _hardware = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION); _max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT); - _max_controls = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT); + _max_controls = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT); _max_relays = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT); _max_transfer = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER) - 2; _max_rc_input = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT); if ((_max_actuators < 1) || (_max_actuators > 255) || - (_max_relays < 1) || (_max_relays > 255) || + (_max_relays > 32) || (_max_transfer < 16) || (_max_transfer > 255) || (_max_rc_input < 1) || (_max_rc_input > 255)) { - log("failed getting parameters from PX4IO"); - mavlink_log_emergency(_mavlink_fd, "[IO] param read fail, abort."); + log("config read error"); + mavlink_log_emergency(_mavlink_fd, "[IO] config read fail, abort."); return -1; } if (_max_rc_input > RC_INPUT_MAX_CHANNELS) @@ -1122,7 +1131,7 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned int ret = _interface->set_reg(page, offset, values, num_values); if (ret != OK) - debug("io_reg_set: error %d", ret); + debug("io_reg_set(%u,%u,%u): error %d", page, offset, num_values, errno); return ret; } @@ -1143,7 +1152,7 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_v int ret = _interface->get_reg(page, offset, values, num_values); if (ret != OK) - debug("io_reg_get: data error %d", ret); + debug("io_reg_get(%u,%u,%u): data error %d", page, offset, num_values, errno); return ret; } @@ -1237,9 +1246,9 @@ void PX4IO::print_status() { /* basic configuration */ - printf("protocol %u software %u bootloader %u buffer %uB\n", + printf("protocol %u hardware %u bootloader %u buffer %uB\n", io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION), - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_SOFTWARE_VERSION), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION), io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_BOOTLOADER_VERSION), io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER)); printf("%u controls %u actuators %u R/C inputs %u analog inputs %u relays\n", @@ -1268,7 +1277,7 @@ PX4IO::print_status() ((flags & PX4IO_P_STATUS_FLAGS_INIT_OK) ? " INIT_OK" : " INIT_FAIL"), ((flags & PX4IO_P_STATUS_FLAGS_FAILSAFE) ? " FAILSAFE" : "")); uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS); - printf("alarms 0x%04x%s%s%s%s%s%s%s\n", + printf("alarms 0x%04x%s%s%s%s%s%s%s%s\n", alarms, ((alarms & PX4IO_P_STATUS_ALARMS_VBATT_LOW) ? " VBATT_LOW" : ""), ((alarms & PX4IO_P_STATUS_ALARMS_TEMPERATURE) ? " TEMPERATURE" : ""), @@ -1276,18 +1285,26 @@ PX4IO::print_status() ((alarms & PX4IO_P_STATUS_ALARMS_ACC_CURRENT) ? " ACC_CURRENT" : ""), ((alarms & PX4IO_P_STATUS_ALARMS_FMU_LOST) ? " FMU_LOST" : ""), ((alarms & PX4IO_P_STATUS_ALARMS_RC_LOST) ? " RC_LOST" : ""), - ((alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) ? " PWM_ERROR" : "")); + ((alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) ? " PWM_ERROR" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_VSERVO_FAULT) ? " VSERVO_FAULT" : "")); /* now clear alarms */ io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, 0xFFFF); - printf("vbatt %u ibatt %u vbatt scale %u\n", - io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT), - io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT), - io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE)); - printf("amp_per_volt %.3f amp_offset %.3f mAh discharged %.3f\n", - (double)_battery_amp_per_volt, - (double)_battery_amp_bias, - (double)_battery_mamphour_total); + if (_hardware == 1) { + printf("vbatt %u ibatt %u vbatt scale %u\n", + io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT), + io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE)); + printf("amp_per_volt %.3f amp_offset %.3f mAh discharged %.3f\n", + (double)_battery_amp_per_volt, + (double)_battery_amp_bias, + (double)_battery_mamphour_total); + } else if (_hardware == 2) { + printf("vservo %u vservo scale %u\n", + io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VSERVO), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE)); + printf("vrssi %u\n", io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VRSSI)); + } printf("actuators"); for (unsigned i = 0; i < _max_actuators; i++) printf(" %u", io_reg_get(PX4IO_PAGE_ACTUATORS, i)); diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 95103964e5..5a95a8aa9b 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -66,7 +66,7 @@ controls_init(void) sbus_init("/dev/ttyS2"); /* default to a 1:1 input map, all enabled */ - for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) { + for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) { unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i; r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = 0; @@ -117,7 +117,7 @@ controls_tick() { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; perf_end(c_gather_ppm); - ASSERT(r_raw_rc_count <= MAX_CONTROL_CHANNELS); + ASSERT(r_raw_rc_count <= PX4IO_CONTROL_CHANNELS); /* * In some cases we may have received a frame, but input has still @@ -190,7 +190,7 @@ controls_tick() { /* and update the scaled/mapped version */ unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; - ASSERT(mapped < MAX_CONTROL_CHANNELS); + ASSERT(mapped < PX4IO_CONTROL_CHANNELS); /* invert channel if pitch - pulling the lever down means pitching up by convention */ if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */ @@ -202,7 +202,7 @@ controls_tick() { } /* set un-assigned controls to zero */ - for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) { + for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) { if (!(assigned_channels & (1 << i))) r_rc_values[i] = 0; } @@ -312,8 +312,8 @@ ppm_input(uint16_t *values, uint16_t *num_values) /* PPM data exists, copy it */ *num_values = ppm_decoded_channels; - if (*num_values > MAX_CONTROL_CHANNELS) - *num_values = MAX_CONTROL_CHANNELS; + if (*num_values > PX4IO_CONTROL_CHANNELS) + *num_values = PX4IO_CONTROL_CHANNELS; for (unsigned i = 0; i < *num_values; i++) values[i] = ppm_buffer[i]; diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index a2193b526b..d8c0e58bad 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -154,7 +154,7 @@ mixer_tick(void) if (source == MIX_FAILSAFE) { /* copy failsafe values to the servo outputs */ - for (unsigned i = 0; i < IO_SERVO_COUNT; i++) { + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { r_page_servos[i] = r_page_servo_failsafe[i]; /* safe actuators for FMU feedback */ @@ -164,11 +164,11 @@ mixer_tick(void) } else if (source != MIX_NONE) { - float outputs[IO_SERVO_COUNT]; + float outputs[PX4IO_SERVO_COUNT]; unsigned mixed; /* mix */ - mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT); + mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT); /* scale to PWM and update the servo outputs as required */ for (unsigned i = 0; i < mixed; i++) { @@ -180,7 +180,7 @@ mixer_tick(void) r_page_servos[i] = (outputs[i] * 600.0f) + 1500; } - for (unsigned i = mixed; i < IO_SERVO_COUNT; i++) + for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) r_page_servos[i] = 0; } @@ -215,7 +215,7 @@ mixer_tick(void) if (mixer_servos_armed) { /* update the servo outputs. */ - for (unsigned i = 0; i < IO_SERVO_COUNT; i++) + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) up_pwm_servo_set(i, r_page_servos[i]); } } @@ -349,11 +349,11 @@ mixer_set_failsafe() return; /* set failsafe defaults to the values for all inputs = 0 */ - float outputs[IO_SERVO_COUNT]; + float outputs[PX4IO_SERVO_COUNT]; unsigned mixed; /* mix */ - mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT); + mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT); /* scale to PWM and update the servo outputs as required */ for (unsigned i = 0; i < mixed; i++) { @@ -364,7 +364,7 @@ mixer_set_failsafe() } /* disable the rest of the outputs */ - for (unsigned i = mixed; i < IO_SERVO_COUNT; i++) + for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) r_page_servo_failsafe[i] = 0; } diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 48ad4316f1..fa57dfc3f9 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -62,12 +62,11 @@ * Note that the implementation of readable pages prefers registers within * readable pages to be densely packed. Page numbers do not need to be * packed. + * + * Definitions marked 1 are only valid on PX4IOv1 boards. Likewise, + * [2] denotes definitions specific to the PX4IOv2 board. */ -#define PX4IO_CONTROL_CHANNELS 8 -#define PX4IO_INPUT_CHANNELS 12 -#define PX4IO_RELAY_CHANNELS 4 - /* Per C, this is safe for all 2's complement systems */ #define REG_TO_SIGNED(_reg) ((int16_t)(_reg)) #define SIGNED_TO_REG(_signed) ((uint16_t)(_signed)) @@ -75,10 +74,12 @@ #define REG_TO_FLOAT(_reg) ((float)REG_TO_SIGNED(_reg) / 10000.0f) #define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)((_float) * 10000.0f)) +#define PX4IO_PROTOCOL_VERSION 2 + /* static configuration page */ #define PX4IO_PAGE_CONFIG 0 -#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* magic numbers TBD */ -#define PX4IO_P_CONFIG_SOFTWARE_VERSION 1 /* magic numbers TBD */ +#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* PX4IO_PROTOCOL_VERSION */ +#define PX4IO_P_CONFIG_HARDWARE_VERSION 1 /* magic numbers TBD */ #define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */ #define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum I2C transfer size */ #define PX4IO_P_CONFIG_CONTROL_COUNT 4 /* hardcoded max control count supported */ @@ -107,16 +108,20 @@ #define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */ #define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */ -#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* VBatt is very close to regulator dropout */ +#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* [1] VBatt is very close to regulator dropout */ #define PX4IO_P_STATUS_ALARMS_TEMPERATURE (1 << 1) /* board temperature is high */ -#define PX4IO_P_STATUS_ALARMS_SERVO_CURRENT (1 << 2) /* servo current limit was exceeded */ -#define PX4IO_P_STATUS_ALARMS_ACC_CURRENT (1 << 3) /* accessory current limit was exceeded */ +#define PX4IO_P_STATUS_ALARMS_SERVO_CURRENT (1 << 2) /* [1] servo current limit was exceeded */ +#define PX4IO_P_STATUS_ALARMS_ACC_CURRENT (1 << 3) /* [1] accessory current limit was exceeded */ #define PX4IO_P_STATUS_ALARMS_FMU_LOST (1 << 4) /* timed out waiting for controls from FMU */ #define PX4IO_P_STATUS_ALARMS_RC_LOST (1 << 5) /* timed out waiting for RC input */ #define PX4IO_P_STATUS_ALARMS_PWM_ERROR (1 << 6) /* PWM configuration or output was bad */ +#define PX4IO_P_STATUS_ALARMS_VSERVO_FAULT (1 << 7) /* [2] VServo was out of the valid range (2.5 - 5.5 V) */ -#define PX4IO_P_STATUS_VBATT 4 /* battery voltage in mV */ -#define PX4IO_P_STATUS_IBATT 5 /* battery current (raw ADC) */ +#define PX4IO_P_STATUS_VBATT 4 /* [1] battery voltage in mV */ +#define PX4IO_P_STATUS_IBATT 5 /* [1] battery current (raw ADC) */ +#define PX4IO_P_STATUS_VSERVO 6 /* [2] servo rail voltage in mV */ +#define PX4IO_P_STATUS_VRSSI 7 /* [2] RSSI voltage */ +#define PX4IO_P_STATUS_PRSSI 8 /* [2] RSSI PWM value */ /* array of post-mix actuator outputs, -10000..10000 */ #define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */ @@ -142,7 +147,7 @@ #define PX4IO_RATE_MAP_BASE 0 /* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */ /* setup page */ -#define PX4IO_PAGE_SETUP 100 +#define PX4IO_PAGE_SETUP 50 #define PX4IO_P_SETUP_FEATURES 0 #define PX4IO_P_SETUP_ARMING 1 /* arming controls */ @@ -155,18 +160,27 @@ #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 */ #define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */ + #define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */ -#define PX4IO_P_SETUP_VBATT_SCALE 6 /* battery voltage correction factor (float) */ +#define PX4IO_P_SETUP_RELAYS_POWER1 (1<<0) /* [1] power relay 1 */ +#define PX4IO_P_SETUP_RELAYS_POWER2 (1<<1) /* [1] power relay 2 */ +#define PX4IO_P_SETUP_RELAYS_ACC1 (1<<2) /* [1] accessory power 1 */ +#define PX4IO_P_SETUP_RELAYS_ACC2 (1<<3) /* [1] accessory power 2 */ + +#define PX4IO_P_SETUP_VBATT_SCALE 6 /* [1] battery voltage correction factor (float) */ +#define PX4IO_P_SETUP_VSERVO_SCALE 6 /* [2] servo voltage correction factor (float) */ + /* 7 */ + /* 8 */ #define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ /* autopilot control values, -10000..10000 */ -#define PX4IO_PAGE_CONTROLS 101 /* 0..CONFIG_CONTROL_COUNT */ +#define PX4IO_PAGE_CONTROLS 51 /* 0..CONFIG_CONTROL_COUNT */ /* raw text load to the mixer parser - ignores offset */ -#define PX4IO_PAGE_MIXERLOAD 102 +#define PX4IO_PAGE_MIXERLOAD 52 /* R/C channel config */ -#define PX4IO_PAGE_RC_CONFIG 103 /* R/C input configuration */ +#define PX4IO_PAGE_RC_CONFIG 53 /* R/C input configuration */ #define PX4IO_P_RC_CONFIG_MIN 0 /* lowest input value */ #define PX4IO_P_RC_CONFIG_CENTER 1 /* center input value */ #define PX4IO_P_RC_CONFIG_MAX 2 /* highest input value */ @@ -178,13 +192,13 @@ #define PX4IO_P_RC_CONFIG_STRIDE 6 /* spacing between channel config data */ /* PWM output - overrides mixer */ -#define PX4IO_PAGE_DIRECT_PWM 104 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +#define PX4IO_PAGE_DIRECT_PWM 54 /* 0..CONFIG_ACTUATOR_COUNT-1 */ /* PWM failsafe values - zero disables the output */ -#define PX4IO_PAGE_FAILSAFE_PWM 105 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +#define PX4IO_PAGE_FAILSAFE_PWM 55 /* 0..CONFIG_ACTUATOR_COUNT-1 */ /* Debug and test page - not used in normal operation */ -#define PX4IO_PAGE_TEST 127 +#define PX4IO_PAGE_TEST 127 #define PX4IO_P_TEST_LED 0 /* set the amber LED on/off */ /** diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 0779ffd8fa..b32782285b 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -54,8 +54,9 @@ /* * Constants and limits. */ -#define MAX_CONTROL_CHANNELS 12 -#define IO_SERVO_COUNT 8 +#define PX4IO_SERVO_COUNT 8 +#define PX4IO_CONTROL_CHANNELS 8 +#define PX4IO_INPUT_CHANNELS 12 /* * Debug logging @@ -124,34 +125,41 @@ extern struct sys_state_s system_state; /* * GPIO handling. */ -#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_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)) + +#ifdef CONFIG_ARCH_BOARD_PX4IOV2 + +# define PX4IO_RELAY_CHANNELS 0 +# define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s)) + +# define VDD_SERVO_FAULT (!stm32_gpioread(GPIO_SERVO_FAULT_DETECT)) + +# define PX4IO_ADC_CHANNEL_COUNT 2 +# define ADC_VSERVO 4 +# define ADC_RSSI 5 + +#else /* CONFIG_ARCH_BOARD_PX4IOV1 */ + +# define PX4IO_RELAY_CHANNELS 4 +# define POWER_SERVO(_s) stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s)) +# define POWER_ACC1(_s) stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s)) +# define POWER_ACC2(_s) stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s)) +# define POWER_RELAY1(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s)) +# define POWER_RELAY2(_s) stm32_gpiowrite(GPIO_RELAY2_EN, (_s)) + +# define OVERCURRENT_ACC (!stm32_gpioread(GPIO_ACC_OC_DETECT)) +# define OVERCURRENT_SERVO (!stm32_gpioread(GPIO_SERVO_OC_DETECT)) + +# define PX4IO_ADC_CHANNEL_COUNT 2 +# define ADC_VBATT 4 +# define ADC_IN5 5 -#ifdef GPIO_SERVO_PWR_EN -# define POWER_SERVO(_s) stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s)) -#endif -#ifdef GPIO_ACC1_PWR_EN -# define POWER_ACC1(_s) stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s)) -#endif -#ifdef GPIO_ACC2_PWR_EN -# define POWER_ACC2(_s) stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s)) -#endif -#ifdef GPIO_RELAY1_EN -# define POWER_RELAY1(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s)) -#endif -#ifdef GPIO_RELAY2_EN -# define POWER_RELAY2(_s) stm32_gpiowrite(GPIO_RELAY2_EN, (_s)) #endif -#define OVERCURRENT_ACC (!stm32_gpioread(GPIO_ACC_OC_DETECT)) -#define OVERCURRENT_SERVO (!stm32_gpioread(GPIO_SERVO_OC_DETECT)) #define BUTTON_SAFETY stm32_gpioread(GPIO_BTN_SAFETY) -#define ADC_VBATT 4 -#define ADC_IN5 5 -#define ADC_CHANNEL_COUNT 2 - /* * Mixer */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index b977375f47..f4541936b0 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -58,14 +58,18 @@ static void pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t alt * Static configuration parameters. */ static const uint16_t r_page_config[] = { - [PX4IO_P_CONFIG_PROTOCOL_VERSION] = 1, /* XXX hardcoded magic number */ - [PX4IO_P_CONFIG_SOFTWARE_VERSION] = 1, /* XXX hardcoded magic number */ + [PX4IO_P_CONFIG_PROTOCOL_VERSION] = PX4IO_PROTOCOL_VERSION, +#ifdef CONFIG_ARCH_BOARD_PX4IOV2 + [PX4IO_P_CONFIG_HARDWARE_VERSION] = 2, +#else + [PX4IO_P_CONFIG_HARDWARE_VERSION] = 1, +#endif [PX4IO_P_CONFIG_BOOTLOADER_VERSION] = 3, /* XXX hardcoded magic number */ [PX4IO_P_CONFIG_MAX_TRANSFER] = 64, /* XXX hardcoded magic number */ [PX4IO_P_CONFIG_CONTROL_COUNT] = PX4IO_CONTROL_CHANNELS, - [PX4IO_P_CONFIG_ACTUATOR_COUNT] = IO_SERVO_COUNT, - [PX4IO_P_CONFIG_RC_INPUT_COUNT] = MAX_CONTROL_CHANNELS, - [PX4IO_P_CONFIG_ADC_INPUT_COUNT] = ADC_CHANNEL_COUNT, + [PX4IO_P_CONFIG_ACTUATOR_COUNT] = PX4IO_SERVO_COUNT, + [PX4IO_P_CONFIG_RC_INPUT_COUNT] = PX4IO_CONTROL_CHANNELS, + [PX4IO_P_CONFIG_ADC_INPUT_COUNT] = PX4IO_ADC_CHANNEL_COUNT, [PX4IO_P_CONFIG_RELAY_COUNT] = PX4IO_RELAY_CHANNELS, }; @@ -80,7 +84,10 @@ uint16_t r_page_status[] = { [PX4IO_P_STATUS_FLAGS] = 0, [PX4IO_P_STATUS_ALARMS] = 0, [PX4IO_P_STATUS_VBATT] = 0, - [PX4IO_P_STATUS_IBATT] = 0 + [PX4IO_P_STATUS_IBATT] = 0, + [PX4IO_P_STATUS_VSERVO] = 0, + [PX4IO_P_STATUS_VRSSI] = 0, + [PX4IO_P_STATUS_PRSSI] = 0 }; /** @@ -88,14 +95,14 @@ uint16_t r_page_status[] = { * * Post-mixed actuator values. */ -uint16_t r_page_actuators[IO_SERVO_COUNT]; +uint16_t r_page_actuators[PX4IO_SERVO_COUNT]; /** * PAGE 3 * * Servo PWM values */ -uint16_t r_page_servos[IO_SERVO_COUNT]; +uint16_t r_page_servos[PX4IO_SERVO_COUNT]; /** * PAGE 4 @@ -105,7 +112,7 @@ uint16_t r_page_servos[IO_SERVO_COUNT]; uint16_t r_page_raw_rc_input[] = { [PX4IO_P_RAW_RC_COUNT] = 0, - [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + MAX_CONTROL_CHANNELS)] = 0 + [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_CONTROL_CHANNELS)] = 0 }; /** @@ -115,7 +122,7 @@ uint16_t r_page_raw_rc_input[] = */ uint16_t r_page_rc_input[] = { [PX4IO_P_RC_VALID] = 0, - [PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + MAX_CONTROL_CHANNELS)] = 0 + [PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + PX4IO_CONTROL_CHANNELS)] = 0 }; /** @@ -148,7 +155,7 @@ volatile uint16_t r_page_setup[] = PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \ PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \ PX4IO_P_SETUP_ARMING_IO_ARM_OK) -#define PX4IO_P_SETUP_RATES_VALID ((1 << IO_SERVO_COUNT) - 1) +#define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1) #define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) /** @@ -167,7 +174,7 @@ volatile uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS]; * * R/C channel input configuration. */ -uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE]; +uint16_t r_page_rc_input_config[PX4IO_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE]; /* valid options */ #define PX4IO_P_RC_CONFIG_OPTIONS_VALID (PX4IO_P_RC_CONFIG_OPTIONS_REVERSE | PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) @@ -183,7 +190,7 @@ uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE * * Disable pulses as default. */ -uint16_t r_page_servo_failsafe[IO_SERVO_COUNT] = { 0 }; +uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0 }; int registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) @@ -234,7 +241,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num case PX4IO_PAGE_FAILSAFE_PWM: /* copy channel data */ - while ((offset < IO_SERVO_COUNT) && (num_values > 0)) { + while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { /* XXX range-check value? */ r_page_servo_failsafe[offset] = *values; @@ -366,6 +373,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) #endif break; + case PX4IO_P_SETUP_VBATT_SCALE: + r_page_setup[PX4IO_P_SETUP_VBATT_SCALE] = value; + break; + case PX4IO_P_SETUP_SET_DEBUG: r_page_setup[PX4IO_P_SETUP_SET_DEBUG] = value; isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]); @@ -388,7 +399,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE; uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE]; - if (channel >= MAX_CONTROL_CHANNELS) + if (channel >= PX4IO_CONTROL_CHANNELS) return -1; /* disable the channel until we have a chance to sanity-check it */ @@ -433,7 +444,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) { count++; } - if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) { + if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_CONTROL_CHANNELS) { count++; } @@ -497,6 +508,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val /* PX4IO_P_STATUS_ALARMS maintained externally */ +#ifdef ADC_VBATT /* PX4IO_P_STATUS_VBATT */ { /* @@ -530,7 +542,8 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val r_page_status[PX4IO_P_STATUS_VBATT] = corrected; } } - +#endif +#ifdef ADC_IBATT /* PX4IO_P_STATUS_IBATT */ { /* @@ -540,26 +553,74 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val FMU sort it out, with user selectable configuration for their sensor */ - unsigned counts = adc_measure(ADC_IN5); + unsigned counts = adc_measure(ADC_IBATT); if (counts != 0xffff) { r_page_status[PX4IO_P_STATUS_IBATT] = counts; } } +#endif +#ifdef ADC_VSERVO + /* PX4IO_P_STATUS_VSERVO */ + { + /* + * Coefficients here derived by measurement of the 5-16V + * range on one unit: + * + * V counts + * 5 1001 + * 6 1219 + * 7 1436 + * 8 1653 + * 9 1870 + * 10 2086 + * 11 2303 + * 12 2522 + * 13 2738 + * 14 2956 + * 15 3172 + * 16 3389 + * + * slope = 0.0046067 + * intercept = 0.3863 + * + * Intercept corrected for best results @ 12V. + */ + unsigned counts = adc_measure(ADC_VSERVO); + if (counts != 0xffff) { + unsigned mV = (4150 + (counts * 46)) / 10 - 200; + unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000; + + r_page_status[PX4IO_P_STATUS_VSERVO] = corrected; + } + } +#endif + /* XXX PX4IO_P_STATUS_VRSSI */ + /* XXX PX4IO_P_STATUS_PRSSI */ SELECT_PAGE(r_page_status); break; case PX4IO_PAGE_RAW_ADC_INPUT: memset(r_page_scratch, 0, sizeof(r_page_scratch)); +#ifdef ADC_VBATT r_page_scratch[0] = adc_measure(ADC_VBATT); - r_page_scratch[1] = adc_measure(ADC_IN5); +#endif +#ifdef ADC_IBATT + r_page_scratch[1] = adc_measure(ADC_IBATT); +#endif +#ifdef ADC_VSERVO + r_page_scratch[0] = adc_measure(ADC_VSERVO); +#endif +#ifdef ADC_RSSI + r_page_scratch[1] = adc_measure(ADC_RSSI); +#endif SELECT_PAGE(r_page_scratch); break; case PX4IO_PAGE_PWM_INFO: memset(r_page_scratch, 0, sizeof(r_page_scratch)); - for (unsigned i = 0; i < IO_SERVO_COUNT; i++) + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) r_page_scratch[PX4IO_RATE_MAP_BASE + i] = up_pwm_servo_get_rate_group(i); SELECT_PAGE(r_page_scratch); @@ -631,7 +692,7 @@ static void pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate) { for (unsigned pass = 0; pass < 2; pass++) { - for (unsigned group = 0; group < IO_SERVO_COUNT; group++) { + for (unsigned group = 0; group < PX4IO_SERVO_COUNT; group++) { /* get the channel mask for this rate group */ uint32_t mask = up_pwm_servo_get_rate_group(group); diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index 4bcce18fb8..6d1ecb3215 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -210,7 +210,7 @@ int preflight_check_main(int argc, char *argv[]) } /* XXX needs inspection of all the _MAP params */ - // if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) { + // if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_CONTROL_CHANNELS) { // mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1); // /* give system time to flush error message in case there are more */ // usleep(100000); From 9fe257c4d151280c770e607bc3160703f9503889 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 7 Jul 2013 12:13:40 -0700 Subject: [PATCH 230/486] A very slender config just for test builds. --- ROMFS/px4fmu_test/init.d/rc.standalone | 13 ++++++++ ROMFS/px4fmu_test/init.d/rcS | 4 +++ makefiles/config_px4fmuv2_test.mk | 45 ++++++++++++++++++++++++++ 3 files changed, 62 insertions(+) create mode 100644 ROMFS/px4fmu_test/init.d/rc.standalone create mode 100755 ROMFS/px4fmu_test/init.d/rcS create mode 100644 makefiles/config_px4fmuv2_test.mk diff --git a/ROMFS/px4fmu_test/init.d/rc.standalone b/ROMFS/px4fmu_test/init.d/rc.standalone new file mode 100644 index 0000000000..67e95215b9 --- /dev/null +++ b/ROMFS/px4fmu_test/init.d/rc.standalone @@ -0,0 +1,13 @@ +#!nsh +# +# Flight startup script for PX4FMU standalone configuration. +# + +echo "[init] doing standalone PX4FMU startup..." + +# +# Start the ORB +# +uorb start + +echo "[init] startup done" diff --git a/ROMFS/px4fmu_test/init.d/rcS b/ROMFS/px4fmu_test/init.d/rcS new file mode 100755 index 0000000000..7f161b053a --- /dev/null +++ b/ROMFS/px4fmu_test/init.d/rcS @@ -0,0 +1,4 @@ +#!nsh +# +# PX4FMU startup script for test hackery. +# diff --git a/makefiles/config_px4fmuv2_test.mk b/makefiles/config_px4fmuv2_test.mk new file mode 100644 index 0000000000..0bd6c18e5f --- /dev/null +++ b/makefiles/config_px4fmuv2_test.mk @@ -0,0 +1,45 @@ +# +# Makefile for the px4fmu_default configuration +# + +# +# Use the configuration's ROMFS. +# +ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_test + +# +# Board support modules +# +MODULES += drivers/device +MODULES += drivers/stm32 +MODULES += drivers/px4io +MODULES += drivers/boards/px4fmuv2 +MODULES += systemcmds/perf +MODULES += systemcmds/reboot + +# needed because things use it for logging +MODULES += modules/mavlink + +# +# Library modules +# +MODULES += modules/systemlib +MODULES += modules/systemlib/mixer +MODULES += modules/uORB + +# +# Transitional support - add commands from the NuttX export archive. +# +# In general, these should move to modules over time. +# +# Each entry here is ... but we use a helper macro +# to make the table a bit more readable. +# +define _B + $(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4) +endef + +# command priority stack entrypoint +BUILTIN_COMMANDS := \ + $(call _B, sercon, , 2048, sercon_main ) \ + $(call _B, serdis, , 2048, serdis_main ) From b4029dd824cec7a0b53c62e960f80d90ddc6e13c Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 7 Jul 2013 17:53:55 -0700 Subject: [PATCH 231/486] Pull v2 pieces up to build with the merge --- ...px4fmuv2.prototype => px4fmu-v2.prototype} | 0 Images/px4io-v2.prototype | 12 + Makefile | 87 +- .../{board_px4fmuv2.mk => board_px4fmu-v2.mk} | 0 .../{board_px4iov2.mk => board_px4io-v2.mk} | 0 ...default.mk => config_px4fmu-v2_default.mk} | 0 ...fmuv2_test.mk => config_px4fmu-v2_test.mk} | 5 - ..._default.mk => config_px4io-v2_default.mk} | 0 makefiles/setup.mk | 1 - makefiles/upload.mk | 2 +- .../px4fmu-v2}/Kconfig | 2 +- .../px4fmu-v2}/common/Make.defs | 0 .../px4fmu-v2}/common/ld.script | 0 .../px4fmu-v2}/include/board.h | 8 +- .../px4fmu-v2}/include/nsh_romfsimg.h | 0 nuttx-configs/px4fmu-v2/nsh/Make.defs | 3 + .../px4fmu-v2}/nsh/appconfig | 0 nuttx-configs/px4fmu-v2/nsh/defconfig | 1022 +++++++++++++++++ .../px4fmu-v2/nsh/defconfig.prev | 20 +- .../px4fmu-v2}/nsh/setenv.sh | 0 .../px4fmu-v2}/src/Makefile | 0 .../px4fmu-v2}/src/empty.c | 0 .../px4io-v2}/README.txt | 0 .../px4io-v2}/common/Make.defs | 0 .../px4io-v2}/common/ld.script | 0 .../px4io-v2}/common/setenv.sh | 0 .../px4io-v2}/include/README.txt | 0 .../px4io-v2}/include/board.h | 17 +- nuttx-configs/px4io-v2/nsh/Make.defs | 3 + .../px4io-v2/nsh}/appconfig | 0 .../px4io-v2/nsh}/defconfig | 26 +- .../px4io-v2/nsh}/setenv.sh | 0 .../px4io-v2}/src/Makefile | 0 .../px4io-v2}/src/README.txt | 0 .../px4io-v2}/src/empty.c | 0 .../px4io-v2/test}/Make.defs | 0 .../px4io-v2/test}/appconfig | 0 .../px4io-v2/test}/defconfig | 25 +- .../px4io-v2/test}/setenv.sh | 0 nuttx/configs/px4fmuv2/nsh/Make.defs | 3 - nuttx/configs/px4iov2/io/Make.defs | 3 - src/drivers/boards/px4fmuv2/px4fmu_init.c | 4 +- src/drivers/boards/px4fmuv2/px4fmu_internal.h | 2 +- .../boards/px4fmuv2/px4fmu_pwm_servo.c | 2 +- src/drivers/boards/px4fmuv2/px4fmu_spi.c | 6 +- src/drivers/boards/px4fmuv2/px4fmu_usb.c | 4 +- src/drivers/boards/px4iov2/px4iov2_init.c | 2 +- src/drivers/boards/px4iov2/px4iov2_internal.h | 2 +- .../boards/px4iov2/px4iov2_pwm_servo.c | 2 +- src/drivers/drv_gpio.h | 40 +- src/drivers/px4fmu/fmu.cpp | 28 +- src/drivers/px4io/interface_serial.cpp | 1 - src/drivers/px4io/px4io.cpp | 8 +- src/drivers/px4io/uploader.cpp | 4 +- src/drivers/stm32/drv_hrt.c | 8 +- src/drivers/stm32/tone_alarm/tone_alarm.cpp | 4 - src/modules/px4iofirmware/module.mk | 4 +- src/modules/px4iofirmware/px4io.h | 30 +- src/modules/px4iofirmware/registers.c | 2 +- src/modules/px4iofirmware/serial.c | 2 +- 60 files changed, 1167 insertions(+), 227 deletions(-) rename Images/{px4fmuv2.prototype => px4fmu-v2.prototype} (100%) create mode 100644 Images/px4io-v2.prototype rename makefiles/{board_px4fmuv2.mk => board_px4fmu-v2.mk} (100%) rename makefiles/{board_px4iov2.mk => board_px4io-v2.mk} (100%) rename makefiles/{config_px4fmuv2_default.mk => config_px4fmu-v2_default.mk} (100%) rename makefiles/{config_px4fmuv2_test.mk => config_px4fmu-v2_test.mk} (89%) rename makefiles/{config_px4iov2_default.mk => config_px4io-v2_default.mk} (100%) rename {nuttx/configs/px4fmuv2 => nuttx-configs/px4fmu-v2}/Kconfig (82%) rename {nuttx/configs/px4fmuv2 => nuttx-configs/px4fmu-v2}/common/Make.defs (100%) rename {nuttx/configs/px4fmuv2 => nuttx-configs/px4fmu-v2}/common/ld.script (100%) rename {nuttx/configs/px4fmuv2 => nuttx-configs/px4fmu-v2}/include/board.h (98%) rename {nuttx/configs/px4fmuv2 => nuttx-configs/px4fmu-v2}/include/nsh_romfsimg.h (100%) create mode 100644 nuttx-configs/px4fmu-v2/nsh/Make.defs rename {nuttx/configs/px4fmuv2 => nuttx-configs/px4fmu-v2}/nsh/appconfig (100%) create mode 100644 nuttx-configs/px4fmu-v2/nsh/defconfig rename nuttx/configs/px4fmuv2/nsh/defconfig => nuttx-configs/px4fmu-v2/nsh/defconfig.prev (98%) rename {nuttx/configs/px4fmuv2 => nuttx-configs/px4fmu-v2}/nsh/setenv.sh (100%) rename {nuttx/configs/px4fmuv2 => nuttx-configs/px4fmu-v2}/src/Makefile (100%) rename {nuttx/configs/px4fmuv2 => nuttx-configs/px4fmu-v2}/src/empty.c (100%) rename {nuttx/configs/px4iov2 => nuttx-configs/px4io-v2}/README.txt (100%) rename {nuttx/configs/px4iov2 => nuttx-configs/px4io-v2}/common/Make.defs (100%) rename {nuttx/configs/px4iov2 => nuttx-configs/px4io-v2}/common/ld.script (100%) rename {nuttx/configs/px4iov2 => nuttx-configs/px4io-v2}/common/setenv.sh (100%) rename {nuttx/configs/px4iov2 => nuttx-configs/px4io-v2}/include/README.txt (100%) rename {nuttx/configs/px4iov2 => nuttx-configs/px4io-v2}/include/board.h (93%) create mode 100644 nuttx-configs/px4io-v2/nsh/Make.defs rename {nuttx/configs/px4iov2/io => nuttx-configs/px4io-v2/nsh}/appconfig (100%) rename {nuttx/configs/px4iov2/io => nuttx-configs/px4io-v2/nsh}/defconfig (96%) rename {nuttx/configs/px4iov2/io => nuttx-configs/px4io-v2/nsh}/setenv.sh (100%) rename {nuttx/configs/px4iov2 => nuttx-configs/px4io-v2}/src/Makefile (100%) rename {nuttx/configs/px4iov2 => nuttx-configs/px4io-v2}/src/README.txt (100%) rename {nuttx/configs/px4iov2 => nuttx-configs/px4io-v2}/src/empty.c (100%) rename {nuttx/configs/px4iov2/nsh => nuttx-configs/px4io-v2/test}/Make.defs (100%) rename {nuttx/configs/px4iov2/nsh => nuttx-configs/px4io-v2/test}/appconfig (100%) rename {nuttx/configs/px4iov2/nsh => nuttx-configs/px4io-v2/test}/defconfig (96%) rename {nuttx/configs/px4iov2/nsh => nuttx-configs/px4io-v2/test}/setenv.sh (100%) delete mode 100644 nuttx/configs/px4fmuv2/nsh/Make.defs delete mode 100644 nuttx/configs/px4iov2/io/Make.defs diff --git a/Images/px4fmuv2.prototype b/Images/px4fmu-v2.prototype similarity index 100% rename from Images/px4fmuv2.prototype rename to Images/px4fmu-v2.prototype diff --git a/Images/px4io-v2.prototype b/Images/px4io-v2.prototype new file mode 100644 index 0000000000..af87924e90 --- /dev/null +++ b/Images/px4io-v2.prototype @@ -0,0 +1,12 @@ +{ + "board_id": 10, + "magic": "PX4FWv2", + "description": "Firmware for the PX4IOv2 board", + "image": "", + "build_time": 0, + "summary": "PX4IOv2", + "version": "2.0", + "image_size": 0, + "git_identity": "", + "board_revision": 0 +} diff --git a/Makefile b/Makefile index 7f98ffaf22..9905f8a63c 100644 --- a/Makefile +++ b/Makefile @@ -100,7 +100,7 @@ all: $(STAGED_FIRMWARES) # is taken care of. # $(STAGED_FIRMWARES): $(IMAGE_DIR)%.px4: $(BUILD_DIR)%.build/firmware.px4 - @echo %% Copying $@ + @$(ECHO) %% Copying $@ $(Q) $(COPY) $< $@ $(Q) $(COPY) $(patsubst %.px4,%.bin,$<) $(patsubst %.px4,%.bin,$@) @@ -111,9 +111,9 @@ $(STAGED_FIRMWARES): $(IMAGE_DIR)%.px4: $(BUILD_DIR)%.build/firmware.px4 $(BUILD_DIR)%.build/firmware.px4: config = $(patsubst $(BUILD_DIR)%.build/firmware.px4,%,$@) $(BUILD_DIR)%.build/firmware.px4: work_dir = $(BUILD_DIR)$(config).build/ $(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4: - @echo %%%% - @echo %%%% Building $(config) in $(work_dir) - @echo %%%% + @$(ECHO) %%%% + @$(ECHO) %%%% Building $(config) in $(work_dir) + @$(ECHO) %%%% $(Q) mkdir -p $(work_dir) $(Q) make -r -C $(work_dir) \ -f $(PX4_MK_DIR)firmware.mk \ @@ -132,8 +132,6 @@ $(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4: # XXX Should support fetching/unpacking from a separate directory to permit # downloads of the prebuilt archives as well... # -# XXX PX4IO configuration name is bad - NuttX configs should probably all be "px4" -# NUTTX_ARCHIVES = $(foreach board,$(BOARDS),$(ARCHIVE_DIR)$(board).export) .PHONY: archives archives: $(NUTTX_ARCHIVES) @@ -146,16 +144,22 @@ endif $(ARCHIVE_DIR)%.export: board = $(notdir $(basename $@)) $(ARCHIVE_DIR)%.export: configuration = nsh -$(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) $(NUTTX_APPS) - @echo %% Configuring NuttX for $(board) +$(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) + @$(ECHO) %% Configuring NuttX for $(board) $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) $(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean + $(Q) (cd $(NUTTX_SRC)/configs && ln -sf $(PX4_BASE)/nuttx-configs/* .) $(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(board)/$(configuration)) - @echo %% Exporting NuttX for $(board) + @$(ECHO) %% Exporting NuttX for $(board) $(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) export $(Q) mkdir -p $(dir $@) $(Q) $(COPY) $(NUTTX_SRC)nuttx-export.zip $@ +$(NUTTX_SRC): + @$(ECHO) "" + @$(ECHO) "NuttX sources missing - clone https://github.com/PX4/NuttX.git and try again." + @$(ECHO) "" + # # Cleanup targets. 'clean' should remove all built products and force # a complete re-compilation, 'distclean' should remove everything @@ -170,46 +174,47 @@ clean: distclean: clean $(Q) $(REMOVE) $(ARCHIVE_DIR)*.export $(Q) make -C $(NUTTX_SRC) -r $(MQUIET) distclean + $(Q) (cd $(NUTTX_SRC)/configs && find . -type l -depth 1 -delete) # # Print some help text # .PHONY: help help: - @echo "" - @echo " PX4 firmware builder" - @echo " ====================" - @echo "" - @echo " Available targets:" - @echo " ------------------" - @echo "" - @echo " archives" - @echo " Build the NuttX RTOS archives that are used by the firmware build." - @echo "" - @echo " all" - @echo " Build all firmware configs: $(CONFIGS)" - @echo " A limited set of configs can be built with CONFIGS=" - @echo "" + @$(ECHO) "" + @$(ECHO) " PX4 firmware builder" + @$(ECHO) " ====================" + @$(ECHO) "" + @$(ECHO) " Available targets:" + @$(ECHO) " ------------------" + @$(ECHO) "" + @$(ECHO) " archives" + @$(ECHO) " Build the NuttX RTOS archives that are used by the firmware build." + @$(ECHO) "" + @$(ECHO) " all" + @$(ECHO) " Build all firmware configs: $(CONFIGS)" + @$(ECHO) " A limited set of configs can be built with CONFIGS=" + @$(ECHO) "" @for config in $(CONFIGS); do \ echo " $$config"; \ echo " Build just the $$config firmware configuration."; \ echo ""; \ done - @echo " clean" - @echo " Remove all firmware build pieces." - @echo "" - @echo " distclean" - @echo " Remove all compilation products, including NuttX RTOS archives." - @echo "" - @echo " upload" - @echo " When exactly one config is being built, add this target to upload the" - @echo " firmware to the board when the build is complete. Not supported for" - @echo " all configurations." - @echo "" - @echo " Common options:" - @echo " ---------------" - @echo "" - @echo " V=1" - @echo " If V is set, more verbose output is printed during the build. This can" - @echo " help when diagnosing issues with the build or toolchain." - @echo "" + @$(ECHO) " clean" + @$(ECHO) " Remove all firmware build pieces." + @$(ECHO) "" + @$(ECHO) " distclean" + @$(ECHO) " Remove all compilation products, including NuttX RTOS archives." + @$(ECHO) "" + @$(ECHO) " upload" + @$(ECHO) " When exactly one config is being built, add this target to upload the" + @$(ECHO) " firmware to the board when the build is complete. Not supported for" + @$(ECHO) " all configurations." + @$(ECHO) "" + @$(ECHO) " Common options:" + @$(ECHO) " ---------------" + @$(ECHO) "" + @$(ECHO) " V=1" + @$(ECHO) " If V is set, more verbose output is printed during the build. This can" + @$(ECHO) " help when diagnosing issues with the build or toolchain." + @$(ECHO) "" diff --git a/makefiles/board_px4fmuv2.mk b/makefiles/board_px4fmu-v2.mk similarity index 100% rename from makefiles/board_px4fmuv2.mk rename to makefiles/board_px4fmu-v2.mk diff --git a/makefiles/board_px4iov2.mk b/makefiles/board_px4io-v2.mk similarity index 100% rename from makefiles/board_px4iov2.mk rename to makefiles/board_px4io-v2.mk diff --git a/makefiles/config_px4fmuv2_default.mk b/makefiles/config_px4fmu-v2_default.mk similarity index 100% rename from makefiles/config_px4fmuv2_default.mk rename to makefiles/config_px4fmu-v2_default.mk diff --git a/makefiles/config_px4fmuv2_test.mk b/makefiles/config_px4fmu-v2_test.mk similarity index 89% rename from makefiles/config_px4fmuv2_test.mk rename to makefiles/config_px4fmu-v2_test.mk index 0bd6c18e5f..9b3013a5bb 100644 --- a/makefiles/config_px4fmuv2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -12,19 +12,14 @@ ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_test # MODULES += drivers/device MODULES += drivers/stm32 -MODULES += drivers/px4io MODULES += drivers/boards/px4fmuv2 MODULES += systemcmds/perf MODULES += systemcmds/reboot -# needed because things use it for logging -MODULES += modules/mavlink - # # Library modules # MODULES += modules/systemlib -MODULES += modules/systemlib/mixer MODULES += modules/uORB # diff --git a/makefiles/config_px4iov2_default.mk b/makefiles/config_px4io-v2_default.mk similarity index 100% rename from makefiles/config_px4iov2_default.mk rename to makefiles/config_px4io-v2_default.mk diff --git a/makefiles/setup.mk b/makefiles/setup.mk index 92461fafbe..a0e880a0d3 100644 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -45,7 +45,6 @@ export PX4_INCLUDE_DIR = $(abspath $(PX4_BASE)/src/include)/ export PX4_MODULE_SRC = $(abspath $(PX4_BASE)/src)/ export PX4_MK_DIR = $(abspath $(PX4_BASE)/makefiles)/ export NUTTX_SRC = $(abspath $(PX4_BASE)/NuttX/nuttx)/ -export NUTTX_APP_SRC = $(abspath $(PX4_BASE)/NuttX/apps)/ export MAVLINK_SRC = $(abspath $(PX4_BASE)/mavlink)/ export ROMFS_SRC = $(abspath $(PX4_BASE)/ROMFS)/ export IMAGE_DIR = $(abspath $(PX4_BASE)/Images)/ diff --git a/makefiles/upload.mk b/makefiles/upload.mk index 4b01b447d0..c55e3f1883 100644 --- a/makefiles/upload.mk +++ b/makefiles/upload.mk @@ -27,7 +27,7 @@ all: upload-$(METHOD)-$(BOARD) upload-serial-px4fmu-v1: $(BUNDLE) $(UPLOADER) $(Q) $(PYTHON) -u $(UPLOADER) --port $(SERIAL_PORTS) $(BUNDLE) -upload-serial-px4fmuv2: $(BUNDLE) $(UPLOADER) +upload-serial-px4fmu-v2: $(BUNDLE) $(UPLOADER) $(Q) $(PYTHON) -u $(UPLOADER) --port $(SERIAL_PORTS) $(BUNDLE) # diff --git a/nuttx/configs/px4fmuv2/Kconfig b/nuttx-configs/px4fmu-v2/Kconfig similarity index 82% rename from nuttx/configs/px4fmuv2/Kconfig rename to nuttx-configs/px4fmu-v2/Kconfig index a10a02ba47..069b25f9e5 100644 --- a/nuttx/configs/px4fmuv2/Kconfig +++ b/nuttx-configs/px4fmu-v2/Kconfig @@ -3,5 +3,5 @@ # see misc/tools/kconfig-language.txt. # -if ARCH_BOARD_PX4FMUV2 +if ARCH_BOARD_PX4FMU_V2 endif diff --git a/nuttx/configs/px4fmuv2/common/Make.defs b/nuttx-configs/px4fmu-v2/common/Make.defs similarity index 100% rename from nuttx/configs/px4fmuv2/common/Make.defs rename to nuttx-configs/px4fmu-v2/common/Make.defs diff --git a/nuttx/configs/px4fmuv2/common/ld.script b/nuttx-configs/px4fmu-v2/common/ld.script similarity index 100% rename from nuttx/configs/px4fmuv2/common/ld.script rename to nuttx-configs/px4fmu-v2/common/ld.script diff --git a/nuttx/configs/px4fmuv2/include/board.h b/nuttx-configs/px4fmu-v2/include/board.h similarity index 98% rename from nuttx/configs/px4fmuv2/include/board.h rename to nuttx-configs/px4fmu-v2/include/board.h index 0055d65e17..a6cdfb4d28 100755 --- a/nuttx/configs/px4fmuv2/include/board.h +++ b/nuttx-configs/px4fmu-v2/include/board.h @@ -46,6 +46,8 @@ # include #endif +#include + /************************************************************************************ * Definitions ************************************************************************************/ @@ -194,10 +196,8 @@ /* High-resolution timer */ -#ifdef CONFIG_HRT_TIMER -# define HRT_TIMER 8 /* use timer8 for the HRT */ -# define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */ -#endif +#define HRT_TIMER 8 /* use timer8 for the HRT */ +#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */ /* Alternate function pin selections ************************************************/ diff --git a/nuttx/configs/px4fmuv2/include/nsh_romfsimg.h b/nuttx-configs/px4fmu-v2/include/nsh_romfsimg.h similarity index 100% rename from nuttx/configs/px4fmuv2/include/nsh_romfsimg.h rename to nuttx-configs/px4fmu-v2/include/nsh_romfsimg.h diff --git a/nuttx-configs/px4fmu-v2/nsh/Make.defs b/nuttx-configs/px4fmu-v2/nsh/Make.defs new file mode 100644 index 0000000000..00257d5467 --- /dev/null +++ b/nuttx-configs/px4fmu-v2/nsh/Make.defs @@ -0,0 +1,3 @@ +include ${TOPDIR}/.config + +include $(TOPDIR)/configs/px4fmu-v2/common/Make.defs diff --git a/nuttx/configs/px4fmuv2/nsh/appconfig b/nuttx-configs/px4fmu-v2/nsh/appconfig similarity index 100% rename from nuttx/configs/px4fmuv2/nsh/appconfig rename to nuttx-configs/px4fmu-v2/nsh/appconfig diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig new file mode 100644 index 0000000000..efbb883a5b --- /dev/null +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -0,0 +1,1022 @@ +# +# Automatically generated file; DO NOT EDIT. +# Nuttx/ Configuration +# +CONFIG_NUTTX_NEWCONFIG=y + +# +# Build Setup +# +# CONFIG_EXPERIMENTAL is not set +# CONFIG_HOST_LINUX is not set +CONFIG_HOST_OSX=y +# CONFIG_HOST_WINDOWS is not set +# CONFIG_HOST_OTHER is not set + +# +# Build Configuration +# +CONFIG_APPS_DIR="../apps" +# CONFIG_BUILD_2PASS is not set + +# +# Binary Output Formats +# +# CONFIG_RRLOAD_BINARY is not set +# CONFIG_INTELHEX_BINARY is not set +# CONFIG_MOTOROLA_SREC is not set +CONFIG_RAW_BINARY=y + +# +# Customize Header Files +# +# CONFIG_ARCH_STDBOOL_H is not set +CONFIG_ARCH_MATH_H=y +# CONFIG_ARCH_FLOAT_H is not set +# CONFIG_ARCH_STDARG_H is not set + +# +# Debug Options +# +# CONFIG_DEBUG is not set +CONFIG_DEBUG_SYMBOLS=y + +# +# System Type +# +# CONFIG_ARCH_8051 is not set +CONFIG_ARCH_ARM=y +# CONFIG_ARCH_AVR is not set +# CONFIG_ARCH_HC is not set +# CONFIG_ARCH_MIPS is not set +# CONFIG_ARCH_RGMP is not set +# CONFIG_ARCH_SH is not set +# CONFIG_ARCH_SIM is not set +# CONFIG_ARCH_X86 is not set +# CONFIG_ARCH_Z16 is not set +# CONFIG_ARCH_Z80 is not set +CONFIG_ARCH="arm" + +# +# ARM Options +# +# CONFIG_ARCH_CHIP_C5471 is not set +# CONFIG_ARCH_CHIP_CALYPSO is not set +# CONFIG_ARCH_CHIP_DM320 is not set +# CONFIG_ARCH_CHIP_IMX is not set +# CONFIG_ARCH_CHIP_KINETIS is not set +# CONFIG_ARCH_CHIP_KL is not set +# CONFIG_ARCH_CHIP_LM is not set +# CONFIG_ARCH_CHIP_LPC17XX is not set +# CONFIG_ARCH_CHIP_LPC214X is not set +# CONFIG_ARCH_CHIP_LPC2378 is not set +# CONFIG_ARCH_CHIP_LPC31XX is not set +# CONFIG_ARCH_CHIP_LPC43XX is not set +# CONFIG_ARCH_CHIP_NUC1XX is not set +# CONFIG_ARCH_CHIP_SAM34 is not set +CONFIG_ARCH_CHIP_STM32=y +# CONFIG_ARCH_CHIP_STR71X is not set +CONFIG_ARCH_CORTEXM4=y +CONFIG_ARCH_FAMILY="armv7-m" +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARCH_HAVE_CMNVECTOR=y +CONFIG_ARMV7M_CMNVECTOR=y +CONFIG_ARCH_HAVE_FPU=y +CONFIG_ARCH_FPU=y +CONFIG_ARCH_HAVE_MPU=y +# CONFIG_ARMV7M_MPU is not set + +# +# ARMV7M Configuration Options +# +# CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set +CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y +CONFIG_ARMV7M_STACKCHECK=y +CONFIG_SERIAL_TERMIOS=y +CONFIG_SDIO_DMA=y +# CONFIG_SDIO_DMAPRIO is not set +# CONFIG_SDIO_WIDTH_D1_ONLY is not set + +# +# STM32 Configuration Options +# +# CONFIG_ARCH_CHIP_STM32L151C6 is not set +# CONFIG_ARCH_CHIP_STM32L151C8 is not set +# CONFIG_ARCH_CHIP_STM32L151CB is not set +# CONFIG_ARCH_CHIP_STM32L151R6 is not set +# CONFIG_ARCH_CHIP_STM32L151R8 is not set +# CONFIG_ARCH_CHIP_STM32L151RB is not set +# CONFIG_ARCH_CHIP_STM32L151V6 is not set +# CONFIG_ARCH_CHIP_STM32L151V8 is not set +# CONFIG_ARCH_CHIP_STM32L151VB is not set +# CONFIG_ARCH_CHIP_STM32L152C6 is not set +# CONFIG_ARCH_CHIP_STM32L152C8 is not set +# CONFIG_ARCH_CHIP_STM32L152CB is not set +# CONFIG_ARCH_CHIP_STM32L152R6 is not set +# CONFIG_ARCH_CHIP_STM32L152R8 is not set +# CONFIG_ARCH_CHIP_STM32L152RB is not set +# CONFIG_ARCH_CHIP_STM32L152V6 is not set +# CONFIG_ARCH_CHIP_STM32L152V8 is not set +# CONFIG_ARCH_CHIP_STM32L152VB is not set +# CONFIG_ARCH_CHIP_STM32F100C8 is not set +# CONFIG_ARCH_CHIP_STM32F100CB is not set +# CONFIG_ARCH_CHIP_STM32F100R8 is not set +# CONFIG_ARCH_CHIP_STM32F100RB is not set +# CONFIG_ARCH_CHIP_STM32F100RC is not set +# CONFIG_ARCH_CHIP_STM32F100RD is not set +# CONFIG_ARCH_CHIP_STM32F100RE is not set +# CONFIG_ARCH_CHIP_STM32F100V8 is not set +# CONFIG_ARCH_CHIP_STM32F100VB is not set +# CONFIG_ARCH_CHIP_STM32F100VC is not set +# CONFIG_ARCH_CHIP_STM32F100VD is not set +# CONFIG_ARCH_CHIP_STM32F100VE is not set +# CONFIG_ARCH_CHIP_STM32F103C4 is not set +# CONFIG_ARCH_CHIP_STM32F103C8 is not set +# CONFIG_ARCH_CHIP_STM32F103RET6 is not set +# CONFIG_ARCH_CHIP_STM32F103VCT6 is not set +# CONFIG_ARCH_CHIP_STM32F103VET6 is not set +# CONFIG_ARCH_CHIP_STM32F103ZET6 is not set +# CONFIG_ARCH_CHIP_STM32F105VBT7 is not set +# CONFIG_ARCH_CHIP_STM32F107VC is not set +# CONFIG_ARCH_CHIP_STM32F207IG is not set +# CONFIG_ARCH_CHIP_STM32F302CB is not set +# CONFIG_ARCH_CHIP_STM32F302CC is not set +# CONFIG_ARCH_CHIP_STM32F302RB is not set +# CONFIG_ARCH_CHIP_STM32F302RC is not set +# CONFIG_ARCH_CHIP_STM32F302VB is not set +# CONFIG_ARCH_CHIP_STM32F302VC is not set +# CONFIG_ARCH_CHIP_STM32F303CB is not set +# CONFIG_ARCH_CHIP_STM32F303CC is not set +# CONFIG_ARCH_CHIP_STM32F303RB is not set +# CONFIG_ARCH_CHIP_STM32F303RC is not set +# CONFIG_ARCH_CHIP_STM32F303VB is not set +# CONFIG_ARCH_CHIP_STM32F303VC is not set +# CONFIG_ARCH_CHIP_STM32F405RG is not set +# CONFIG_ARCH_CHIP_STM32F405VG is not set +# CONFIG_ARCH_CHIP_STM32F405ZG is not set +# CONFIG_ARCH_CHIP_STM32F407VE is not set +# CONFIG_ARCH_CHIP_STM32F407VG is not set +# CONFIG_ARCH_CHIP_STM32F407ZE is not set +# CONFIG_ARCH_CHIP_STM32F407ZG is not set +# CONFIG_ARCH_CHIP_STM32F407IE is not set +# CONFIG_ARCH_CHIP_STM32F407IG is not set +CONFIG_ARCH_CHIP_STM32F427V=y +# CONFIG_ARCH_CHIP_STM32F427Z is not set +# CONFIG_ARCH_CHIP_STM32F427I is not set +# CONFIG_STM32_STM32L15XX is not set +# CONFIG_STM32_ENERGYLITE is not set +# CONFIG_STM32_STM32F10XX is not set +# CONFIG_STM32_VALUELINE is not set +# CONFIG_STM32_CONNECTIVITYLINE is not set +# CONFIG_STM32_PERFORMANCELINE is not set +# CONFIG_STM32_HIGHDENSITY is not set +# CONFIG_STM32_MEDIUMDENSITY is not set +# CONFIG_STM32_LOWDENSITY is not set +# CONFIG_STM32_STM32F20XX is not set +# CONFIG_STM32_STM32F30XX is not set +CONFIG_STM32_STM32F40XX=y +CONFIG_STM32_STM32F427=y +# CONFIG_STM32_DFU is not set + +# +# STM32 Peripheral Support +# +CONFIG_STM32_ADC1=y +# CONFIG_STM32_ADC2 is not set +# CONFIG_STM32_ADC3 is not set +CONFIG_STM32_BKPSRAM=y +# CONFIG_STM32_CAN1 is not set +# CONFIG_STM32_CAN2 is not set +CONFIG_STM32_CCMDATARAM=y +# CONFIG_STM32_CRC is not set +# CONFIG_STM32_CRYP is not set +CONFIG_STM32_DMA1=y +CONFIG_STM32_DMA2=y +# CONFIG_STM32_DAC1 is not set +# CONFIG_STM32_DAC2 is not set +# CONFIG_STM32_DCMI is not set +# CONFIG_STM32_ETHMAC is not set +# CONFIG_STM32_FSMC is not set +# CONFIG_STM32_HASH is not set +CONFIG_STM32_I2C1=y +CONFIG_STM32_I2C2=y +# CONFIG_STM32_I2C3 is not set +CONFIG_STM32_OTGFS=y +# CONFIG_STM32_OTGHS is not set +CONFIG_STM32_PWR=y +# CONFIG_STM32_RNG is not set +CONFIG_STM32_SDIO=y +CONFIG_STM32_SPI1=y +CONFIG_STM32_SPI2=y +# CONFIG_STM32_SPI3 is not set +# CONFIG_STM32_SPI4 is not set +# CONFIG_STM32_SPI5 is not set +# CONFIG_STM32_SPI6 is not set +CONFIG_STM32_SYSCFG=y +CONFIG_STM32_TIM1=y +# CONFIG_STM32_TIM2 is not set +CONFIG_STM32_TIM3=y +CONFIG_STM32_TIM4=y +# CONFIG_STM32_TIM5 is not set +# CONFIG_STM32_TIM6 is not set +# CONFIG_STM32_TIM7 is not set +# CONFIG_STM32_TIM8 is not set +CONFIG_STM32_TIM9=y +CONFIG_STM32_TIM10=y +CONFIG_STM32_TIM11=y +# CONFIG_STM32_TIM12 is not set +# CONFIG_STM32_TIM13 is not set +# CONFIG_STM32_TIM14 is not set +CONFIG_STM32_USART1=y +CONFIG_STM32_USART2=y +CONFIG_STM32_USART3=y +CONFIG_STM32_UART4=y +# CONFIG_STM32_UART5 is not set +CONFIG_STM32_USART6=y +CONFIG_STM32_UART7=y +CONFIG_STM32_UART8=y +# CONFIG_STM32_IWDG is not set +CONFIG_STM32_WWDG=y +CONFIG_STM32_ADC=y +CONFIG_STM32_SPI=y +CONFIG_STM32_I2C=y + +# +# Alternate Pin Mapping +# +CONFIG_STM32_FLASH_PREFETCH=y +# CONFIG_STM32_JTAG_DISABLE is not set +# CONFIG_STM32_JTAG_FULL_ENABLE is not set +# CONFIG_STM32_JTAG_NOJNTRST_ENABLE is not set +CONFIG_STM32_JTAG_SW_ENABLE=y +CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y +# CONFIG_STM32_FORCEPOWER is not set +# CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set +# CONFIG_STM32_CCMEXCLUDE is not set +CONFIG_STM32_DMACAPABLE=y +# CONFIG_STM32_TIM1_PWM is not set +# CONFIG_STM32_TIM3_PWM is not set +# CONFIG_STM32_TIM4_PWM is not set +# CONFIG_STM32_TIM9_PWM is not set +# CONFIG_STM32_TIM10_PWM is not set +# CONFIG_STM32_TIM11_PWM is not set +# CONFIG_STM32_TIM1_ADC is not set +# CONFIG_STM32_TIM3_ADC is not set +# CONFIG_STM32_TIM4_ADC is not set +CONFIG_STM32_USART=y + +# +# U[S]ART Configuration +# +# CONFIG_USART1_RS485 is not set +CONFIG_USART1_RXDMA=y +# CONFIG_USART2_RS485 is not set +CONFIG_USART2_RXDMA=y +# CONFIG_USART3_RS485 is not set +CONFIG_USART3_RXDMA=y +# CONFIG_UART4_RS485 is not set +CONFIG_UART4_RXDMA=y +# CONFIG_UART5_RXDMA is not set +# CONFIG_USART6_RS485 is not set +# CONFIG_USART6_RXDMA is not set +# CONFIG_USART7_RXDMA is not set +CONFIG_USART8_RXDMA=y +CONFIG_STM32_USART_SINGLEWIRE=y + +# +# SPI Configuration +# +# CONFIG_STM32_SPI_INTERRUPTS is not set +# CONFIG_STM32_SPI_DMA is not set + +# +# I2C Configuration +# +# CONFIG_STM32_I2C_DYNTIMEO is not set +CONFIG_STM32_I2CTIMEOSEC=0 +CONFIG_STM32_I2CTIMEOMS=10 +CONFIG_STM32_I2CTIMEOTICKS=500 +# CONFIG_STM32_I2C_DUTY16_9 is not set + +# +# SDIO Configuration +# +CONFIG_SDIO_PRI=128 + +# +# USB Host Configuration +# + +# +# USB Device Configuration +# + +# +# External Memory Configuration +# + +# +# Architecture Options +# +# CONFIG_ARCH_NOINTC is not set +# CONFIG_ARCH_VECNOTIRQ is not set +CONFIG_ARCH_DMA=y +CONFIG_ARCH_IRQPRIO=y +# CONFIG_CUSTOM_STACK is not set +# CONFIG_ADDRENV is not set +CONFIG_ARCH_HAVE_VFORK=y +CONFIG_ARCH_STACKDUMP=y +# CONFIG_ENDIAN_BIG is not set +# CONFIG_ARCH_HAVE_RAMFUNCS is not set +CONFIG_ARCH_HAVE_RAMVECTORS=y +# CONFIG_ARCH_RAMVECTORS is not set + +# +# Board Settings +# +CONFIG_BOARD_LOOPSPERMSEC=16717 +# CONFIG_ARCH_CALIBRATION is not set +CONFIG_DRAM_START=0x20000000 +CONFIG_DRAM_SIZE=262144 +CONFIG_ARCH_HAVE_INTERRUPTSTACK=y +CONFIG_ARCH_INTERRUPTSTACK=0 + +# +# Boot options +# +# CONFIG_BOOT_RUNFROMEXTSRAM is not set +CONFIG_BOOT_RUNFROMFLASH=y +# CONFIG_BOOT_RUNFROMISRAM is not set +# CONFIG_BOOT_RUNFROMSDRAM is not set +# CONFIG_BOOT_COPYTORAM is not set + +# +# Board Selection +# +CONFIG_ARCH_BOARD_PX4FMU_V2=y +# CONFIG_ARCH_BOARD_CUSTOM is not set +CONFIG_ARCH_BOARD="px4fmu-v2" + +# +# Common Board Options +# +CONFIG_NSH_MMCSDMINOR=0 + +# +# Board-Specific Options +# + +# +# RTOS Features +# +# CONFIG_BOARD_INITIALIZE is not set +CONFIG_MSEC_PER_TICK=1 +CONFIG_RR_INTERVAL=0 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_TASK_NAME_SIZE=24 +# CONFIG_SCHED_HAVE_PARENT is not set +# CONFIG_JULIAN_TIME is not set +CONFIG_START_YEAR=1970 +CONFIG_START_MONTH=1 +CONFIG_START_DAY=1 +# CONFIG_DEV_CONSOLE is not set +# CONFIG_MUTEX_TYPES is not set +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_SEM_PREALLOCHOLDERS=8 +CONFIG_SEM_NNESTPRIO=8 +# CONFIG_FDCLONE_DISABLE is not set +CONFIG_FDCLONE_STDIO=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SCHED_WAITPID=y +# CONFIG_SCHED_STARTHOOK is not set +CONFIG_SCHED_ATEXIT=y +CONFIG_SCHED_ATEXIT_MAX=1 +# CONFIG_SCHED_ONEXIT is not set +CONFIG_USER_ENTRYPOINT="nsh_main" +# CONFIG_DISABLE_OS_API is not set + +# +# Signal Numbers +# +CONFIG_SIG_SIGUSR1=1 +CONFIG_SIG_SIGUSR2=2 +CONFIG_SIG_SIGALARM=3 +CONFIG_SIG_SIGCONDTIMEDOUT=16 +CONFIG_SIG_SIGWORK=4 + +# +# Sizes of configurable things (0 disables) +# +CONFIG_MAX_TASKS=32 +CONFIG_MAX_TASK_ARGS=10 +CONFIG_NPTHREAD_KEYS=4 +CONFIG_NFILE_DESCRIPTORS=32 +CONFIG_NFILE_STREAMS=25 +CONFIG_NAME_MAX=32 +CONFIG_PREALLOC_MQ_MSGS=4 +CONFIG_MQ_MAXMSGSIZE=32 +CONFIG_MAX_WDOGPARMS=2 +CONFIG_PREALLOC_WDOGS=50 +CONFIG_PREALLOC_TIMERS=50 + +# +# Stack and heap information +# +CONFIG_IDLETHREAD_STACKSIZE=6000 +CONFIG_USERMAIN_STACKSIZE=4096 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_PTHREAD_STACK_DEFAULT=2048 + +# +# Device Drivers +# +# CONFIG_DISABLE_POLL is not set +CONFIG_DEV_NULL=y +# CONFIG_DEV_ZERO is not set +# CONFIG_LOOP is not set +# CONFIG_RAMDISK is not set +# CONFIG_CAN is not set +# CONFIG_PWM is not set +CONFIG_I2C=y +# CONFIG_I2C_SLAVE is not set +CONFIG_I2C_TRANSFER=y +# CONFIG_I2C_WRITEREAD is not set +# CONFIG_I2C_POLLED is not set +# CONFIG_I2C_TRACE is not set +CONFIG_ARCH_HAVE_I2CRESET=y +CONFIG_I2C_RESET=y +CONFIG_SPI=y +# CONFIG_SPI_OWNBUS is not set +CONFIG_SPI_EXCHANGE=y +# CONFIG_SPI_CMDDATA is not set +# CONFIG_RTC is not set +CONFIG_WATCHDOG=y +# CONFIG_ANALOG is not set +# CONFIG_AUDIO_DEVICES is not set +# CONFIG_BCH is not set +# CONFIG_INPUT is not set +# CONFIG_LCD is not set +# CONFIG_MMCSD is not set +CONFIG_MTD=y + +# +# MTD Configuration +# +# CONFIG_MTD_PARTITION is not set +# CONFIG_MTD_BYTE_WRITE is not set + +# +# MTD Device Drivers +# +# CONFIG_RAMMTD is not set +# CONFIG_MTD_AT24XX is not set +# CONFIG_MTD_AT45DB is not set +# CONFIG_MTD_M25P is not set +# CONFIG_MTD_SMART is not set +CONFIG_MTD_RAMTRON=y +# CONFIG_MTD_SST25 is not set +# CONFIG_MTD_SST39FV is not set +# CONFIG_MTD_W25 is not set +CONFIG_PIPES=y +# CONFIG_PM is not set +# CONFIG_POWER is not set +# CONFIG_SENSORS is not set +CONFIG_SERIAL=y +# CONFIG_DEV_LOWCONSOLE is not set +CONFIG_SERIAL_REMOVABLE=y +# CONFIG_16550_UART is not set +CONFIG_ARCH_HAVE_UART4=y +CONFIG_ARCH_HAVE_UART7=y +CONFIG_ARCH_HAVE_UART8=y +CONFIG_ARCH_HAVE_USART1=y +CONFIG_ARCH_HAVE_USART2=y +CONFIG_ARCH_HAVE_USART3=y +CONFIG_ARCH_HAVE_USART6=y +CONFIG_MCU_SERIAL=y +CONFIG_STANDARD_SERIAL=y +CONFIG_SERIAL_NPOLLWAITERS=2 +# CONFIG_USART1_SERIAL_CONSOLE is not set +# CONFIG_USART2_SERIAL_CONSOLE is not set +# CONFIG_USART3_SERIAL_CONSOLE is not set +# CONFIG_UART4_SERIAL_CONSOLE is not set +# CONFIG_USART6_SERIAL_CONSOLE is not set +# CONFIG_UART7_SERIAL_CONSOLE is not set +# CONFIG_UART8_SERIAL_CONSOLE is not set +CONFIG_NO_SERIAL_CONSOLE=y + +# +# USART1 Configuration +# +CONFIG_USART1_RXBUFSIZE=16 +CONFIG_USART1_TXBUFSIZE=16 +CONFIG_USART1_BAUD=115200 +CONFIG_USART1_BITS=8 +CONFIG_USART1_PARITY=0 +CONFIG_USART1_2STOP=0 +# CONFIG_USART1_IFLOWCONTROL is not set +# CONFIG_USART1_OFLOWCONTROL is not set + +# +# USART2 Configuration +# +CONFIG_USART2_RXBUFSIZE=512 +CONFIG_USART2_TXBUFSIZE=512 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_BITS=8 +CONFIG_USART2_PARITY=0 +CONFIG_USART2_2STOP=0 +CONFIG_USART2_IFLOWCONTROL=y +CONFIG_USART2_OFLOWCONTROL=y + +# +# USART3 Configuration +# +CONFIG_USART3_RXBUFSIZE=512 +CONFIG_USART3_TXBUFSIZE=512 +CONFIG_USART3_BAUD=115200 +CONFIG_USART3_BITS=8 +CONFIG_USART3_PARITY=0 +CONFIG_USART3_2STOP=0 +CONFIG_USART3_IFLOWCONTROL=y +CONFIG_USART3_OFLOWCONTROL=y + +# +# UART4 Configuration +# +CONFIG_UART4_RXBUFSIZE=128 +CONFIG_UART4_TXBUFSIZE=128 +CONFIG_UART4_BAUD=57600 +CONFIG_UART4_BITS=8 +CONFIG_UART4_PARITY=0 +CONFIG_UART4_2STOP=0 +# CONFIG_UART4_IFLOWCONTROL is not set +# CONFIG_UART4_OFLOWCONTROL is not set + +# +# USART6 Configuration +# +CONFIG_USART6_RXBUFSIZE=16 +CONFIG_USART6_TXBUFSIZE=16 +CONFIG_USART6_BAUD=115200 +CONFIG_USART6_BITS=8 +CONFIG_USART6_PARITY=0 +CONFIG_USART6_2STOP=0 +# CONFIG_USART6_IFLOWCONTROL is not set +# CONFIG_USART6_OFLOWCONTROL is not set + +# +# UART7 Configuration +# +CONFIG_UART7_RXBUFSIZE=128 +CONFIG_UART7_TXBUFSIZE=128 +CONFIG_UART7_BAUD=57600 +CONFIG_UART7_BITS=8 +CONFIG_UART7_PARITY=0 +CONFIG_UART7_2STOP=0 +# CONFIG_UART7_IFLOWCONTROL is not set +# CONFIG_UART7_OFLOWCONTROL is not set + +# +# UART8 Configuration +# +CONFIG_UART8_RXBUFSIZE=128 +CONFIG_UART8_TXBUFSIZE=128 +CONFIG_UART8_BAUD=57600 +CONFIG_UART8_BITS=8 +CONFIG_UART8_PARITY=0 +CONFIG_UART8_2STOP=0 +# CONFIG_UART8_IFLOWCONTROL is not set +# CONFIG_UART8_OFLOWCONTROL is not set +CONFIG_SERIAL_IFLOWCONTROL=y +CONFIG_SERIAL_OFLOWCONTROL=y +CONFIG_USBDEV=y + +# +# USB Device Controller Driver Options +# +# CONFIG_USBDEV_ISOCHRONOUS is not set +# CONFIG_USBDEV_DUALSPEED is not set +# CONFIG_USBDEV_SELFPOWERED is not set +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +# CONFIG_USBDEV_DMA is not set +# CONFIG_USBDEV_TRACE is not set + +# +# USB Device Class Driver Options +# +# CONFIG_USBDEV_COMPOSITE is not set +# CONFIG_PL2303 is not set +CONFIG_CDCACM=y +CONFIG_CDCACM_CONSOLE=y +CONFIG_CDCACM_EP0MAXPACKET=64 +CONFIG_CDCACM_EPINTIN=1 +CONFIG_CDCACM_EPINTIN_FSSIZE=64 +CONFIG_CDCACM_EPINTIN_HSSIZE=64 +CONFIG_CDCACM_EPBULKOUT=3 +CONFIG_CDCACM_EPBULKOUT_FSSIZE=64 +CONFIG_CDCACM_EPBULKOUT_HSSIZE=512 +CONFIG_CDCACM_EPBULKIN=2 +CONFIG_CDCACM_EPBULKIN_FSSIZE=64 +CONFIG_CDCACM_EPBULKIN_HSSIZE=512 +CONFIG_CDCACM_NWRREQS=4 +CONFIG_CDCACM_NRDREQS=4 +CONFIG_CDCACM_BULKIN_REQLEN=96 +CONFIG_CDCACM_RXBUFSIZE=256 +CONFIG_CDCACM_TXBUFSIZE=256 +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_PRODUCTID=0x0010 +CONFIG_CDCACM_VENDORSTR="3D Robotics" +CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v2.x" +# CONFIG_USBMSC is not set +# CONFIG_USBHOST is not set +# CONFIG_WIRELESS is not set + +# +# System Logging Device Options +# + +# +# System Logging +# +# CONFIG_RAMLOG is not set + +# +# Networking Support +# +# CONFIG_NET is not set + +# +# File Systems +# + +# +# File system configuration +# +# CONFIG_DISABLE_MOUNTPOINT is not set +# CONFIG_FS_RAMMAP is not set +CONFIG_FS_FAT=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_MAXFNAME=32 +CONFIG_FS_FATTIME=y +# CONFIG_FAT_DMAMEMORY is not set +CONFIG_FS_NXFFS=y +CONFIG_NXFFS_PREALLOCATED=y +CONFIG_NXFFS_ERASEDSTATE=0xff +CONFIG_NXFFS_PACKTHRESHOLD=32 +CONFIG_NXFFS_MAXNAMLEN=32 +CONFIG_NXFFS_TAILTHRESHOLD=2048 +CONFIG_FS_ROMFS=y +# CONFIG_FS_SMARTFS is not set +CONFIG_FS_BINFS=y + +# +# System Logging +# +# CONFIG_SYSLOG_ENABLE is not set +CONFIG_SYSLOG=y +CONFIG_SYSLOG_CHAR=y +CONFIG_SYSLOG_DEVPATH="/dev/ttyS0" + +# +# Graphics Support +# +# CONFIG_NX is not set + +# +# Memory Management +# +# CONFIG_MM_MULTIHEAP is not set +# CONFIG_MM_SMALL is not set +CONFIG_MM_REGIONS=2 +CONFIG_GRAN=y +CONFIG_GRAN_SINGLE=y +CONFIG_GRAN_INTR=y + +# +# Audio Support +# +# CONFIG_AUDIO is not set + +# +# Binary Formats +# +# CONFIG_BINFMT_DISABLE is not set +# CONFIG_BINFMT_EXEPATH is not set +# CONFIG_NXFLAT is not set +# CONFIG_ELF is not set +CONFIG_BUILTIN=y +# CONFIG_PIC is not set +# CONFIG_SYMTAB_ORDEREDBYNAME is not set + +# +# Library Routines +# + +# +# Standard C Library Options +# +CONFIG_STDIO_BUFFER_SIZE=32 +CONFIG_STDIO_LINEBUFFER=y +CONFIG_NUNGET_CHARS=2 +CONFIG_LIB_HOMEDIR="/" +# CONFIG_NOPRINTF_FIELDWIDTH is not set +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIB_RAND_ORDER=1 +# CONFIG_EOL_IS_CR is not set +# CONFIG_EOL_IS_LF is not set +# CONFIG_EOL_IS_BOTH_CRLF is not set +CONFIG_EOL_IS_EITHER_CRLF=y +# CONFIG_LIBC_EXECFUNCS is not set +CONFIG_POSIX_SPAWN_PROXY_STACKSIZE=1024 +CONFIG_TASK_SPAWN_DEFAULT_STACKSIZE=2048 +CONFIG_LIBC_STRERROR=y +# CONFIG_LIBC_STRERROR_SHORT is not set +# CONFIG_LIBC_PERROR_STDOUT is not set +CONFIG_ARCH_LOWPUTC=y +CONFIG_LIB_SENDFILE_BUFSIZE=512 +# CONFIG_ARCH_ROMGETC is not set +CONFIG_ARCH_OPTIMIZED_FUNCTIONS=y +CONFIG_ARCH_MEMCPY=y +# CONFIG_ARCH_MEMCMP is not set +# CONFIG_ARCH_MEMMOVE is not set +# CONFIG_ARCH_MEMSET is not set +# CONFIG_MEMSET_OPTSPEED is not set +# CONFIG_ARCH_STRCHR is not set +# CONFIG_ARCH_STRCMP is not set +# CONFIG_ARCH_STRCPY is not set +# CONFIG_ARCH_STRNCPY is not set +# CONFIG_ARCH_STRLEN is not set +# CONFIG_ARCH_STRNLEN is not set +# CONFIG_ARCH_BZERO is not set + +# +# Non-standard Library Support +# +CONFIG_SCHED_WORKQUEUE=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_WORKPRIORITY=192 +CONFIG_SCHED_WORKPERIOD=5000 +CONFIG_SCHED_WORKSTACKSIZE=2048 +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKPERIOD=50000 +CONFIG_SCHED_LPWORKSTACKSIZE=2048 +# CONFIG_LIB_KBDCODEC is not set +# CONFIG_LIB_SLCDCODEC is not set + +# +# Basic CXX Support +# +CONFIG_C99_BOOL8=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +# CONFIG_CXX_NEWLONG is not set + +# +# uClibc++ Standard C++ Library +# +# CONFIG_UCLIBCXX is not set + +# +# Application Configuration +# + +# +# Built-In Applications +# +CONFIG_BUILTIN_PROXY_STACKSIZE=1024 + +# +# Examples +# +# CONFIG_EXAMPLES_BUTTONS is not set +# CONFIG_EXAMPLES_CAN is not set +CONFIG_EXAMPLES_CDCACM=y +# CONFIG_EXAMPLES_COMPOSITE is not set +# CONFIG_EXAMPLES_CXXTEST is not set +# CONFIG_EXAMPLES_DHCPD is not set +# CONFIG_EXAMPLES_ELF is not set +# CONFIG_EXAMPLES_FTPC is not set +# CONFIG_EXAMPLES_FTPD is not set +# CONFIG_EXAMPLES_HELLO is not set +# CONFIG_EXAMPLES_HELLOXX is not set +# CONFIG_EXAMPLES_JSON is not set +# CONFIG_EXAMPLES_HIDKBD is not set +# CONFIG_EXAMPLES_KEYPADTEST is not set +# CONFIG_EXAMPLES_IGMP is not set +# CONFIG_EXAMPLES_LCDRW is not set +# CONFIG_EXAMPLES_MM is not set +# CONFIG_EXAMPLES_MODBUS is not set +CONFIG_EXAMPLES_MOUNT=y +# CONFIG_EXAMPLES_NRF24L01TERM is not set +CONFIG_EXAMPLES_NSH=y +# CONFIG_EXAMPLES_NULL is not set +# CONFIG_EXAMPLES_NX is not set +# CONFIG_EXAMPLES_NXCONSOLE is not set +# CONFIG_EXAMPLES_NXFFS is not set +# CONFIG_EXAMPLES_NXFLAT is not set +# CONFIG_EXAMPLES_NXHELLO is not set +# CONFIG_EXAMPLES_NXIMAGE is not set +# CONFIG_EXAMPLES_NXLINES is not set +# CONFIG_EXAMPLES_NXTEXT is not set +# CONFIG_EXAMPLES_OSTEST is not set +# CONFIG_EXAMPLES_PASHELLO is not set +# CONFIG_EXAMPLES_PIPE is not set +# CONFIG_EXAMPLES_POSIXSPAWN is not set +# CONFIG_EXAMPLES_QENCODER is not set +# CONFIG_EXAMPLES_RGMP is not set +# CONFIG_EXAMPLES_ROMFS is not set +# CONFIG_EXAMPLES_SENDMAIL is not set +# CONFIG_EXAMPLES_SERLOOP is not set +# CONFIG_EXAMPLES_SLCD is not set +# CONFIG_EXAMPLES_SMART_TEST is not set +# CONFIG_EXAMPLES_SMART is not set +# CONFIG_EXAMPLES_TCPECHO is not set +# CONFIG_EXAMPLES_TELNETD is not set +# CONFIG_EXAMPLES_THTTPD is not set +# CONFIG_EXAMPLES_TIFF is not set +# CONFIG_EXAMPLES_TOUCHSCREEN is not set +# CONFIG_EXAMPLES_UDP is not set +# CONFIG_EXAMPLES_UIP is not set +# CONFIG_EXAMPLES_USBSERIAL is not set +# CONFIG_EXAMPLES_USBMSC is not set +# CONFIG_EXAMPLES_USBTERM is not set +# CONFIG_EXAMPLES_WATCHDOG is not set + +# +# Graphics Support +# +# CONFIG_TIFF is not set + +# +# Interpreters +# +# CONFIG_INTERPRETERS_FICL is not set +# CONFIG_INTERPRETERS_PCODE is not set + +# +# Network Utilities +# + +# +# Networking Utilities +# +# CONFIG_NETUTILS_CODECS is not set +# CONFIG_NETUTILS_DHCPC is not set +# CONFIG_NETUTILS_DHCPD is not set +# CONFIG_NETUTILS_FTPC is not set +# CONFIG_NETUTILS_FTPD is not set +# CONFIG_NETUTILS_JSON is not set +# CONFIG_NETUTILS_RESOLV is not set +# CONFIG_NETUTILS_SMTP is not set +# CONFIG_NETUTILS_TELNETD is not set +# CONFIG_NETUTILS_TFTPC is not set +# CONFIG_NETUTILS_THTTPD is not set +# CONFIG_NETUTILS_UIPLIB is not set +# CONFIG_NETUTILS_WEBCLIENT is not set + +# +# FreeModBus +# +# CONFIG_MODBUS is not set + +# +# NSH Library +# +CONFIG_NSH_LIBRARY=y +CONFIG_NSH_BUILTIN_APPS=y + +# +# Disable Individual commands +# +# CONFIG_NSH_DISABLE_CAT is not set +# CONFIG_NSH_DISABLE_CD is not set +# CONFIG_NSH_DISABLE_CP is not set +# CONFIG_NSH_DISABLE_DD is not set +# CONFIG_NSH_DISABLE_ECHO is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_FREE is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_HELP is not set +# CONFIG_NSH_DISABLE_HEXDUMP is not set +# CONFIG_NSH_DISABLE_IFCONFIG is not set +# CONFIG_NSH_DISABLE_KILL is not set +# CONFIG_NSH_DISABLE_LOSETUP is not set +# CONFIG_NSH_DISABLE_LS is not set +# CONFIG_NSH_DISABLE_MB is not set +# CONFIG_NSH_DISABLE_MKDIR is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_MKFIFO is not set +# CONFIG_NSH_DISABLE_MKRD is not set +# CONFIG_NSH_DISABLE_MH is not set +# CONFIG_NSH_DISABLE_MOUNT is not set +# CONFIG_NSH_DISABLE_MW is not set +# CONFIG_NSH_DISABLE_NSFMOUNT is not set +# CONFIG_NSH_DISABLE_PS is not set +# CONFIG_NSH_DISABLE_PING is not set +# CONFIG_NSH_DISABLE_PUT is not set +# CONFIG_NSH_DISABLE_PWD is not set +# CONFIG_NSH_DISABLE_RM is not set +# CONFIG_NSH_DISABLE_RMDIR is not set +# CONFIG_NSH_DISABLE_SET is not set +# CONFIG_NSH_DISABLE_SH is not set +# CONFIG_NSH_DISABLE_SLEEP is not set +# CONFIG_NSH_DISABLE_TEST is not set +# CONFIG_NSH_DISABLE_UMOUNT is not set +# CONFIG_NSH_DISABLE_UNSET is not set +# CONFIG_NSH_DISABLE_USLEEP is not set +# CONFIG_NSH_DISABLE_WGET is not set +# CONFIG_NSH_DISABLE_XD is not set + +# +# Configure Command Options +# +# CONFIG_NSH_CMDOPT_DF_H is not set +CONFIG_NSH_CODECS_BUFSIZE=128 +CONFIG_NSH_FILEIOSIZE=512 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=12 +CONFIG_NSH_NESTDEPTH=8 +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLEBG is not set +CONFIG_NSH_ROMFSETC=y +# CONFIG_NSH_ROMFSRC is not set +CONFIG_NSH_ROMFSMOUNTPT="/etc" +CONFIG_NSH_INITSCRIPT="init.d/rcS" +CONFIG_NSH_ROMFSDEVNO=0 +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_ARCHROMFS=y +CONFIG_NSH_FATDEVNO=1 +CONFIG_NSH_FATSECTSIZE=512 +CONFIG_NSH_FATNSECTORS=1024 +CONFIG_NSH_FATMOUNTPT="/tmp" +CONFIG_NSH_CONSOLE=y +# CONFIG_NSH_USBCONSOLE is not set + +# +# USB Trace Support +# +# CONFIG_NSH_CONDEV is not set +CONFIG_NSH_ARCHINIT=y + +# +# NxWidgets/NxWM +# + +# +# System NSH Add-Ons +# + +# +# Custom Free Memory Command +# +# CONFIG_SYSTEM_FREE is not set + +# +# I2C tool +# +# CONFIG_SYSTEM_I2CTOOL is not set + +# +# FLASH Program Installation +# +# CONFIG_SYSTEM_INSTALL is not set + +# +# FLASH Erase-all Command +# + +# +# readline() +# +CONFIG_SYSTEM_READLINE=y +CONFIG_READLINE_ECHO=y + +# +# Power Off +# +# CONFIG_SYSTEM_POWEROFF is not set + +# +# RAMTRON +# +# CONFIG_SYSTEM_RAMTRON is not set + +# +# SD Card +# +# CONFIG_SYSTEM_SDCARD is not set + +# +# Sysinfo +# +CONFIG_SYSTEM_SYSINFO=y + +# +# USB Monitor +# diff --git a/nuttx/configs/px4fmuv2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig.prev similarity index 98% rename from nuttx/configs/px4fmuv2/nsh/defconfig rename to nuttx-configs/px4fmu-v2/nsh/defconfig.prev index 17803c4d72..42910ce0a8 100755 --- a/nuttx/configs/px4fmuv2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig.prev @@ -75,8 +75,8 @@ CONFIG_ARCH_ARM=y CONFIG_ARCH_CORTEXM4=y CONFIG_ARCH_CHIP="stm32" CONFIG_ARCH_CHIP_STM32F427V=y -CONFIG_ARCH_BOARD="px4fmuv2" -CONFIG_ARCH_BOARD_PX4FMUV2=y +CONFIG_ARCH_BOARD="px4fmu-v2" +CONFIG_ARCH_BOARD_PX4FMU_V2=y CONFIG_BOARD_LOOPSPERMSEC=16717 CONFIG_DRAM_SIZE=0x00040000 CONFIG_DRAM_START=0x20000000 @@ -297,22 +297,6 @@ CONFIG_USART6_RXDMA=y CONFIG_UART7_RXDMA=n CONFIG_UART8_RXDMA=n -# -# PX4FMU specific driver settings -# -# CONFIG_HRT_TIMER -# Enables the high-resolution timer. The board definition must -# set HRT_TIMER and HRT_TIMER_CHANNEL to the timer and capture/ -# compare channels to be used. -# CONFIG_HRT_PPM -# Enables R/C PPM input using the HRT. The board definition must -# set HRT_PPM_CHANNEL to the timer capture/compare channel to be -# used, and define GPIO_PPM_IN to configure the appropriate timer -# GPIO. -# -CONFIG_HRT_TIMER=y -CONFIG_HRT_PPM=n - # # STM32F40xxx specific SPI device driver settings # diff --git a/nuttx/configs/px4fmuv2/nsh/setenv.sh b/nuttx-configs/px4fmu-v2/nsh/setenv.sh similarity index 100% rename from nuttx/configs/px4fmuv2/nsh/setenv.sh rename to nuttx-configs/px4fmu-v2/nsh/setenv.sh diff --git a/nuttx/configs/px4fmuv2/src/Makefile b/nuttx-configs/px4fmu-v2/src/Makefile similarity index 100% rename from nuttx/configs/px4fmuv2/src/Makefile rename to nuttx-configs/px4fmu-v2/src/Makefile diff --git a/nuttx/configs/px4fmuv2/src/empty.c b/nuttx-configs/px4fmu-v2/src/empty.c similarity index 100% rename from nuttx/configs/px4fmuv2/src/empty.c rename to nuttx-configs/px4fmu-v2/src/empty.c diff --git a/nuttx/configs/px4iov2/README.txt b/nuttx-configs/px4io-v2/README.txt similarity index 100% rename from nuttx/configs/px4iov2/README.txt rename to nuttx-configs/px4io-v2/README.txt diff --git a/nuttx/configs/px4iov2/common/Make.defs b/nuttx-configs/px4io-v2/common/Make.defs similarity index 100% rename from nuttx/configs/px4iov2/common/Make.defs rename to nuttx-configs/px4io-v2/common/Make.defs diff --git a/nuttx/configs/px4iov2/common/ld.script b/nuttx-configs/px4io-v2/common/ld.script similarity index 100% rename from nuttx/configs/px4iov2/common/ld.script rename to nuttx-configs/px4io-v2/common/ld.script diff --git a/nuttx/configs/px4iov2/common/setenv.sh b/nuttx-configs/px4io-v2/common/setenv.sh similarity index 100% rename from nuttx/configs/px4iov2/common/setenv.sh rename to nuttx-configs/px4io-v2/common/setenv.sh diff --git a/nuttx/configs/px4iov2/include/README.txt b/nuttx-configs/px4io-v2/include/README.txt similarity index 100% rename from nuttx/configs/px4iov2/include/README.txt rename to nuttx-configs/px4io-v2/include/README.txt diff --git a/nuttx/configs/px4iov2/include/board.h b/nuttx-configs/px4io-v2/include/board.h similarity index 93% rename from nuttx/configs/px4iov2/include/board.h rename to nuttx-configs/px4io-v2/include/board.h index 21aacda871..b93ad42203 100755 --- a/nuttx/configs/px4iov2/include/board.h +++ b/nuttx-configs/px4io-v2/include/board.h @@ -45,11 +45,8 @@ #include #ifndef __ASSEMBLY__ # include -# include #endif -#include -#include -#include +#include /************************************************************************************ * Definitions @@ -118,10 +115,8 @@ /* * High-resolution timer */ -#ifdef CONFIG_HRT_TIMER -# define HRT_TIMER 1 /* use timer1 for the HRT */ -# define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 2 */ -#endif +#define HRT_TIMER 1 /* use timer1 for the HRT */ +#define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 2 */ /* * PPM @@ -130,10 +125,8 @@ * * Pin is PA8, timer 1, channel 1 */ -#if defined(CONFIG_HRT_TIMER) && defined (CONFIG_HRT_PPM) -# define HRT_PPM_CHANNEL 1 /* use capture/compare channel 1 */ -# define GPIO_PPM_IN GPIO_TIM1_CH1IN -#endif +#define HRT_PPM_CHANNEL 1 /* use capture/compare channel 1 */ +#define GPIO_PPM_IN GPIO_TIM1_CH1IN /* LED definitions ******************************************************************/ /* PX4 has two LEDs that we will encode as: */ diff --git a/nuttx-configs/px4io-v2/nsh/Make.defs b/nuttx-configs/px4io-v2/nsh/Make.defs new file mode 100644 index 0000000000..bdfc4e3e2c --- /dev/null +++ b/nuttx-configs/px4io-v2/nsh/Make.defs @@ -0,0 +1,3 @@ +include ${TOPDIR}/.config + +include $(TOPDIR)/configs/px4io-v2/common/Make.defs diff --git a/nuttx/configs/px4iov2/io/appconfig b/nuttx-configs/px4io-v2/nsh/appconfig similarity index 100% rename from nuttx/configs/px4iov2/io/appconfig rename to nuttx-configs/px4io-v2/nsh/appconfig diff --git a/nuttx/configs/px4iov2/io/defconfig b/nuttx-configs/px4io-v2/nsh/defconfig similarity index 96% rename from nuttx/configs/px4iov2/io/defconfig rename to nuttx-configs/px4io-v2/nsh/defconfig index 1eaf4f2d11..846ea8fb9f 100755 --- a/nuttx/configs/px4iov2/io/defconfig +++ b/nuttx-configs/px4io-v2/nsh/defconfig @@ -74,8 +74,8 @@ CONFIG_ARCH_ARM=y CONFIG_ARCH_CORTEXM3=y CONFIG_ARCH_CHIP="stm32" CONFIG_ARCH_CHIP_STM32F100C8=y -CONFIG_ARCH_BOARD="px4iov2" -CONFIG_ARCH_BOARD_PX4IOV2=y +CONFIG_ARCH_BOARD="px4io-v2" +CONFIG_ARCH_BOARD_PX4IO_V2=y CONFIG_BOARD_LOOPSPERMSEC=2000 CONFIG_DRAM_SIZE=0x00002000 CONFIG_DRAM_START=0x20000000 @@ -195,28 +195,6 @@ SERIAL_HAVE_CONSOLE_DMA=y CONFIG_USART2_RXDMA=n CONFIG_USART3_RXDMA=y -# -# PX4IO specific driver settings -# -# CONFIG_HRT_TIMER -# Enables the high-resolution timer. The board definition must -# set HRT_TIMER and HRT_TIMER_CHANNEL to the timer and capture/ -# compare channels to be used. -# CONFIG_HRT_PPM -# Enables R/C PPM input using the HRT. The board definition must -# set HRT_PPM_CHANNEL to the timer capture/compare channel to be -# used, and define GPIO_PPM_IN to configure the appropriate timer -# GPIO. -# CONFIG_PWM_SERVO -# Enables the PWM servo driver. The driver configuration must be -# supplied by the board support at initialisation time. -# Note that USART2 must be disabled on the PX4 board for this to -# be available. -# -# -CONFIG_HRT_TIMER=y -CONFIG_HRT_PPM=y - # # General build options # diff --git a/nuttx/configs/px4iov2/io/setenv.sh b/nuttx-configs/px4io-v2/nsh/setenv.sh similarity index 100% rename from nuttx/configs/px4iov2/io/setenv.sh rename to nuttx-configs/px4io-v2/nsh/setenv.sh diff --git a/nuttx/configs/px4iov2/src/Makefile b/nuttx-configs/px4io-v2/src/Makefile similarity index 100% rename from nuttx/configs/px4iov2/src/Makefile rename to nuttx-configs/px4io-v2/src/Makefile diff --git a/nuttx/configs/px4iov2/src/README.txt b/nuttx-configs/px4io-v2/src/README.txt similarity index 100% rename from nuttx/configs/px4iov2/src/README.txt rename to nuttx-configs/px4io-v2/src/README.txt diff --git a/nuttx/configs/px4iov2/src/empty.c b/nuttx-configs/px4io-v2/src/empty.c similarity index 100% rename from nuttx/configs/px4iov2/src/empty.c rename to nuttx-configs/px4io-v2/src/empty.c diff --git a/nuttx/configs/px4iov2/nsh/Make.defs b/nuttx-configs/px4io-v2/test/Make.defs similarity index 100% rename from nuttx/configs/px4iov2/nsh/Make.defs rename to nuttx-configs/px4io-v2/test/Make.defs diff --git a/nuttx/configs/px4iov2/nsh/appconfig b/nuttx-configs/px4io-v2/test/appconfig similarity index 100% rename from nuttx/configs/px4iov2/nsh/appconfig rename to nuttx-configs/px4io-v2/test/appconfig diff --git a/nuttx/configs/px4iov2/nsh/defconfig b/nuttx-configs/px4io-v2/test/defconfig similarity index 96% rename from nuttx/configs/px4iov2/nsh/defconfig rename to nuttx-configs/px4io-v2/test/defconfig index 14d7a64012..19dfefdf84 100755 --- a/nuttx/configs/px4iov2/nsh/defconfig +++ b/nuttx-configs/px4io-v2/test/defconfig @@ -75,7 +75,7 @@ CONFIG_ARCH_CORTEXM3=y CONFIG_ARCH_CHIP=stm32 CONFIG_ARCH_CHIP_STM32F100C8=y CONFIG_ARCH_BOARD=px4iov2 -CONFIG_ARCH_BOARD_PX4IOV2=y +CONFIG_ARCH_BOARD_PX4IO_V2=y CONFIG_BOARD_LOOPSPERMSEC=24000 CONFIG_DRAM_SIZE=0x00002000 CONFIG_DRAM_START=0x20000000 @@ -187,29 +187,6 @@ CONFIG_USART1_2STOP=0 CONFIG_USART2_2STOP=0 CONFIG_USART3_2STOP=0 -# -# PX4IO specific driver settings -# -# CONFIG_HRT_TIMER -# Enables the high-resolution timer. The board definition must -# set HRT_TIMER and HRT_TIMER_CHANNEL to the timer and capture/ -# compare channels to be used. -# CONFIG_HRT_PPM -# Enables R/C PPM input using the HRT. The board definition must -# set HRT_PPM_CHANNEL to the timer capture/compare channel to be -# used, and define GPIO_PPM_IN to configure the appropriate timer -# GPIO. -# CONFIG_PWM_SERVO -# Enables the PWM servo driver. The driver configuration must be -# supplied by the board support at initialisation time. -# Note that USART2 must be disabled on the PX4 board for this to -# be available. -# -# -CONFIG_HRT_TIMER=y -CONFIG_HRT_PPM=y -CONFIG_PWM_SERVO=y - # # General build options # diff --git a/nuttx/configs/px4iov2/nsh/setenv.sh b/nuttx-configs/px4io-v2/test/setenv.sh similarity index 100% rename from nuttx/configs/px4iov2/nsh/setenv.sh rename to nuttx-configs/px4io-v2/test/setenv.sh diff --git a/nuttx/configs/px4fmuv2/nsh/Make.defs b/nuttx/configs/px4fmuv2/nsh/Make.defs deleted file mode 100644 index 3306e4bf10..0000000000 --- a/nuttx/configs/px4fmuv2/nsh/Make.defs +++ /dev/null @@ -1,3 +0,0 @@ -include ${TOPDIR}/.config - -include $(TOPDIR)/configs/px4fmuv2/common/Make.defs diff --git a/nuttx/configs/px4iov2/io/Make.defs b/nuttx/configs/px4iov2/io/Make.defs deleted file mode 100644 index 369772d03f..0000000000 --- a/nuttx/configs/px4iov2/io/Make.defs +++ /dev/null @@ -1,3 +0,0 @@ -include ${TOPDIR}/.config - -include $(TOPDIR)/configs/px4io/common/Make.defs diff --git a/src/drivers/boards/px4fmuv2/px4fmu_init.c b/src/drivers/boards/px4fmuv2/px4fmu_init.c index 03ec5a2559..f9d1b80226 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu_init.c +++ b/src/drivers/boards/px4fmuv2/px4fmu_init.c @@ -59,9 +59,9 @@ #include #include -#include "stm32_internal.h" +#include #include "px4fmu_internal.h" -#include "stm32_uart.h" +#include #include diff --git a/src/drivers/boards/px4fmuv2/px4fmu_internal.h b/src/drivers/boards/px4fmuv2/px4fmu_internal.h index dd291b9b7f..dce51bcda2 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu_internal.h +++ b/src/drivers/boards/px4fmuv2/px4fmu_internal.h @@ -50,7 +50,7 @@ __BEGIN_DECLS /* these headers are not C++ safe */ -#include +#include /**************************************************************************************************** diff --git a/src/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c index 14e4052be1..d1656005b8 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c +++ b/src/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c @@ -46,7 +46,7 @@ #include #include -#include +#include #include #include diff --git a/src/drivers/boards/px4fmuv2/px4fmu_spi.c b/src/drivers/boards/px4fmuv2/px4fmu_spi.c index f90f250716..5e90c227d7 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu_spi.c +++ b/src/drivers/boards/px4fmuv2/px4fmu_spi.c @@ -50,9 +50,9 @@ #include #include -#include "up_arch.h" -#include "chip.h" -#include "stm32_internal.h" +#include +#include +#include #include "px4fmu_internal.h" /************************************************************************************ diff --git a/src/drivers/boards/px4fmuv2/px4fmu_usb.c b/src/drivers/boards/px4fmuv2/px4fmu_usb.c index b0b669fbed..3492e2c684 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu_usb.c +++ b/src/drivers/boards/px4fmuv2/px4fmu_usb.c @@ -51,8 +51,8 @@ #include #include -#include "up_arch.h" -#include "stm32_internal.h" +#include +#include #include "px4fmu_internal.h" /************************************************************************************ diff --git a/src/drivers/boards/px4iov2/px4iov2_init.c b/src/drivers/boards/px4iov2/px4iov2_init.c index 5d7b225609..e95298bf5d 100644 --- a/src/drivers/boards/px4iov2/px4iov2_init.c +++ b/src/drivers/boards/px4iov2/px4iov2_init.c @@ -54,7 +54,7 @@ #include -#include "stm32_internal.h" +#include #include "px4iov2_internal.h" #include diff --git a/src/drivers/boards/px4iov2/px4iov2_internal.h b/src/drivers/boards/px4iov2/px4iov2_internal.h index 81a75431c3..2bf65e0470 100755 --- a/src/drivers/boards/px4iov2/px4iov2_internal.h +++ b/src/drivers/boards/px4iov2/px4iov2_internal.h @@ -48,7 +48,7 @@ #include /* these headers are not C++ safe */ -#include +#include /****************************************************************************** diff --git a/src/drivers/boards/px4iov2/px4iov2_pwm_servo.c b/src/drivers/boards/px4iov2/px4iov2_pwm_servo.c index 5e1aafa494..4f1b9487cf 100644 --- a/src/drivers/boards/px4iov2/px4iov2_pwm_servo.c +++ b/src/drivers/boards/px4iov2/px4iov2_pwm_servo.c @@ -46,7 +46,7 @@ #include #include -#include +#include #include #include diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h index a4c59d2180..faeb9cf60a 100644 --- a/src/drivers/drv_gpio.h +++ b/src/drivers/drv_gpio.h @@ -42,7 +42,7 @@ #include -#ifdef CONFIG_ARCH_BOARD_PX4FMU +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 /* * PX4FMU GPIO numbers. * @@ -67,7 +67,7 @@ #endif -#ifdef CONFIG_ARCH_BOARD_PX4FMUV2 +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 /* * PX4FMUv2 GPIO numbers. * @@ -93,36 +93,14 @@ #endif -#ifdef CONFIG_ARCH_BOARD_PX4IO -/* - * PX4IO GPIO numbers. - * - * XXX note that these are here for reference/future use; currently - * there is no good way to wire these up without a common STM32 GPIO - * driver, which isn't implemented yet. - */ -/* outputs */ -# define GPIO_ACC1_POWER (1<<0) /**< accessory power 1 */ -# define GPIO_ACC2_POWER (1<<1) /**< accessory power 2 */ -# define GPIO_SERVO_POWER (1<<2) /**< servo power */ -# define GPIO_RELAY1 (1<<3) /**< relay 1 */ -# define GPIO_RELAY2 (1<<4) /**< relay 2 */ -# define GPIO_LED_BLUE (1<<5) /**< blue LED */ -# define GPIO_LED_AMBER (1<<6) /**< amber/red LED */ -# define GPIO_LED_SAFETY (1<<7) /**< safety LED */ - -/* inputs */ -# define GPIO_ACC_OVERCURRENT (1<<8) /**< accessory 1/2 overcurrent detect */ -# define GPIO_SERVO_OVERCURRENT (1<<9) /**< servo overcurrent detect */ -# define GPIO_SAFETY_BUTTON (1<<10) /**< safety button pressed */ - -/** - * Default GPIO device - other devices may also support this protocol if - * they also export GPIO-like things. This is always the GPIOs on the - * main board. - */ -# define GPIO_DEVICE_PATH "/dev/px4io" +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 +/* no GPIO driver on the PX4IOv1 board */ +# define GPIO_DEVICE_PATH "/nonexistent" +#endif +#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 +/* no GPIO driver on the PX4IOv2 board */ +# define GPIO_DEVICE_PATH "/nonexistent" #endif #ifndef GPIO_DEVICE_PATH diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index ff99de02f1..7d3af4b0d2 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -59,10 +59,10 @@ #include #include -#if defined(CONFIG_ARCH_BOARD_PX4FMU) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) # include # define FMU_HAVE_PPM -#elif defined(CONFIG_ARCH_BOARD_PX4FMUV2) +#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) # include # undef FMU_HAVE_PPM #else @@ -158,7 +158,7 @@ private: }; const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { -#if defined(CONFIG_ARCH_BOARD_PX4FMU) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, GPIO_USART2_CTS_1}, @@ -168,7 +168,7 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { {GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, GPIO_CAN2_TX_2}, {GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, GPIO_CAN2_RX_2}, #endif -#if defined(CONFIG_ARCH_BOARD_PX4FMUV2) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, 0}, @@ -873,7 +873,7 @@ PX4FMU::gpio_reset(void) } } -#if defined(CONFIG_ARCH_BOARD_PX4FMU) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) /* if we have a GPIO direction control, set it to zero (input) */ stm32_gpiowrite(GPIO_GPIO_DIR, 0); stm32_configgpio(GPIO_GPIO_DIR); @@ -883,7 +883,7 @@ PX4FMU::gpio_reset(void) void PX4FMU::gpio_set_function(uint32_t gpios, int function) { -#if defined(CONFIG_ARCH_BOARD_PX4FMU) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) /* * GPIOs 0 and 1 must have the same direction as they are buffered * by a shared 2-port driver. Any attempt to set either sets both. @@ -918,7 +918,7 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function) } } -#if defined(CONFIG_ARCH_BOARD_PX4FMU) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) /* flip buffer to input mode if required */ if ((GPIO_SET_INPUT == function) && (gpios & 3)) stm32_gpiowrite(GPIO_GPIO_DIR, 0); @@ -1024,17 +1024,17 @@ fmu_new_mode(PortMode new_mode) break; case PORT_FULL_PWM: -#if defined(CONFIG_ARCH_BOARD_PX4FMU) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) /* select 4-pin PWM mode */ servo_mode = PX4FMU::MODE_4PWM; #endif -#if defined(CONFIG_ARCH_BOARD_PX4FMUV2) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) servo_mode = PX4FMU::MODE_6PWM; #endif break; /* mixed modes supported on v1 board only */ -#if defined(CONFIG_ARCH_BOARD_PX4FMU) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) case PORT_FULL_SERIAL: /* set all multi-GPIOs to serial mode */ gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4; @@ -1116,7 +1116,7 @@ test(void) if (ioctl(fd, PWM_SERVO_SET(3), 1600) < 0) err(1, "servo 4 set failed"); -#if defined(CONFIG_ARCH_BOARD_PX4FMUV2) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) if (ioctl(fd, PWM_SERVO_SET(4), 1800) < 0) err(1, "servo 5 set failed"); if (ioctl(fd, PWM_SERVO_SET(5), 2000) < 0) err(1, "servo 6 set failed"); @@ -1183,7 +1183,7 @@ fmu_main(int argc, char *argv[]) } else if (!strcmp(verb, "mode_pwm")) { new_mode = PORT_FULL_PWM; -#if defined(CONFIG_ARCH_BOARD_PX4FMU) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) } else if (!strcmp(verb, "mode_serial")) { new_mode = PORT_FULL_SERIAL; @@ -1217,9 +1217,9 @@ fmu_main(int argc, char *argv[]) fake(argc - 1, argv + 1); fprintf(stderr, "FMU: unrecognised command, try:\n"); -#if defined(CONFIG_ARCH_BOARD_PX4FMU) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n"); -#elif defined(CONFIG_ARCH_BOARD_PX4FMUV2) +#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) fprintf(stderr, " mode_gpio, mode_pwm\n"); #endif exit(1); diff --git a/src/drivers/px4io/interface_serial.cpp b/src/drivers/px4io/interface_serial.cpp index 9727daa717..06b955971e 100644 --- a/src/drivers/px4io/interface_serial.cpp +++ b/src/drivers/px4io/interface_serial.cpp @@ -56,7 +56,6 @@ #include #include #include -#include #include diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 15e213afb7..29624018f6 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1604,9 +1604,9 @@ start(int argc, char *argv[]) PX4IO_interface *interface; -#if defined(CONFIG_ARCH_BOARD_PX4FMUV2) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) interface = io_serial_interface(); -#elif defined(CONFIG_ARCH_BOARD_PX4FMU) +#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V1) interface = io_i2c_interface(PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO); #else # error Unknown board - cannot select interface. @@ -1741,9 +1741,9 @@ if_test(unsigned mode) { PX4IO_interface *interface; -#if defined(CONFIG_ARCH_BOARD_PX4FMUV2) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) interface = io_serial_interface(); -#elif defined(CONFIG_ARCH_BOARD_PX4FMU) +#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V1) interface = io_i2c_interface(PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO); #else # error Unknown board - cannot select interface. diff --git a/src/drivers/px4io/uploader.cpp b/src/drivers/px4io/uploader.cpp index 28c5844380..ec22a5e174 100644 --- a/src/drivers/px4io/uploader.cpp +++ b/src/drivers/px4io/uploader.cpp @@ -56,10 +56,10 @@ #include "uploader.h" -#ifdef CONFIG_ARCH_BOARD_PX4FMUV2 +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 #include #endif -#ifdef CONFIG_ARCH_BOARD_PX4FMU +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 #include #endif diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index 7ef3db970e..1cc18afda7 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -70,7 +70,7 @@ #include "stm32_gpio.h" #include "stm32_tim.h" -#ifdef CONFIG_HRT_TIMER +#ifdef HRT_TIMER /* HRT configuration */ #if HRT_TIMER == 1 @@ -275,7 +275,7 @@ static void hrt_call_invoke(void); /* * Specific registers and bits used by PPM sub-functions */ -#ifdef CONFIG_HRT_PPM +#ifdef HRT_PPM_CHANNEL /* * If the timer hardware doesn't support GTIM_CCER_CCxNP, then we will work around it. * @@ -377,7 +377,7 @@ static void hrt_ppm_decode(uint32_t status); # define CCMR1_PPM 0 # define CCMR2_PPM 0 # define CCER_PPM 0 -#endif /* CONFIG_HRT_PPM */ +#endif /* HRT_PPM_CHANNEL */ /* * Initialise the timer we are going to use. @@ -907,4 +907,4 @@ hrt_latency_update(void) } -#endif /* CONFIG_HRT_TIMER */ +#endif /* HRT_TIMER */ diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index 167ef30a8e..2284be84de 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -117,10 +117,6 @@ #include -#ifndef CONFIG_HRT_TIMER -# error This driver requires CONFIG_HRT_TIMER -#endif - /* Tone alarm configuration */ #if TONE_ALARM_TIMER == 2 # define TONE_ALARM_BASE STM32_TIM2_BASE diff --git a/src/modules/px4iofirmware/module.mk b/src/modules/px4iofirmware/module.mk index 4dd1aa8d73..59f470a94e 100644 --- a/src/modules/px4iofirmware/module.mk +++ b/src/modules/px4iofirmware/module.mk @@ -15,10 +15,10 @@ SRCS = adc.c \ ../systemlib/mixer/mixer_multirotor.cpp \ ../systemlib/mixer/mixer_simple.cpp \ -ifeq ($(BOARD),px4io) +ifeq ($(BOARD),px4io-v1) SRCS += i2c.c endif -ifeq ($(BOARD),px4iov2) +ifeq ($(BOARD),px4io-v2) SRCS += serial.c \ ../systemlib/hx_stream.c endif diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index b32782285b..ccf175e459 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -42,10 +42,10 @@ #include #include -#ifdef CONFIG_ARCH_BOARD_PX4IO +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 # include #endif -#ifdef CONFIG_ARCH_BOARD_PX4IOV2 +#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 # include #endif @@ -129,18 +129,7 @@ extern struct sys_state_s system_state; #define LED_AMBER(_s) stm32_gpiowrite(GPIO_LED2, !(_s)) #define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s)) -#ifdef CONFIG_ARCH_BOARD_PX4IOV2 - -# define PX4IO_RELAY_CHANNELS 0 -# define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s)) - -# define VDD_SERVO_FAULT (!stm32_gpioread(GPIO_SERVO_FAULT_DETECT)) - -# define PX4IO_ADC_CHANNEL_COUNT 2 -# define ADC_VSERVO 4 -# define ADC_RSSI 5 - -#else /* CONFIG_ARCH_BOARD_PX4IOV1 */ +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 # define PX4IO_RELAY_CHANNELS 4 # define POWER_SERVO(_s) stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s)) @@ -158,6 +147,19 @@ extern struct sys_state_s system_state; #endif +#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 + +# define PX4IO_RELAY_CHANNELS 0 +# define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s)) + +# define VDD_SERVO_FAULT (!stm32_gpioread(GPIO_SERVO_FAULT_DETECT)) + +# define PX4IO_ADC_CHANNEL_COUNT 2 +# define ADC_VSERVO 4 +# define ADC_RSSI 5 + +#endif + #define BUTTON_SAFETY stm32_gpioread(GPIO_BTN_SAFETY) /* diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index f4541936b0..873ee73f1a 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -59,7 +59,7 @@ static void pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t alt */ static const uint16_t r_page_config[] = { [PX4IO_P_CONFIG_PROTOCOL_VERSION] = PX4IO_PROTOCOL_VERSION, -#ifdef CONFIG_ARCH_BOARD_PX4IOV2 +#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 [PX4IO_P_CONFIG_HARDWARE_VERSION] = 2, #else [PX4IO_P_CONFIG_HARDWARE_VERSION] = 1, diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index e4bc68f580..2f31846238 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -50,7 +50,7 @@ #include #include #include -#include +#include #include //#define DEBUG From 0efb1d6cebbbe1889aceb12329a97a1c85126cce Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 7 Jul 2013 18:50:04 -0700 Subject: [PATCH 232/486] Fix the px4io serial port device name now that we're not using UART8 as the console. --- src/drivers/boards/px4fmuv2/px4fmu_internal.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/boards/px4fmuv2/px4fmu_internal.h b/src/drivers/boards/px4fmuv2/px4fmu_internal.h index dce51bcda2..76aa042f64 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu_internal.h +++ b/src/drivers/boards/px4fmuv2/px4fmu_internal.h @@ -59,7 +59,7 @@ __BEGIN_DECLS /* Configuration ************************************************************************************/ /* PX4IO connection configuration */ -#define PX4IO_SERIAL_DEVICE "/dev/ttyS5" +#define PX4IO_SERIAL_DEVICE "/dev/ttyS4" #define PX4IO_SERIAL_TX_GPIO GPIO_USART6_TX #define PX4IO_SERIAL_RX_GPIO GPIO_USART6_RX #define PX4IO_SERIAL_BASE STM32_USART6_BASE /* hardwired on the board */ From e2458677c99f7b74462381a3cd9dec3321901190 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 7 Jul 2013 20:42:03 -0700 Subject: [PATCH 233/486] Tweak IO serial packet error handling slightly; on reception of a serial error send a line break back to FMU. This should cause FMU to stop sending immediately. Flag these cases and discard the packet rather than processing it, rather than simply dropping the received packet and letting FMU time out. --- src/modules/px4iofirmware/serial.c | 44 +++++++++++++++++++++++------- 1 file changed, 34 insertions(+), 10 deletions(-) diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index 2f31846238..94d7407dff 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -221,6 +221,10 @@ rx_handle_packet(void) static void rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) { + /* + * We are here because DMA completed, or UART reception stopped and + * we think we have a packet in the buffer. + */ perf_begin(pc_txns); /* disable UART DMA */ @@ -235,7 +239,7 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) /* re-set DMA for reception first, so we are ready to receive before we start sending */ dma_reset(); - /* send the reply to the previous request */ + /* send the reply to the just-processed request */ dma_packet.crc = 0; dma_packet.crc = crc_packet(&dma_packet); stm32_dmasetup( @@ -256,6 +260,8 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) static int serial_interrupt(int irq, void *context) { + static bool abort_on_idle = false; + uint32_t sr = rSR; /* get UART status register */ (void)rDR; /* required to clear any of the interrupt status that brought us here */ @@ -271,28 +277,46 @@ serial_interrupt(int irq, void *context) if (sr & USART_SR_FE) perf_count(pc_fe); - /* reset DMA state machine back to listening-for-packet */ - dma_reset(); + /* send a line break - this will abort transmission/reception on the other end */ + rCR1 |= USART_CR1_SBK; - /* don't attempt to handle IDLE if it's set - things went bad */ - return 0; + /* when the line goes idle, abort rather than look at the packet */ + abort_on_idle = true; } if (sr & USART_SR_IDLE) { - /* the packet might have been short - go check */ + /* + * If we saw an error, don't bother looking at the packet - it should have + * been aborted by the sender and will definitely be bad. Get the DMA reconfigured + * ready for their retry. + */ + if (abort_on_idle) { + + abort_on_idle = false; + dma_reset(); + return 0; + } + + /* + * The sender has stopped sending - this is probably the end of a packet. + * Check the received length against the length in the header to see if + * we have something that looks like a packet. + */ unsigned length = sizeof(dma_packet) - stm32_dmaresidual(rx_dma); if ((length < 1) || (length < PKT_SIZE(dma_packet))) { + + /* it was too short - possibly truncated */ perf_count(pc_badidle); return 0; } + /* + * Looks like we received a packet. Stop the DMA and go process the + * packet. + */ perf_count(pc_idle); - - /* stop the receive DMA */ stm32_dmastop(rx_dma); - - /* and treat this like DMA completion */ rx_dma_callback(rx_dma, DMA_STATUS_TCIF, NULL); } From 04fbed493aab1dede6c2200aaab2b990d24a66e7 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 9 Jul 2013 14:09:48 +0400 Subject: [PATCH 234/486] multirotor_pos_control: use separate PID controllers for position and velocity --- src/modules/multirotor_pos_control/module.mk | 3 +- .../multirotor_pos_control.c | 197 ++++++++++-------- .../multirotor_pos_control_params.c | 63 +++--- .../multirotor_pos_control_params.h | 42 ++-- .../multirotor_pos_control/thrust_pid.c | 179 ++++++++++++++++ .../multirotor_pos_control/thrust_pid.h | 75 +++++++ .../position_estimator_inav_main.c | 2 +- 7 files changed, 423 insertions(+), 138 deletions(-) create mode 100644 src/modules/multirotor_pos_control/thrust_pid.c create mode 100644 src/modules/multirotor_pos_control/thrust_pid.h diff --git a/src/modules/multirotor_pos_control/module.mk b/src/modules/multirotor_pos_control/module.mk index d048477455..bc4b48fb40 100644 --- a/src/modules/multirotor_pos_control/module.mk +++ b/src/modules/multirotor_pos_control/module.mk @@ -38,4 +38,5 @@ MODULE_COMMAND = multirotor_pos_control SRCS = multirotor_pos_control.c \ - multirotor_pos_control_params.c + multirotor_pos_control_params.c \ + thrust_pid.c diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index acae03fae0..d56c3d58f8 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -61,9 +61,11 @@ #include #include #include +#include #include #include "multirotor_pos_control_params.h" +#include "thrust_pid.h" static bool thread_should_exit = false; /**< Deamon exit flag */ @@ -84,8 +86,6 @@ static void usage(const char *reason); static float scale_control(float ctl, float end, float dz); -static float limit_value(float v, float limit); - static float norm(float x, float y); static void usage(const char *reason) { @@ -110,11 +110,12 @@ int multirotor_pos_control_main(int argc, char *argv[]) { if (!strcmp(argv[1], "start")) { if (thread_running) { - printf("multirotor_pos_control already running\n"); + warnx("already running"); /* this is not an error */ exit(0); } + warnx("start"); thread_should_exit = false; deamon_task = task_spawn_cmd("multirotor_pos_control", SCHED_DEFAULT, @@ -126,15 +127,16 @@ int multirotor_pos_control_main(int argc, char *argv[]) { } if (!strcmp(argv[1], "stop")) { + warnx("stop"); thread_should_exit = true; exit(0); } if (!strcmp(argv[1], "status")) { if (thread_running) { - printf("\tmultirotor_pos_control app is running\n"); + warnx("app is running"); } else { - printf("\tmultirotor_pos_control app not started\n"); + warnx("app not started"); } exit(0); } @@ -153,22 +155,13 @@ static float scale_control(float ctl, float end, float dz) { } } -static float limit_value(float v, float limit) { - if (v > limit) { - v = limit; - } else if (v < -limit) { - v = -limit; - } - return v; -} - static float norm(float x, float y) { return sqrtf(x * x + y * y); } static int multirotor_pos_control_thread_main(int argc, char *argv[]) { /* welcome user */ - warnx("started."); + warnx("started"); static int mavlink_fd; mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); mavlink_log_info(mavlink_fd, "[multirotor_pos_control] started"); @@ -203,15 +196,17 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { bool reset_sp_alt = true; bool reset_sp_pos = true; hrt_abstime t_prev = 0; - float alt_integral = 0.0f; /* integrate in NED frame to estimate wind but not attitude offset */ - float pos_x_integral = 0.0f; - float pos_y_integral = 0.0f; const float alt_ctl_dz = 0.2f; const float pos_ctl_dz = 0.05f; float home_alt = 0.0f; hrt_abstime home_alt_t = 0; + static PID_t xy_pos_pids[2]; + static PID_t xy_vel_pids[2]; + static PID_t z_pos_pid; + static thrust_pid_t z_vel_pid; + thread_running = true; struct multirotor_position_control_params params; @@ -219,6 +214,13 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { parameters_init(¶ms_h); parameters_update(¶ms_h, ¶ms); + for (int i = 0; i < 2; i++) { + pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, params.xy_vel_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); + pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.slope_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); + } + pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); + thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); + int paramcheck_counter = 0; while (!thread_should_exit) { @@ -231,6 +233,12 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { orb_check(param_sub, ¶m_updated); if (param_updated) { parameters_update(¶ms_h, ¶ms); + for (int i = 0; i < 2; i++) { + pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, params.xy_vel_max); + pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.slope_max); + } + pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max); + thrust_pid_set_parameters(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min); } paramcheck_counter = 0; } @@ -269,7 +277,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { if (reset_sp_alt) { reset_sp_alt = false; local_pos_sp.z = local_pos.z; - alt_integral = manual.throttle; + z_vel_pid.integral = -manual.throttle; // thrust PID uses Z downside mavlink_log_info(mavlink_fd, "reset alt setpoint: z = %.2f, throttle = %.2f", local_pos_sp.z, manual.throttle); } @@ -277,18 +285,17 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { reset_sp_pos = false; local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; - pos_x_integral = 0.0f; - pos_y_integral = 0.0f; + xy_vel_pids[0].integral = 0.0f; + xy_vel_pids[1].integral = 0.0f; mavlink_log_info(mavlink_fd, "reset pos setpoint: x = %.2f, y = %.2f", local_pos_sp.x, local_pos_sp.y); } - float alt_err_linear_limit = params.alt_d / params.alt_p * params.alt_rate_max; - float pos_err_linear_limit = params.pos_d / params.pos_p * params.pos_rate_max; + float z_sp_offs_max = params.z_vel_max / params.z_p * 2.0f; + float xy_sp_offs_max = params.xy_vel_max / params.xy_p * 2.0f; - float pos_sp_speed_x = 0.0f; - float pos_sp_speed_y = 0.0f; - float pos_sp_speed_z = 0.0f; + float sp_move_rate[3] = { 0.0f, 0.0f, 0.0f }; + /* manual control */ if (status.flag_control_manual_enabled) { if (local_pos.home_timestamp != home_alt_t) { if (home_alt_t != 0) { @@ -299,14 +306,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { home_alt = local_pos.home_alt; } /* move altitude setpoint with manual controls */ - float alt_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz); - if (alt_sp_ctl != 0.0f) { - pos_sp_speed_z = -alt_sp_ctl * params.alt_rate_max; - local_pos_sp.z += pos_sp_speed_z * dt; - if (local_pos_sp.z > local_pos.z + alt_err_linear_limit) { - local_pos_sp.z = local_pos.z + alt_err_linear_limit; - } else if (local_pos_sp.z < local_pos.z - alt_err_linear_limit) { - local_pos_sp.z = local_pos.z - alt_err_linear_limit; + float z_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz); + if (z_sp_ctl != 0.0f) { + sp_move_rate[2] = -z_sp_ctl * params.z_vel_max; + local_pos_sp.z += sp_move_rate[2] * dt; + if (local_pos_sp.z > local_pos.z + z_sp_offs_max) { + local_pos_sp.z = local_pos.z + z_sp_offs_max; + } else if (local_pos_sp.z < local_pos.z - z_sp_offs_max) { + local_pos_sp.z = local_pos.z - z_sp_offs_max; } } @@ -316,76 +323,84 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { float pos_roll_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz); if (pos_pitch_sp_ctl != 0.0f || pos_roll_sp_ctl != 0.0f) { /* calculate direction and increment of control in NED frame */ - float pos_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl); - float pos_sp_ctl_speed = norm(pos_pitch_sp_ctl, pos_roll_sp_ctl) * params.pos_rate_max; - pos_sp_speed_x = cosf(pos_sp_ctl_dir) * pos_sp_ctl_speed; - pos_sp_speed_y = sinf(pos_sp_ctl_dir) * pos_sp_ctl_speed; - local_pos_sp.x += pos_sp_speed_x * dt; - local_pos_sp.y += pos_sp_speed_y * dt; + float xy_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl); + float xy_sp_ctl_speed = norm(pos_pitch_sp_ctl, pos_roll_sp_ctl) * params.xy_vel_max; + sp_move_rate[0] = cosf(xy_sp_ctl_dir) * xy_sp_ctl_speed; + sp_move_rate[1] = sinf(xy_sp_ctl_dir) * xy_sp_ctl_speed; + local_pos_sp.x += sp_move_rate[0] * dt; + local_pos_sp.y += sp_move_rate[1] * dt; /* limit maximum setpoint from position offset and preserve direction */ float pos_vec_x = local_pos_sp.x - local_pos.x; float pos_vec_y = local_pos_sp.y - local_pos.y; - float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / pos_err_linear_limit; + float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / xy_sp_offs_max; if (pos_vec_norm > 1.0f) { local_pos_sp.x = local_pos.x + pos_vec_x / pos_vec_norm; local_pos_sp.y = local_pos.y + pos_vec_y / pos_vec_norm; } } } - - if (params.hard == 0) { - pos_sp_speed_x = 0.0f; - pos_sp_speed_y = 0.0f; - pos_sp_speed_z = 0.0f; - } } - /* PID for altitude */ - /* don't accelerate more than ALT_RATE_MAX, limit error to corresponding value */ - float alt_err = limit_value(local_pos_sp.z - local_pos.z, alt_err_linear_limit); - /* P and D components */ - float thrust_ctl_pd = -(alt_err * params.alt_p + (pos_sp_speed_z - local_pos.vz) * params.alt_d); // altitude = -z - /* integrate */ - alt_integral += thrust_ctl_pd / params.alt_p * params.alt_i * dt; - if (alt_integral < params.thr_min) { - alt_integral = params.thr_min; - } else if (alt_integral > params.thr_max) { - alt_integral = params.thr_max; - } - /* add I component */ - float thrust_ctl = thrust_ctl_pd + alt_integral; - if (thrust_ctl < params.thr_min) { - thrust_ctl = params.thr_min; - } else if (thrust_ctl > params.thr_max) { - thrust_ctl = params.thr_max; - } + /* run position & altitude controllers, calculate velocity setpoint */ + float vel_sp[3] = { 0.0f, 0.0f, 0.0f }; + vel_sp[2] = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz, dt); if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) { - /* PID for position */ - /* don't accelerate more than POS_RATE_MAX, limit error to corresponding value */ - float pos_x_err = limit_value(local_pos.x - local_pos_sp.x, pos_err_linear_limit); - float pos_y_err = limit_value(local_pos.y - local_pos_sp.y, pos_err_linear_limit); - /* P and D components */ - float pos_x_ctl_pd = - pos_x_err * params.pos_p + (pos_sp_speed_x - local_pos.vx) * params.pos_d; - float pos_y_ctl_pd = - pos_y_err * params.pos_p + (pos_sp_speed_y - local_pos.vy) * params.pos_d; - /* integrate */ - pos_x_integral = limit_value(pos_x_integral + pos_x_ctl_pd / params.pos_p * params.pos_i * dt, params.slope_max); - pos_y_integral = limit_value(pos_y_integral + pos_y_ctl_pd / params.pos_p * params.pos_i * dt, params.slope_max); - /* add I component */ - float pos_x_ctl = pos_x_ctl_pd + pos_x_integral; - float pos_y_ctl = pos_y_ctl_pd + pos_y_integral; - /* calculate direction and slope in NED frame */ - float dir = atan2f(pos_y_ctl, pos_x_ctl); - /* use approximation: slope ~ sin(slope) = force */ - float slope = limit_value(sqrtf(pos_x_ctl * pos_x_ctl + pos_y_ctl * pos_y_ctl), params.slope_max); - /* convert direction to body frame */ - dir -= att.yaw; - /* calculate roll and pitch */ - att_sp.pitch_body = -cosf(dir) * slope; // reverse pitch - att_sp.roll_body = sinf(dir) * slope; + /* calculate velocity set point in NED frame */ + vel_sp[0] = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx, dt); + vel_sp[1] = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy, dt); } else { reset_sp_pos = true; } - att_sp.thrust = thrust_ctl; + /* calculate direction and norm of thrust in NED frame + * limit 3D speed by ellipsoid: + * (vx/xy_vel_max)^2 + (vy/xy_vel_max)^2 + (vz/z_vel_max)^2 = 1 */ + float v; + float vel_sp_norm = 0.0f; + v = vel_sp[0] / params.xy_vel_max; + vel_sp_norm += v * v; + v = vel_sp[1] / params.xy_vel_max; + vel_sp_norm += v * v; + v = vel_sp[2] / params.z_vel_max; + vel_sp_norm += v * v; + vel_sp_norm = sqrtf(vel_sp_norm); + if (vel_sp_norm > 1.0f) { + vel_sp[0] /= vel_sp_norm; + vel_sp[1] /= vel_sp_norm; + vel_sp[2] /= vel_sp_norm; + } + + /* run velocity controllers, calculate thrust vector */ + float thrust_sp[3] = { 0.0f, 0.0f, 0.0f }; + thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, vel_sp[2], local_pos.vz, dt); + if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) { + /* calculate velocity set point in NED frame */ + thrust_sp[0] = pid_calculate(&xy_vel_pids[0], vel_sp[0], local_pos.vx, 0.0f, dt); + thrust_sp[1] = pid_calculate(&xy_vel_pids[1], vel_sp[1], local_pos.vy, 0.0f, dt); + } + /* thrust_vector now contains desired acceleration (but not in m/s^2) in NED frame */ + /* limit horizontal part of thrust */ + float thrust_xy_dir = atan2f(thrust_sp[1], thrust_sp[0]); + float thrust_xy_norm = norm(thrust_sp[0], thrust_sp[1]); + if (thrust_xy_norm > params.slope_max) { + thrust_xy_norm = params.slope_max; + } + /* use approximation: slope ~ sin(slope) = force */ + /* convert direction to body frame */ + thrust_xy_dir -= att.yaw; + if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) { + /* calculate roll and pitch */ + att_sp.roll_body = sinf(thrust_xy_dir) * thrust_xy_norm; + att_sp.pitch_body = -cosf(thrust_xy_dir) * thrust_xy_norm / cosf(att_sp.roll_body); // reverse pitch + } + /* attitude-thrust compensation */ + float att_comp; + if (att.R[2][2] > 0.8f) + att_comp = 1.0f / att.R[2][2]; + else if (att.R[2][2] > 0.0f) + att_comp = ((1.0f / 0.8f - 1.0f) / 0.8f) * att.R[2][2] + 1.0f; + else + att_comp = 1.0f; + att_sp.thrust = -thrust_sp[2] * att_comp; att_sp.timestamp = hrt_absolute_time(); if (status.flag_control_manual_enabled) { /* publish local position setpoint in manual mode */ @@ -403,8 +418,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { } - printf("[multirotor_pos_control] exiting\n"); - mavlink_log_info(mavlink_fd, "[multirotor_pos_control] exiting"); + warnx("stopped"); + mavlink_log_info(mavlink_fd, "[multirotor_pos_control] stopped"); thread_running = false; diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c index d284de433c..f8a982c6c0 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -44,31 +44,37 @@ /* controller parameters */ PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.3f); PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.7f); -PARAM_DEFINE_FLOAT(MPC_ALT_P, 0.1f); -PARAM_DEFINE_FLOAT(MPC_ALT_I, 0.1f); -PARAM_DEFINE_FLOAT(MPC_ALT_D, 0.1f); -PARAM_DEFINE_FLOAT(MPC_ALT_RATE_MAX, 3.0f); -PARAM_DEFINE_FLOAT(MPC_POS_P, 0.1f); -PARAM_DEFINE_FLOAT(MPC_POS_I, 0.0f); -PARAM_DEFINE_FLOAT(MPC_POS_D, 0.2f); -PARAM_DEFINE_FLOAT(MPC_POS_RATE_MAX, 10.0f); +PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f); +PARAM_DEFINE_FLOAT(MPC_Z_D, 0.0f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.1f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.0f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 3.0f); +PARAM_DEFINE_FLOAT(MPC_XY_P, 0.5f); +PARAM_DEFINE_FLOAT(MPC_XY_D, 0.0f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.2f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.0f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.0f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 10.0f); PARAM_DEFINE_FLOAT(MPC_SLOPE_MAX, 0.5f); -PARAM_DEFINE_INT32(MPC_HARD, 0); int parameters_init(struct multirotor_position_control_param_handles *h) { h->thr_min = param_find("MPC_THR_MIN"); h->thr_max = param_find("MPC_THR_MAX"); - h->alt_p = param_find("MPC_ALT_P"); - h->alt_i = param_find("MPC_ALT_I"); - h->alt_d = param_find("MPC_ALT_D"); - h->alt_rate_max = param_find("MPC_ALT_RATE_MAX"); - h->pos_p = param_find("MPC_POS_P"); - h->pos_i = param_find("MPC_POS_I"); - h->pos_d = param_find("MPC_POS_D"); - h->pos_rate_max = param_find("MPC_POS_RATE_MAX"); + h->z_p = param_find("MPC_Z_P"); + h->z_d = param_find("MPC_Z_D"); + h->z_vel_p = param_find("MPC_Z_VEL_P"); + h->z_vel_i = param_find("MPC_Z_VEL_I"); + h->z_vel_d = param_find("MPC_Z_VEL_D"); + h->z_vel_max = param_find("MPC_Z_VEL_MAX"); + h->xy_p = param_find("MPC_XY_P"); + h->xy_d = param_find("MPC_XY_D"); + h->xy_vel_p = param_find("MPC_XY_VEL_P"); + h->xy_vel_i = param_find("MPC_XY_VEL_I"); + h->xy_vel_d = param_find("MPC_XY_VEL_D"); + h->xy_vel_max = param_find("MPC_XY_VEL_MAX"); h->slope_max = param_find("MPC_SLOPE_MAX"); - h->hard = param_find("MPC_HARD"); h->rc_scale_pitch = param_find("RC_SCALE_PITCH"); h->rc_scale_roll = param_find("RC_SCALE_ROLL"); @@ -81,16 +87,19 @@ int parameters_update(const struct multirotor_position_control_param_handles *h, { param_get(h->thr_min, &(p->thr_min)); param_get(h->thr_max, &(p->thr_max)); - param_get(h->alt_p, &(p->alt_p)); - param_get(h->alt_i, &(p->alt_i)); - param_get(h->alt_d, &(p->alt_d)); - param_get(h->alt_rate_max, &(p->alt_rate_max)); - param_get(h->pos_p, &(p->pos_p)); - param_get(h->pos_i, &(p->pos_i)); - param_get(h->pos_d, &(p->pos_d)); - param_get(h->pos_rate_max, &(p->pos_rate_max)); + param_get(h->z_p, &(p->z_p)); + param_get(h->z_d, &(p->z_d)); + param_get(h->z_vel_p, &(p->z_vel_p)); + param_get(h->z_vel_i, &(p->z_vel_i)); + param_get(h->z_vel_d, &(p->z_vel_d)); + param_get(h->z_vel_max, &(p->z_vel_max)); + param_get(h->xy_p, &(p->xy_p)); + param_get(h->xy_d, &(p->xy_d)); + param_get(h->xy_vel_p, &(p->xy_vel_p)); + param_get(h->xy_vel_i, &(p->xy_vel_i)); + param_get(h->xy_vel_d, &(p->xy_vel_d)); + param_get(h->xy_vel_max, &(p->xy_vel_max)); param_get(h->slope_max, &(p->slope_max)); - param_get(h->hard, &(p->hard)); param_get(h->rc_scale_pitch, &(p->rc_scale_pitch)); param_get(h->rc_scale_roll, &(p->rc_scale_roll)); diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h index 13b667ad30..e9b1f5c390 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h @@ -44,16 +44,19 @@ struct multirotor_position_control_params { float thr_min; float thr_max; - float alt_p; - float alt_i; - float alt_d; - float alt_rate_max; - float pos_p; - float pos_i; - float pos_d; - float pos_rate_max; + float z_p; + float z_d; + float z_vel_p; + float z_vel_i; + float z_vel_d; + float z_vel_max; + float xy_p; + float xy_d; + float xy_vel_p; + float xy_vel_i; + float xy_vel_d; + float xy_vel_max; float slope_max; - int hard; float rc_scale_pitch; float rc_scale_roll; @@ -63,16 +66,19 @@ struct multirotor_position_control_params { struct multirotor_position_control_param_handles { param_t thr_min; param_t thr_max; - param_t alt_p; - param_t alt_i; - param_t alt_d; - param_t alt_rate_max; - param_t pos_p; - param_t pos_i; - param_t pos_d; - param_t pos_rate_max; + param_t z_p; + param_t z_d; + param_t z_vel_p; + param_t z_vel_i; + param_t z_vel_d; + param_t z_vel_max; + param_t xy_p; + param_t xy_d; + param_t xy_vel_p; + param_t xy_vel_i; + param_t xy_vel_d; + param_t xy_vel_max; param_t slope_max; - param_t hard; param_t rc_scale_pitch; param_t rc_scale_roll; diff --git a/src/modules/multirotor_pos_control/thrust_pid.c b/src/modules/multirotor_pos_control/thrust_pid.c new file mode 100644 index 0000000000..89efe1334e --- /dev/null +++ b/src/modules/multirotor_pos_control/thrust_pid.c @@ -0,0 +1,179 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Laurens Mackay + * Tobias Naegeli + * Martin Rutschmann + * Anton Babushkin + * Julian Oes + * + * 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 thrust_pid.c + * + * Implementation of generic PID control interface. + * + * @author Laurens Mackay + * @author Tobias Naegeli + * @author Martin Rutschmann + * @author Anton Babushkin + * @author Julian Oes + */ + +#include "thrust_pid.h" +#include + +__EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min) +{ + pid->kp = kp; + pid->ki = ki; + pid->kd = kd; + pid->limit_min = limit_min; + pid->limit_max = limit_max; + pid->mode = mode; + pid->dt_min = dt_min; + pid->last_output = 0.0f; + pid->sp = 0.0f; + pid->error_previous = 0.0f; + pid->integral = 0.0f; +} + +__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max) +{ + int ret = 0; + + if (isfinite(kp)) { + pid->kp = kp; + + } else { + ret = 1; + } + + if (isfinite(ki)) { + pid->ki = ki; + + } else { + ret = 1; + } + + if (isfinite(kd)) { + pid->kd = kd; + + } else { + ret = 1; + } + + if (isfinite(limit_min)) { + pid->limit_min = limit_min; + + } else { + ret = 1; + } + + if (isfinite(limit_max)) { + pid->limit_max = limit_max; + + } else { + ret = 1; + } + + return ret; +} + +__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt) +{ + /* Alternative integral component calculation + error = setpoint - actual_position + integral = integral + (Ki*error*dt) + derivative = (error - previous_error)/dt + output = (Kp*error) + integral + (Kd*derivative) + previous_error = error + wait(dt) + goto start + */ + + if (!isfinite(sp) || !isfinite(val) || !isfinite(dt)) { + return pid->last_output; + } + + float i, d; + pid->sp = sp; + + // Calculated current error value + float error = pid->sp - val; + + // Calculate or measured current error derivative + if (pid->mode == THRUST_PID_MODE_DERIVATIV_CALC) { + d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min); + pid->error_previous = error; + + } else if (pid->mode == THRUST_PID_MODE_DERIVATIV_CALC_NO_SP) { + d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min); + pid->error_previous = -val; + + } else { + d = 0.0f; + } + + if (!isfinite(d)) { + d = 0.0f; + } + + // Calculate the error integral and check for saturation + i = pid->integral + (pid->ki * error * dt); + + float output = (error * pid->kp) + i + (d * pid->kd); + if (output < pid->limit_min || output > pid->limit_max) { + i = pid->integral; // If saturated then do not update integral value + // recalculate output with old integral + output = (error * pid->kp) + i + (d * pid->kd); + + } else { + if (!isfinite(i)) { + i = 0.0f; + } + + pid->integral = i; + } + + if (isfinite(output)) { + if (output > pid->limit_max) { + output = pid->limit_max; + + } else if (output < pid->limit_min) { + output = pid->limit_min; + } + + pid->last_output = output; + } + + return pid->last_output; +} diff --git a/src/modules/multirotor_pos_control/thrust_pid.h b/src/modules/multirotor_pos_control/thrust_pid.h new file mode 100644 index 0000000000..65ee33c51c --- /dev/null +++ b/src/modules/multirotor_pos_control/thrust_pid.h @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin + * + * 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 thrust_pid.h + * + * Definition of thrust control PID interface. + * + * @author Anton Babushkin + */ + +#ifndef THRUST_PID_H_ +#define THRUST_PID_H_ + +#include + +__BEGIN_DECLS + +/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error */ +#define THRUST_PID_MODE_DERIVATIV_CALC 0 +/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, setpoint derivative is ignored */ +#define THRUST_PID_MODE_DERIVATIV_CALC_NO_SP 1 + +typedef struct { + float kp; + float ki; + float kd; + float sp; + float integral; + float error_previous; + float last_output; + float limit_min; + float limit_max; + float dt_min; + uint8_t mode; +} thrust_pid_t; + +__EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min); +__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max); +__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt); + +__END_DECLS + +#endif /* THRUST_PID_H_ */ diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index fb5a779bc0..9db2633c68 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -120,7 +120,7 @@ int position_estimator_inav_main(int argc, char *argv[]) verbose_mode = true; thread_should_exit = false; - position_estimator_inav_task = task_spawn("position_estimator_inav", + position_estimator_inav_task = task_spawn_cmd("position_estimator_inav", SCHED_RR, SCHED_PRIORITY_MAX - 5, 4096, position_estimator_inav_thread_main, (argv) ? (const char **) &argv[2] : (const char **) NULL); From f5b91e109df755a6171264b59e92099b3ab20dbe Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 10 Jul 2013 23:50:37 -0700 Subject: [PATCH 235/486] More GPIO and general pin assignment fixes. --- src/drivers/drv_gpio.h | 9 ++++++--- src/drivers/px4fmu/fmu.cpp | 22 +++++++++++++--------- src/drivers/stm32/adc/adc.cpp | 12 +++++++++--- 3 files changed, 28 insertions(+), 15 deletions(-) diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h index faeb9cf60a..5b06f91a82 100644 --- a/src/drivers/drv_gpio.h +++ b/src/drivers/drv_gpio.h @@ -80,9 +80,12 @@ # define GPIO_SERVO_5 (1<<4) /**< servo 5 output */ # define GPIO_SERVO_6 (1<<5) /**< servo 6 output */ -# define GPIO_5V_PERIPH_EN (1<<6) /**< PA8 - VDD_5V_PERIPH_EN */ -# define GPIO_5V_HIPOWER_OC (1<<7) /**< PE10 - !VDD_5V_HIPOWER_OC */ -# define GPIO_5V_PERIPH_OC (1<<8) /**< PE15 - !VDD_5V_PERIPH_OC */ +# define GPIO_5V_PERIPH_EN (1<<6) /**< PA8 - !VDD_5V_PERIPH_EN */ +# define GPIO_3V3_SENSORS_EN (1<<7) /**< PE3 - VDD_3V3_SENSORS_EN */ +# define GPIO_BRICK_VALID (1<<8) /**< PB5 - !VDD_BRICK_VALID */ +# define GPIO_SERVO_VALID (1<<9) /**< PB7 - !VDD_SERVO_VALID */ +# define GPIO_5V_HIPOWER_OC (1<<10) /**< PE10 - !VDD_5V_HIPOWER_OC */ +# define GPIO_5V_PERIPH_OC (1<<11) /**< PE10 - !VDD_5V_PERIPH_OC */ /** * Default GPIO device - other devices may also support this protocol if diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 7d3af4b0d2..7928752d5f 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -169,15 +169,19 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { {GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, GPIO_CAN2_RX_2}, #endif #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) - {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, - {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, - {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, 0}, - {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, 0}, - {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, 0}, - {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, 0}, - {0, GPIO_VDD_5V_PERIPH_EN, 0}, - {GPIO_5V_HIPOWER_OC, 0, 0}, - {GPIO_5V_PERIPH_OC, 0, 0}, + {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, + {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, + {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, 0}, + {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, 0}, + {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, 0}, + {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, 0}, + + {0, GPIO_VDD_5V_PERIPH_EN, 0}, + {0, GPIO_VDD_3V3_SENSORS_EN, 0}, + {GPIO_VDD_BRICK_VALID, 0, 0}, + {GPIO_VDD_SERVO_VALID, 0, 0}, + {GPIO_VDD_5V_HIPOWER_OC, 0, 0}, + {GPIO_VDD_5V_PERIPH_OC, 0, 0}, #endif }; diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp index 1020eb9465..48c95b3dd8 100644 --- a/src/drivers/stm32/adc/adc.cpp +++ b/src/drivers/stm32/adc/adc.cpp @@ -227,7 +227,6 @@ ADC::init() if ((hrt_absolute_time() - now) > 500) { log("sample timeout"); return -1; - return 0xffff; } } @@ -282,7 +281,7 @@ ADC::close_last(struct file *filp) void ADC::_tick_trampoline(void *arg) { - ((ADC *)arg)->_tick(); + (reinterpret_cast(arg))->_tick(); } void @@ -366,8 +365,15 @@ int adc_main(int argc, char *argv[]) { if (g_adc == nullptr) { - /* XXX this hardcodes the default channel set for PX4FMU - should be configurable */ +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + /* XXX this hardcodes the default channel set for PX4FMUv1 - should be configurable */ g_adc = new ADC((1 << 10) | (1 << 11) | (1 << 12) | (1 << 13)); +#endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + /* XXX this hardcodes the default channel set for PX4FMUv2 - should be configurable */ + g_adc = new ADC((1 << 2) | (1 << 3) | (1 << 4) | + (1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) | (1 << 14) | (1 << 15)); +#endif if (g_adc == nullptr) errx(1, "couldn't allocate the ADC driver"); From c51c211bbaef70defe73c91f6acd6ab99f0f3a04 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 12 Jul 2013 08:45:49 +0400 Subject: [PATCH 236/486] position_estimator_inav: global position altitude fixed --- .../position_estimator_inav/position_estimator_inav_main.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 9db2633c68..3b0fbd7823 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -548,7 +548,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); global_pos.lat = (int32_t)(est_lat * 1e7); global_pos.lon = (int32_t)(est_lon * 1e7); - global_pos.alt = -local_pos.z - local_pos.home_alt; + global_pos.alt = local_pos.home_alt - local_pos.z; global_pos.relative_alt = -local_pos.z; global_pos.vx = local_pos.vx; global_pos.vy = local_pos.vy; From 16cb0a793d60e55efe0b851ff52f31a4c7770834 Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 11 Jul 2013 23:23:10 -0700 Subject: [PATCH 237/486] Some more v2 pin / gpio configs missed in the previous commit --- src/drivers/boards/px4fmuv2/px4fmu_init.c | 18 +++++++++++++----- src/drivers/boards/px4fmuv2/px4fmu_internal.h | 4 +++- 2 files changed, 16 insertions(+), 6 deletions(-) diff --git a/src/drivers/boards/px4fmuv2/px4fmu_init.c b/src/drivers/boards/px4fmuv2/px4fmu_init.c index f9d1b80226..57f2812b87 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu_init.c +++ b/src/drivers/boards/px4fmuv2/px4fmu_init.c @@ -149,13 +149,21 @@ __EXPORT int nsh_archinitialize(void) { /* configure ADC pins */ - stm32_configgpio(GPIO_ADC1_IN10); - stm32_configgpio(GPIO_ADC1_IN11); - stm32_configgpio(GPIO_ADC1_IN12); + stm32_configgpio(GPIO_ADC1_IN2); /* BATT_VOLTAGE_SENS */ + stm32_configgpio(GPIO_ADC1_IN3); /* BATT_CURRENT_SENS */ + stm32_configgpio(GPIO_ADC1_IN4); /* VDD_5V_SENS */ + stm32_configgpio(GPIO_ADC1_IN10); /* unrouted */ + stm32_configgpio(GPIO_ADC1_IN11); /* unrouted */ + stm32_configgpio(GPIO_ADC1_IN12); /* unrouted */ + stm32_configgpio(GPIO_ADC1_IN13); /* FMU_AUX_ADC_1 */ + stm32_configgpio(GPIO_ADC1_IN14); /* FMU_AUX_ADC_2 */ + stm32_configgpio(GPIO_ADC1_IN15); /* PRESSURE_SENS */ - /* configure power supply control pins */ + /* configure power supply control/sense pins */ stm32_configgpio(GPIO_VDD_5V_PERIPH_EN); - stm32_configgpio(GPIO_VDD_2V8_SENSORS_EN); + stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); + stm32_configgpio(GPIO_VDD_BRICK_VALID); + stm32_configgpio(GPIO_VDD_SERVO_VALID); stm32_configgpio(GPIO_VDD_5V_HIPOWER_OC); stm32_configgpio(GPIO_VDD_5V_PERIPH_OC); diff --git a/src/drivers/boards/px4fmuv2/px4fmu_internal.h b/src/drivers/boards/px4fmuv2/px4fmu_internal.h index 76aa042f64..ad66ce563c 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu_internal.h +++ b/src/drivers/boards/px4fmuv2/px4fmu_internal.h @@ -102,7 +102,9 @@ __BEGIN_DECLS /* Power supply control and monitoring GPIOs */ #define GPIO_VDD_5V_PERIPH_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN8) -#define GPIO_VDD_2V8_SENSORS_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3) +#define GPIO_VDD_BRICK_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) +#define GPIO_VDD_SERVO_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN7) +#define GPIO_VDD_3V3_SENSORS_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3) #define GPIO_VDD_5V_HIPOWER_OC (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN10) #define GPIO_VDD_5V_PERIPH_OC (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN15) From 95f59f521d7c18bf5348a23addb0471ce81f016e Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 11 Jul 2013 23:23:58 -0700 Subject: [PATCH 238/486] Add builtin command dependency on module makefiles, since they can also create / remove commands --- makefiles/firmware.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index f1c1b496a0..3ad13088b0 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -385,7 +385,7 @@ define BUILTIN_DEF endef # Don't generate until modules have updated their command files -$(BUILTIN_CSRC): $(GLOBAL_DEPS) $(MODULE_OBJS) $(BUILTIN_COMMAND_FILES) +$(BUILTIN_CSRC): $(GLOBAL_DEPS) $(MODULE_OBJS) $(MODULE_MKFILES) $(BUILTIN_COMMAND_FILES) @$(ECHO) "CMDS: $@" $(Q) $(ECHO) '/* builtin command list - automatically generated, do not edit */' > $@ $(Q) $(ECHO) '#include ' >> $@ From c459901f263b19d13466c47fcb8e2dce828ab59c Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 11 Jul 2013 23:52:36 -0700 Subject: [PATCH 239/486] Let's have some direct-access I/O methods as well. --- src/drivers/device/cdev.cpp | 35 +++++++++++++++++++++------ src/drivers/device/device.h | 47 +++++++++++++++++++++++++++++++++---- 2 files changed, 70 insertions(+), 12 deletions(-) diff --git a/src/drivers/device/cdev.cpp b/src/drivers/device/cdev.cpp index 422321850a..dcbac25e30 100644 --- a/src/drivers/device/cdev.cpp +++ b/src/drivers/device/cdev.cpp @@ -111,21 +111,21 @@ CDev::~CDev() int CDev::init() { - int ret = OK; - // base class init first - ret = Device::init(); + int ret = Device::init(); if (ret != OK) goto out; // now register the driver - ret = register_driver(_devname, &fops, 0666, (void *)this); + if (_devname != nullptr) { + ret = register_driver(_devname, &fops, 0666, (void *)this); - if (ret != OK) - goto out; + if (ret != OK) + goto out; - _registered = true; + _registered = true; + } out: return ret; @@ -395,4 +395,25 @@ cdev_poll(struct file *filp, struct pollfd *fds, bool setup) return cdev->poll(filp, fds, setup); } +int +CDev::read(unsigned offset, void *data, unsigned count) +{ + errno = ENODEV; + return -1; +} + +int +CDev::write(unsigned offset, void *data, unsigned count) +{ + errno = ENODEV; + return -1; +} + +int +CDev::ioctl(unsigned operation, unsigned &arg) +{ + errno = ENODEV; + return -1; +} + } // namespace device \ No newline at end of file diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h index 7d375aab91..2cac86636d 100644 --- a/src/drivers/device/device.h +++ b/src/drivers/device/device.h @@ -85,7 +85,7 @@ protected: */ Device(const char *name, int irq = 0); - ~Device(); + virtual ~Device(); /** * Initialise the driver and make it ready for use. @@ -189,7 +189,7 @@ public: /** * Destructor */ - ~CDev(); + virtual ~CDev(); virtual int init(); @@ -287,6 +287,43 @@ public: */ bool is_open() { return _open_count > 0; } + /* + * Direct access methods. + */ + + /** + * Read directly from the device. + * + * The actual size of each unit quantity is device-specific. + * + * @param offset The device offset at which to start reading + * @param data The buffer into which the read values should be placed. + * @param count The number of items to read, defaults to 1. + * @return count on success, < 0 on error. + */ + virtual int read(unsigned offset, void *data, unsigned count = 1); + + /** + * Write directly to the device. + * + * The actual size of each unit quantity is device-specific. + * + * @param address The device address at which to start writing. + * @param data The buffer from which values should be read. + * @param count The number of registers to write, defaults to 1. + * @return count on success, < 0 on error. + */ + virtual int write(unsigned address, void *data, unsigned count = 1); + + /** + * Perform a device-specific operation. + * + * @param operation The operation to perform + * @param arg An argument to the operation. + * @return < 0 on error + */ + virtual int ioctl(unsigned operation, unsigned &arg); + protected: /** * Pointer to the default cdev file operations table; useful for @@ -396,9 +433,9 @@ public: const char *devname, uint32_t base, int irq = 0); - ~PIO(); + virtual ~PIO(); - int init(); + virtual int init(); protected: @@ -407,7 +444,7 @@ protected: * * @param offset Register offset in bytes from the base address. */ - uint32_t reg(uint32_t offset) { + uint32_t reg(uint32_t offset) { return *(volatile uint32_t *)(_base + offset); } From d4c6ebde338c8aa1b0d2c9574d3bbdeead525cea Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 12 Jul 2013 13:17:42 +0400 Subject: [PATCH 240/486] multirotor_pos_control: global_position_setpoint support in AUTO mode --- .../multirotor_pos_control.c | 39 ++++++++++++++++++- 1 file changed, 38 insertions(+), 1 deletion(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index d56c3d58f8..3e5857c265 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -60,6 +60,7 @@ #include #include #include +#include #include #include #include @@ -179,6 +180,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { memset(&local_pos, 0, sizeof(local_pos)); struct vehicle_local_position_setpoint_s local_pos_sp; memset(&local_pos_sp, 0, sizeof(local_pos_sp)); + struct vehicle_global_position_setpoint_s global_pos_sp; + memset(&local_pos_sp, 0, sizeof(local_pos_sp)); /* subscribe to attitude, motor setpoints and system state */ int param_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -188,6 +191,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); int local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); /* publish setpoint */ orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &local_pos_sp); @@ -201,6 +205,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { const float pos_ctl_dz = 0.05f; float home_alt = 0.0f; hrt_abstime home_alt_t = 0; + uint64_t local_home_timestamp = 0; static PID_t xy_pos_pids[2]; static PID_t xy_vel_pids[2]; @@ -222,6 +227,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); int paramcheck_counter = 0; + bool global_pos_sp_updated = false; while (!thread_should_exit) { orb_copy(ORB_ID(vehicle_status), state_sub, &status); @@ -243,6 +249,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { paramcheck_counter = 0; } + /* only check global position setpoint updates but not read */ + if (!global_pos_sp_updated) { + orb_check(global_pos_sp_sub, &global_pos_sp_updated); + } + /* Check if controller should act */ bool act = status.flag_system_armed && ( /* SAS modes */ @@ -271,7 +282,33 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); if (status.state_machine == SYSTEM_STATE_AUTO) { - orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); + //orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); + if (local_pos.home_timestamp != local_home_timestamp) { + local_home_timestamp = local_pos.home_timestamp; + /* init local projection using local position home */ + double lat_home = local_pos.home_lat * 1e-7; + double lon_home = local_pos.home_lon * 1e-7; + map_projection_init(lat_home, lon_home); + warnx("local pos home: lat = %.10f, lon = %.10f", lat_home, lon_home); + mavlink_log_info(mavlink_fd, "local pos home: %.7f, %.7f", lat_home, lon_home); + } + if (global_pos_sp_updated) { + global_pos_sp_updated = false; + orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp); + double sp_lat = global_pos_sp.lat * 1e-7; + double sp_lon = global_pos_sp.lon * 1e-7; + /* project global setpoint to local setpoint */ + map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y)); + if (global_pos_sp.altitude_is_relative) { + local_pos_sp.z = -global_pos_sp.altitude; + } else { + local_pos_sp.z = local_pos.home_alt - global_pos_sp.altitude; + } + warnx("new setpoint: lat = %.10f, lon = %.10f, x = %.2f, y = %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); + mavlink_log_info(mavlink_fd, "new setpoint: %.7f, %.7f, %.2f, %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); + /* publish local position setpoint as projection of global position setpoint */ + orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); + } } if (reset_sp_alt) { From 5937c3a31b39d2fe5b70c2eef6327562208f1042 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 12 Jul 2013 16:30:11 +0400 Subject: [PATCH 241/486] uORB topic vehicle_global_velocity_setpoint added --- src/modules/uORB/objects_common.cpp | 3 + .../topics/vehicle_global_velocity_setpoint.h | 64 +++++++++++++++++++ 2 files changed, 67 insertions(+) create mode 100644 src/modules/uORB/topics/vehicle_global_velocity_setpoint.h diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index ae5fc6c61c..1d5f174059 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -110,6 +110,9 @@ ORB_DEFINE(vehicle_global_position_setpoint, struct vehicle_global_position_setp #include "topics/vehicle_global_position_set_triplet.h" ORB_DEFINE(vehicle_global_position_set_triplet, struct vehicle_global_position_set_triplet_s); +#include "topics/vehicle_global_velocity_setpoint.h" +ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s); + #include "topics/mission.h" ORB_DEFINE(mission, struct mission_s); diff --git a/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h b/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h new file mode 100644 index 0000000000..73961cdfed --- /dev/null +++ b/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h @@ -0,0 +1,64 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: @author Anton Babushkin + * + * 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 vehicle_global_velocity_setpoint.h + * Definition of the global velocity setpoint uORB topic. + */ + +#ifndef TOPIC_VEHICLE_GLOBAL_VELOCITY_SETPOINT_H_ +#define TOPIC_VEHICLE_GLOBAL_VELOCITY_SETPOINT_H_ + +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +struct vehicle_global_velocity_setpoint_s +{ + float vx; /**< in m/s NED */ + float vy; /**< in m/s NED */ + float vz; /**< in m/s NED */ +}; /**< Velocity setpoint in NED frame */ + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_global_velocity_setpoint); + +#endif From eb5af244b9281e8d654d5f87feedde3e652b5fc5 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 12 Jul 2013 21:57:46 +0400 Subject: [PATCH 242/486] sdlog2: GVSP (global velocity setpoint) message added, cleanup --- src/modules/sdlog2/sdlog2.c | 70 ++++++++++++---------------- src/modules/sdlog2/sdlog2_messages.h | 9 ++++ 2 files changed, 40 insertions(+), 39 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index ec1b7628d7..6641d88f32 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -75,6 +75,7 @@ #include #include #include +#include #include #include #include @@ -94,7 +95,6 @@ log_msgs_written++; \ } else { \ log_msgs_skipped++; \ - /*printf("skip\n");*/ \ } #define LOG_ORB_SUBSCRIBE(_var, _topic) subs.##_var##_sub = orb_subscribe(ORB_ID(##_topic##)); \ @@ -102,9 +102,6 @@ fds[fdsc_count].events = POLLIN; \ fdsc_count++; - -//#define SDLOG2_DEBUG - static bool main_thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int deamon_task; /**< Handle of deamon task / thread */ @@ -233,7 +230,7 @@ int sdlog2_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (thread_running) { - printf("sdlog2 already running\n"); + warnx("already running"); /* this is not an error */ exit(0); } @@ -250,7 +247,7 @@ int sdlog2_main(int argc, char *argv[]) if (!strcmp(argv[1], "stop")) { if (!thread_running) { - printf("\tsdlog2 is not started\n"); + warnx("not started"); } main_thread_should_exit = true; @@ -262,7 +259,7 @@ int sdlog2_main(int argc, char *argv[]) sdlog2_status(); } else { - printf("\tsdlog2 not started\n"); + warnx("not started\n"); } exit(0); @@ -387,11 +384,6 @@ static void *logwriter_thread(void *arg) /* only get pointer to thread-safe data, do heavy I/O a few lines down */ int available = logbuffer_get_ptr(logbuf, &read_ptr, &is_part); -#ifdef SDLOG2_DEBUG - int rp = logbuf->read_ptr; - int wp = logbuf->write_ptr; -#endif - /* continue */ pthread_mutex_unlock(&logbuffer_mutex); @@ -407,9 +399,6 @@ static void *logwriter_thread(void *arg) n = write(log_file, read_ptr, n); should_wait = (n == available) && !is_part; -#ifdef SDLOG2_DEBUG - printf("write %i %i of %i rp=%i wp=%i, is_part=%i, should_wait=%i\n", log_bytes_written, n, available, rp, wp, (int)is_part, (int)should_wait); -#endif if (n < 0) { main_thread_should_exit = true; @@ -422,14 +411,8 @@ static void *logwriter_thread(void *arg) } else { n = 0; -#ifdef SDLOG2_DEBUG - printf("no data available, main_thread_should_exit=%i, logwriter_should_exit=%i\n", (int)main_thread_should_exit, (int)logwriter_should_exit); -#endif /* exit only with empty buffer */ if (main_thread_should_exit || logwriter_should_exit) { -#ifdef SDLOG2_DEBUG - printf("break logwriter thread\n"); -#endif break; } should_wait = true; @@ -444,10 +427,6 @@ static void *logwriter_thread(void *arg) fsync(log_file); close(log_file); -#ifdef SDLOG2_DEBUG - printf("logwriter thread exit\n"); -#endif - return OK; } @@ -614,14 +593,6 @@ int sdlog2_thread_main(int argc, char *argv[]) errx(1, "can't allocate log buffer, exiting."); } - /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ - /* number of messages */ - const ssize_t fdsc = 19; - /* Sanity check variable and index */ - ssize_t fdsc_count = 0; - /* file descriptors to wait for */ - struct pollfd fds[fdsc]; - struct vehicle_status_s buf_status; memset(&buf_status, 0, sizeof(buf_status)); @@ -646,6 +617,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct differential_pressure_s diff_pres; struct airspeed_s airspeed; struct esc_status_s esc; + struct vehicle_global_velocity_setpoint_s global_vel_sp; } buf; memset(&buf, 0, sizeof(buf)); @@ -669,6 +641,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int rc_sub; int airspeed_sub; int esc_sub; + int global_vel_sp_sub; } subs; /* log message buffer: header + body */ @@ -694,6 +667,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GPOS_s log_GPOS; struct log_GPSP_s log_GPSP; struct log_ESC_s log_ESC; + struct log_GVSP_s log_GVSP; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -701,6 +675,14 @@ int sdlog2_thread_main(int argc, char *argv[]) #pragma pack(pop) memset(&log_msg.body, 0, sizeof(log_msg.body)); + /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ + /* number of messages */ + const ssize_t fdsc = 20; + /* Sanity check variable and index */ + ssize_t fdsc_count = 0; + /* file descriptors to wait for */ + struct pollfd fds[fdsc]; + /* --- VEHICLE COMMAND --- */ subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); fds[fdsc_count].fd = subs.cmd_sub; @@ -815,6 +797,12 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- GLOBAL VELOCITY SETPOINT --- */ + subs.global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint)); + fds[fdsc_count].fd = subs.global_vel_sp_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* WARNING: If you get the error message below, * then the number of registered messages (fdsc) * differs from the number of messages in the above list. @@ -1166,14 +1154,18 @@ int sdlog2_thread_main(int argc, char *argv[]) } } -#ifdef SDLOG2_DEBUG - printf("fill rp=%i wp=%i count=%i\n", lb.read_ptr, lb.write_ptr, logbuffer_count(&lb)); -#endif + /* --- GLOBAL VELOCITY SETPOINT --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_global_velocity_setpoint), subs.global_vel_sp_sub, &buf.global_vel_sp); + log_msg.msg_type = LOG_GVSP_MSG; + log_msg.body.log_GVSP.vx = buf.global_vel_sp.vx; + log_msg.body.log_GVSP.vy = buf.global_vel_sp.vy; + log_msg.body.log_GVSP.vz = buf.global_vel_sp.vz; + LOGBUFFER_WRITE_AND_COUNT(GVSP); + } + /* signal the other thread new data, but not yet unlock */ if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) { -#ifdef SDLOG2_DEBUG - printf("signal rp=%i wp=%i count=%i\n", lb.read_ptr, lb.write_ptr, logbuffer_count(&lb)); -#endif /* only request write if several packets can be written at once */ pthread_cond_signal(&logbuffer_cond); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 0e479b524b..06e640b5b3 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -244,6 +244,14 @@ struct log_ESC_s { uint16_t esc_setpoint_raw; }; +/* --- GVSP - GLOBAL VELOCITY SETPOINT --- */ +#define LOG_GVSP_MSG 19 +struct log_GVSP_s { + float vx; + float vy; + float vz; +}; + #pragma pack(pop) /* construct list of all message formats */ @@ -267,6 +275,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"), LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"), LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,No,Version,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), + LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), }; static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); From 87782300509572ea593f309cbbf0a1ca64a53775 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 12 Jul 2013 21:59:49 +0400 Subject: [PATCH 243/486] multirotor_pos_control fixes, systemlib/pid now accepts limit = 0.0, means no limit --- .../multirotor_pos_control.c | 147 +++++++++++------- src/modules/systemlib/pid/pid.c | 14 +- 2 files changed, 103 insertions(+), 58 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 3e5857c265..1d2dd384ac 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -61,6 +61,7 @@ #include #include #include +#include #include #include #include @@ -89,9 +90,11 @@ static float scale_control(float ctl, float end, float dz); static float norm(float x, float y); -static void usage(const char *reason) { +static void usage(const char *reason) +{ if (reason) fprintf(stderr, "%s\n", reason); + fprintf(stderr, "usage: deamon {start|stop|status} [-p ]\n\n"); exit(1); } @@ -100,11 +103,12 @@ static void usage(const char *reason) { * The deamon app only briefly exists to start * the background job. The stack size assigned in the * Makefile does only apply to this management task. - * + * * The actual stack size should be set in the call * to task_spawn(). */ -int multirotor_pos_control_main(int argc, char *argv[]) { +int multirotor_pos_control_main(int argc, char *argv[]) +{ if (argc < 1) usage("missing command"); @@ -119,11 +123,11 @@ int multirotor_pos_control_main(int argc, char *argv[]) { warnx("start"); thread_should_exit = false; deamon_task = task_spawn_cmd("multirotor_pos_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 60, - 4096, - multirotor_pos_control_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 60, + 4096, + multirotor_pos_control_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); } @@ -136,9 +140,11 @@ int multirotor_pos_control_main(int argc, char *argv[]) { if (!strcmp(argv[1], "status")) { if (thread_running) { warnx("app is running"); + } else { warnx("app not started"); } + exit(0); } @@ -146,26 +152,31 @@ int multirotor_pos_control_main(int argc, char *argv[]) { exit(1); } -static float scale_control(float ctl, float end, float dz) { +static float scale_control(float ctl, float end, float dz) +{ if (ctl > dz) { return (ctl - dz) / (end - dz); + } else if (ctl < -dz) { return (ctl + dz) / (end - dz); + } else { return 0.0f; } } -static float norm(float x, float y) { +static float norm(float x, float y) +{ return sqrtf(x * x + y * y); } -static int multirotor_pos_control_thread_main(int argc, char *argv[]) { +static int multirotor_pos_control_thread_main(int argc, char *argv[]) +{ /* welcome user */ warnx("started"); static int mavlink_fd; mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(mavlink_fd, "[multirotor_pos_control] started"); + mavlink_log_info(mavlink_fd, "[mpc] started"); /* structures */ struct vehicle_status_s status; @@ -181,7 +192,9 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { struct vehicle_local_position_setpoint_s local_pos_sp; memset(&local_pos_sp, 0, sizeof(local_pos_sp)); struct vehicle_global_position_setpoint_s global_pos_sp; - memset(&local_pos_sp, 0, sizeof(local_pos_sp)); + memset(&global_pos_sp, 0, sizeof(local_pos_sp)); + struct vehicle_global_velocity_setpoint_s global_vel_sp; + memset(&global_vel_sp, 0, sizeof(global_vel_sp)); /* subscribe to attitude, motor setpoints and system state */ int param_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -195,6 +208,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { /* publish setpoint */ orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &local_pos_sp); + orb_advert_t global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &global_vel_sp); orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); bool reset_sp_alt = true; @@ -220,10 +234,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { parameters_update(¶ms_h, ¶ms); for (int i = 0; i < 2; i++) { - pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, params.xy_vel_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); + pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.slope_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); } - pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); + + pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); int paramcheck_counter = 0; @@ -234,18 +249,23 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { /* check parameters at 1 Hz*/ paramcheck_counter++; + if (paramcheck_counter == 50) { bool param_updated; orb_check(param_sub, ¶m_updated); + if (param_updated) { parameters_update(¶ms_h, ¶ms); + for (int i = 0; i < 2; i++) { - pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, params.xy_vel_max); + pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f); pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.slope_max); } + pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max); thrust_pid_set_parameters(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min); } + paramcheck_counter = 0; } @@ -256,31 +276,36 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { /* Check if controller should act */ bool act = status.flag_system_armed && ( - /* SAS modes */ - ( - status.flag_control_manual_enabled && - status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS && ( - status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ALTITUDE || - status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE - ) - ) || - /* AUTO mode */ - status.state_machine == SYSTEM_STATE_AUTO - ); + /* SAS modes */ + ( + status.flag_control_manual_enabled && + status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS && ( + status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ALTITUDE || + status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE + ) + ) || + /* AUTO mode */ + status.state_machine == SYSTEM_STATE_AUTO + ); hrt_abstime t = hrt_absolute_time(); float dt; + if (t_prev != 0) { dt = (t - t_prev) * 0.000001f; + } else { dt = 0.0f; } + t_prev = t; + if (act) { orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); + if (status.state_machine == SYSTEM_STATE_AUTO) { //orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); if (local_pos.home_timestamp != local_home_timestamp) { @@ -292,6 +317,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { warnx("local pos home: lat = %.10f, lon = %.10f", lat_home, lon_home); mavlink_log_info(mavlink_fd, "local pos home: %.7f, %.7f", lat_home, lon_home); } + if (global_pos_sp_updated) { global_pos_sp_updated = false; orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp); @@ -299,11 +325,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { double sp_lon = global_pos_sp.lon * 1e-7; /* project global setpoint to local setpoint */ map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y)); + if (global_pos_sp.altitude_is_relative) { local_pos_sp.z = -global_pos_sp.altitude; + } else { local_pos_sp.z = local_pos.home_alt - global_pos_sp.altitude; } + warnx("new setpoint: lat = %.10f, lon = %.10f, x = %.2f, y = %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); mavlink_log_info(mavlink_fd, "new setpoint: %.7f, %.7f, %.2f, %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); /* publish local position setpoint as projection of global position setpoint */ @@ -339,16 +368,21 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { /* home alt changed, don't follow large ground level changes in manual flight */ local_pos_sp.z += local_pos.home_alt - home_alt; } + home_alt_t = local_pos.home_timestamp; home_alt = local_pos.home_alt; } + /* move altitude setpoint with manual controls */ float z_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz); + if (z_sp_ctl != 0.0f) { sp_move_rate[2] = -z_sp_ctl * params.z_vel_max; local_pos_sp.z += sp_move_rate[2] * dt; + if (local_pos_sp.z > local_pos.z + z_sp_offs_max) { local_pos_sp.z = local_pos.z + z_sp_offs_max; + } else if (local_pos_sp.z < local_pos.z - z_sp_offs_max) { local_pos_sp.z = local_pos.z - z_sp_offs_max; } @@ -358,6 +392,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { /* move position setpoint with manual controls */ float pos_pitch_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz); float pos_roll_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz); + if (pos_pitch_sp_ctl != 0.0f || pos_roll_sp_ctl != 0.0f) { /* calculate direction and increment of control in NED frame */ float xy_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl); @@ -370,6 +405,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { float pos_vec_x = local_pos_sp.x - local_pos.x; float pos_vec_y = local_pos_sp.y - local_pos.y; float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / xy_sp_offs_max; + if (pos_vec_norm > 1.0f) { local_pos_sp.x = local_pos.x + pos_vec_x / pos_vec_norm; local_pos_sp.y = local_pos.y + pos_vec_y / pos_vec_norm; @@ -379,72 +415,79 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { } /* run position & altitude controllers, calculate velocity setpoint */ - float vel_sp[3] = { 0.0f, 0.0f, 0.0f }; - vel_sp[2] = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz, dt); + global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz, dt); + if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) { /* calculate velocity set point in NED frame */ - vel_sp[0] = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx, dt); - vel_sp[1] = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy, dt); + global_vel_sp.vx = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx, dt); + global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy, dt); + + /* limit horizontal speed */ + float xy_vel_sp_norm = norm(global_vel_sp.vx, global_vel_sp.vy) / params.xy_vel_max; + if (xy_vel_sp_norm > 1.0f) { + global_vel_sp.vx /= xy_vel_sp_norm; + global_vel_sp.vy /= xy_vel_sp_norm; + } + } else { reset_sp_pos = true; + global_vel_sp.vx = 0.0f; + global_vel_sp.vy = 0.0f; } - /* calculate direction and norm of thrust in NED frame - * limit 3D speed by ellipsoid: - * (vx/xy_vel_max)^2 + (vy/xy_vel_max)^2 + (vz/z_vel_max)^2 = 1 */ - float v; - float vel_sp_norm = 0.0f; - v = vel_sp[0] / params.xy_vel_max; - vel_sp_norm += v * v; - v = vel_sp[1] / params.xy_vel_max; - vel_sp_norm += v * v; - v = vel_sp[2] / params.z_vel_max; - vel_sp_norm += v * v; - vel_sp_norm = sqrtf(vel_sp_norm); - if (vel_sp_norm > 1.0f) { - vel_sp[0] /= vel_sp_norm; - vel_sp[1] /= vel_sp_norm; - vel_sp[2] /= vel_sp_norm; - } + + /* publish new velocity setpoint */ + orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp); /* run velocity controllers, calculate thrust vector */ float thrust_sp[3] = { 0.0f, 0.0f, 0.0f }; - thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, vel_sp[2], local_pos.vz, dt); + thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt); + if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) { /* calculate velocity set point in NED frame */ - thrust_sp[0] = pid_calculate(&xy_vel_pids[0], vel_sp[0], local_pos.vx, 0.0f, dt); - thrust_sp[1] = pid_calculate(&xy_vel_pids[1], vel_sp[1], local_pos.vy, 0.0f, dt); + thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt); + thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt); } + /* thrust_vector now contains desired acceleration (but not in m/s^2) in NED frame */ /* limit horizontal part of thrust */ float thrust_xy_dir = atan2f(thrust_sp[1], thrust_sp[0]); float thrust_xy_norm = norm(thrust_sp[0], thrust_sp[1]); + if (thrust_xy_norm > params.slope_max) { thrust_xy_norm = params.slope_max; } + /* use approximation: slope ~ sin(slope) = force */ /* convert direction to body frame */ thrust_xy_dir -= att.yaw; + if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) { /* calculate roll and pitch */ att_sp.roll_body = sinf(thrust_xy_dir) * thrust_xy_norm; att_sp.pitch_body = -cosf(thrust_xy_dir) * thrust_xy_norm / cosf(att_sp.roll_body); // reverse pitch } + /* attitude-thrust compensation */ float att_comp; + if (att.R[2][2] > 0.8f) att_comp = 1.0f / att.R[2][2]; else if (att.R[2][2] > 0.0f) att_comp = ((1.0f / 0.8f - 1.0f) / 0.8f) * att.R[2][2] + 1.0f; else att_comp = 1.0f; + att_sp.thrust = -thrust_sp[2] * att_comp; att_sp.timestamp = hrt_absolute_time(); + if (status.flag_control_manual_enabled) { /* publish local position setpoint in manual mode */ orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); } + /* publish new attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + } else { reset_sp_alt = true; reset_sp_pos = true; @@ -456,7 +499,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { } warnx("stopped"); - mavlink_log_info(mavlink_fd, "[multirotor_pos_control] stopped"); + mavlink_log_info(mavlink_fd, "[mpc] stopped"); thread_running = false; diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 4996a8f661..562f3ac04b 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -168,8 +168,8 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo // Calculate the error integral and check for saturation i = pid->integral + (error * dt); - if (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit || - fabsf(i) > pid->intmax) { + if ((pid->limit != 0.0f && (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit)) || + fabsf(i) > pid->intmax) { i = pid->integral; // If saturated then do not update integral value pid->saturated = 1; @@ -186,11 +186,13 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd); if (isfinite(output)) { - if (output > pid->limit) { - output = pid->limit; + if (pid->limit != 0.0f) { + if (output > pid->limit) { + output = pid->limit; - } else if (output < -pid->limit) { - output = -pid->limit; + } else if (output < -pid->limit) { + output = -pid->limit; + } } pid->last_output = output; From e67f6a51a320a9b8bf0a27c4ffb55a9485805680 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 13 Jul 2013 20:13:53 -0700 Subject: [PATCH 244/486] Make px4io driver filenames less ambiguous. --- src/drivers/px4io/module.mk | 6 +++--- src/drivers/px4io/{interface_i2c.cpp => px4io_i2c.cpp} | 0 .../px4io/{interface_serial.cpp => px4io_serial.cpp} | 0 src/drivers/px4io/{uploader.cpp => px4io_uploader.cpp} | 0 4 files changed, 3 insertions(+), 3 deletions(-) rename src/drivers/px4io/{interface_i2c.cpp => px4io_i2c.cpp} (100%) rename src/drivers/px4io/{interface_serial.cpp => px4io_serial.cpp} (100%) rename src/drivers/px4io/{uploader.cpp => px4io_uploader.cpp} (100%) diff --git a/src/drivers/px4io/module.mk b/src/drivers/px4io/module.mk index d5bab6599f..2054faa127 100644 --- a/src/drivers/px4io/module.mk +++ b/src/drivers/px4io/module.mk @@ -38,9 +38,9 @@ MODULE_COMMAND = px4io SRCS = px4io.cpp \ - uploader.cpp \ - interface_serial.cpp \ - interface_i2c.cpp + px4io_uploader.cpp \ + px4io_serial.cpp \ + px4io_i2c.cpp # XXX prune to just get UART registers INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common diff --git a/src/drivers/px4io/interface_i2c.cpp b/src/drivers/px4io/px4io_i2c.cpp similarity index 100% rename from src/drivers/px4io/interface_i2c.cpp rename to src/drivers/px4io/px4io_i2c.cpp diff --git a/src/drivers/px4io/interface_serial.cpp b/src/drivers/px4io/px4io_serial.cpp similarity index 100% rename from src/drivers/px4io/interface_serial.cpp rename to src/drivers/px4io/px4io_serial.cpp diff --git a/src/drivers/px4io/uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp similarity index 100% rename from src/drivers/px4io/uploader.cpp rename to src/drivers/px4io/px4io_uploader.cpp From 26eb0b9d724654bd87edd9191e72a726f5ce240b Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 13 Jul 2013 21:00:03 -0700 Subject: [PATCH 245/486] =?UTF-8?q?Move=20the=20direct-access=20methods=20?= =?UTF-8?q?from=20CDev=20to=20Device=E2=80=A6=20makes=20more=20sense=20tha?= =?UTF-8?q?t=20way.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/drivers/device/cdev.cpp | 21 -------- src/drivers/device/device.cpp | 26 ++++++++++ src/drivers/device/device.h | 90 ++++++++++++++++++++--------------- src/drivers/device/i2c.h | 12 ++--- 4 files changed, 83 insertions(+), 66 deletions(-) diff --git a/src/drivers/device/cdev.cpp b/src/drivers/device/cdev.cpp index dcbac25e30..1972d2efcb 100644 --- a/src/drivers/device/cdev.cpp +++ b/src/drivers/device/cdev.cpp @@ -395,25 +395,4 @@ cdev_poll(struct file *filp, struct pollfd *fds, bool setup) return cdev->poll(filp, fds, setup); } -int -CDev::read(unsigned offset, void *data, unsigned count) -{ - errno = ENODEV; - return -1; -} - -int -CDev::write(unsigned offset, void *data, unsigned count) -{ - errno = ENODEV; - return -1; -} - -int -CDev::ioctl(unsigned operation, unsigned &arg) -{ - errno = ENODEV; - return -1; -} - } // namespace device \ No newline at end of file diff --git a/src/drivers/device/device.cpp b/src/drivers/device/device.cpp index 04a5222c36..dd8074460e 100644 --- a/src/drivers/device/device.cpp +++ b/src/drivers/device/device.cpp @@ -223,5 +223,31 @@ interrupt(int irq, void *context) return OK; } +int +Device::probe() +{ + return -1; +} + +int +Device::read(unsigned offset, void *data, unsigned count) +{ + errno = ENODEV; + return -1; +} + +int +Device::write(unsigned offset, void *data, unsigned count) +{ + errno = ENODEV; + return -1; +} + +int +Device::ioctl(unsigned operation, unsigned &arg) +{ + errno = ENODEV; + return -1; +} } // namespace device \ No newline at end of file diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h index 2cac86636d..d69d1b2d6b 100644 --- a/src/drivers/device/device.h +++ b/src/drivers/device/device.h @@ -68,11 +68,62 @@ namespace device __EXPORT class __EXPORT Device { public: + /** + * Destructor. + * + * Public so that anonymous devices can be destroyed. + */ + virtual ~Device(); + /** * Interrupt handler. */ virtual void interrupt(void *ctx); /**< interrupt handler */ + /* + * Direct access methods. + */ + + /** + * Probe to test whether the device is present. + * + * @return Zero if present, < 0 (error) otherwise. + */ + virtual int probe(); + + /** + * Read directly from the device. + * + * The actual size of each unit quantity is device-specific. + * + * @param offset The device address at which to start reading + * @param data The buffer into which the read values should be placed. + * @param count The number of items to read, defaults to 1. + * @return count on success, < 0 on error. + */ + virtual int read(unsigned address, void *data, unsigned count = 1); + + /** + * Write directly to the device. + * + * The actual size of each unit quantity is device-specific. + * + * @param address The device address at which to start writing. + * @param data The buffer from which values should be read. + * @param count The number of registers to write, defaults to 1. + * @return count on success, < 0 on error. + */ + virtual int write(unsigned address, void *data, unsigned count = 1); + + /** + * Perform a device-specific operation. + * + * @param operation The operation to perform + * @param arg An argument to the operation. + * @return < 0 on error + */ + virtual int ioctl(unsigned operation, unsigned &arg); + protected: const char *_name; /**< driver name */ bool _debug_enabled; /**< if true, debug messages are printed */ @@ -85,7 +136,6 @@ protected: */ Device(const char *name, int irq = 0); - virtual ~Device(); /** * Initialise the driver and make it ready for use. @@ -282,48 +332,12 @@ public: * Test whether the device is currently open. * * This can be used to avoid tearing down a device that is still active. + * Note - not virtual, cannot be overridden by a subclass. * * @return True if the device is currently open. */ bool is_open() { return _open_count > 0; } - /* - * Direct access methods. - */ - - /** - * Read directly from the device. - * - * The actual size of each unit quantity is device-specific. - * - * @param offset The device offset at which to start reading - * @param data The buffer into which the read values should be placed. - * @param count The number of items to read, defaults to 1. - * @return count on success, < 0 on error. - */ - virtual int read(unsigned offset, void *data, unsigned count = 1); - - /** - * Write directly to the device. - * - * The actual size of each unit quantity is device-specific. - * - * @param address The device address at which to start writing. - * @param data The buffer from which values should be read. - * @param count The number of registers to write, defaults to 1. - * @return count on success, < 0 on error. - */ - virtual int write(unsigned address, void *data, unsigned count = 1); - - /** - * Perform a device-specific operation. - * - * @param operation The operation to perform - * @param arg An argument to the operation. - * @return < 0 on error - */ - virtual int ioctl(unsigned operation, unsigned &arg); - protected: /** * Pointer to the default cdev file operations table; useful for diff --git a/src/drivers/device/i2c.h b/src/drivers/device/i2c.h index b4a9cdd536..8c4fab4c3e 100644 --- a/src/drivers/device/i2c.h +++ b/src/drivers/device/i2c.h @@ -55,13 +55,11 @@ class __EXPORT I2C : public CDev public: - /** - * Get the address - */ - uint16_t get_address() { - return _address; - } - + /** + * Get the address + */ + int16_t get_address() { return _address; } + protected: /** * The number of times a read or write operation will be retried on From 4f0ae1cdeac75111e232001e86e9d0dc2f531935 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 13 Jul 2013 21:00:27 -0700 Subject: [PATCH 246/486] Build the px4io interfaces on top of the Device direct-access API. --- src/drivers/px4io/px4io.cpp | 37 ++++--- src/drivers/px4io/px4io_i2c.cpp | 102 +++++++------------ src/drivers/px4io/px4io_serial.cpp | 151 ++++++++++++++++------------- 3 files changed, 140 insertions(+), 150 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 29624018f6..1b4f20de09 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -85,7 +85,9 @@ #include #include "uploader.h" -#include "interface.h" + +extern device::Device *PX4IO_i2c_interface() weak_function; +extern device::Device *PX4IO_serial_interface() weak_function; #define PX4IO_SET_DEBUG _IOC(0xff00, 0) #define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1) @@ -93,7 +95,7 @@ class PX4IO : public device::CDev { public: - PX4IO(PX4IO_interface *interface); + PX4IO(device::Device *interface); virtual ~PX4IO(); virtual int init(); @@ -131,7 +133,7 @@ public: void print_status(); private: - PX4IO_interface *_interface; + device::Device *_interface; // XXX unsigned _hardware; @@ -316,7 +318,7 @@ PX4IO *g_dev; } -PX4IO::PX4IO(PX4IO_interface *interface) : +PX4IO::PX4IO(device::Device *interface) : CDev("px4io", "/dev/px4io"), _interface(interface), _hardware(0), @@ -1129,7 +1131,7 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned return -EINVAL; } - int ret = _interface->set_reg(page, offset, values, num_values); + int ret = _interface->write((page << 8) | offset, (void *)values, num_values); if (ret != OK) debug("io_reg_set(%u,%u,%u): error %d", page, offset, num_values, errno); return ret; @@ -1150,7 +1152,7 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_v return -EINVAL; } - int ret = _interface->get_reg(page, offset, values, num_values); + int ret = _interface->read((page << 8) | offset, reinterpret_cast(values), num_values); if (ret != OK) debug("io_reg_get(%u,%u,%u): data error %d", page, offset, num_values, errno); return ret; @@ -1602,12 +1604,12 @@ start(int argc, char *argv[]) if (g_dev != nullptr) errx(1, "already loaded"); - PX4IO_interface *interface; + device::Device *interface; #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) - interface = io_serial_interface(); + interface = PX4IO_serial_interface(); #elif defined(CONFIG_ARCH_BOARD_PX4FMU_V1) - interface = io_i2c_interface(PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO); + interface = PX4IO_i2c_interface(); #else # error Unknown board - cannot select interface. #endif @@ -1615,7 +1617,7 @@ start(int argc, char *argv[]) if (interface == nullptr) errx(1, "cannot alloc interface"); - if (!interface->ok()) { + if (interface->probe()) { delete interface; errx(1, "interface init failed"); } @@ -1739,12 +1741,12 @@ monitor(void) void if_test(unsigned mode) { - PX4IO_interface *interface; + device::Device *interface; #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) - interface = io_serial_interface(); + interface = PX4IO_serial_interface(); #elif defined(CONFIG_ARCH_BOARD_PX4FMU_V1) - interface = io_i2c_interface(PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO); + interface = PX4IO_i2c_interface(); #else # error Unknown board - cannot select interface. #endif @@ -1752,20 +1754,17 @@ if_test(unsigned mode) if (interface == nullptr) errx(1, "cannot alloc interface"); - if (!interface->ok()) { + if (interface->probe()) { delete interface; errx(1, "interface init failed"); } else { - - int result = interface->test(mode); + int result = interface->ioctl(1, mode); /* XXX magic numbers */ delete interface; errx(0, "test returned %d", result); } - - exit(0); } -} +} /* namespace */ int px4io_main(int argc, char *argv[]) diff --git a/src/drivers/px4io/px4io_i2c.cpp b/src/drivers/px4io/px4io_i2c.cpp index 45a707883e..317326e40a 100644 --- a/src/drivers/px4io/px4io_i2c.cpp +++ b/src/drivers/px4io/px4io_i2c.cpp @@ -52,109 +52,93 @@ #include +#include #include #include "uploader.h" #include -#include "interface.h" +device::Device *PX4IO_i2c_interface(); -class PX4IO_I2C : public PX4IO_interface +class PX4IO_I2C : public device::I2C { public: PX4IO_I2C(int bus, uint8_t address); virtual ~PX4IO_I2C(); - virtual int set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values); - virtual int get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values); - - virtual bool ok(); - - virtual int test(unsigned mode); + virtual int probe(); + virtual int read(unsigned offset, void *data, unsigned count = 1); + virtual int write(unsigned address, void *data, unsigned count = 1); + virtual int ioctl(unsigned operation, unsigned &arg); private: - static const unsigned _retries = 2; - struct i2c_dev_s *_dev; - uint8_t _address; }; -PX4IO_interface *io_i2c_interface(int bus, uint8_t address) +device::Device +*PX4IO_i2c_interface() { - return new PX4IO_I2C(bus, address); +#ifdef PX4_I2C_OBDEV_PX4IO + return new PX4IO_I2C(PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO); +#endif + return nullptr; } PX4IO_I2C::PX4IO_I2C(int bus, uint8_t address) : - _dev(nullptr), - _address(address) + I2C("PX4IO_i2c", nullptr, bus, address, 400000) { - _dev = up_i2cinitialize(bus); - if (_dev) - I2C_SETFREQUENCY(_dev, 400000); + _retries = 3; } PX4IO_I2C::~PX4IO_I2C() { - if (_dev) - up_i2cuninitialize(_dev); -} - -bool -PX4IO_I2C::ok() -{ - if (!_dev) - return false; - - /* check any other status here */ - - return true; } int -PX4IO_I2C::test(unsigned mode) +PX4IO_I2C::probe() +{ + /* XXX really should do something here */ + + return 0; +} + +int +PX4IO_I2C::ioctl(unsigned operation, unsigned &arg) { return 0; } int -PX4IO_I2C::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) +PX4IO_I2C::write(unsigned address, void *data, unsigned count) { - int ret; + uint8_t page = address >> 8; + uint8_t offset = address & 0xff; + const uint16_t *values = reinterpret_cast(data); /* set up the transfer */ uint8_t addr[2] = { page, offset }; + i2c_msg_s msgv[2]; - msgv[0].addr = _address; msgv[0].flags = 0; msgv[0].buffer = addr; msgv[0].length = 2; - msgv[1].addr = _address; msgv[1].flags = I2C_M_NORESTART; msgv[1].buffer = (uint8_t *)values; - msgv[1].length = num_values * sizeof(*values); + msgv[1].length = 2 * count; - unsigned tries = 0; - do { - - /* perform the transfer */ - ret = I2C_TRANSFER(_dev, msgv, 2); - - if (ret == OK) - break; - - } while (tries++ < _retries); - - return ret; + return transfer(msgv, 2); } int -PX4IO_I2C::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) +PX4IO_I2C::read(unsigned address, void *data, unsigned count) { - int ret; + uint8_t page = address >> 8; + uint8_t offset = address & 0xff; + const uint16_t *values = reinterpret_cast(data); /* set up the transfer */ uint8_t addr[2] = { @@ -163,25 +147,13 @@ PX4IO_I2C::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_ }; i2c_msg_s msgv[2]; - msgv[0].addr = _address; msgv[0].flags = 0; msgv[0].buffer = addr; msgv[0].length = 2; - msgv[1].addr = _address; msgv[1].flags = I2C_M_READ; msgv[1].buffer = (uint8_t *)values; - msgv[1].length = num_values * sizeof(*values); + msgv[1].length = 2 * count; - unsigned tries = 0; - do { - /* perform the transfer */ - ret = I2C_TRANSFER(_dev, msgv, 2); - - if (ret == OK) - break; - - } while (tries++ < _retries); - - return ret; + return transfer(msgv, 2); } diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp index 06b955971e..c48b6ec4c6 100644 --- a/src/drivers/px4io/px4io_serial.cpp +++ b/src/drivers/px4io/px4io_serial.cpp @@ -59,6 +59,7 @@ #include +#include #include #include /* XXX should really not be hardcoding v2 here */ @@ -66,7 +67,7 @@ #include -#include "interface.h" +device::Device *PX4IO_serial_interface(); /* serial register accessors */ #define REG(_x) (*(volatile uint32_t *)(PX4IO_SERIAL_BASE + _x)) @@ -78,17 +79,16 @@ #define rCR3 REG(STM32_USART_CR3_OFFSET) #define rGTPR REG(STM32_USART_GTPR_OFFSET) -class PX4IO_serial : public PX4IO_interface +class PX4IO_serial : public device::Device { public: PX4IO_serial(); virtual ~PX4IO_serial(); - virtual int set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values); - virtual int get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values); - - virtual bool ok(); - virtual int test(unsigned mode); + virtual int probe(); + virtual int read(unsigned offset, void *data, unsigned count = 1); + virtual int write(unsigned address, void *data, unsigned count = 1); + virtual int ioctl(unsigned operation, unsigned &arg); private: /* @@ -159,12 +159,14 @@ private: IOPacket PX4IO_serial::_dma_buffer; static PX4IO_serial *g_interface; -PX4IO_interface *io_serial_interface() +device::Device +*PX4IO_serial_interface() { return new PX4IO_serial(); } PX4IO_serial::PX4IO_serial() : + Device("PX4IO_serial"), _tx_dma(nullptr), _rx_dma(nullptr), _rx_dma_status(_dma_status_inactive), @@ -262,74 +264,87 @@ PX4IO_serial::~PX4IO_serial() g_interface = nullptr; } -bool -PX4IO_serial::ok() +int +PX4IO_serial::probe() { - if (_tx_dma == nullptr) - return false; - if (_rx_dma == nullptr) - return false; + /* XXX this could try talking to IO */ - return true; + if (_tx_dma == nullptr) + return -1; + if (_rx_dma == nullptr) + return -1; + + return 0; } int -PX4IO_serial::test(unsigned mode) +PX4IO_serial::ioctl(unsigned operation, unsigned &arg) { - switch (mode) { - case 0: - lowsyslog("test 0\n"); + switch (operation) { - /* kill DMA, this is a PIO test */ - stm32_dmastop(_tx_dma); - stm32_dmastop(_rx_dma); - rCR3 &= ~(USART_CR3_DMAR | USART_CR3_DMAT); + case 1: /* XXX magic number - test operation */ + switch (arg) { + case 0: + lowsyslog("test 0\n"); - for (;;) { - while (!(rSR & USART_SR_TXE)) - ; - rDR = 0x55; - } - return 0; + /* kill DMA, this is a PIO test */ + stm32_dmastop(_tx_dma); + stm32_dmastop(_rx_dma); + rCR3 &= ~(USART_CR3_DMAR | USART_CR3_DMAT); - case 1: - { - unsigned fails = 0; - for (unsigned count = 0;; count++) { - uint16_t value = count & 0xffff; - - if (set_reg(PX4IO_PAGE_TEST, PX4IO_P_TEST_LED, &value, 1) != 0) - fails++; - - if (count >= 5000) { - lowsyslog("==== test 1 : %u failures ====\n", fails); - perf_print_counter(_pc_txns); - perf_print_counter(_pc_dmasetup); - perf_print_counter(_pc_retries); - perf_print_counter(_pc_timeouts); - perf_print_counter(_pc_crcerrs); - perf_print_counter(_pc_dmaerrs); - perf_print_counter(_pc_protoerrs); - perf_print_counter(_pc_uerrs); - perf_print_counter(_pc_idle); - perf_print_counter(_pc_badidle); - count = 0; - } + for (;;) { + while (!(rSR & USART_SR_TXE)) + ; + rDR = 0x55; } return 0; + + case 1: + { + unsigned fails = 0; + for (unsigned count = 0;; count++) { + uint16_t value = count & 0xffff; + + if (write((PX4IO_PAGE_TEST << 8) | PX4IO_P_TEST_LED, &value, 1) != 0) + fails++; + + if (count >= 5000) { + lowsyslog("==== test 1 : %u failures ====\n", fails); + perf_print_counter(_pc_txns); + perf_print_counter(_pc_dmasetup); + perf_print_counter(_pc_retries); + perf_print_counter(_pc_timeouts); + perf_print_counter(_pc_crcerrs); + perf_print_counter(_pc_dmaerrs); + perf_print_counter(_pc_protoerrs); + perf_print_counter(_pc_uerrs); + perf_print_counter(_pc_idle); + perf_print_counter(_pc_badidle); + count = 0; + } + } + return 0; + } + case 2: + lowsyslog("test 2\n"); + return 0; } - case 2: - lowsyslog("test 2\n"); - return 0; + default: + break; } + return -1; } int -PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) +PX4IO_serial::write(unsigned address, void *data, unsigned count) { - if (num_values > PKT_MAX_REGS) { + uint8_t page = address >> 8; + uint8_t offset = address & 0xff; + const uint16_t *values = reinterpret_cast(data); + + if (count > PKT_MAX_REGS) { errno = EINVAL; return -1; } @@ -339,11 +354,11 @@ PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsi int result; for (unsigned retries = 0; retries < 3; retries++) { - _dma_buffer.count_code = num_values | PKT_CODE_WRITE; + _dma_buffer.count_code = count | PKT_CODE_WRITE; _dma_buffer.page = page; _dma_buffer.offset = offset; - memcpy((void *)&_dma_buffer.regs[0], (void *)values, (2 * num_values)); - for (unsigned i = num_values; i < PKT_MAX_REGS; i++) + memcpy((void *)&_dma_buffer.regs[0], (void *)values, (2 * count)); + for (unsigned i = count; i < PKT_MAX_REGS; i++) _dma_buffer.regs[i] = 0x55aa; /* XXX implement check byte */ @@ -373,9 +388,13 @@ PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsi } int -PX4IO_serial::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) +PX4IO_serial::read(unsigned address, void *data, unsigned count) { - if (num_values > PKT_MAX_REGS) + uint8_t page = address >> 8; + uint8_t offset = address & 0xff; + uint16_t *values = reinterpret_cast(data); + + if (count > PKT_MAX_REGS) return -EINVAL; sem_wait(&_bus_semaphore); @@ -383,7 +402,7 @@ PX4IO_serial::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned n int result; for (unsigned retries = 0; retries < 3; retries++) { - _dma_buffer.count_code = num_values | PKT_CODE_READ; + _dma_buffer.count_code = count | PKT_CODE_READ; _dma_buffer.page = page; _dma_buffer.offset = offset; @@ -402,7 +421,7 @@ PX4IO_serial::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned n perf_count(_pc_protoerrs); /* compare the received count with the expected count */ - } else if (PKT_COUNT(_dma_buffer) != num_values) { + } else if (PKT_COUNT(_dma_buffer) != count) { /* IO returned the wrong number of registers - no point retrying */ errno = EIO; @@ -413,14 +432,14 @@ PX4IO_serial::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned n } else { /* copy back the result */ - memcpy(values, &_dma_buffer.regs[0], (2 * num_values)); + memcpy(values, &_dma_buffer.regs[0], (2 * count)); } break; } perf_count(_pc_retries); } -out: + sem_post(&_bus_semaphore); return result; } From 28f996b026f4de7e572f0676834ac5eafa5dee7f Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 13 Jul 2013 21:34:31 -0700 Subject: [PATCH 247/486] rename the px4io serial perf counters so it's clearer what they belong to --- src/drivers/px4io/px4io_serial.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp index c48b6ec4c6..20b89accb6 100644 --- a/src/drivers/px4io/px4io_serial.cpp +++ b/src/drivers/px4io/px4io_serial.cpp @@ -170,16 +170,16 @@ PX4IO_serial::PX4IO_serial() : _tx_dma(nullptr), _rx_dma(nullptr), _rx_dma_status(_dma_status_inactive), - _pc_txns(perf_alloc(PC_ELAPSED, "txns ")), - _pc_dmasetup(perf_alloc(PC_ELAPSED, "dmasetup ")), - _pc_retries(perf_alloc(PC_COUNT, "retries ")), - _pc_timeouts(perf_alloc(PC_COUNT, "timeouts ")), - _pc_crcerrs(perf_alloc(PC_COUNT, "crcerrs ")), - _pc_dmaerrs(perf_alloc(PC_COUNT, "dmaerrs ")), - _pc_protoerrs(perf_alloc(PC_COUNT, "protoerrs")), - _pc_uerrs(perf_alloc(PC_COUNT, "uarterrs ")), - _pc_idle(perf_alloc(PC_COUNT, "idle ")), - _pc_badidle(perf_alloc(PC_COUNT, "badidle ")) + _pc_txns(perf_alloc(PC_ELAPSED, "io_txns ")), + _pc_dmasetup(perf_alloc(PC_ELAPSED, "io_dmasetup ")), + _pc_retries(perf_alloc(PC_COUNT, "io_retries ")), + _pc_timeouts(perf_alloc(PC_COUNT, "io_timeouts ")), + _pc_crcerrs(perf_alloc(PC_COUNT, "io_crcerrs ")), + _pc_dmaerrs(perf_alloc(PC_COUNT, "io_dmaerrs ")), + _pc_protoerrs(perf_alloc(PC_COUNT, "io_protoerrs")), + _pc_uerrs(perf_alloc(PC_COUNT, "io_uarterrs ")), + _pc_idle(perf_alloc(PC_COUNT, "io_idle ")), + _pc_badidle(perf_alloc(PC_COUNT, "io_badidle ")) { /* allocate DMA */ _tx_dma = stm32_dmachannel(PX4IO_SERIAL_TX_DMAMAP); From 7c83e928a5ac190631047ef5b1758f1ca6b01871 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 14 Jul 2013 11:42:56 -0700 Subject: [PATCH 248/486] destructors for I2C and SPI should be virtual. --- src/drivers/device/i2c.h | 2 +- src/drivers/device/spi.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/device/i2c.h b/src/drivers/device/i2c.h index 8c4fab4c3e..5498793523 100644 --- a/src/drivers/device/i2c.h +++ b/src/drivers/device/i2c.h @@ -88,7 +88,7 @@ protected: uint16_t address, uint32_t frequency, int irq = 0); - ~I2C(); + virtual ~I2C(); virtual int init(); diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h index d2d01efb35..e0122372a0 100644 --- a/src/drivers/device/spi.h +++ b/src/drivers/device/spi.h @@ -71,7 +71,7 @@ protected: enum spi_mode_e mode, uint32_t frequency, int irq = 0); - ~SPI(); + virtual ~SPI(); virtual int init(); From 12b84597d8058412002de6292d5def559b19c7e6 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 14 Jul 2013 11:43:43 -0700 Subject: [PATCH 249/486] Direct access functions return errors directly, not touching errno. --- src/drivers/device/device.cpp | 15 ++----- src/drivers/device/device.h | 85 ++++++++++++++++------------------- 2 files changed, 42 insertions(+), 58 deletions(-) diff --git a/src/drivers/device/device.cpp b/src/drivers/device/device.cpp index dd8074460e..1c6f9b7f46 100644 --- a/src/drivers/device/device.cpp +++ b/src/drivers/device/device.cpp @@ -223,31 +223,22 @@ interrupt(int irq, void *context) return OK; } -int -Device::probe() -{ - return -1; -} - int Device::read(unsigned offset, void *data, unsigned count) { - errno = ENODEV; - return -1; + return -ENODEV; } int Device::write(unsigned offset, void *data, unsigned count) { - errno = ENODEV; - return -1; + return -ENODEV; } int Device::ioctl(unsigned operation, unsigned &arg) { - errno = ENODEV; - return -1; + return -ENODEV; } } // namespace device \ No newline at end of file diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h index d69d1b2d6b..75f35ff0f1 100644 --- a/src/drivers/device/device.h +++ b/src/drivers/device/device.h @@ -80,49 +80,49 @@ public: */ virtual void interrupt(void *ctx); /**< interrupt handler */ - /* - * Direct access methods. - */ + /* + * Direct access methods. + */ - /** - * Probe to test whether the device is present. - * - * @return Zero if present, < 0 (error) otherwise. - */ - virtual int probe(); + /** + * Initialise the driver and make it ready for use. + * + * @return OK if the driver initialized OK, negative errno otherwise; + */ + virtual int init(); - /** - * Read directly from the device. - * - * The actual size of each unit quantity is device-specific. - * - * @param offset The device address at which to start reading - * @param data The buffer into which the read values should be placed. - * @param count The number of items to read, defaults to 1. - * @return count on success, < 0 on error. - */ - virtual int read(unsigned address, void *data, unsigned count = 1); + /** + * Read directly from the device. + * + * The actual size of each unit quantity is device-specific. + * + * @param offset The device address at which to start reading + * @param data The buffer into which the read values should be placed. + * @param count The number of items to read. + * @return The number of items read on success, negative errno otherwise. + */ + virtual int read(unsigned address, void *data, unsigned count); - /** - * Write directly to the device. - * - * The actual size of each unit quantity is device-specific. - * - * @param address The device address at which to start writing. - * @param data The buffer from which values should be read. - * @param count The number of registers to write, defaults to 1. - * @return count on success, < 0 on error. - */ - virtual int write(unsigned address, void *data, unsigned count = 1); + /** + * Write directly to the device. + * + * The actual size of each unit quantity is device-specific. + * + * @param address The device address at which to start writing. + * @param data The buffer from which values should be read. + * @param count The number of items to write. + * @return The number of items written on success, negative errno otherwise. + */ + virtual int write(unsigned address, void *data, unsigned count); - /** - * Perform a device-specific operation. - * - * @param operation The operation to perform - * @param arg An argument to the operation. - * @return < 0 on error - */ - virtual int ioctl(unsigned operation, unsigned &arg); + /** + * Perform a device-specific operation. + * + * @param operation The operation to perform. + * @param arg An argument to the operation. + * @return Negative errno on error, OK or positive value on success. + */ + virtual int ioctl(unsigned operation, unsigned &arg); protected: const char *_name; /**< driver name */ @@ -137,13 +137,6 @@ protected: Device(const char *name, int irq = 0); - /** - * Initialise the driver and make it ready for use. - * - * @return OK if the driver initialised OK. - */ - virtual int init(); - /** * Enable the device interrupt */ From 6c7f1e883e0e0e8f09618ba1a80075f39faadf0b Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 14 Jul 2013 11:44:46 -0700 Subject: [PATCH 250/486] Direct-access device functions return errors directly. Move to using ::init rather than ::probe in keeping with device changes. --- src/drivers/px4io/interface.h | 85 ---------------------- src/drivers/px4io/px4io.cpp | 12 +-- src/drivers/px4io/px4io_i2c.cpp | 25 +++++-- src/drivers/px4io/px4io_serial.cpp | 113 ++++++++++++++--------------- 4 files changed, 79 insertions(+), 156 deletions(-) delete mode 100644 src/drivers/px4io/interface.h diff --git a/src/drivers/px4io/interface.h b/src/drivers/px4io/interface.h deleted file mode 100644 index cb7da1081d..0000000000 --- a/src/drivers/px4io/interface.h +++ /dev/null @@ -1,85 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file interface.h - * - * PX4IO interface classes. - */ - -#include - -#include - -class PX4IO_interface -{ -public: - /** - * Check that the interface initialised OK. - * - * Does not check that communication has been established. - */ - virtual bool ok() = 0; - - /** - * Set PX4IO registers. - * - * @param page The register page to write - * @param offset Offset of the first register to write - * @param values Pointer to values to write - * @param num_values The number of values to write - * @return Zero on success. - */ - virtual int set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) = 0; - - /** - * Get PX4IO registers. - * - * @param page The register page to read - * @param offset Offset of the first register to read - * @param values Pointer to store values read - * @param num_values The number of values to read - * @return Zero on success. - */ - virtual int get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) = 0; - - /** - * Perform an interface test. - * - * @param mode Optional test mode. - */ - virtual int test(unsigned mode) = 0; -}; - -extern PX4IO_interface *io_i2c_interface(int bus, uint8_t address); -extern PX4IO_interface *io_serial_interface(); diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 1b4f20de09..904da84c4c 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1132,8 +1132,8 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned } int ret = _interface->write((page << 8) | offset, (void *)values, num_values); - if (ret != OK) - debug("io_reg_set(%u,%u,%u): error %d", page, offset, num_values, errno); + if (ret != num_values) + debug("io_reg_set(%u,%u,%u): error %d", page, offset, num_values, ret); return ret; } @@ -1153,8 +1153,8 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_v } int ret = _interface->read((page << 8) | offset, reinterpret_cast(values), num_values); - if (ret != OK) - debug("io_reg_get(%u,%u,%u): data error %d", page, offset, num_values, errno); + if (ret != num_values) + debug("io_reg_get(%u,%u,%u): data error %d", page, offset, num_values, ret); return ret; } @@ -1617,7 +1617,7 @@ start(int argc, char *argv[]) if (interface == nullptr) errx(1, "cannot alloc interface"); - if (interface->probe()) { + if (interface->init()) { delete interface; errx(1, "interface init failed"); } @@ -1754,7 +1754,7 @@ if_test(unsigned mode) if (interface == nullptr) errx(1, "cannot alloc interface"); - if (interface->probe()) { + if (interface->init()) { delete interface; errx(1, "interface init failed"); } else { diff --git a/src/drivers/px4io/px4io_i2c.cpp b/src/drivers/px4io/px4io_i2c.cpp index 317326e40a..585b995fee 100644 --- a/src/drivers/px4io/px4io_i2c.cpp +++ b/src/drivers/px4io/px4io_i2c.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file interface_i2c.cpp + * @file px4io_i2c.cpp * * I2C interface for PX4IO */ @@ -65,7 +65,7 @@ public: PX4IO_I2C(int bus, uint8_t address); virtual ~PX4IO_I2C(); - virtual int probe(); + virtual int init(); virtual int read(unsigned offset, void *data, unsigned count = 1); virtual int write(unsigned address, void *data, unsigned count = 1); virtual int ioctl(unsigned operation, unsigned &arg); @@ -94,10 +94,17 @@ PX4IO_I2C::~PX4IO_I2C() } int -PX4IO_I2C::probe() +PX4IO_I2C::init() { - /* XXX really should do something here */ + int ret; + ret = I2C::init(); + if (ret != OK) + goto out; + + /* XXX really should do something more here */ + +out: return 0; } @@ -130,7 +137,10 @@ PX4IO_I2C::write(unsigned address, void *data, unsigned count) msgv[1].buffer = (uint8_t *)values; msgv[1].length = 2 * count; - return transfer(msgv, 2); + int ret = transfer(msgv, 2); + if (ret == OK) + ret = count; + return ret; } int @@ -155,5 +165,8 @@ PX4IO_I2C::read(unsigned address, void *data, unsigned count) msgv[1].buffer = (uint8_t *)values; msgv[1].length = 2 * count; - return transfer(msgv, 2); + int ret = transfer(msgv, 2); + if (ret == OK) + ret = count; + return ret; } diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp index 20b89accb6..6a0b4d33fb 100644 --- a/src/drivers/px4io/px4io_serial.cpp +++ b/src/drivers/px4io/px4io_serial.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file interface_serial.cpp + * @file px4io_serial.cpp * * Serial interface for PX4IO */ @@ -85,7 +85,7 @@ public: PX4IO_serial(); virtual ~PX4IO_serial(); - virtual int probe(); + virtual int init(); virtual int read(unsigned offset, void *data, unsigned count = 1); virtual int write(unsigned address, void *data, unsigned count = 1); virtual int ioctl(unsigned operation, unsigned &arg); @@ -181,43 +181,6 @@ PX4IO_serial::PX4IO_serial() : _pc_idle(perf_alloc(PC_COUNT, "io_idle ")), _pc_badidle(perf_alloc(PC_COUNT, "io_badidle ")) { - /* allocate DMA */ - _tx_dma = stm32_dmachannel(PX4IO_SERIAL_TX_DMAMAP); - _rx_dma = stm32_dmachannel(PX4IO_SERIAL_RX_DMAMAP); - if ((_tx_dma == nullptr) || (_rx_dma == nullptr)) - return; - - /* configure pins for serial use */ - stm32_configgpio(PX4IO_SERIAL_TX_GPIO); - stm32_configgpio(PX4IO_SERIAL_RX_GPIO); - - /* reset & configure the UART */ - rCR1 = 0; - rCR2 = 0; - rCR3 = 0; - - /* eat any existing interrupt status */ - (void)rSR; - (void)rDR; - - /* configure line speed */ - uint32_t usartdiv32 = PX4IO_SERIAL_CLOCK / (PX4IO_SERIAL_BITRATE / 2); - uint32_t mantissa = usartdiv32 >> 5; - uint32_t fraction = (usartdiv32 - (mantissa << 5) + 1) >> 1; - rBRR = (mantissa << USART_BRR_MANT_SHIFT) | (fraction << USART_BRR_FRAC_SHIFT); - - /* attach serial interrupt handler */ - irq_attach(PX4IO_SERIAL_VECTOR, _interrupt); - up_enable_irq(PX4IO_SERIAL_VECTOR); - - /* enable UART in DMA mode, enable error and line idle interrupts */ - /* rCR3 = USART_CR3_EIE;*/ - rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE; - - /* create semaphores */ - sem_init(&_completion_semaphore, 0, 0); - sem_init(&_bus_semaphore, 0, 1); - g_interface = this; } @@ -265,14 +228,47 @@ PX4IO_serial::~PX4IO_serial() } int -PX4IO_serial::probe() +PX4IO_serial::init() { - /* XXX this could try talking to IO */ + /* allocate DMA */ + _tx_dma = stm32_dmachannel(PX4IO_SERIAL_TX_DMAMAP); + _rx_dma = stm32_dmachannel(PX4IO_SERIAL_RX_DMAMAP); + if ((_tx_dma == nullptr) || (_rx_dma == nullptr)) + return -1; - if (_tx_dma == nullptr) - return -1; - if (_rx_dma == nullptr) - return -1; + /* configure pins for serial use */ + stm32_configgpio(PX4IO_SERIAL_TX_GPIO); + stm32_configgpio(PX4IO_SERIAL_RX_GPIO); + + /* reset & configure the UART */ + rCR1 = 0; + rCR2 = 0; + rCR3 = 0; + + /* eat any existing interrupt status */ + (void)rSR; + (void)rDR; + + /* configure line speed */ + uint32_t usartdiv32 = PX4IO_SERIAL_CLOCK / (PX4IO_SERIAL_BITRATE / 2); + uint32_t mantissa = usartdiv32 >> 5; + uint32_t fraction = (usartdiv32 - (mantissa << 5) + 1) >> 1; + rBRR = (mantissa << USART_BRR_MANT_SHIFT) | (fraction << USART_BRR_FRAC_SHIFT); + + /* attach serial interrupt handler */ + irq_attach(PX4IO_SERIAL_VECTOR, _interrupt); + up_enable_irq(PX4IO_SERIAL_VECTOR); + + /* enable UART in DMA mode, enable error and line idle interrupts */ + /* rCR3 = USART_CR3_EIE;*/ + rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE; + + /* create semaphores */ + sem_init(&_completion_semaphore, 0, 0); + sem_init(&_bus_semaphore, 0, 1); + + + /* XXX this could try talking to IO */ return 0; } @@ -344,10 +340,8 @@ PX4IO_serial::write(unsigned address, void *data, unsigned count) uint8_t offset = address & 0xff; const uint16_t *values = reinterpret_cast(data); - if (count > PKT_MAX_REGS) { - errno = EINVAL; - return -1; - } + if (count > PKT_MAX_REGS) + return -EINVAL; sem_wait(&_bus_semaphore); @@ -373,8 +367,7 @@ PX4IO_serial::write(unsigned address, void *data, unsigned count) if (PKT_CODE(_dma_buffer) == PKT_CODE_ERROR) { /* IO didn't like it - no point retrying */ - errno = EINVAL; - result = -1; + result = -EINVAL; perf_count(_pc_protoerrs); } @@ -384,6 +377,9 @@ PX4IO_serial::write(unsigned address, void *data, unsigned count) } sem_post(&_bus_semaphore); + + if (result == OK) + result = count; return result; } @@ -416,16 +412,14 @@ PX4IO_serial::read(unsigned address, void *data, unsigned count) if (PKT_CODE(_dma_buffer) == PKT_CODE_ERROR) { /* IO didn't like it - no point retrying */ - errno = EINVAL; - result = -1; + result = -EINVAL; perf_count(_pc_protoerrs); /* compare the received count with the expected count */ } else if (PKT_COUNT(_dma_buffer) != count) { /* IO returned the wrong number of registers - no point retrying */ - errno = EIO; - result = -1; + result = -EIO; perf_count(_pc_protoerrs); /* successful read */ @@ -441,6 +435,9 @@ PX4IO_serial::read(unsigned address, void *data, unsigned count) } sem_post(&_bus_semaphore); + + if (result == OK) + result = count; return result; } @@ -524,8 +521,7 @@ PX4IO_serial::_wait_complete() /* check for DMA errors */ if (_rx_dma_status & DMA_STATUS_TEIF) { perf_count(_pc_dmaerrs); - ret = -1; - errno = EIO; + ret = -EIO; break; } @@ -534,8 +530,7 @@ PX4IO_serial::_wait_complete() _dma_buffer.crc = 0; if ((crc != crc_packet(&_dma_buffer)) | (PKT_CODE(_dma_buffer) == PKT_CODE_CORRUPT)) { perf_count(_pc_crcerrs); - ret = -1; - errno = EIO; + ret = -EIO; break; } From 5350c2be09af7eb88233cb89f8c013974a586e53 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 14 Jul 2013 11:45:21 -0700 Subject: [PATCH 251/486] Refactor MS5611 driver to use interface nubs for the I2C and SPI versions of the chip. This reduces the amount of duplicated code. --- src/drivers/ms5611/ms5611.cpp | 1435 ++++------------------------- src/drivers/ms5611/ms5611.h | 86 ++ src/drivers/ms5611/ms5611_i2c.cpp | 264 ++++++ src/drivers/ms5611/ms5611_spi.cpp | 228 +++++ 4 files changed, 782 insertions(+), 1231 deletions(-) create mode 100644 src/drivers/ms5611/ms5611.h create mode 100644 src/drivers/ms5611/ms5611_i2c.cpp create mode 100644 src/drivers/ms5611/ms5611_spi.cpp diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 76acf7ccd7..b8c396f7bf 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -38,9 +38,6 @@ #include -#include -#include - #include #include #include @@ -60,12 +57,14 @@ #include +#include +#include #include #include #include -#include +#include "ms5611.h" /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -77,313 +76,6 @@ static const int ERROR = -1; # error This requires CONFIG_SCHED_WORKQUEUE. #endif -/* SPI protocol address bits */ -#define DIR_READ (1<<7) -#define DIR_WRITE (0<<7) -#define ADDR_INCREMENT (1<<6) - -/** - * Calibration PROM as reported by the device. - */ -#pragma pack(push,1) -struct ms5611_prom_s { - uint16_t factory_setup; - uint16_t c1_pressure_sens; - uint16_t c2_pressure_offset; - uint16_t c3_temp_coeff_pres_sens; - uint16_t c4_temp_coeff_pres_offset; - uint16_t c5_reference_temp; - uint16_t c6_temp_coeff_temp; - uint16_t serial_and_crc; -}; - -/** - * Grody hack for crc4() - */ -union ms5611_prom_u { - uint16_t c[8]; - struct ms5611_prom_s s; -}; -#pragma pack(pop) - -class MS5611_I2C : public device::I2C -{ -public: - MS5611_I2C(int bus); - virtual ~MS5611_I2C(); - - virtual int init(); - - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - - /** - * Diagnostics - print some basic information about the driver. - */ - void print_info(); - -protected: - virtual int probe(); - union ms5611_prom_u _prom; - - struct work_s _work; - unsigned _measure_ticks; - - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; - struct baro_report *_reports; - - bool _collect_phase; - unsigned _measure_phase; - - /* intermediate temperature values per MS5611 datasheet */ - int32_t _TEMP; - int64_t _OFF; - int64_t _SENS; - - /* altitude conversion calibration */ - unsigned _msl_pressure; /* in kPa */ - - orb_advert_t _baro_topic; - - perf_counter_t _sample_perf; - perf_counter_t _measure_perf; - perf_counter_t _comms_errors; - perf_counter_t _buffer_overflows; - - /** - * Initialize the automatic measurement state machine and start it. - * - * @note This function is called at open and error time. It might make sense - * to make it more aggressive about resetting the bus in case of errors. - */ - void start_cycle(); - - /** - * Stop the automatic measurement state machine. - */ - void stop_cycle(); - - /** - * Perform a poll cycle; collect from the previous measurement - * and start a new one. - * - * This is the heart of the measurement state machine. This function - * alternately starts a measurement, or collects the data from the - * previous measurement. - * - * When the interval between measurements is greater than the minimum - * measurement interval, a gap is inserted between collection - * and measurement to provide the most recent measurement possible - * at the next interval. - */ - void cycle(); - - /** - * Static trampoline from the workq context; because we don't have a - * generic workq wrapper yet. - * - * @param arg Instance pointer for the driver that is polling. - */ - static void cycle_trampoline(void *arg); - - /** - * Issue a measurement command for the current state. - * - * @return OK if the measurement command was successful. - */ - virtual int measure(); - - /** - * Collect the result of the most recent measurement. - */ - virtual int collect(); - - /** - * Send a reset command to the MS5611. - * - * This is required after any bus reset. - */ - virtual int cmd_reset(); - - /** - * Read the MS5611 PROM - * - * @return OK if the PROM reads successfully. - */ - virtual int read_prom(); - - /** - * Execute the bus-specific ioctl (I2C or SPI) - * - * @return the bus-specific ioctl return value - */ - virtual int bus_ioctl(struct file *filp, int cmd, unsigned long arg); - - /** - * PROM CRC routine ported from MS5611 application note - * - * @param n_prom Pointer to words read from PROM. - * @return True if the CRC matches. - */ - bool crc4(uint16_t *n_prom); - -private: - - /** - * Test whether the device supported by the driver is present at a - * specific address. - * - * @param address The I2C bus address to probe. - * @return True if the device is present. - */ - int probe_address(uint8_t address); - -}; - -class MS5611_SPI : public device::SPI -{ -public: - MS5611_SPI(int bus, const char* path, spi_dev_e device); - virtual ~MS5611_SPI(); - - virtual int init(); - - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - - /** - * Diagnostics - print some basic information about the driver. - */ - void print_info(); - -protected: - virtual int probe(); - union ms5611_prom_u _prom; - - struct work_s _work; - unsigned _measure_ticks; - - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; - struct baro_report *_reports; - - bool _collect_phase; - unsigned _measure_phase; - - /* intermediate temperature values per MS5611 datasheet */ - int32_t _TEMP; - int64_t _OFF; - int64_t _SENS; - - /* altitude conversion calibration */ - unsigned _msl_pressure; /* in kPa */ - - orb_advert_t _baro_topic; - - perf_counter_t _sample_perf; - perf_counter_t _measure_perf; - perf_counter_t _comms_errors; - perf_counter_t _buffer_overflows; - - /** - * Initialize the automatic measurement state machine and start it. - * - * @note This function is called at open and error time. It might make sense - * to make it more aggressive about resetting the bus in case of errors. - */ - void start_cycle(); - - /** - * Stop the automatic measurement state machine. - */ - void stop_cycle(); - - /** - * Perform a poll cycle; collect from the previous measurement - * and start a new one. - * - * This is the heart of the measurement state machine. This function - * alternately starts a measurement, or collects the data from the - * previous measurement. - * - * When the interval between measurements is greater than the minimum - * measurement interval, a gap is inserted between collection - * and measurement to provide the most recent measurement possible - * at the next interval. - */ - void cycle(); - - /** - * Static trampoline from the workq context; because we don't have a - * generic workq wrapper yet. - * - * @param arg Instance pointer for the driver that is polling. - */ - static void cycle_trampoline(void *arg); - - /** - * Issue a measurement command for the current state. - * - * @return OK if the measurement command was successful. - */ - virtual int measure(); - - /** - * Collect the result of the most recent measurement. - */ - virtual int collect(); - - /** - * Send a reset command to the MS5611. - * - * This is required after any bus reset. - */ - virtual int cmd_reset(); - - /** - * Read the MS5611 PROM - * - * @return OK if the PROM reads successfully. - */ - virtual int read_prom(); - - /** - * Execute the bus-specific ioctl (I2C or SPI) - * - * @return the bus-specific ioctl return value - */ - virtual int bus_ioctl(struct file *filp, int cmd, unsigned long arg); - - /** - * PROM CRC routine ported from MS5611 application note - * - * @param n_prom Pointer to words read from PROM. - * @return True if the CRC matches. - */ - bool crc4(uint16_t *n_prom); - - // XXX this should really go into the SPI base class, as its common code - uint8_t read_reg(unsigned reg); - uint16_t read_reg16(unsigned reg); - void write_reg(unsigned reg, uint8_t value); - void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); - -private: - - /** - * Test whether the device supported by the driver is present at a - * specific address. - * - * @param address The I2C bus address to probe. - * @return True if the device is present. - */ - int probe_address(uint8_t address); - -}; - /* helper macro for handling report buffer indices */ #define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) @@ -398,29 +90,110 @@ private: #define MS5611_CONVERSION_INTERVAL 10000 /* microseconds */ #define MS5611_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */ -#ifndef PX4_I2C_BUS_ONBOARD - #define MS5611_BUS 1 -#else - #define MS5611_BUS PX4_I2C_BUS_ONBOARD -#endif +class MS5611 : public device::CDev +{ +public: + MS5611(device::Device *interface); + ~MS5611(); -#define MS5611_ADDRESS_1 0x76 /* address select pins pulled high (PX4FMU series v1.6+) */ -#define MS5611_ADDRESS_2 0x77 /* address select pins pulled low (PX4FMU prototypes) */ + virtual int init(); -#define ADDR_RESET_CMD 0x1E /* write to this address to reset chip */ -#define ADDR_CMD_CONVERT_D1 0x48 /* write to this address to start temperature conversion */ -#define ADDR_CMD_CONVERT_D2 0x58 /* write to this address to start pressure conversion */ -#define ADDR_DATA 0x00 /* address of 3 bytes / 32bit pressure data */ -#define ADDR_PROM_SETUP 0xA0 /* address of 8x 2 bytes factory and calibration data */ -#define ADDR_PROM_C1 0xA2 /* address of 6x 2 bytes calibration data */ + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + Device *_interface; + + ms5611::prom_u _prom; + + struct work_s _work; + unsigned _measure_ticks; + + unsigned _num_reports; + volatile unsigned _next_report; + volatile unsigned _oldest_report; + struct baro_report *_reports; + + bool _collect_phase; + unsigned _measure_phase; + + /* intermediate temperature values per MS5611 datasheet */ + int32_t _TEMP; + int64_t _OFF; + int64_t _SENS; + + /* altitude conversion calibration */ + unsigned _msl_pressure; /* in kPa */ + + orb_advert_t _baro_topic; + + perf_counter_t _sample_perf; + perf_counter_t _measure_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + + /** + * Initialize the automatic measurement state machine and start it. + * + * @note This function is called at open and error time. It might make sense + * to make it more aggressive about resetting the bus in case of errors. + */ + void start_cycle(); + + /** + * Stop the automatic measurement state machine. + */ + void stop_cycle(); + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + * + * This is the heart of the measurement state machine. This function + * alternately starts a measurement, or collects the data from the + * previous measurement. + * + * When the interval between measurements is greater than the minimum + * measurement interval, a gap is inserted between collection + * and measurement to provide the most recent measurement possible + * at the next interval. + */ + void cycle(); + + /** + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); + + /** + * Issue a measurement command for the current state. + * + * @return OK if the measurement command was successful. + */ + virtual int measure(); + + /** + * Collect the result of the most recent measurement. + */ + virtual int collect(); +}; /* * Driver 'main' command. */ extern "C" __EXPORT int ms5611_main(int argc, char *argv[]); -MS5611_I2C::MS5611_I2C(int bus) : - I2C("MS5611", BARO_DEVICE_PATH, bus, 0, 400000), +MS5611::MS5611(device::Device *interface) : + CDev("MS5611", BARO_DEVICE_PATH), + _interface(interface), _measure_ticks(0), _num_reports(0), _next_report(0), @@ -438,36 +211,11 @@ MS5611_I2C::MS5611_I2C(int bus) : _comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows")) { - // work_cancel in the dtor will explode if we don't do this... + // work_cancel in stop_cycle called from the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); - warnx("MS5611 I2C constructor"); } -MS5611_SPI::MS5611_SPI(int bus, const char* path, spi_dev_e device) : - SPI("MS5611", path, bus, device, SPIDEV_MODE3, 2000000), - _measure_ticks(0), - _num_reports(0), - _next_report(0), - _oldest_report(0), - _reports(nullptr), - _collect_phase(false), - _measure_phase(0), - _TEMP(0), - _OFF(0), - _SENS(0), - _msl_pressure(101325), - _baro_topic(-1), - _sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")), - _measure_perf(perf_alloc(PC_ELAPSED, "ms5611_measure")), - _comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")), - _buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows")) -{ - // work_cancel in the dtor will explode if we don't do this... - memset(&_work, 0, sizeof(_work)); - warnx("MS5611 SPI constructor"); -} - -MS5611_I2C::~MS5611_I2C() +MS5611::~MS5611() { /* make sure we are truly inactive */ stop_cycle(); @@ -475,33 +223,32 @@ MS5611_I2C::~MS5611_I2C() /* free any existing reports */ if (_reports != nullptr) delete[] _reports; -} -MS5611_SPI::~MS5611_SPI() -{ - /* make sure we are truly inactive */ - stop_cycle(); - - /* free any existing reports */ - if (_reports != nullptr) - delete[] _reports; + delete _interface; } int -MS5611_I2C::init() +MS5611::init() { - int ret = ERROR; - /* do I2C init (and probe) first */ - if (I2C::init() != OK) + /* verify that the interface is ok */ + unsigned arg = (unsigned)&_prom; + _interface->ioctl(IOCTL_SET_PROMBUFFER, arg); + int ret = _interface->init(); + if (ret != OK) { + debug("interface init failed"); goto out; + } /* allocate basic report buffers */ _num_reports = 2; _reports = new struct baro_report[_num_reports]; - if (_reports == nullptr) + if (_reports == nullptr) { + debug("can't get memory for reports"); + ret = -ENOMEM; goto out; + } _oldest_report = _next_report = 0; @@ -509,175 +256,19 @@ MS5611_I2C::init() memset(&_reports[0], 0, sizeof(_reports[0])); _baro_topic = orb_advertise(ORB_ID(sensor_baro), &_reports[0]); - if (_baro_topic < 0) + if (_baro_topic < 0) { debug("failed to create sensor_baro object"); + ret = -ENOSPC; + goto out; + } ret = OK; out: return ret; } -int -MS5611_I2C::probe() -{ -#ifdef PX4_I2C_OBDEV_MS5611 - _retries = 10; - - if ((OK == probe_address(MS5611_ADDRESS_1)) || - (OK == probe_address(MS5611_ADDRESS_2))) { - /* - * Disable retries; we may enable them selectively in some cases, - * but the device gets confused if we retry some of the commands. - */ - _retries = 0; - return OK; - } -#endif - - return -EIO; -} - -int -MS5611_SPI::init() -{ - int ret = ERROR; - - /* do SPI init (and probe) first */ - warnx("MS5611 SPI init function"); - if (SPI::init() != OK) { - warnx("SPI init failed"); - goto out; - } else { - warnx("SPI init ok"); - } - - /* allocate basic report buffers */ - _num_reports = 2; - _reports = new struct baro_report[_num_reports]; - - if (_reports == nullptr) - goto out; - - _oldest_report = _next_report = 0; - - /* get a publish handle on the baro topic */ - memset(&_reports[0], 0, sizeof(_reports[0])); - _baro_topic = orb_advertise(ORB_ID(sensor_baro), &_reports[0]); - - if (_baro_topic < 0) - debug("failed to create sensor_baro object"); - - ret = OK; -out: - return ret; -} - -int -MS5611_SPI::probe() -{ - - warnx("SPI probe"); - /* send reset command */ - if (OK != cmd_reset()) - return -EIO; - - /* read PROM */ - if (OK != read_prom()) - return -EIO; - - return OK; -} - -int -MS5611_I2C::probe_address(uint8_t address) -{ - /* select the address we are going to try */ - set_address(address); - - /* send reset command */ - if (OK != cmd_reset()) - return -EIO; - - /* read PROM */ - if (OK != read_prom()) - return -EIO; - - return OK; -} - ssize_t -MS5611_I2C::read(struct file *filp, char *buffer, size_t buflen) -{ - unsigned count = buflen / sizeof(struct baro_report); - int ret = 0; - - /* buffer must be large enough */ - if (count < 1) - return -ENOSPC; - - /* if automatic measurement is enabled */ - if (_measure_ticks > 0) { - - /* - * While there is space in the caller's buffer, and reports, copy them. - * Note that we may be pre-empted by the workq thread while we are doing this; - * we are careful to avoid racing with them. - */ - while (count--) { - if (_oldest_report != _next_report) { - memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); - ret += sizeof(_reports[0]); - INCREMENT(_oldest_report, _num_reports); - } - } - - /* if there was no data, warn the caller */ - return ret ? ret : -EAGAIN; - } - - /* manual measurement - run one conversion */ - /* XXX really it'd be nice to lock against other readers here */ - do { - _measure_phase = 0; - _oldest_report = _next_report = 0; - - /* do temperature first */ - if (OK != measure()) { - ret = -EIO; - break; - } - - usleep(MS5611_CONVERSION_INTERVAL); - - if (OK != collect()) { - ret = -EIO; - break; - } - - /* now do a pressure measurement */ - if (OK != measure()) { - ret = -EIO; - break; - } - - usleep(MS5611_CONVERSION_INTERVAL); - - if (OK != collect()) { - ret = -EIO; - break; - } - - /* state machine will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); - - } while (0); - - return ret; -} - -ssize_t -MS5611_SPI::read(struct file *filp, char *buffer, size_t buflen) +MS5611::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct baro_report); int ret = 0; @@ -748,21 +339,7 @@ MS5611_SPI::read(struct file *filp, char *buffer, size_t buflen) } int -MS5611_SPI::bus_ioctl(struct file *filp, int cmd, unsigned long arg) -{ - /* give it to the superclass */ - return SPI::ioctl(filp, cmd, arg); -} - -int -MS5611_I2C::bus_ioctl(struct file *filp, int cmd, unsigned long arg) -{ - /* give it to the superclass */ - return I2C::ioctl(filp, cmd, arg); -} - -int -MS5611_I2C::ioctl(struct file *filp, int cmd, unsigned long arg) +MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { @@ -877,130 +454,11 @@ MS5611_I2C::ioctl(struct file *filp, int cmd, unsigned long arg) /* give it to the bus-specific superclass */ // return bus_ioctl(filp, cmd, arg); - return I2C::ioctl(filp, cmd, arg); -} - -int -MS5611_SPI::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - - case SENSORIOCSPOLLRATE: { - switch (arg) { - - /* switching to manual polling */ - case SENSOR_POLLRATE_MANUAL: - stop_cycle(); - _measure_ticks = 0; - return OK; - - /* external signalling not supported */ - case SENSOR_POLLRATE_EXTERNAL: - - /* zero would be bad */ - case 0: - return -EINVAL; - - /* set default/max polling rate */ - case SENSOR_POLLRATE_MAX: - case SENSOR_POLLRATE_DEFAULT: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); - - /* set interval for next measurement to minimum legal value */ - _measure_ticks = USEC2TICK(MS5611_CONVERSION_INTERVAL); - - /* if we need to start the poll state machine, do it */ - if (want_start) - start_cycle(); - - return OK; - } - - /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); - - /* convert hz to tick interval via microseconds */ - unsigned ticks = USEC2TICK(1000000 / arg); - - /* check against maximum rate */ - if (ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL)) - return -EINVAL; - - /* update interval for next measurement */ - _measure_ticks = ticks; - - /* if we need to start the poll state machine, do it */ - if (want_start) - start_cycle(); - - return OK; - } - } - } - - case SENSORIOCGPOLLRATE: - if (_measure_ticks == 0) - return SENSOR_POLLRATE_MANUAL; - - return (1000 / _measure_ticks); - - case SENSORIOCSQUEUEDEPTH: { - /* add one to account for the sentinel in the ring */ - arg++; - - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 2) || (arg > 100)) - return -EINVAL; - - /* allocate new buffer */ - struct baro_report *buf = new struct baro_report[arg]; - - if (nullptr == buf) - return -ENOMEM; - - /* reset the measurement state machine with the new buffer, free the old */ - stop_cycle(); - delete[] _reports; - _num_reports = arg; - _reports = buf; - start_cycle(); - - return OK; - } - - case SENSORIOCGQUEUEDEPTH: - return _num_reports - 1; - - case SENSORIOCRESET: - /* XXX implement this */ - return -EINVAL; - - case BAROIOCSMSLPRESSURE: - - /* range-check for sanity */ - if ((arg < 80000) || (arg > 120000)) - return -EINVAL; - - _msl_pressure = arg; - return OK; - - case BAROIOCGMSLPRESSURE: - return _msl_pressure; - - default: - break; - } - - /* give it to the bus-specific superclass */ - // return bus_ioctl(filp, cmd, arg); - return SPI::ioctl(filp, cmd, arg); + return CDev::ioctl(filp, cmd, arg); } void -MS5611_I2C::start_cycle() +MS5611::start_cycle() { /* reset the report ring and state machine */ @@ -1009,25 +467,25 @@ MS5611_I2C::start_cycle() _oldest_report = _next_report = 0; /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&MS5611_I2C::cycle_trampoline, this, 1); + work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, 1); } void -MS5611_I2C::stop_cycle() +MS5611::stop_cycle() { work_cancel(HPWORK, &_work); } void -MS5611_I2C::cycle_trampoline(void *arg) +MS5611::cycle_trampoline(void *arg) { - MS5611_I2C *dev = (MS5611_I2C *)arg; + MS5611 *dev = reinterpret_cast(arg); dev->cycle(); } void -MS5611_I2C::cycle() +MS5611::cycle() { int ret; @@ -1041,7 +499,7 @@ MS5611_I2C::cycle() /* * The ms5611 seems to regularly fail to respond to * its address; this happens often enough that we'd rather not - * spam the console with the message. + * spam the console with a message for this. */ } else { //log("collection error %d", ret); @@ -1065,7 +523,7 @@ MS5611_I2C::cycle() /* schedule a fresh cycle call when we are ready to measure again */ work_queue(HPWORK, &_work, - (worker_t)&MS5611_I2C::cycle_trampoline, + (worker_t)&MS5611::cycle_trampoline, this, _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL)); @@ -1088,107 +546,13 @@ MS5611_I2C::cycle() /* schedule a fresh cycle call when the measurement is done */ work_queue(HPWORK, &_work, - (worker_t)&MS5611_I2C::cycle_trampoline, - this, - USEC2TICK(MS5611_CONVERSION_INTERVAL)); -} - -void -MS5611_SPI::start_cycle() -{ - - /* reset the report ring and state machine */ - _collect_phase = false; - _measure_phase = 0; - _oldest_report = _next_report = 0; - - /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&MS5611_SPI::cycle_trampoline, this, 1); -} - -void -MS5611_SPI::stop_cycle() -{ - work_cancel(HPWORK, &_work); -} - -void -MS5611_SPI::cycle_trampoline(void *arg) -{ - MS5611_SPI *dev = (MS5611_SPI *)arg; - - dev->cycle(); -} - -void -MS5611_SPI::cycle() -{ - int ret; - - /* collection phase? */ - if (_collect_phase) { - - /* perform collection */ - ret = collect(); - if (ret != OK) { - if (ret == -6) { - /* - * The ms5611 seems to regularly fail to respond to - * its address; this happens often enough that we'd rather not - * spam the console with the message. - */ - } else { - //log("collection error %d", ret); - } - /* reset the collection state machine and try again */ - start_cycle(); - return; - } - - /* next phase is measurement */ - _collect_phase = false; - - /* - * Is there a collect->measure gap? - * Don't inject one after temperature measurements, so we can keep - * doing pressure measurements at something close to the desired rate. - */ - if ((_measure_phase != 0) && - (_measure_ticks > USEC2TICK(MS5611_CONVERSION_INTERVAL))) { - - /* schedule a fresh cycle call when we are ready to measure again */ - work_queue(HPWORK, - &_work, - (worker_t)&MS5611_SPI::cycle_trampoline, - this, - _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL)); - - return; - } - } - - /* measurement phase */ - ret = measure(); - if (ret != OK) { - //log("measure error %d", ret); - /* reset the collection state machine and try again */ - start_cycle(); - return; - } - - /* next phase is collection */ - _collect_phase = true; - - /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, - &_work, - (worker_t)&MS5611_SPI::cycle_trampoline, + (worker_t)&MS5611::cycle_trampoline, this, USEC2TICK(MS5611_CONVERSION_INTERVAL)); } int -MS5611_I2C::measure() +MS5611::measure() { int ret; @@ -1197,17 +561,12 @@ MS5611_I2C::measure() /* * In phase zero, request temperature; in other phases, request pressure. */ - uint8_t cmd_data = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1; + unsigned addr = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1; /* * Send the command to begin measuring. - * - * Disable retries on this command; we can't know whether failure - * means the device did or did not see the write. */ - _retries = 0; - ret = transfer(&cmd_data, 1, nullptr, 0); - + ret = _interface->ioctl(IOCTL_MEASURE, addr); if (OK != ret) perf_count(_comms_errors); @@ -1217,38 +576,24 @@ MS5611_I2C::measure() } int -MS5611_I2C::collect() +MS5611::collect() { int ret; - uint8_t cmd; - uint8_t data[3]; - union { - uint8_t b[4]; - uint32_t w; - } cvt; - - /* read the most recent measurement */ - cmd = 0; + uint32_t raw; perf_begin(_sample_perf); /* this should be fairly close to the end of the conversion, so the best approximation of the time */ _reports[_next_report].timestamp = hrt_absolute_time(); - ret = transfer(&cmd, 1, &data[0], 3); - if (ret != OK) { + /* read the most recent measurement - read offset/size are hardcoded in the interface */ + ret = _interface->read(0, (void *)&raw, 0); + if (ret < 0) { perf_count(_comms_errors); perf_end(_sample_perf); return ret; } - /* fetch the raw value */ - cvt.b[0] = data[2]; - cvt.b[1] = data[1]; - cvt.b[2] = data[0]; - cvt.b[3] = 0; - uint32_t raw = cvt.w; - /* handle a measurement */ if (_measure_phase == 0) { @@ -1352,421 +697,8 @@ MS5611_I2C::collect() return OK; } -int -MS5611_I2C::cmd_reset() -{ - unsigned old_retrycount = _retries; - uint8_t cmd = ADDR_RESET_CMD; - int result; - - /* bump the retry count */ - _retries = 10; - result = transfer(&cmd, 1, nullptr, 0); - _retries = old_retrycount; - - return result; -} - -int -MS5611_I2C::read_prom() -{ - uint8_t prom_buf[2]; - union { - uint8_t b[2]; - uint16_t w; - } cvt; - - /* - * Wait for PROM contents to be in the device (2.8 ms) in the case we are - * called immediately after reset. - */ - usleep(3000); - - /* read and convert PROM words */ - for (int i = 0; i < 8; i++) { - uint8_t cmd = ADDR_PROM_SETUP + (i * 2); - - if (OK != transfer(&cmd, 1, &prom_buf[0], 2)) - break; - - /* assemble 16 bit value and convert from big endian (sensor) to little endian (MCU) */ - cvt.b[0] = prom_buf[1]; - cvt.b[1] = prom_buf[0]; - _prom.c[i] = cvt.w; - } - - /* calculate CRC and return success/failure accordingly */ - return crc4(&_prom.c[0]) ? OK : -EIO; -} - -uint8_t -MS5611_SPI::read_reg(unsigned reg) -{ - uint8_t cmd[2]; - - cmd[0] = reg | DIR_READ; - - transfer(cmd, cmd, sizeof(cmd)); - - return cmd[1]; -} - -uint16_t -MS5611_SPI::read_reg16(unsigned reg) -{ - uint8_t cmd[3]; - - cmd[0] = reg | DIR_READ; - - transfer(cmd, cmd, sizeof(cmd)); - - return (uint16_t)(cmd[1] << 8) | cmd[2]; -} - void -MS5611_SPI::write_reg(unsigned reg, uint8_t value) -{ - uint8_t cmd[2]; - - cmd[0] = reg | DIR_WRITE; - cmd[1] = value; - - transfer(cmd, nullptr, sizeof(cmd)); -} - -void -MS5611_SPI::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) -{ - uint8_t val; - - val = read_reg(reg); - val &= ~clearbits; - val |= setbits; - write_reg(reg, val); -} - -int -MS5611_SPI::measure() -{ - int ret; - - perf_begin(_measure_perf); - - /* - * In phase zero, request temperature; in other phases, request pressure. - */ - uint8_t cmd_data = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1; - cmd_data |= DIR_WRITE; - - /* - * Send the command to begin measuring. - * - * Disable retries on this command; we can't know whether failure - * means the device did or did not see the write. - */ - ret = transfer(&cmd_data, nullptr, 1); - - if (OK != ret) - perf_count(_comms_errors); - - perf_end(_measure_perf); - - return ret; -} - -int -MS5611_SPI::collect() -{ - int ret; - - uint8_t data[4]; - union { - uint8_t b[4]; - uint32_t w; - } cvt; - - /* read the most recent measurement */ - data[0] = 0 | DIR_WRITE; - - perf_begin(_sample_perf); - - /* this should be fairly close to the end of the conversion, so the best approximation of the time */ - _reports[_next_report].timestamp = hrt_absolute_time(); - - ret = transfer(&data[0], &data[0], sizeof(data)); - if (ret != OK) { - perf_count(_comms_errors); - perf_end(_sample_perf); - return ret; - } - - /* fetch the raw value */ - cvt.b[0] = data[3]; - cvt.b[1] = data[2]; - cvt.b[2] = data[1]; - cvt.b[3] = 0; - uint32_t raw = cvt.w; - - /* handle a measurement */ - if (_measure_phase == 0) { - - /* temperature offset (in ADC units) */ - int32_t dT = (int32_t)raw - ((int32_t)_prom.s.c5_reference_temp << 8); - - /* absolute temperature in centidegrees - note intermediate value is outside 32-bit range */ - _TEMP = 2000 + (int32_t)(((int64_t)dT * _prom.s.c6_temp_coeff_temp) >> 23); - - /* base sensor scale/offset values */ - _SENS = ((int64_t)_prom.s.c1_pressure_sens << 15) + (((int64_t)_prom.s.c3_temp_coeff_pres_sens * dT) >> 8); - _OFF = ((int64_t)_prom.s.c2_pressure_offset << 16) + (((int64_t)_prom.s.c4_temp_coeff_pres_offset * dT) >> 7); - - /* temperature compensation */ - if (_TEMP < 2000) { - - int32_t T2 = POW2(dT) >> 31; - - int64_t f = POW2((int64_t)_TEMP - 2000); - int64_t OFF2 = 5 * f >> 1; - int64_t SENS2 = 5 * f >> 2; - - if (_TEMP < -1500) { - int64_t f2 = POW2(_TEMP + 1500); - OFF2 += 7 * f2; - SENS2 += 11 * f2 >> 1; - } - - _TEMP -= T2; - _OFF -= OFF2; - _SENS -= SENS2; - } - - } else { - - /* pressure calculation, result in Pa */ - int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15; - - /* generate a new report */ - _reports[_next_report].temperature = _TEMP / 100.0f; - _reports[_next_report].pressure = P / 100.0f; /* convert to millibar */ - - /* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */ - - /* - * PERFORMANCE HINT: - * - * The single precision calculation is 50 microseconds faster than the double - * precision variant. It is however not obvious if double precision is required. - * Pending more inspection and tests, we'll leave the double precision variant active. - * - * Measurements: - * double precision: ms5611_read: 992 events, 258641us elapsed, min 202us max 305us - * single precision: ms5611_read: 963 events, 208066us elapsed, min 202us max 241us - */ - - /* tropospheric properties (0-11km) for standard atmosphere */ - const double T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */ - const double a = -6.5 / 1000; /* temperature gradient in degrees per metre */ - const double g = 9.80665; /* gravity constant in m/s/s */ - const double R = 287.05; /* ideal gas constant in J/kg/K */ - - /* current pressure at MSL in kPa */ - double p1 = _msl_pressure / 1000.0; - - /* measured pressure in kPa */ - double p = P / 1000.0; - - /* - * Solve: - * - * / -(aR / g) \ - * | (p / p1) . T1 | - T1 - * \ / - * h = ------------------------------- + h1 - * a - */ - _reports[_next_report].altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; - - /* publish it */ - orb_publish(ORB_ID(sensor_baro), _baro_topic, &_reports[_next_report]); - - /* post a report to the ring - note, not locked */ - INCREMENT(_next_report, _num_reports); - - /* if we are running up against the oldest report, toss it */ - if (_next_report == _oldest_report) { - perf_count(_buffer_overflows); - INCREMENT(_oldest_report, _num_reports); - } - - /* notify anyone waiting for data */ - poll_notify(POLLIN); - } - - /* update the measurement state machine */ - INCREMENT(_measure_phase, MS5611_MEASUREMENT_RATIO + 1); - - perf_end(_sample_perf); - - return OK; -} - -int -MS5611_SPI::cmd_reset() -{ - uint8_t cmd = ADDR_RESET_CMD | DIR_WRITE; - int result; - - result = transfer(&cmd, nullptr, 1); - warnx("transferred reset, result: %d", result); - - return result; -} - -int -MS5611_SPI::read_prom() -{ - uint8_t prom_buf[3]; - union { - uint8_t b[2]; - uint16_t w; - } cvt; - - /* - * Wait for PROM contents to be in the device (2.8 ms) in the case we are - * called immediately after reset. - */ - usleep(3000); - - /* read and convert PROM words */ - for (int i = 0; i < 8; i++) { - uint8_t cmd = (ADDR_PROM_SETUP + (i * 2)); - _prom.c[i] = read_reg16(cmd); - } - - warnx("going for CRC"); - - /* calculate CRC and return success/failure accordingly */ - int ret = crc4(&_prom.c[0]) ? OK : -EIO; - if (ret == OK) { - warnx("CRC OK"); - } else { - warnx("CRC FAIL"); - } - return ret; -} - -bool -MS5611_I2C::crc4(uint16_t *n_prom) -{ - int16_t cnt; - uint16_t n_rem; - uint16_t crc_read; - uint8_t n_bit; - - n_rem = 0x00; - - /* save the read crc */ - crc_read = n_prom[7]; - - /* remove CRC byte */ - n_prom[7] = (0xFF00 & (n_prom[7])); - - for (cnt = 0; cnt < 16; cnt++) { - /* uneven bytes */ - if (cnt & 1) { - n_rem ^= (uint8_t)((n_prom[cnt >> 1]) & 0x00FF); - - } else { - n_rem ^= (uint8_t)(n_prom[cnt >> 1] >> 8); - } - - for (n_bit = 8; n_bit > 0; n_bit--) { - if (n_rem & 0x8000) { - n_rem = (n_rem << 1) ^ 0x3000; - - } else { - n_rem = (n_rem << 1); - } - } - } - - /* final 4 bit remainder is CRC value */ - n_rem = (0x000F & (n_rem >> 12)); - n_prom[7] = crc_read; - - /* return true if CRCs match */ - return (0x000F & crc_read) == (n_rem ^ 0x00); -} - -void -MS5611_I2C::print_info() -{ - perf_print_counter(_sample_perf); - perf_print_counter(_comms_errors); - perf_print_counter(_buffer_overflows); - printf("poll interval: %u ticks\n", _measure_ticks); - printf("report queue: %u (%u/%u @ %p)\n", - _num_reports, _oldest_report, _next_report, _reports); - printf("TEMP: %d\n", _TEMP); - printf("SENS: %lld\n", _SENS); - printf("OFF: %lld\n", _OFF); - printf("MSL pressure: %10.4f\n", (double)(_msl_pressure / 100.f)); - - printf("factory_setup %u\n", _prom.s.factory_setup); - printf("c1_pressure_sens %u\n", _prom.s.c1_pressure_sens); - printf("c2_pressure_offset %u\n", _prom.s.c2_pressure_offset); - printf("c3_temp_coeff_pres_sens %u\n", _prom.s.c3_temp_coeff_pres_sens); - printf("c4_temp_coeff_pres_offset %u\n", _prom.s.c4_temp_coeff_pres_offset); - printf("c5_reference_temp %u\n", _prom.s.c5_reference_temp); - printf("c6_temp_coeff_temp %u\n", _prom.s.c6_temp_coeff_temp); - printf("serial_and_crc %u\n", _prom.s.serial_and_crc); -} - -bool -MS5611_SPI::crc4(uint16_t *n_prom) -{ - int16_t cnt; - uint16_t n_rem; - uint16_t crc_read; - uint8_t n_bit; - - n_rem = 0x00; - - /* save the read crc */ - crc_read = n_prom[7]; - - /* remove CRC byte */ - n_prom[7] = (0xFF00 & (n_prom[7])); - - for (cnt = 0; cnt < 16; cnt++) { - /* uneven bytes */ - if (cnt & 1) { - n_rem ^= (uint8_t)((n_prom[cnt >> 1]) & 0x00FF); - - } else { - n_rem ^= (uint8_t)(n_prom[cnt >> 1] >> 8); - } - - for (n_bit = 8; n_bit > 0; n_bit--) { - if (n_rem & 0x8000) { - n_rem = (n_rem << 1) ^ 0x3000; - - } else { - n_rem = (n_rem << 1); - } - } - } - - /* final 4 bit remainder is CRC value */ - n_rem = (0x000F & (n_rem >> 12)); - n_prom[7] = crc_read; - - /* return true if CRCs match */ - return (0x000F & crc_read) == (n_rem ^ 0x00); -} - -void -MS5611_SPI::print_info() +MS5611::print_info() { perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); @@ -1795,7 +727,7 @@ MS5611_SPI::print_info() namespace ms5611 { -device::CDev *g_dev; +MS5611 *g_dev; void start(); void test(); @@ -1803,6 +735,53 @@ void reset(); void info(); void calibrate(unsigned altitude); +/** + * MS5611 crc4 cribbed from the datasheet + */ +bool +crc4(uint16_t *n_prom) +{ + int16_t cnt; + uint16_t n_rem; + uint16_t crc_read; + uint8_t n_bit; + + n_rem = 0x00; + + /* save the read crc */ + crc_read = n_prom[7]; + + /* remove CRC byte */ + n_prom[7] = (0xFF00 & (n_prom[7])); + + for (cnt = 0; cnt < 16; cnt++) { + /* uneven bytes */ + if (cnt & 1) { + n_rem ^= (uint8_t)((n_prom[cnt >> 1]) & 0x00FF); + + } else { + n_rem ^= (uint8_t)(n_prom[cnt >> 1] >> 8); + } + + for (n_bit = 8; n_bit > 0; n_bit--) { + if (n_rem & 0x8000) { + n_rem = (n_rem << 1) ^ 0x3000; + + } else { + n_rem = (n_rem << 1); + } + } + } + + /* final 4 bit remainder is CRC value */ + n_rem = (0x000F & (n_rem >> 12)); + n_prom[7] = crc_read; + + /* return true if CRCs match */ + return (0x000F & crc_read) == (n_rem ^ 0x00); +} + + /** * Start the driver. */ @@ -1814,34 +793,29 @@ start() if (g_dev != nullptr) errx(1, "already started"); - /* create the driver, try SPI first, fall back to I2C if required */ - #ifdef PX4_SPIDEV_BARO - { - warnx("Attempting SPI start"); - g_dev = new MS5611_SPI(1 /* XXX magic number, SPI1 */, BARO_DEVICE_PATH,(spi_dev_e)PX4_SPIDEV_BARO); + device::Device *interface = nullptr; + + /* create the driver, try SPI first, fall back to I2C if unsuccessful */ + if (MS5611_spi_interface != nullptr) + interface = MS5611_spi_interface(); + if (interface == nullptr && (MS5611_i2c_interface != nullptr)) + interface = MS5611_i2c_interface(); + + if (interface == nullptr) + errx(1, "failed to allocate an interface"); + + g_dev = new MS5611(interface); + if (g_dev == nullptr) { + delete interface; + errx(1, "failed to allocate driver"); } - #endif - /* try I2C if configuration exists and SPI failed*/ - #ifdef MS5611_BUS - if (g_dev == nullptr) { - warnx("Attempting I2C start"); - g_dev = new MS5611_I2C(MS5611_BUS); - } - - #endif - - if (g_dev == nullptr) - goto fail; - - if (OK != g_dev->init()) + if (g_dev->init() != OK) goto fail; /* set the poll rate to default, starts automatic data collection */ fd = open(BARO_DEVICE_PATH, O_RDONLY); - if (fd < 0) goto fail; - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) goto fail; @@ -1952,8 +926,7 @@ info() errx(1, "driver not running"); printf("state @ %p\n", g_dev); - MS5611_SPI* spi = (MS5611_SPI*)g_dev; - spi->print_info(); + g_dev->print_info(); exit(0); } diff --git a/src/drivers/ms5611/ms5611.h b/src/drivers/ms5611/ms5611.h new file mode 100644 index 0000000000..872011dc94 --- /dev/null +++ b/src/drivers/ms5611/ms5611.h @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ms5611.h + * + * Shared defines for the ms5611 driver. + */ + +#define ADDR_RESET_CMD 0x1E /* write to this address to reset chip */ +#define ADDR_CMD_CONVERT_D1 0x48 /* write to this address to start temperature conversion */ +#define ADDR_CMD_CONVERT_D2 0x58 /* write to this address to start pressure conversion */ +#define ADDR_DATA 0x00 /* address of 3 bytes / 32bit pressure data */ +#define ADDR_PROM_SETUP 0xA0 /* address of 8x 2 bytes factory and calibration data */ +#define ADDR_PROM_C1 0xA2 /* address of 6x 2 bytes calibration data */ + +/* interface ioctls */ +#define IOCTL_SET_PROMBUFFER 1 +#define IOCTL_RESET 2 +#define IOCTL_MEASURE 3 + + +/* interface factories */ +extern device::Device *MS5611_spi_interface() weak_function; +extern device::Device *MS5611_i2c_interface() weak_function; + +namespace ms5611 +{ + +/** + * Calibration PROM as reported by the device. + */ +#pragma pack(push,1) +struct prom_s { + uint16_t factory_setup; + uint16_t c1_pressure_sens; + uint16_t c2_pressure_offset; + uint16_t c3_temp_coeff_pres_sens; + uint16_t c4_temp_coeff_pres_offset; + uint16_t c5_reference_temp; + uint16_t c6_temp_coeff_temp; + uint16_t serial_and_crc; +}; + +/** + * Grody hack for crc4() + */ +union prom_u { + uint16_t c[8]; + prom_s s; +}; +#pragma pack(pop) + +extern bool crc4(uint16_t *n_prom); + +} /* namespace */ diff --git a/src/drivers/ms5611/ms5611_i2c.cpp b/src/drivers/ms5611/ms5611_i2c.cpp new file mode 100644 index 0000000000..7f5667c28a --- /dev/null +++ b/src/drivers/ms5611/ms5611_i2c.cpp @@ -0,0 +1,264 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + /** + * @file ms5611_i2c.cpp + * + * I2C interface for MS5611 + */ + +#ifndef PX4_I2C_BUS_ONBOARD + #define MS5611_BUS 1 +#else + #define MS5611_BUS PX4_I2C_BUS_ONBOARD +#endif + +#define MS5611_ADDRESS_1 0x76 /* address select pins pulled high (PX4FMU series v1.6+) */ +#define MS5611_ADDRESS_2 0x77 /* address select pins pulled low (PX4FMU prototypes) */ + + + +device::Device *MS5611_i2c_interface(); + +class MS5611_I2C : device::I2C +{ +public: + MS5611_I2C(int bus); + virtual ~MS5611_I2C(); + + virtual int init(); + virtual int read(unsigned offset, void *data, unsigned count); + virtual int ioctl(unsigned operation, unsigned &arg); + +protected: + virtual int probe(); + +private: + ms5611::prom_u *_prom + + int _probe_address(uint8_t address); + + /** + * Send a reset command to the MS5611. + * + * This is required after any bus reset. + */ + int _reset(); + + /** + * Send a measure command to the MS5611. + * + * @param addr Which address to use for the measure operation. + */ + int _measure(unsigned addr); + + /** + * Read the MS5611 PROM + * + * @return OK if the PROM reads successfully. + */ + int _read_prom(); + +}; + +device::Device * +MS5611_i2c_interface() +{ +#ifdef PX4_I2C_OBDEV_MS5611 + return new MS5611_I2C(MS5611_BUS); +#endif + return nullptr; +} + +MS5611_I2C::MS5611_I2C(int bus, ms5611_prom_u &prom) : + I2C("MS5611_I2C", nullptr, bus, 0, 400000), + _prom(prom) +{ +} + +MS5611_I2C::~MS5611_I2C() +{ +} + +int +MS5611_I2C::init() +{ + /* this will call probe(), and thereby _probe_address */ + return I2C::init(); +} + +int +MS5611_I2C::read(unsigned offset, void *data, unsigned count) +{ + union _cvt { + uint8_t b[4]; + uint32_t w; + } *cvt = (_cvt *)data; + uint8_t buf[3]; + + /* read the most recent measurement */ + uint8_t cmd = 0; + int ret = transfer(&cmd, 1, &buf[0], 3); + if (ret == OK) { + /* fetch the raw value */ + cvt->b[0] = buf[2]; + cvt->b[1] = buf[1]; + cvt->b[2] = buf[0]; + cvt->b[3] = 0; + } + + return ret; +} + +int +MS5611_I2C::ioctl(unsigned operation, unsigned &arg) +{ + int ret; + + switch (operation) { + case IOCTL_SET_PROMBUFFER: + _prom = reinterpret_cast(arg); + ret = OK; + break; + + case IOCTL_RESET: + ret = _reset(); + break; + + case IOCTL_MEASURE: + ret = _measure(arg); + break; + + default: + ret = EINVAL; + } + + return ret; +} + +int +MS5611_I2C::probe() +{ + _retries = 10; + + if ((OK == _probe_address(MS5611_ADDRESS_1)) || + (OK == _probe_address(MS5611_ADDRESS_2))) { + /* + * Disable retries; we may enable them selectively in some cases, + * but the device gets confused if we retry some of the commands. + */ + _retries = 0; + return OK; + } + + return -EIO; +} + +int +MS5611_I2C::_probe_address(uint8_t address) +{ + /* select the address we are going to try */ + set_address(address); + + /* send reset command */ + if (OK != _reset()) + return -EIO; + + /* read PROM */ + if (OK != _read_prom()) + return -EIO; + + return OK; +} + + +int +MS5611_I2C::_reset() +{ + unsigned old_retrycount = _retries; + uint8_t cmd = ADDR_RESET_CMD; + int result; + + /* bump the retry count */ + _retries = 10; + result = transfer(&cmd, 1, nullptr, 0); + _retries = old_retrycount; + + return result; +} + +int +MS5611_I2C::_measure(unsigned addr) +{ + /* + * Disable retries on this command; we can't know whether failure + * means the device did or did not see the command. + */ + _retries = 0; + + uint8_t cmd = addr; + return transfer(&cmd, 1, nullptr, 0); +} + +int +MS5611_I2C::_read_prom() +{ + uint8_t prom_buf[2]; + union { + uint8_t b[2]; + uint16_t w; + } cvt; + + /* + * Wait for PROM contents to be in the device (2.8 ms) in the case we are + * called immediately after reset. + */ + usleep(3000); + + /* read and convert PROM words */ + for (int i = 0; i < 8; i++) { + uint8_t cmd = ADDR_PROM_SETUP + (i * 2); + + if (OK != transfer(&cmd, 1, &prom_buf[0], 2)) + break; + + /* assemble 16 bit value and convert from big endian (sensor) to little endian (MCU) */ + cvt.b[0] = prom_buf[1]; + cvt.b[1] = prom_buf[0]; + _prom->c[i] = cvt.w; + } + + /* calculate CRC and return success/failure accordingly */ + return ms5611::crc4(&_prom->c[0]) ? OK : -EIO; +} + diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp new file mode 100644 index 0000000000..eed8e16970 --- /dev/null +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -0,0 +1,228 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + /** + * @file ms5611_spi.cpp + * + * SPI interface for MS5611 + */ + + +/* SPI protocol address bits */ +#define DIR_READ (1<<7) +#define DIR_WRITE (0<<7) +#define ADDR_INCREMENT (1<<6) + +device::Device *MS5611_spi_interface(); + +class MS5611_SPI : device::SPI +{ +public: + MS5611_SPI(int bus, spi_dev_e device); + virtual ~MS5611_SPI(); + + virtual int init(); + virtual int read(unsigned offset, void *data, unsigned count); + virtual int ioctl(unsigned operation, unsigned &arg); + +protected: + virtual int probe(); + +private: + ms5611::prom_u *_prom + + int _probe_address(uint8_t address); + + /** + * Send a reset command to the MS5611. + * + * This is required after any bus reset. + */ + int _reset(); + + /** + * Send a measure command to the MS5611. + * + * @param addr Which address to use for the measure operation. + */ + int _measure(unsigned addr); + + /** + * Read the MS5611 PROM + * + * @return OK if the PROM reads successfully. + */ + int _read_prom(); + + /** + * Read a 16-bit register value. + * + * @param reg The register to read. + */ + uint16_t _reg16(unsigned reg); +}; + +device::Device * +MS5611_spi_interface() +{ +#ifdef PX4_SPIDEV_BARO + return new MS5611_SPI(1 /* XXX MAGIC NUMBER */, (spi_dev_e)PX4_SPIDEV_BARO); +#endif + return nullptr; +} + +int +MS5611_SPI::init() +{ + int ret; + + ret = SPI::init(); + if (ret != OK) + goto out; + + /* send reset command */ + ret = _reset(); + if (ret != OK) + goto out; + + /* read PROM */ + ret = _read_prom(); + if (ret != OK) + goto out; + +out: + return ret; +} + +int +MS5611_SPI::read(unsigned offset, void *data, unsigned count) +{ + union _cvt { + uint8_t b[4]; + uint32_t w; + } *cvt = (_cvt *)data; + uint8_t buf[4]; + + /* read the most recent measurement */ + buf[0] = 0 | DIR_WRITE; + int ret = transfer(&buf[0], &buf[0], sizeof(buf)); + + if (ret == OK) { + /* fetch the raw value */ + cvt->b[0] = data[3]; + cvt->b[1] = data[2]; + cvt->b[2] = data[1]; + cvt->b[3] = 0; + + ret = count; + } + + return ret; +} + +int +MS5611_SPI::ioctl(unsigned operation, unsigned &arg) +{ + int ret; + + switch (operation) { + case IOCTL_SET_PROMBUFFER: + _prom = reinterpret_cast(arg); + return OK; + + case IOCTL_RESET: + ret = _reset(); + break; + + case IOCTL_MEASURE: + ret = _measure(arg); + break; + + default: + ret = EINVAL; + } + + if (ret != OK) { + errno = ret; + return -1; + } + return 0; +} + +int +MS5611_SPI::_reset() +{ + uint8_t cmd = ADDR_RESET_CMD | DIR_WRITE; + + return transfer(&cmd, nullptr, 1); +} + +int +MS5611_SPI::_measure(unsigned addr) +{ + uint8_t cmd = addr | DIR_WRITE; + + return transfer(&cmd, nullptr, 1); +} + + +int +MS5611_SPI::_read_prom() +{ + /* + * Wait for PROM contents to be in the device (2.8 ms) in the case we are + * called immediately after reset. + */ + usleep(3000); + + /* read and convert PROM words */ + for (int i = 0; i < 8; i++) { + uint8_t cmd = (ADDR_PROM_SETUP + (i * 2)); + _prom.c[i] = _reg16(cmd); + } + + /* calculate CRC and return success/failure accordingly */ + return ms5611::crc4(&_prom.c[0]) ? OK : -EIO; +} + +uint16_t +MS5611_SPI::_reg16(unsigned reg) +{ + uint8_t cmd[3]; + + cmd[0] = reg | DIR_READ; + + transfer(cmd, cmd, sizeof(cmd)); + + return (uint16_t)(cmd[1] << 8) | cmd[2]; +} From f8f6a43feac372c310f3d2444a8533b09e201d6c Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 14 Jul 2013 11:50:35 -0700 Subject: [PATCH 252/486] Use common, board-type-agnostic code to allocate the PX4IO interface. --- src/drivers/px4io/px4io.cpp | 70 ++++++++++++++++++------------------- 1 file changed, 34 insertions(+), 36 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 904da84c4c..9f84c09505 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1598,29 +1598,42 @@ extern "C" __EXPORT int px4io_main(int argc, char *argv[]); namespace { +device::Device * +get_interface() +{ + device::Device *interface = nullptr; + + /* try for a serial interface */ + if (PX4IO_serial_interface != nullptr) + interface = PX4IO_serial_interface(); + if (interface != nullptr) + goto got; + + /* try for an I2C interface if we haven't got a serial one */ + if (PX4IO_i2c_interface != nullptr) + interface = PX4IO_i2c_interface(); + if (interface != nullptr) + goto got; + + errx(1, "cannot alloc interface"); + +got: + if (interface->init() != OK) { + delete interface; + errx(1, "interface init failed"); + } + + return interface; +} + void start(int argc, char *argv[]) { if (g_dev != nullptr) errx(1, "already loaded"); - device::Device *interface; - -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) - interface = PX4IO_serial_interface(); -#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V1) - interface = PX4IO_i2c_interface(); -#else -# error Unknown board - cannot select interface. -#endif - - if (interface == nullptr) - errx(1, "cannot alloc interface"); - - if (interface->init()) { - delete interface; - errx(1, "interface init failed"); - } + /* allocate the interface */ + device::Device *interface = get_interface(); /* create the driver - it will set g_dev */ (void)new PX4IO(interface); @@ -1741,27 +1754,12 @@ monitor(void) void if_test(unsigned mode) { - device::Device *interface; + device::Device *interface = get_interface(); -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) - interface = PX4IO_serial_interface(); -#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V1) - interface = PX4IO_i2c_interface(); -#else -# error Unknown board - cannot select interface. -#endif + int result = interface->ioctl(1, mode); /* XXX magic numbers */ + delete interface; - if (interface == nullptr) - errx(1, "cannot alloc interface"); - - if (interface->init()) { - delete interface; - errx(1, "interface init failed"); - } else { - int result = interface->ioctl(1, mode); /* XXX magic numbers */ - delete interface; - errx(0, "test returned %d", result); - } + errx(0, "test returned %d", result); } } /* namespace */ From b11e05d614c372c45a3492e2c3b45ab252defced Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 14 Jul 2013 12:40:26 -0700 Subject: [PATCH 253/486] Don't build interface drivers we don't have config for. --- src/drivers/px4io/px4io_i2c.cpp | 12 ++++-------- src/drivers/px4io/px4io_serial.cpp | 4 ++++ 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/drivers/px4io/px4io_i2c.cpp b/src/drivers/px4io/px4io_i2c.cpp index 585b995fee..f561b4359a 100644 --- a/src/drivers/px4io/px4io_i2c.cpp +++ b/src/drivers/px4io/px4io_i2c.cpp @@ -50,12 +50,9 @@ #include -#include - #include -#include -#include "uploader.h" -#include + +#ifdef PX4_I2C_OBDEV_PX4IO device::Device *PX4IO_i2c_interface(); @@ -77,10 +74,7 @@ private: device::Device *PX4IO_i2c_interface() { -#ifdef PX4_I2C_OBDEV_PX4IO return new PX4IO_I2C(PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO); -#endif - return nullptr; } PX4IO_I2C::PX4IO_I2C(int bus, uint8_t address) : @@ -170,3 +164,5 @@ PX4IO_I2C::read(unsigned address, void *data, unsigned count) ret = count; return ret; } + +#endif /* PX4_I2C_OBDEV_PX4IO */ \ No newline at end of file diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp index 6a0b4d33fb..840b96f5be 100644 --- a/src/drivers/px4io/px4io_serial.cpp +++ b/src/drivers/px4io/px4io_serial.cpp @@ -67,6 +67,8 @@ #include +#ifdef PX4IO_SERIAL_BASE + device::Device *PX4IO_serial_interface(); /* serial register accessors */ @@ -667,3 +669,5 @@ PX4IO_serial::_abort_dma() stm32_dmastop(_tx_dma); stm32_dmastop(_rx_dma); } + +#endif /* PX4IO_SERIAL_BASE */ \ No newline at end of file From 6cf120831289368015b7b4f51db4f99f418e7129 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 14 Jul 2013 12:42:51 -0700 Subject: [PATCH 254/486] Don't build interface drivers we don't have configs for. Make the interface drivers build. Change the way we handle the prom buffer so that we can init the interface before constructing the driver. --- src/drivers/ms5611/module.mk | 2 +- src/drivers/ms5611/ms5611.cpp | 59 ++++++++++++++----------- src/drivers/ms5611/ms5611.h | 11 +++-- src/drivers/ms5611/ms5611_i2c.cpp | 46 ++++++++++++-------- src/drivers/ms5611/ms5611_spi.cpp | 72 ++++++++++++++++++++----------- 5 files changed, 117 insertions(+), 73 deletions(-) diff --git a/src/drivers/ms5611/module.mk b/src/drivers/ms5611/module.mk index 3c4b0f0938..20f8aa1737 100644 --- a/src/drivers/ms5611/module.mk +++ b/src/drivers/ms5611/module.mk @@ -37,4 +37,4 @@ MODULE_COMMAND = ms5611 -SRCS = ms5611.cpp +SRCS = ms5611.cpp ms5611_spi.cpp ms5611_i2c.cpp diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index b8c396f7bf..9d9888a905 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -93,7 +93,7 @@ static const int ERROR = -1; class MS5611 : public device::CDev { public: - MS5611(device::Device *interface); + MS5611(device::Device *interface, ms5611::prom_u &prom_buf); ~MS5611(); virtual int init(); @@ -109,7 +109,7 @@ public: protected: Device *_interface; - ms5611::prom_u _prom; + ms5611::prom_s _prom; struct work_s _work; unsigned _measure_ticks; @@ -191,9 +191,10 @@ protected: */ extern "C" __EXPORT int ms5611_main(int argc, char *argv[]); -MS5611::MS5611(device::Device *interface) : +MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) : CDev("MS5611", BARO_DEVICE_PATH), _interface(interface), + _prom(prom_buf.s), _measure_ticks(0), _num_reports(0), _next_report(0), @@ -230,13 +231,11 @@ MS5611::~MS5611() int MS5611::init() { + int ret; - /* verify that the interface is ok */ - unsigned arg = (unsigned)&_prom; - _interface->ioctl(IOCTL_SET_PROMBUFFER, arg); - int ret = _interface->init(); + ret = CDev::init(); if (ret != OK) { - debug("interface init failed"); + debug("CDev init failed"); goto out; } @@ -598,14 +597,14 @@ MS5611::collect() if (_measure_phase == 0) { /* temperature offset (in ADC units) */ - int32_t dT = (int32_t)raw - ((int32_t)_prom.s.c5_reference_temp << 8); + int32_t dT = (int32_t)raw - ((int32_t)_prom.c5_reference_temp << 8); /* absolute temperature in centidegrees - note intermediate value is outside 32-bit range */ - _TEMP = 2000 + (int32_t)(((int64_t)dT * _prom.s.c6_temp_coeff_temp) >> 23); + _TEMP = 2000 + (int32_t)(((int64_t)dT * _prom.c6_temp_coeff_temp) >> 23); /* base sensor scale/offset values */ - _SENS = ((int64_t)_prom.s.c1_pressure_sens << 15) + (((int64_t)_prom.s.c3_temp_coeff_pres_sens * dT) >> 8); - _OFF = ((int64_t)_prom.s.c2_pressure_offset << 16) + (((int64_t)_prom.s.c4_temp_coeff_pres_offset * dT) >> 7); + _SENS = ((int64_t)_prom.c1_pressure_sens << 15) + (((int64_t)_prom.c3_temp_coeff_pres_sens * dT) >> 8); + _OFF = ((int64_t)_prom.c2_pressure_offset << 16) + (((int64_t)_prom.c4_temp_coeff_pres_offset * dT) >> 7); /* temperature compensation */ if (_TEMP < 2000) { @@ -711,14 +710,14 @@ MS5611::print_info() printf("OFF: %lld\n", _OFF); printf("MSL pressure: %10.4f\n", (double)(_msl_pressure / 100.f)); - printf("factory_setup %u\n", _prom.s.factory_setup); - printf("c1_pressure_sens %u\n", _prom.s.c1_pressure_sens); - printf("c2_pressure_offset %u\n", _prom.s.c2_pressure_offset); - printf("c3_temp_coeff_pres_sens %u\n", _prom.s.c3_temp_coeff_pres_sens); - printf("c4_temp_coeff_pres_offset %u\n", _prom.s.c4_temp_coeff_pres_offset); - printf("c5_reference_temp %u\n", _prom.s.c5_reference_temp); - printf("c6_temp_coeff_temp %u\n", _prom.s.c6_temp_coeff_temp); - printf("serial_and_crc %u\n", _prom.s.serial_and_crc); + printf("factory_setup %u\n", _prom.factory_setup); + printf("c1_pressure_sens %u\n", _prom.c1_pressure_sens); + printf("c2_pressure_offset %u\n", _prom.c2_pressure_offset); + printf("c3_temp_coeff_pres_sens %u\n", _prom.c3_temp_coeff_pres_sens); + printf("c4_temp_coeff_pres_offset %u\n", _prom.c4_temp_coeff_pres_offset); + printf("c5_reference_temp %u\n", _prom.c5_reference_temp); + printf("c6_temp_coeff_temp %u\n", _prom.c6_temp_coeff_temp); + printf("serial_and_crc %u\n", _prom.serial_and_crc); } /** @@ -789,6 +788,7 @@ void start() { int fd; + prom_u prom_buf; if (g_dev != nullptr) errx(1, "already started"); @@ -797,14 +797,19 @@ start() /* create the driver, try SPI first, fall back to I2C if unsuccessful */ if (MS5611_spi_interface != nullptr) - interface = MS5611_spi_interface(); + interface = MS5611_spi_interface(prom_buf); if (interface == nullptr && (MS5611_i2c_interface != nullptr)) - interface = MS5611_i2c_interface(); + interface = MS5611_i2c_interface(prom_buf); if (interface == nullptr) errx(1, "failed to allocate an interface"); - g_dev = new MS5611(interface); + if (interface->init() != OK) { + delete interface; + errx(1, "interface init failed"); + } + + g_dev = new MS5611(interface, prom_buf); if (g_dev == nullptr) { delete interface; errx(1, "failed to allocate driver"); @@ -814,10 +819,14 @@ start() /* set the poll rate to default, starts automatic data collection */ fd = open(BARO_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { + warnx("can't open baro device"); goto fail; - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + } + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + warnx("failed setting default poll rate"); goto fail; + } exit(0); diff --git a/src/drivers/ms5611/ms5611.h b/src/drivers/ms5611/ms5611.h index 872011dc94..76fb84de8e 100644 --- a/src/drivers/ms5611/ms5611.h +++ b/src/drivers/ms5611/ms5611.h @@ -45,15 +45,9 @@ #define ADDR_PROM_C1 0xA2 /* address of 6x 2 bytes calibration data */ /* interface ioctls */ -#define IOCTL_SET_PROMBUFFER 1 #define IOCTL_RESET 2 #define IOCTL_MEASURE 3 - -/* interface factories */ -extern device::Device *MS5611_spi_interface() weak_function; -extern device::Device *MS5611_i2c_interface() weak_function; - namespace ms5611 { @@ -84,3 +78,8 @@ union prom_u { extern bool crc4(uint16_t *n_prom); } /* namespace */ + +/* interface factories */ +extern device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf) weak_function; +extern device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf) weak_function; + diff --git a/src/drivers/ms5611/ms5611_i2c.cpp b/src/drivers/ms5611/ms5611_i2c.cpp index 7f5667c28a..06867cc9df 100644 --- a/src/drivers/ms5611/ms5611_i2c.cpp +++ b/src/drivers/ms5611/ms5611_i2c.cpp @@ -37,6 +37,25 @@ * I2C interface for MS5611 */ +/* XXX trim includes */ +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include "ms5611.h" + +#ifdef PX4_I2C_OBDEV_MS5611 + #ifndef PX4_I2C_BUS_ONBOARD #define MS5611_BUS 1 #else @@ -48,12 +67,12 @@ -device::Device *MS5611_i2c_interface(); +device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf); -class MS5611_I2C : device::I2C +class MS5611_I2C : public device::I2C { public: - MS5611_I2C(int bus); + MS5611_I2C(int bus, ms5611::prom_u &prom_buf); virtual ~MS5611_I2C(); virtual int init(); @@ -64,7 +83,7 @@ protected: virtual int probe(); private: - ms5611::prom_u *_prom + ms5611::prom_u &_prom; int _probe_address(uint8_t address); @@ -92,15 +111,12 @@ private: }; device::Device * -MS5611_i2c_interface() +MS5611_i2c_interface(ms5611::prom_u &prom_buf) { -#ifdef PX4_I2C_OBDEV_MS5611 - return new MS5611_I2C(MS5611_BUS); -#endif - return nullptr; + return new MS5611_I2C(MS5611_BUS, prom_buf); } -MS5611_I2C::MS5611_I2C(int bus, ms5611_prom_u &prom) : +MS5611_I2C::MS5611_I2C(int bus, ms5611::prom_u &prom) : I2C("MS5611_I2C", nullptr, bus, 0, 400000), _prom(prom) { @@ -146,11 +162,6 @@ MS5611_I2C::ioctl(unsigned operation, unsigned &arg) int ret; switch (operation) { - case IOCTL_SET_PROMBUFFER: - _prom = reinterpret_cast(arg); - ret = OK; - break; - case IOCTL_RESET: ret = _reset(); break; @@ -255,10 +266,11 @@ MS5611_I2C::_read_prom() /* assemble 16 bit value and convert from big endian (sensor) to little endian (MCU) */ cvt.b[0] = prom_buf[1]; cvt.b[1] = prom_buf[0]; - _prom->c[i] = cvt.w; + _prom.c[i] = cvt.w; } /* calculate CRC and return success/failure accordingly */ - return ms5611::crc4(&_prom->c[0]) ? OK : -EIO; + return ms5611::crc4(&_prom.c[0]) ? OK : -EIO; } +#endif /* PX4_I2C_OBDEV_MS5611 */ diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index eed8e16970..156832a618 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -37,31 +37,44 @@ * SPI interface for MS5611 */ +/* XXX trim includes */ +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include "ms5611.h" /* SPI protocol address bits */ #define DIR_READ (1<<7) #define DIR_WRITE (0<<7) #define ADDR_INCREMENT (1<<6) -device::Device *MS5611_spi_interface(); +#ifdef PX4_SPIDEV_BARO -class MS5611_SPI : device::SPI +device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf); + +class MS5611_SPI : public device::SPI { public: - MS5611_SPI(int bus, spi_dev_e device); + MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf); virtual ~MS5611_SPI(); virtual int init(); virtual int read(unsigned offset, void *data, unsigned count); virtual int ioctl(unsigned operation, unsigned &arg); -protected: - virtual int probe(); - private: - ms5611::prom_u *_prom - - int _probe_address(uint8_t address); + ms5611::prom_u &_prom; /** * Send a reset command to the MS5611. @@ -93,12 +106,19 @@ private: }; device::Device * -MS5611_spi_interface() +MS5611_spi_interface(ms5611::prom_u &prom_buf) +{ + return new MS5611_SPI(1 /* XXX MAGIC NUMBER */, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf); +} + +MS5611_SPI::MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf) : + SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 2000000), + _prom(prom_buf) +{ +} + +MS5611_SPI::~MS5611_SPI() { -#ifdef PX4_SPIDEV_BARO - return new MS5611_SPI(1 /* XXX MAGIC NUMBER */, (spi_dev_e)PX4_SPIDEV_BARO); -#endif - return nullptr; } int @@ -107,18 +127,24 @@ MS5611_SPI::init() int ret; ret = SPI::init(); - if (ret != OK) + if (ret != OK) { + debug("SPI init failed"); goto out; + } /* send reset command */ ret = _reset(); - if (ret != OK) + if (ret != OK) { + debug("reset failed"); goto out; + } /* read PROM */ ret = _read_prom(); - if (ret != OK) + if (ret != OK) { + debug("prom readout failed"); goto out; + } out: return ret; @@ -139,9 +165,9 @@ MS5611_SPI::read(unsigned offset, void *data, unsigned count) if (ret == OK) { /* fetch the raw value */ - cvt->b[0] = data[3]; - cvt->b[1] = data[2]; - cvt->b[2] = data[1]; + cvt->b[0] = buf[3]; + cvt->b[1] = buf[2]; + cvt->b[2] = buf[1]; cvt->b[3] = 0; ret = count; @@ -156,10 +182,6 @@ MS5611_SPI::ioctl(unsigned operation, unsigned &arg) int ret; switch (operation) { - case IOCTL_SET_PROMBUFFER: - _prom = reinterpret_cast(arg); - return OK; - case IOCTL_RESET: ret = _reset(); break; @@ -226,3 +248,5 @@ MS5611_SPI::_reg16(unsigned reg) return (uint16_t)(cmd[1] << 8) | cmd[2]; } + +#endif /* PX4_SPIDEV_BARO */ From b174a6051527dacc858c6ed54b5d113888c5d4de Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 15 Jul 2013 09:11:52 +0400 Subject: [PATCH 255/486] multirotor_pos_control: PID ontroller derivative mode fixed --- .../multirotor_pos_control/multirotor_pos_control.c | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 1d2dd384ac..4bae7efa4a 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -234,11 +234,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) parameters_update(¶ms_h, ¶ms); for (int i = 0; i < 2; i++) { - pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); + pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f); pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.slope_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); } - pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); + pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f); thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); int paramcheck_counter = 0; @@ -401,7 +401,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) sp_move_rate[1] = sinf(xy_sp_ctl_dir) * xy_sp_ctl_speed; local_pos_sp.x += sp_move_rate[0] * dt; local_pos_sp.y += sp_move_rate[1] * dt; - /* limit maximum setpoint from position offset and preserve direction */ + /* limit maximum setpoint from position offset and preserve direction + * fail safe, should not happen in normal operation */ float pos_vec_x = local_pos_sp.x - local_pos.x; float pos_vec_y = local_pos_sp.y - local_pos.y; float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / xy_sp_offs_max; From 1b38cf715d85b15f2200d49b64fbe22a05b71937 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 15 Jul 2013 22:15:15 +0200 Subject: [PATCH 256/486] Renamed actuator_safety back to actuator_armed, compiling but untested --- .../ardrone_interface/ardrone_interface.c | 19 ++-- src/drivers/blinkm/blinkm.cpp | 83 ++++++++++-------- src/drivers/hil/hil.cpp | 18 ++-- src/drivers/mkblctrl/mkblctrl.cpp | 20 ++--- src/drivers/px4fmu/fmu.cpp | 28 +++--- src/drivers/px4io/px4io.cpp | 37 ++++---- .../flow_position_control_main.c | 10 +-- .../flow_position_estimator_main.c | 20 ++--- .../flow_speed_control_main.c | 12 +-- src/modules/commander/commander.c | 87 ++++++++++--------- src/modules/commander/commander_helper.h | 2 +- src/modules/commander/state_machine_helper.c | 26 +++--- src/modules/commander/state_machine_helper.h | 4 +- src/modules/gpio_led/gpio_led.c | 14 +-- src/modules/mavlink/orb_listener.c | 18 ++-- src/modules/mavlink/orb_topics.h | 3 +- src/modules/mavlink_onboard/mavlink.c | 10 +-- src/modules/mavlink_onboard/orb_topics.h | 2 +- src/modules/mavlink_onboard/util.h | 2 +- .../multirotor_att_control_main.c | 18 ++-- src/modules/sdlog2/sdlog2.c | 32 +++---- src/modules/uORB/objects_common.cpp | 9 +- .../{actuator_safety.h => actuator_armed.h} | 22 ++--- src/modules/uORB/topics/safety.h | 57 ++++++++++++ .../uORB/topics/vehicle_control_mode.h | 6 +- 25 files changed, 316 insertions(+), 243 deletions(-) rename src/modules/uORB/topics/{actuator_safety.h => actuator_armed.h} (73%) create mode 100644 src/modules/uORB/topics/safety.h diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c index 187820372b..b88f61ce80 100644 --- a/src/drivers/ardrone_interface/ardrone_interface.c +++ b/src/drivers/ardrone_interface/ardrone_interface.c @@ -53,9 +53,10 @@ #include #include #include -#include +#include #include -#include +#include +#include #include @@ -244,17 +245,15 @@ int ardrone_interface_thread_main(int argc, char *argv[]) int led_counter = 0; /* declare and safely initialize all structs */ - struct vehicle_status_s state; - memset(&state, 0, sizeof(state)); struct actuator_controls_s actuator_controls; memset(&actuator_controls, 0, sizeof(actuator_controls)); - struct actuator_safety_s safety; - safety.armed = false; + struct actuator_armed_s armed; + //XXX is this necessairy? + armed.armed = false; /* subscribe to attitude, motor setpoints and system state */ int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - int state_sub = orb_subscribe(ORB_ID(vehicle_status)); - int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); printf("[ardrone_interface] Motors initialized - ready.\n"); fflush(stdout); @@ -325,12 +324,12 @@ int ardrone_interface_thread_main(int argc, char *argv[]) /* get a local copy of the actuator controls */ orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls); - orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); /* for now only spin if armed and immediately shut down * if in failsafe */ - if (safety.armed && !safety.lockdown) { + if (armed.armed && !armed.lockdown) { ardrone_mixing_and_output(ardrone_write, &actuator_controls); } else { diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index a11f884773..e83a87aa93 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -116,7 +116,8 @@ #include #include #include -#include +#include +#include #include static const float MAX_CELL_VOLTAGE = 4.3f; @@ -376,8 +377,9 @@ BlinkM::led() { static int vehicle_status_sub_fd; + static int vehicle_control_mode_sub_fd; static int vehicle_gps_position_sub_fd; - static int actuator_safety_sub_fd; + static int actuator_armed_sub_fd; static int num_of_cells = 0; static int detected_cells_runcount = 0; @@ -388,7 +390,8 @@ BlinkM::led() static int led_interval = 1000; static int no_data_vehicle_status = 0; - static int no_data_actuator_safety = 0; + static int no_data_vehicle_control_mode = 0; + static int no_data_actuator_armed = 0; static int no_data_vehicle_gps_position = 0; static bool topic_initialized = false; @@ -401,8 +404,11 @@ BlinkM::led() vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status)); orb_set_interval(vehicle_status_sub_fd, 1000); - actuator_safety_sub_fd = orb_subscribe(ORB_ID(actuator_safety)); - orb_set_interval(actuator_safety_sub_fd, 1000); + vehicle_control_mode_sub_fd = orb_subscribe(ORB_ID(vehicle_control_mode)); + orb_set_interval(vehicle_control_mode_sub_fd, 1000); + + actuator_armed_sub_fd = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(actuator_armed_sub_fd, 1000); vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position)); orb_set_interval(vehicle_gps_position_sub_fd, 1000); @@ -458,14 +464,16 @@ BlinkM::led() if (led_thread_runcount == 15) { /* obtained data for the first file descriptor */ struct vehicle_status_s vehicle_status_raw; - struct actuator_safety_s actuator_safety; + struct vehicle_control_mode_s vehicle_control_mode; + struct actuator_armed_s actuator_armed; struct vehicle_gps_position_s vehicle_gps_position_raw; memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw)); memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw)); bool new_data_vehicle_status; - bool new_data_actuator_safety; + bool new_data_vehicle_control_mode; + bool new_data_actuator_armed; bool new_data_vehicle_gps_position; orb_check(vehicle_status_sub_fd, &new_data_vehicle_status); @@ -479,15 +487,26 @@ BlinkM::led() no_data_vehicle_status = 3; } - orb_check(actuator_safety_sub_fd, &new_data_actuator_safety); + orb_check(vehicle_control_mode_sub_fd, &new_data_vehicle_control_mode); - if (new_data_actuator_safety) { - orb_copy(ORB_ID(actuator_safety), actuator_safety_sub_fd, &actuator_safety); - no_data_actuator_safety = 0; + if (new_data_vehicle_control_mode) { + orb_copy(ORB_ID(vehicle_control_mode), vehicle_control_mode_sub_fd, &vehicle_control_mode); + no_data_vehicle_control_mode = 0; } else { - no_data_actuator_safety++; - if(no_data_vehicle_status >= 3) - no_data_vehicle_status = 3; + no_data_vehicle_control_mode++; + if(no_data_vehicle_control_mode >= 3) + no_data_vehicle_control_mode = 3; + } + + orb_check(actuator_armed_sub_fd, &new_data_actuator_armed); + + if (new_data_actuator_armed) { + orb_copy(ORB_ID(actuator_armed), actuator_armed_sub_fd, &actuator_armed); + no_data_actuator_armed = 0; + } else { + no_data_actuator_armed++; + if(no_data_actuator_armed >= 3) + no_data_actuator_armed = 3; } orb_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position); @@ -549,7 +568,7 @@ BlinkM::led() } else { /* no battery warnings here */ - if(actuator_safety.armed == false) { + if(actuator_armed.armed == false) { /* system not armed */ led_color_1 = LED_RED; led_color_2 = LED_RED; @@ -573,25 +592,21 @@ BlinkM::led() led_color_8 = LED_OFF; led_blink = LED_BLINK; - /* handle 4th led - flightmode indicator */ -#warning fix this -// switch((int)vehicle_status_raw.flight_mode) { -// case VEHICLE_FLIGHT_MODE_MANUAL: -// led_color_4 = LED_AMBER; -// break; -// -// case VEHICLE_FLIGHT_MODE_STAB: -// led_color_4 = LED_YELLOW; -// break; -// -// case VEHICLE_FLIGHT_MODE_HOLD: -// led_color_4 = LED_BLUE; -// break; -// -// case VEHICLE_FLIGHT_MODE_AUTO: -// led_color_4 = LED_GREEN; -// break; -// } + if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) { + + //XXX please check + if (vehicle_control_mode.flag_control_position_enabled) + led_color_4 = LED_GREEN; + else if (vehicle_control_mode.flag_control_velocity_enabled) + led_color_4 = LED_BLUE; + else if (vehicle_control_mode.flag_control_attitude_enabled) + led_color_4 = LED_YELLOW; + else if (vehicle_control_mode.flag_control_manual_enabled) + led_color_4 = LED_AMBER; + else + led_color_4 = LED_OFF; + + } if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) { /* handling used sat�s */ diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index 15fbf8c882..3e30e32927 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -75,7 +75,7 @@ #include #include -#include +#include #include #include @@ -109,7 +109,7 @@ private: int _current_update_rate; int _task; int _t_actuators; - int _t_safety; + int _t_armed; orb_advert_t _t_outputs; unsigned _num_outputs; bool _primary_pwm_device; @@ -162,7 +162,7 @@ HIL::HIL() : _current_update_rate(0), _task(-1), _t_actuators(-1), - _t_safety(-1), + _t_armed(-1), _t_outputs(0), _num_outputs(0), _primary_pwm_device(false), @@ -322,8 +322,8 @@ HIL::task_main() /* force a reset of the update rate */ _current_update_rate = 0; - _t_safety = orb_subscribe(ORB_ID(actuator_safety)); - orb_set_interval(_t_safety, 200); /* 5Hz update rate */ + _t_armed = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(_t_armed, 200); /* 5Hz update rate */ /* advertise the mixed control outputs */ actuator_outputs_s outputs; @@ -335,7 +335,7 @@ HIL::task_main() pollfd fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; - fds[1].fd = _t_safety; + fds[1].fd = _t_armed; fds[1].events = POLLIN; unsigned num_outputs; @@ -427,15 +427,15 @@ HIL::task_main() /* how about an arming update? */ if (fds[1].revents & POLLIN) { - actuator_safety_s aa; + actuator_armed_s aa; /* get new value */ - orb_copy(ORB_ID(actuator_safety), _t_safety, &aa); + orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); } } ::close(_t_actuators); - ::close(_t_safety); + ::close(_t_armed); /* make sure servos are off */ // up_pwm_servo_deinit(); diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 3fc1054e6c..06930db388 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -74,7 +74,7 @@ #include #include #include -#include +#include #include #include @@ -135,7 +135,7 @@ private: int _current_update_rate; int _task; int _t_actuators; - int _t_actuator_safety; + int _t_actuator_armed; unsigned int _motor; int _px4mode; int _frametype; @@ -248,7 +248,7 @@ MK::MK(int bus) : _update_rate(50), _task(-1), _t_actuators(-1), - _t_actuator_safety(-1), + _t_actuator_armed(-1), _t_outputs(0), _t_actuators_effective(0), _t_esc_status(0), @@ -513,8 +513,8 @@ MK::task_main() /* force a reset of the update rate */ _current_update_rate = 0; - _t_actuator_safety = orb_subscribe(ORB_ID(actuator_safety)); - orb_set_interval(_t_actuator_safety, 200); /* 5Hz update rate */ + _t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */ /* advertise the mixed control outputs */ actuator_outputs_s outputs; @@ -540,7 +540,7 @@ MK::task_main() pollfd fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; - fds[1].fd = _t_actuator_safety; + fds[1].fd = _t_actuator_armed; fds[1].events = POLLIN; log("starting"); @@ -651,13 +651,13 @@ MK::task_main() /* how about an arming update? */ if (fds[1].revents & POLLIN) { - actuator_safety_s as; + actuator_armed_s aa; /* get new value */ - orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &as); + orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa); /* update PWM servo armed status if armed and not locked down */ - mk_servo_arm(as.armed && !as.lockdown); + mk_servo_arm(aa.armed && !aa.lockdown); } @@ -700,7 +700,7 @@ MK::task_main() //::close(_t_esc_status); ::close(_t_actuators); ::close(_t_actuators_effective); - ::close(_t_actuator_safety); + ::close(_t_actuator_armed); /* make sure servos are off */ diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index d0499d2fd6..41c8c8bb72 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -69,7 +69,7 @@ #include #include #include -#include +#include #include #include @@ -105,7 +105,7 @@ private: unsigned _current_update_rate; int _task; int _t_actuators; - int _t_actuator_safety; + int _t_actuator_armed; orb_advert_t _t_outputs; orb_advert_t _t_actuators_effective; unsigned _num_outputs; @@ -175,7 +175,7 @@ PX4FMU::PX4FMU() : _current_update_rate(0), _task(-1), _t_actuators(-1), - _t_actuator_safety(-1), + _t_actuator_armed(-1), _t_outputs(0), _t_actuators_effective(0), _num_outputs(0), @@ -377,8 +377,8 @@ PX4FMU::task_main() /* force a reset of the update rate */ _current_update_rate = 0; - _t_actuator_safety = orb_subscribe(ORB_ID(actuator_safety)); - orb_set_interval(_t_actuator_safety, 200); /* 5Hz update rate */ + _t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */ /* advertise the mixed control outputs */ actuator_outputs_s outputs; @@ -397,7 +397,7 @@ PX4FMU::task_main() pollfd fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; - fds[1].fd = _t_actuator_safety; + fds[1].fd = _t_actuator_armed; fds[1].events = POLLIN; unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4; @@ -500,13 +500,13 @@ PX4FMU::task_main() /* how about an arming update? */ if (fds[1].revents & POLLIN) { - actuator_safety_s as; + actuator_armed_s aa; /* get new value */ - orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &as); + orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa); /* update PWM servo armed status if armed and not locked down */ - bool set_armed = as.armed && !as.lockdown; + bool set_armed = aa.armed && !aa.lockdown; if (set_armed != _armed) { _armed = set_armed; up_pwm_servo_arm(set_armed); @@ -536,7 +536,7 @@ PX4FMU::task_main() ::close(_t_actuators); ::close(_t_actuators_effective); - ::close(_t_actuator_safety); + ::close(_t_actuator_armed); /* make sure servos are off */ up_pwm_servo_deinit(); @@ -1022,12 +1022,12 @@ fake(int argc, char *argv[]) if (handle < 0) errx(1, "advertise failed"); - actuator_safety_s as; + actuator_armed_s aa; - as.armed = true; - as.lockdown = false; + aa.armed = true; + aa.lockdown = false; - handle = orb_advertise(ORB_ID(actuator_safety), &as); + handle = orb_advertise(ORB_ID(actuator_armed), &aa); if (handle < 0) errx(1, "advertise failed 2"); diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 94f2318211..ea732eccd1 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -75,7 +75,8 @@ #include #include #include -#include +#include +#include #include #include #include @@ -178,7 +179,7 @@ private: /* subscribed topics */ int _t_actuators; ///< actuator controls topic - int _t_actuator_safety; ///< system armed control topic + int _t_actuator_armed; ///< system armed control topic int _t_vstatus; ///< system / vehicle status int _t_param; ///< parameter update topic @@ -360,7 +361,7 @@ PX4IO::PX4IO() : _status(0), _alarms(0), _t_actuators(-1), - _t_actuator_safety(-1), + _t_actuator_armed(-1), _t_vstatus(-1), _t_param(-1), _to_input_rc(0), @@ -505,9 +506,9 @@ PX4IO::init() * remains untouched (so manual override is still available). */ - int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + int safety_sub = orb_subscribe(ORB_ID(actuator_armed)); /* fill with initial values, clear updated flag */ - struct actuator_safety_s safety; + struct actuator_armed_s safety; uint64_t try_start_time = hrt_absolute_time(); bool updated = false; @@ -518,7 +519,7 @@ PX4IO::init() if (updated) { /* got data, copy and exit loop */ - orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + orb_copy(ORB_ID(actuator_armed), safety_sub, &safety); break; } @@ -559,7 +560,7 @@ PX4IO::init() orb_check(safety_sub, &updated); if (updated) { - orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + orb_copy(ORB_ID(actuator_armed), safety_sub, &safety); } /* wait 50 ms */ @@ -643,8 +644,8 @@ PX4IO::task_main() ORB_ID(actuator_controls_1)); orb_set_interval(_t_actuators, 20); /* default to 50Hz */ - _t_actuator_safety = orb_subscribe(ORB_ID(actuator_safety)); - orb_set_interval(_t_actuator_safety, 200); /* 5Hz update rate */ + _t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */ _t_vstatus = orb_subscribe(ORB_ID(vehicle_status)); orb_set_interval(_t_vstatus, 200); /* 5Hz update rate max. */ @@ -656,7 +657,7 @@ PX4IO::task_main() pollfd fds[4]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; - fds[1].fd = _t_actuator_safety; + fds[1].fd = _t_actuator_armed; fds[1].events = POLLIN; fds[2].fd = _t_vstatus; fds[2].events = POLLIN; @@ -832,21 +833,21 @@ PX4IO::set_idle_values(const uint16_t *vals, unsigned len) int PX4IO::io_set_arming_state() { - actuator_safety_s safety; ///< system armed state + actuator_armed_s armed; ///< system armed state vehicle_status_s vstatus; ///< overall system state - orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &safety); + orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &armed); orb_copy(ORB_ID(vehicle_status), _t_vstatus, &vstatus); uint16_t set = 0; uint16_t clear = 0; - if (safety.armed && !safety.lockdown) { + if (armed.armed && !armed.lockdown) { set |= PX4IO_P_SETUP_ARMING_FMU_ARMED; } else { clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED; } - if (safety.ready_to_arm) { + if (armed.ready_to_arm) { set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; } else { clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; @@ -1004,10 +1005,10 @@ PX4IO::io_handle_status(uint16_t status) /** * Get and handle the safety status */ - struct actuator_safety_s safety; + struct safety_s safety; safety.timestamp = hrt_absolute_time(); - orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &safety); + orb_copy(ORB_ID(safety), _to_safety, &safety); if (status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { safety.safety_off = true; @@ -1019,9 +1020,9 @@ PX4IO::io_handle_status(uint16_t status) /* lazily publish the safety status */ if (_to_safety > 0) { - orb_publish(ORB_ID(actuator_safety), _to_safety, &safety); + orb_publish(ORB_ID(safety), _to_safety, &safety); } else { - _to_safety = orb_advertise(ORB_ID(actuator_safety), &safety); + _to_safety = orb_advertise(ORB_ID(safety), &safety); } return ret; diff --git a/src/examples/flow_position_control/flow_position_control_main.c b/src/examples/flow_position_control/flow_position_control_main.c index 0b94045822..621d3253f0 100644 --- a/src/examples/flow_position_control/flow_position_control_main.c +++ b/src/examples/flow_position_control/flow_position_control_main.c @@ -55,7 +55,7 @@ #include #include #include -#include +#include #include #include #include @@ -159,7 +159,7 @@ flow_position_control_thread_main(int argc, char *argv[]) const float time_scale = powf(10.0f,-6.0f); /* structures */ - struct actuator_safety_s safety; + struct actuator_armed_s armed; struct vehicle_control_mode_s control_mode; struct vehicle_attitude_s att; struct manual_control_setpoint_s manual; @@ -171,7 +171,7 @@ flow_position_control_thread_main(int argc, char *argv[]) /* subscribe to attitude, motor setpoints and system state */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow)); @@ -261,7 +261,7 @@ flow_position_control_thread_main(int argc, char *argv[]) perf_begin(mc_loop_perf); /* get a local copy of the vehicle state */ - orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); /* get a local copy of manual setpoint */ orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual); /* get a local copy of attitude */ @@ -578,7 +578,7 @@ flow_position_control_thread_main(int argc, char *argv[]) close(parameter_update_sub); close(vehicle_attitude_sub); close(vehicle_local_position_sub); - close(safety_sub); + close(armed_sub); close(control_mode_sub); close(manual_control_setpoint_sub); close(speed_sp_pub); diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c index 557fdf37c4..5e80066a70 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_main.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c @@ -56,7 +56,7 @@ #include #include #include -#include +#include #include #include #include @@ -159,8 +159,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) static float sonar_lp = 0.0f; /* subscribe to vehicle status, attitude, sensors and flow*/ - struct actuator_safety_s safety; - memset(&safety, 0, sizeof(safety)); + struct actuator_armed_s armed; + memset(&armed, 0, sizeof(armed)); struct vehicle_control_mode_s control_mode; memset(&control_mode, 0, sizeof(control_mode)); struct vehicle_attitude_s att; @@ -173,8 +173,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) /* subscribe to parameter changes */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); - /* subscribe to safety topic */ - int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + /* subscribe to armed topic */ + int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); /* subscribe to safety topic */ int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); @@ -270,7 +270,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) /* got flow, updating attitude and status as well */ orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); orb_copy(ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub, &att_sp); - orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); /* vehicle state estimation */ @@ -284,12 +284,12 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) if (!vehicle_liftoff) { - if (safety.armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f) + if (armed.armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f) vehicle_liftoff = true; } else { - if (!safety.armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f)) + if (!armed.armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f)) vehicle_liftoff = false; } @@ -356,7 +356,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) } /* filtering ground distance */ - if (!vehicle_liftoff || !safety.armed) + if (!vehicle_liftoff || !armed.armed) { /* not possible to fly */ sonar_valid = false; @@ -453,7 +453,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) close(vehicle_attitude_setpoint_sub); close(vehicle_attitude_sub); - close(safety_sub); + close(armed_sub); close(control_mode_sub); close(parameter_update_sub); close(optical_flow_sub); diff --git a/src/examples/flow_speed_control/flow_speed_control_main.c b/src/examples/flow_speed_control/flow_speed_control_main.c index 8032a6264a..6af955cd7d 100644 --- a/src/examples/flow_speed_control/flow_speed_control_main.c +++ b/src/examples/flow_speed_control/flow_speed_control_main.c @@ -55,7 +55,7 @@ #include #include #include -#include +#include #include #include #include @@ -156,7 +156,7 @@ flow_speed_control_thread_main(int argc, char *argv[]) uint32_t counter = 0; /* structures */ - struct actuator_safety_s safety; + struct actuator_armed_s armed; struct vehicle_control_mode_s control_mode; struct filtered_bottom_flow_s filtered_flow; struct vehicle_bodyframe_speed_setpoint_s speed_sp; @@ -166,7 +166,7 @@ flow_speed_control_thread_main(int argc, char *argv[]) /* subscribe to attitude, motor setpoints and system state */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow)); int vehicle_bodyframe_speed_setpoint_sub = orb_subscribe(ORB_ID(vehicle_bodyframe_speed_setpoint)); @@ -229,8 +229,8 @@ flow_speed_control_thread_main(int argc, char *argv[]) { perf_begin(mc_loop_perf); - /* get a local copy of the safety topic */ - orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + /* get a local copy of the armed topic */ + orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); /* get a local copy of the control mode */ orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); /* get a local copy of filtered bottom flow */ @@ -355,7 +355,7 @@ flow_speed_control_thread_main(int argc, char *argv[]) close(vehicle_attitude_sub); close(vehicle_bodyframe_speed_setpoint_sub); close(filtered_bottom_flow_sub); - close(safety_sub); + close(armed_sub); close(control_mode_sub); close(att_sp_pub); diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index 2b0b3a415e..c4ee605ccf 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -76,9 +76,10 @@ #include #include #include -#include +#include #include #include +#include #include #include @@ -177,7 +178,7 @@ int led_off(int led); void do_reboot(void); -void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd, int safety_pub, struct actuator_safety_s *safety); +void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed); // int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state); @@ -253,7 +254,7 @@ void do_reboot() -void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd, int safety_pub, struct actuator_safety_s *safety) +void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed) { /* result of the command */ uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; @@ -273,13 +274,13 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } if ((int)cmd->param1 & VEHICLE_MODE_FLAG_SAFETY_ARMED) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, safety_pub, safety, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; } } else { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, safety_pub, safety, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -292,14 +293,14 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request to arm */ if ((int)cmd->param1 == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, safety_pub, safety, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; } /* request to disarm */ } else if ((int)cmd->param1 == 0) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, safety_pub, safety, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -312,7 +313,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request for an autopilot reboot */ if ((int)cmd->param1 == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_REBOOT, safety_pub, safety, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_REBOOT, armed_pub, armed, mavlink_fd)) { /* reboot is executed later, after positive tune is sent */ result = VEHICLE_CMD_RESULT_ACCEPTED; tune_positive(); @@ -361,7 +362,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION; @@ -380,7 +381,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION; @@ -400,7 +401,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta // if(low_prio_task == LOW_PRIO_TASK_NONE) { // /* try to go to INIT/PREFLIGHT arming state */ - // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { + // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { // result = VEHICLE_CMD_RESULT_ACCEPTED; // /* now set the task for the low prio thread */ // low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION; @@ -421,7 +422,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta // if(low_prio_task == LOW_PRIO_TASK_NONE) { // /* try to go to INIT/PREFLIGHT arming state */ - // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { + // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { // result = VEHICLE_CMD_RESULT_ACCEPTED; // /* now set the task for the low prio thread */ // low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION; @@ -441,7 +442,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION; @@ -460,7 +461,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION; @@ -734,12 +735,11 @@ int commander_thread_main(int argc, char *argv[]) /* make sure we are in preflight state */ memset(¤t_status, 0, sizeof(current_status)); - /* safety topic */ - struct actuator_safety_s safety; - orb_advert_t safety_pub; - /* Initialize safety with all false */ - memset(&safety, 0, sizeof(safety)); - + /* armed topic */ + struct actuator_armed_s armed; + orb_advert_t armed_pub; + /* Initialize armed with all false */ + memset(&armed, 0, sizeof(armed)); /* flags for control apps */ struct vehicle_control_mode_s control_mode; @@ -768,8 +768,8 @@ int commander_thread_main(int argc, char *argv[]) /* set battery warning flag */ current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; - /* set safety device detection flag */ + /* set safety device detection flag */ /* XXX do we need this? */ //current_status.flag_safety_present = false; @@ -789,10 +789,7 @@ int commander_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(vehicle_status), status_pub, ¤t_status); - safety_pub = orb_advertise(ORB_ID(actuator_safety), &safety); - - /* but also subscribe to it */ - int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); @@ -844,6 +841,11 @@ int commander_thread_main(int argc, char *argv[]) /* XXX what exactly is this for? */ uint64_t last_print_time = 0; + /* Subscribe to safety topic */ + int safety_sub = orb_subscribe(ORB_ID(safety)); + struct safety_s safety; + memset(&safety, 0, sizeof(safety)); + /* Subscribe to manual control data */ int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); struct manual_control_setpoint_s sp_man; @@ -917,7 +919,6 @@ int commander_thread_main(int argc, char *argv[]) /* XXX use this! */ //uint64_t failsave_ll_start_time = 0; - enum VEHICLE_MANUAL_SAS_MODE manual_sas_mode; bool state_changed = true; bool param_init_forced = true; @@ -936,7 +937,7 @@ int commander_thread_main(int argc, char *argv[]) /* update parameters */ - if (!safety.armed) { + if (!armed.armed) { if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { warnx("failed setting new system type"); } @@ -990,7 +991,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - handle_command(status_pub, ¤t_status, &cmd, safety_pub, &safety); + handle_command(status_pub, ¤t_status, &cmd, armed_pub, &armed); } @@ -1003,7 +1004,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); /* update parameters */ - if (!safety.armed) { + if (!armed.armed) { if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { warnx("failed setting new system type"); } @@ -1028,7 +1029,7 @@ int commander_thread_main(int argc, char *argv[]) orb_check(safety_sub, &new_data); if (new_data) { - orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + orb_copy(ORB_ID(safety), safety_sub, &safety); } /* update global position estimate */ @@ -1170,7 +1171,7 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (current_status.arming_state == ARMING_STATE_INIT) { // XXX check for sensors - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); } else { // XXX: Add emergency stuff if sensors are lost } @@ -1287,7 +1288,7 @@ int commander_thread_main(int argc, char *argv[]) && (vdop_m < vdop_threshold_m) // XXX note that vdop is 0 for mtk && !home_position_set && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) - && !safety.armed) { + && !armed.armed) { warnx("setting home position"); /* copy position data to uORB home message, store it locally as well */ @@ -1579,7 +1580,7 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "DISARM DENY, go manual mode first"); tune_negative(); } else { - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); tune_positive(); } @@ -1595,13 +1596,12 @@ int commander_thread_main(int argc, char *argv[]) } } - /* check if left stick is in lower right position --> arm */ - if (current_status.flag_control_manual_enabled && - current_status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS && + /* check if left stick is in lower right position and we're in manual --> arm */ + if (!control_mode.flag_control_position_enabled && !control_mode.flag_control_velocity_enabled && sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, safety_pub, &safety, mavlink_fd); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, armed_pub, &armed, mavlink_fd); stick_on_counter = 0; tune_positive(); @@ -1704,13 +1704,13 @@ int commander_thread_main(int argc, char *argv[]) // XXX check if this is correct /* arm / disarm on request */ - if (sp_offboard.armed && !safety.armed) { + if (sp_offboard.armed && !armed.armed) { - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, safety_pub, &safety, mavlink_fd); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, armed_pub, &armed, mavlink_fd); - } else if (!sp_offboard.armed && safety.armed) { + } else if (!sp_offboard.armed && armed.armed) { - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); } } else { @@ -1772,7 +1772,7 @@ int commander_thread_main(int argc, char *argv[]) /* play tone according to evaluation result */ /* check if we recently armed */ - if (!arm_tune_played && safety.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available))) { + if (!arm_tune_played && armed.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available))) { if (tune_arm() == OK) arm_tune_played = true; @@ -1789,7 +1789,7 @@ int commander_thread_main(int argc, char *argv[]) } /* reset arm_tune_played when disarmed */ - if (!(safety.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available)))) { + if (!(armed.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available)))) { arm_tune_played = false; } @@ -1812,6 +1812,7 @@ int commander_thread_main(int argc, char *argv[]) close(sp_offboard_sub); close(global_position_sub); close(sensor_sub); + close(safety_sub); close(cmd_sub); warnx("exiting"); diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index ea96d72a6a..b06e851691 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -43,7 +43,7 @@ #include #include -#include +#include #include diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index c15fc91a05..0b241d108d 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -57,7 +57,7 @@ #include "commander_helper.h" -int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int safety_pub, struct actuator_safety_s *safety, const int mavlink_fd) { +int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int armed_pub, struct actuator_armed_s *armed, const int mavlink_fd) { int ret = ERROR; @@ -73,8 +73,8 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* allow going back from INIT for calibration */ if (current_state->arming_state == ARMING_STATE_STANDBY) { ret = OK; - safety->armed = false; - safety->ready_to_arm = false; + armed->armed = false; + armed->ready_to_arm = false; } break; case ARMING_STATE_STANDBY: @@ -86,8 +86,8 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* sensors need to be initialized for STANDBY state */ if (current_state->condition_system_sensors_initialized) { ret = OK; - safety->armed = false; - safety->ready_to_arm = true; + armed->armed = false; + armed->ready_to_arm = true; } else { mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized"); } @@ -101,7 +101,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* XXX conditions for arming? */ ret = OK; - safety->armed = true; + armed->armed = true; } break; case ARMING_STATE_ARMED_ERROR: @@ -111,7 +111,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* XXX conditions for an error state? */ ret = OK; - safety->armed = true; + armed->armed = true; } break; case ARMING_STATE_STANDBY_ERROR: @@ -120,8 +120,8 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta || current_state->arming_state == ARMING_STATE_INIT || current_state->arming_state == ARMING_STATE_ARMED_ERROR) { ret = OK; - safety->armed = false; - safety->ready_to_arm = false; + armed->armed = false; + armed->ready_to_arm = false; } break; case ARMING_STATE_REBOOT: @@ -132,8 +132,8 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta || current_state->arming_state == ARMING_STATE_STANDBY_ERROR) { ret = OK; - safety->armed = false; - safety->ready_to_arm = false; + armed->armed = false; + armed->ready_to_arm = false; } break; @@ -151,8 +151,8 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta current_state->timestamp = hrt_absolute_time(); orb_publish(ORB_ID(vehicle_status), status_pub, current_state); - safety->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(actuator_safety), safety_pub, safety); + armed->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(actuator_armed), armed_pub, armed); } } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index b553a4b56a..e591d2fef9 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -46,11 +46,11 @@ #include #include -#include +#include #include -int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int safety_pub, struct actuator_safety_s *safety, const int mavlink_fd); +int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int armed_pub, struct actuator_armed_s *armed, const int mavlink_fd); int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, int control_mode_pub, struct vehicle_control_mode_s *control_mode, const int mavlink_fd); diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 97b585cb77..6eb4257058 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -51,7 +51,7 @@ #include #include #include -#include +#include #include #include #include @@ -63,8 +63,8 @@ struct gpio_led_s { int pin; struct vehicle_status_s status; int vehicle_status_sub; - struct actuator_safety_s safety; - int actuator_safety_sub; + struct actuator_armed_s armed; + int actuator_armed_sub; bool led_state; int counter; }; @@ -233,12 +233,12 @@ void gpio_led_cycle(FAR void *arg) orb_check(priv->vehicle_status_sub, &status_updated); if (status_updated) - orb_copy(ORB_ID(actuator_safety), priv->actuator_safety_sub, &priv->safety); + orb_copy(ORB_ID(actuator_armed), priv->actuator_armed_sub, &priv->armed); /* select pattern for current status */ int pattern = 0; - if (priv->safety.armed) { + if (priv->armed.armed) { if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { pattern = 0x3f; // ****** solid (armed) @@ -247,10 +247,10 @@ void gpio_led_cycle(FAR void *arg) } } else { - if (priv->safety.ready_to_arm) { + if (priv->armed.ready_to_arm) { pattern = 0x00; // ______ off (disarmed, preflight check) - } else if (priv->safety.ready_to_arm && priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { + } else if (priv->armed.ready_to_arm && priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { pattern = 0x38; // ***___ slow blink (disarmed, ready) } else { diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 95bc8fdc0d..57a5d5dd50 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -70,7 +70,7 @@ struct vehicle_local_position_s local_pos; struct vehicle_status_s v_status; struct rc_channels_s rc; struct rc_input_values rc_raw; -struct actuator_safety_s safety; +struct actuator_armed_s armed; struct actuator_controls_effective_s actuators_0; struct vehicle_attitude_s att; @@ -110,7 +110,7 @@ static void l_global_position_setpoint(const struct listener *l); static void l_local_position_setpoint(const struct listener *l); static void l_attitude_setpoint(const struct listener *l); static void l_actuator_outputs(const struct listener *l); -static void l_actuator_safety(const struct listener *l); +static void l_actuator_armed(const struct listener *l); static void l_manual_control_setpoint(const struct listener *l); static void l_vehicle_attitude_controls(const struct listener *l); static void l_debug_key_value(const struct listener *l); @@ -135,7 +135,7 @@ static const struct listener listeners[] = { {l_actuator_outputs, &mavlink_subs.act_1_sub, 1}, {l_actuator_outputs, &mavlink_subs.act_2_sub, 2}, {l_actuator_outputs, &mavlink_subs.act_3_sub, 3}, - {l_actuator_safety, &mavlink_subs.safety_sub, 0}, + {l_actuator_armed, &mavlink_subs.armed_sub, 0}, {l_manual_control_setpoint, &mavlink_subs.man_control_sp_sub, 0}, {l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0}, {l_debug_key_value, &mavlink_subs.debug_key_value, 0}, @@ -269,7 +269,7 @@ l_vehicle_status(const struct listener *l) { /* immediately communicate state changes back to user */ orb_copy(ORB_ID(vehicle_status), status_sub, &v_status); - orb_copy(ORB_ID(actuator_safety), mavlink_subs.safety_sub, &safety); + orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); /* enable or disable HIL */ set_hil_on_off(v_status.flag_hil_enabled); @@ -466,7 +466,7 @@ l_actuator_outputs(const struct listener *l) act_outputs.output[7]); /* only send in HIL mode */ - if (mavlink_hil_enabled && safety.armed) { + if (mavlink_hil_enabled && armed.armed) { /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; @@ -538,9 +538,9 @@ l_actuator_outputs(const struct listener *l) } void -l_actuator_safety(const struct listener *l) +l_actuator_armed(const struct listener *l) { - orb_copy(ORB_ID(actuator_safety), mavlink_subs.safety_sub, &safety); + orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); } void @@ -759,8 +759,8 @@ uorb_receive_start(void) orb_set_interval(mavlink_subs.act_3_sub, 100); /* 10Hz updates */ /* --- ACTUATOR ARMED VALUE --- */ - mavlink_subs.safety_sub = orb_subscribe(ORB_ID(actuator_safety)); - orb_set_interval(mavlink_subs.safety_sub, 100); /* 10Hz updates */ + mavlink_subs.armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(mavlink_subs.armed_sub, 100); /* 10Hz updates */ /* --- MAPPED MANUAL CONTROL INPUTS --- */ mavlink_subs.man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h index 3b968b9110..506f73105d 100644 --- a/src/modules/mavlink/orb_topics.h +++ b/src/modules/mavlink/orb_topics.h @@ -60,7 +60,7 @@ #include #include #include -#include +#include #include #include #include @@ -79,6 +79,7 @@ struct mavlink_subscriptions { int man_control_sp_sub; int safety_sub; int actuators_sub; + int armed_sub; int local_pos_sub; int spa_sub; int spl_sub; diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c index 2d5a64ee87..9ed2c6c12e 100644 --- a/src/modules/mavlink_onboard/mavlink.c +++ b/src/modules/mavlink_onboard/mavlink.c @@ -273,7 +273,7 @@ void mavlink_update_system(void) } void -get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_safety_s *safety, +get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_armed_s *armed, uint8_t *mavlink_state, uint8_t *mavlink_mode) { /* reset MAVLink mode bitfield */ @@ -284,12 +284,12 @@ get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, co *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; } - if (safety->hil_enabled) { + if (control_mode->flag_system_hil_enabled) { *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; } /* set arming state */ - if (safety->armed) { + if (armed->armed) { *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } else { *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; @@ -370,7 +370,7 @@ int mavlink_thread_main(int argc, char *argv[]) /* XXX this is never written? */ struct vehicle_status_s v_status; struct vehicle_control_mode_s control_mode; - struct actuator_safety_s safety; + struct actuator_armed_s armed; /* work around some stupidity in task_create's argv handling */ argc -= 2; @@ -438,7 +438,7 @@ int mavlink_thread_main(int argc, char *argv[]) /* translate the current system state to mavlink state and mode */ uint8_t mavlink_state = 0; uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&control_mode, &safety, &mavlink_state, &mavlink_mode); + get_mavlink_mode_and_state(&control_mode, &armed, &mavlink_state, &mavlink_mode); /* send heartbeat */ mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state); diff --git a/src/modules/mavlink_onboard/orb_topics.h b/src/modules/mavlink_onboard/orb_topics.h index f01f09e12d..1b49c9ce4e 100644 --- a/src/modules/mavlink_onboard/orb_topics.h +++ b/src/modules/mavlink_onboard/orb_topics.h @@ -56,7 +56,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/src/modules/mavlink_onboard/util.h b/src/modules/mavlink_onboard/util.h index c6a2e52bf0..c84b6fd26e 100644 --- a/src/modules/mavlink_onboard/util.h +++ b/src/modules/mavlink_onboard/util.h @@ -51,5 +51,5 @@ extern volatile bool thread_should_exit; * Translate the custom state into standard mavlink modes and state. */ extern void -get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_safety_s *safety, +get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_armed_s *armed, uint8_t *mavlink_state, uint8_t *mavlink_mode); diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 70a61fc4ec..20502c0eae 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -58,7 +58,7 @@ #include #include #include -#include +#include #include #include #include @@ -94,8 +94,8 @@ mc_thread_main(int argc, char *argv[]) /* declare and safely initialize all structs */ struct vehicle_control_mode_s control_mode; memset(&control_mode, 0, sizeof(control_mode)); - struct actuator_safety_s safety; - memset(&safety, 0, sizeof(safety)); + struct actuator_armed_s armed; + memset(&armed, 0, sizeof(armed)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_attitude_setpoint_s att_sp; @@ -121,7 +121,7 @@ mc_thread_main(int argc, char *argv[]) int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); @@ -210,10 +210,10 @@ mc_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); } - orb_check(safety_sub, &updated); + orb_check(armed_sub, &updated); if (updated) { - orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); } /* get a local copy of manual setpoint */ @@ -261,7 +261,7 @@ mc_thread_main(int argc, char *argv[]) /* initialize to current yaw if switching to manual or att control */ if (control_mode.flag_control_attitude_enabled != flag_control_attitude_enabled || control_mode.flag_control_manual_enabled != flag_control_manual_enabled || - safety.armed != flag_armed) { + armed.armed != flag_armed) { att_sp.yaw_body = att.yaw; } @@ -275,7 +275,7 @@ mc_thread_main(int argc, char *argv[]) att_sp.pitch_body = manual.pitch; /* set attitude if arming */ - if (!flag_control_attitude_enabled && safety.armed) { + if (!flag_control_attitude_enabled && armed.armed) { att_sp.yaw_body = att.yaw; } @@ -366,7 +366,7 @@ mc_thread_main(int argc, char *argv[]) flag_control_manual_enabled = control_mode.flag_control_manual_enabled; flag_control_position_enabled = control_mode.flag_control_position_enabled; flag_control_velocity_enabled = control_mode.flag_control_velocity_enabled; - flag_armed = safety.armed; + flag_armed = armed.armed; perf_end(mc_loop_perf); } /* end of poll call for attitude updates */ diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 80ee3adeef..b20d38b5ee 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -60,7 +60,7 @@ #include #include -#include +#include #include #include #include @@ -194,7 +194,7 @@ static int file_copy(const char *file_old, const char *file_new); static void handle_command(struct vehicle_command_s *cmd); -static void handle_status(struct actuator_safety_s *safety); +static void handle_status(struct actuator_armed_s *armed); /** * Create folder for current logging session. Store folder name in 'log_folder'. @@ -628,8 +628,8 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_status_s buf_status; memset(&buf_status, 0, sizeof(buf_status)); - struct actuator_safety_s buf_safety; - memset(&buf_safety, 0, sizeof(buf_safety)); + struct actuator_armed_s buf_armed; + memset(&buf_armed, 0, sizeof(buf_armed)); /* warning! using union here to save memory, elements should be used separately! */ union { @@ -659,7 +659,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct { int cmd_sub; int status_sub; - int safety_sub; + int armed_sub; int sensor_sub; int att_sub; int att_sp_sub; @@ -717,9 +717,9 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; - /* --- SAFETY --- */ - subs.safety_sub = orb_subscribe(ORB_ID(actuator_safety)); - fds[fdsc_count].fd = subs.safety_sub; + /* --- ACTUATOR ARMED --- */ + subs.armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + fds[fdsc_count].fd = subs.armed_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; @@ -896,12 +896,12 @@ int sdlog2_thread_main(int argc, char *argv[]) handled_topics++; } - /* --- SAFETY- LOG MANAGEMENT --- */ + /* --- ARMED- LOG MANAGEMENT --- */ if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(actuator_safety), subs.safety_sub, &buf_safety); + orb_copy(ORB_ID(actuator_armed), subs.armed_sub, &buf_armed); if (log_when_armed) { - handle_status(&buf_safety); + handle_status(&buf_armed); } handled_topics++; @@ -912,7 +912,7 @@ int sdlog2_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf_status); //if (log_when_armed) { - // handle_status(&buf_safety); + // handle_status(&buf_armed); //} //handled_topics++; @@ -944,7 +944,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_STAT.flight_mode = 0; log_msg.body.log_STAT.manual_control_mode = 0; log_msg.body.log_STAT.manual_sas_mode = 0; - log_msg.body.log_STAT.armed = (unsigned char) buf_safety.armed; /* XXX fmu armed correct? */ + log_msg.body.log_STAT.armed = (unsigned char) buf_armed.armed; /* XXX fmu armed correct? */ log_msg.body.log_STAT.battery_voltage = buf_status.voltage_battery; log_msg.body.log_STAT.battery_current = buf_status.current_battery; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; @@ -1348,10 +1348,10 @@ void handle_command(struct vehicle_command_s *cmd) } } -void handle_status(struct actuator_safety_s *safety) +void handle_status(struct actuator_armed_s *armed) { - if (safety->armed != flag_system_armed) { - flag_system_armed = safety->armed; + if (armed->armed != flag_system_armed) { + flag_system_armed = armed->armed; if (flag_system_armed) { sdlog2_start_log(); diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index a061fe676f..ed77bb7eff 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -77,6 +77,9 @@ ORB_DEFINE(home_position, struct home_position_s); #include "topics/vehicle_status.h" ORB_DEFINE(vehicle_status, struct vehicle_status_s); +#include "topics/safety.h" +ORB_DEFINE(safety, struct safety_s); + #include "topics/battery_status.h" ORB_DEFINE(battery_status, struct battery_status_s); @@ -153,8 +156,8 @@ ORB_DEFINE(actuator_controls_1, struct actuator_controls_s); ORB_DEFINE(actuator_controls_2, struct actuator_controls_s); ORB_DEFINE(actuator_controls_3, struct actuator_controls_s); -#include "topics/actuator_safety.h" -ORB_DEFINE(actuator_safety, struct actuator_safety_s); +#include "topics/actuator_armed.h" +ORB_DEFINE(actuator_armed, struct actuator_armed_s); /* actuator controls, as set by actuators / mixers after limiting */ #include "topics/actuator_controls_effective.h" diff --git a/src/modules/uORB/topics/actuator_safety.h b/src/modules/uORB/topics/actuator_armed.h similarity index 73% rename from src/modules/uORB/topics/actuator_safety.h rename to src/modules/uORB/topics/actuator_armed.h index c431217ab1..6e944ffee3 100644 --- a/src/modules/uORB/topics/actuator_safety.h +++ b/src/modules/uORB/topics/actuator_armed.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,35 +32,27 @@ ****************************************************************************/ /** - * @file actuator_controls.h + * @file actuator_armed.h * - * Actuator control topics - mixer inputs. + * Actuator armed topic * - * Values published to these topics are the outputs of the vehicle control - * system, and are expected to be mixed and used to drive the actuators - * (servos, speed controls, etc.) that operate the vehicle. - * - * Each topic can be published by a single controller */ -#ifndef TOPIC_ACTUATOR_SAFETY_H -#define TOPIC_ACTUATOR_SAFETY_H +#ifndef TOPIC_ACTUATOR_ARMED_H +#define TOPIC_ACTUATOR_ARMED_H #include #include "../uORB.h" /** global 'actuator output is live' control. */ -struct actuator_safety_s { +struct actuator_armed_s { uint64_t timestamp; - bool safety_switch_available; /**< Set to true if a safety switch is connected */ - bool safety_off; /**< Set to true if safety is off */ bool armed; /**< Set to true if system is armed */ bool ready_to_arm; /**< Set to true if system is ready to be armed */ bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ - bool hil_enabled; /**< Set to true if hardware-in-the-loop (HIL) is enabled */ }; -ORB_DECLARE(actuator_safety); +ORB_DECLARE(actuator_armed); #endif \ No newline at end of file diff --git a/src/modules/uORB/topics/safety.h b/src/modules/uORB/topics/safety.h new file mode 100644 index 0000000000..a5d21cd4ad --- /dev/null +++ b/src/modules/uORB/topics/safety.h @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file safety.h + * + * Safety topic to pass safety state from px4io driver to commander + * This concerns only the safety button of the px4io but has nothing to do + * with arming/disarming. + */ + +#ifndef TOPIC_SAFETY_H +#define TOPIC_SAFETY_H + +#include +#include "../uORB.h" + +struct safety_s { + + uint64_t timestamp; + bool safety_switch_available; /**< Set to true if a safety switch is connected */ + bool safety_off; /**< Set to true if safety is off */ +}; + +ORB_DECLARE(safety); + +#endif \ No newline at end of file diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 177e30898b..02bf5568ab 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -69,9 +69,13 @@ struct vehicle_control_mode_s bool flag_system_emergency; bool flag_preflight_calibration; + // XXX needs yet to be set by state machine helper + bool flag_system_hil_enabled; + bool flag_control_manual_enabled; /**< true if manual input is mixed in */ bool flag_control_offboard_enabled; /**< true if offboard control input is on */ - bool flag_auto_enabled; + // XXX shouldn't be necessairy + //bool flag_auto_enabled; bool flag_control_rates_enabled; /**< true if rates are stabilized */ bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ From ce8e2fd72668d64e3a56d3b1df5d7f9079fcdf55 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 16 Jul 2013 09:00:21 +0200 Subject: [PATCH 257/486] Fixed compile error due to bad merge --- src/drivers/px4io/px4io.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index e9f26b0d3f..8f8555f1f3 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1510,7 +1510,7 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) uint32_t bits = (1 << _max_relays) - 1; /* don't touch relay1 if it's controlling RX vcc */ if (_dsm_vcc_ctl) - bits &= ~PX4IO_RELAY1; + bits &= ~PX4IO_P_SETUP_RELAYS_POWER1; ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, bits, 0); break; } @@ -1518,7 +1518,7 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) case GPIO_SET: arg &= ((1 << _max_relays) - 1); /* don't touch relay1 if it's controlling RX vcc */ - if (_dsm_vcc_ctl & (arg & PX4IO_RELAY1)) + if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1)) ret = -EINVAL; else ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg); @@ -1527,7 +1527,7 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) case GPIO_CLEAR: arg &= ((1 << _max_relays) - 1); /* don't touch relay1 if it's controlling RX vcc */ - if (_dsm_vcc_ctl & (arg & PX4IO_RELAY1)) + if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1)) ret = -EINVAL; else ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0); From 2f76c811474aa34dd11973df491283278234957a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 16 Jul 2013 09:08:05 +0200 Subject: [PATCH 258/486] More compile fixes --- .gitignore | 8 +++++++- makefiles/config_px4fmu-v2_default.mk | 3 ++- src/modules/gpio_led/gpio_led.c | 10 +++++----- src/modules/px4iofirmware/dsm.c | 5 +++++ 4 files changed, 19 insertions(+), 7 deletions(-) diff --git a/.gitignore b/.gitignore index 0372b60c80..5a4f7683cf 100644 --- a/.gitignore +++ b/.gitignore @@ -24,4 +24,10 @@ Firmware.sublime-workspace Images/*.bin Images/*.px4 mavlink/include/mavlink/v0.9/ -NuttX \ No newline at end of file +NuttX +nuttx-configs/px4io-v2/src/.depend +nuttx-configs/px4io-v2/src/Make.dep +nuttx-configs/px4io-v2/src/libboard.a +nuttx-configs/px4io-v1/src/.depend +nuttx-configs/px4io-v1/src/Make.dep +nuttx-configs/px4io-v1/src/libboard.a \ No newline at end of file diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index c4499048c1..8e64bd754b 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -26,7 +26,8 @@ MODULES += drivers/ms5611 MODULES += drivers/mb12xx MODULES += drivers/gps MODULES += drivers/hil -MODULES += drivers/hott_telemetry +MODULES += drivers/hott/hott_telemetry +MODULES += drivers/hott/hott_sensors MODULES += drivers/blinkm MODULES += modules/sensors diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 1aef739c7b..7466dfdb92 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -109,19 +109,19 @@ int gpio_led_main(int argc, char *argv[]) } else if (!strcmp(argv[3], "a1")) { use_io = true; - pin = PX4IO_ACC1; + pin = PX4IO_P_SETUP_RELAYS_ACC1; } else if (!strcmp(argv[3], "a2")) { use_io = true; - pin = PX4IO_ACC2; + pin = PX4IO_P_SETUP_RELAYS_ACC2; } else if (!strcmp(argv[3], "r1")) { use_io = true; - pin = PX4IO_RELAY1; + pin = PX4IO_P_SETUP_RELAYS_POWER1; } else if (!strcmp(argv[3], "r2")) { use_io = true; - pin = PX4IO_RELAY2; + pin = PX4IO_P_SETUP_RELAYS_POWER2; } else { errx(1, "unsupported pin: %s", argv[3]); @@ -142,7 +142,7 @@ int gpio_led_main(int argc, char *argv[]) char pin_name[24]; if (use_io) { - if (pin & (PX4IO_ACC1 | PX4IO_ACC2)) { + if (pin & (PX4IO_P_SETUP_RELAYS_ACC1 | PX4IO_P_SETUP_RELAYS_ACC2)) { sprintf(pin_name, "PX4IO ACC%i", (pin >> 3)); } else { diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index ab6e3fec43..598bcee342 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -110,6 +110,10 @@ dsm_bind(uint16_t cmd, int pulses) if (dsm_fd < 0) return; +#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 + // XXX implement + #warning DSM BIND NOT IMPLEMENTED ON PX4IO V2 +#else switch (cmd) { case dsm_bind_power_down: // power down DSM satellite @@ -135,6 +139,7 @@ dsm_bind(uint16_t cmd, int pulses) stm32_configgpio(GPIO_USART1_RX); break; } +#endif } bool From 3e161049ac4e953f8c0084b1872b544de6189f5d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 16 Jul 2013 09:24:21 +0200 Subject: [PATCH 259/486] Got rid of useless orb_receive_loop, moved some helper functions --- src/modules/commander/commander.c | 196 ++++++----------------- src/modules/commander/commander_helper.c | 46 ++++++ src/modules/commander/commander_helper.h | 7 +- 3 files changed, 103 insertions(+), 146 deletions(-) diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index c4ee605ccf..a74c9ebe93 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -132,8 +132,7 @@ extern struct system_load_s system_load; #define LOCAL_POSITION_TIMEOUT 1000000 /**< consider the local position estimate invalid after 1s */ -/* File descriptors */ -static int leds; +/* Mavlink file descriptors */ static int mavlink_fd; /* flags */ @@ -159,8 +158,9 @@ enum { } low_prio_task; -/* pthread loops */ -void *orb_receive_loop(void *arg); +/** + * Loop that runs at a lower rate and priority for calibration and parameter tasks. + */ void *commander_low_prio_loop(void *arg); __EXPORT int commander_main(int argc, char *argv[]); @@ -170,88 +170,16 @@ __EXPORT int commander_main(int argc, char *argv[]); */ int commander_thread_main(int argc, char *argv[]); -int led_init(void); -void led_deinit(void); -int led_toggle(int led); -int led_on(int led); -int led_off(int led); -void do_reboot(void); - - +/** + * React to commands that are sent e.g. from the mavlink module. + */ void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed); -// int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state); - - - /** * Print the correct usage. */ void usage(const char *reason); -/** - * Sort calibration values. - * - * Sorts the calibration values with bubble sort. - * - * @param a The array to sort - * @param n The number of entries in the array - */ -// static void cal_bsort(float a[], int n); - - -int led_init() -{ - leds = open(LED_DEVICE_PATH, 0); - - if (leds < 0) { - warnx("LED: open fail\n"); - return ERROR; - } - - if (ioctl(leds, LED_ON, LED_BLUE) || ioctl(leds, LED_ON, LED_AMBER)) { - warnx("LED: ioctl fail\n"); - return ERROR; - } - - return 0; -} - -void led_deinit() -{ - close(leds); -} - -int led_toggle(int led) -{ - static int last_blue = LED_ON; - static int last_amber = LED_ON; - - if (led == LED_BLUE) last_blue = (last_blue == LED_ON) ? LED_OFF : LED_ON; - - if (led == LED_AMBER) last_amber = (last_amber == LED_ON) ? LED_OFF : LED_ON; - - return ioctl(leds, ((led == LED_BLUE) ? last_blue : last_amber), led); -} - -int led_on(int led) -{ - return ioctl(leds, LED_ON, led); -} - -int led_off(int led) -{ - return ioctl(leds, LED_OFF, led); -} - -void do_reboot() -{ - mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); - usleep(500000); - up_systemreset(); - /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ -} - void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed) @@ -317,8 +245,10 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* reboot is executed later, after positive tune is sent */ result = VEHICLE_CMD_RESULT_ACCEPTED; tune_positive(); - /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ - do_reboot(); + mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); + usleep(500000); + up_systemreset(); + /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ } else { /* system may return here */ @@ -526,59 +456,6 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } -void *orb_receive_loop(void *arg) //handles status information coming from subsystems (present, enabled, health), these values do not indicate the quality (variance) of the signal -{ - /* Set thread name */ - prctl(PR_SET_NAME, "commander orb rcv", getpid()); - - /* Subscribe to command topic */ - int subsys_sub = orb_subscribe(ORB_ID(subsystem_info)); - struct subsystem_info_s info; - - struct vehicle_status_s *vstatus = (struct vehicle_status_s *)arg; - - while (!thread_should_exit) { - struct pollfd fds[1] = { { .fd = subsys_sub, .events = POLLIN } }; - - if (poll(fds, 1, 5000) == 0) { - /* timeout, but this is no problem, silently ignore */ - } else { - /* got command */ - orb_copy(ORB_ID(subsystem_info), subsys_sub, &info); - - warnx("Subsys changed: %d\n", (int)info.subsystem_type); - - /* mark / unmark as present */ - if (info.present) { - vstatus->onboard_control_sensors_present |= info.subsystem_type; - - } else { - vstatus->onboard_control_sensors_present &= ~info.subsystem_type; - } - - /* mark / unmark as enabled */ - if (info.enabled) { - vstatus->onboard_control_sensors_enabled |= info.subsystem_type; - - } else { - vstatus->onboard_control_sensors_enabled &= ~info.subsystem_type; - } - - /* mark / unmark as ok */ - if (info.ok) { - vstatus->onboard_control_sensors_health |= info.subsystem_type; - - } else { - vstatus->onboard_control_sensors_health &= ~info.subsystem_type; - } - } - } - - close(subsys_sub); - - return NULL; -} - /* * Provides a coarse estimate of remaining battery power. * @@ -709,9 +586,7 @@ int commander_thread_main(int argc, char *argv[]) /* welcome user */ warnx("[commander] starting"); - /* pthreads for command and subsystem info handling */ - // pthread_t command_handling_thread; - pthread_t subsystem_info_thread; + /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; /* initialize */ @@ -807,12 +682,6 @@ int commander_thread_main(int argc, char *argv[]) // XXX needed? mavlink_log_info(mavlink_fd, "system is running"); - /* create pthreads */ - pthread_attr_t subsystem_info_attr; - pthread_attr_init(&subsystem_info_attr); - pthread_attr_setstacksize(&subsystem_info_attr, 2048); - pthread_create(&subsystem_info_thread, &subsystem_info_attr, orb_receive_loop, ¤t_status); - pthread_attr_t commander_low_prio_attr; pthread_attr_init(&commander_low_prio_attr); pthread_attr_setstacksize(&commander_low_prio_attr, 2048); @@ -905,6 +774,11 @@ int commander_thread_main(int argc, char *argv[]) memset(&battery, 0, sizeof(battery)); battery.voltage_v = 0.0f; + /* Subscribe to subsystem info topic */ + int subsys_sub = orb_subscribe(ORB_ID(subsystem_info)); + struct subsystem_info_s info; + memset(&info, 0, sizeof(info)); + // uint8_t vehicle_state_previous = current_status.state_machine; float voltage_previous = 0.0f; @@ -1073,6 +947,39 @@ int commander_thread_main(int argc, char *argv[]) } } + /* update subsystem */ + orb_check(subsys_sub, &new_data); + + if (new_data) { + orb_copy(ORB_ID(subsystem_info), subsys_sub, &info); + + warnx("Subsys changed: %d\n", (int)info.subsystem_type); + + /* mark / unmark as present */ + if (info.present) { + current_status.onboard_control_sensors_present |= info.subsystem_type; + + } else { + current_status.onboard_control_sensors_present &= ~info.subsystem_type; + } + + /* mark / unmark as enabled */ + if (info.enabled) { + current_status.onboard_control_sensors_enabled |= info.subsystem_type; + + } else { + current_status.onboard_control_sensors_enabled &= ~info.subsystem_type; + } + + /* mark / unmark as ok */ + if (info.ok) { + current_status.onboard_control_sensors_health |= info.subsystem_type; + + } else { + current_status.onboard_control_sensors_health &= ~info.subsystem_type; + } + } + /* Slow but important 8 Hz checks */ if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) { @@ -1801,8 +1708,6 @@ int commander_thread_main(int argc, char *argv[]) } /* wait for threads to complete */ - // pthread_join(command_handling_thread, NULL); - pthread_join(subsystem_info_thread, NULL); pthread_join(commander_low_prio_thread, NULL); /* close fds */ @@ -1814,6 +1719,7 @@ int commander_thread_main(int argc, char *argv[]) close(sensor_sub); close(safety_sub); close(cmd_sub); + close(subsys_sub); warnx("exiting"); fflush(stdout); diff --git a/src/modules/commander/commander_helper.c b/src/modules/commander/commander_helper.c index a75b5dec3c..fb5c478850 100644 --- a/src/modules/commander/commander_helper.c +++ b/src/modules/commander/commander_helper.c @@ -51,6 +51,7 @@ #include #include #include +#include #include "commander_helper.h" @@ -127,3 +128,48 @@ void tune_stop() ioctl(buzzer, TONE_SET_ALARM, 0); } +static int leds; + +int led_init() +{ + leds = open(LED_DEVICE_PATH, 0); + + if (leds < 0) { + warnx("LED: open fail\n"); + return ERROR; + } + + if (ioctl(leds, LED_ON, LED_BLUE) || ioctl(leds, LED_ON, LED_AMBER)) { + warnx("LED: ioctl fail\n"); + return ERROR; + } + + return 0; +} + +void led_deinit() +{ + close(leds); +} + +int led_toggle(int led) +{ + static int last_blue = LED_ON; + static int last_amber = LED_ON; + + if (led == LED_BLUE) last_blue = (last_blue == LED_ON) ? LED_OFF : LED_ON; + + if (led == LED_AMBER) last_amber = (last_amber == LED_ON) ? LED_OFF : LED_ON; + + return ioctl(leds, ((led == LED_BLUE) ? last_blue : last_amber), led); +} + +int led_on(int led) +{ + return ioctl(leds, LED_ON, led); +} + +int led_off(int led) +{ + return ioctl(leds, LED_OFF, led); +} \ No newline at end of file diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index b06e851691..71a257c86d 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -60,7 +60,12 @@ void tune_negative(void); int tune_arm(void); int tune_critical_bat(void); int tune_low_bat(void); - void tune_stop(void); +int led_init(void); +void led_deinit(void); +int led_toggle(int led); +int led_on(int led); +int led_off(int led); + #endif /* COMMANDER_HELPER_H_ */ From 08926019ea4203760a225e957d27328862182ce1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 16 Jul 2013 09:35:31 +0200 Subject: [PATCH 260/486] Just some reordering in commander --- src/modules/commander/commander.c | 183 ++++++++--------------- src/modules/commander/commander_helper.c | 43 ++++++ src/modules/commander/commander_helper.h | 9 ++ 3 files changed, 118 insertions(+), 117 deletions(-) diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index a74c9ebe93..d7bf6ac57f 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -159,16 +159,19 @@ enum { /** - * Loop that runs at a lower rate and priority for calibration and parameter tasks. + * The daemon app only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). */ -void *commander_low_prio_loop(void *arg); - __EXPORT int commander_main(int argc, char *argv[]); /** - * Mainloop of commander. + * Print the correct usage. */ -int commander_thread_main(int argc, char *argv[]); +void usage(const char *reason); /** * React to commands that are sent e.g. from the mavlink module. @@ -176,11 +179,67 @@ int commander_thread_main(int argc, char *argv[]); void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed); /** - * Print the correct usage. + * Mainloop of commander. */ -void usage(const char *reason); +int commander_thread_main(int argc, char *argv[]); + +/** + * Loop that runs at a lower rate and priority for calibration and parameter tasks. + */ +void *commander_low_prio_loop(void *arg); +int commander_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + warnx("commander already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + daemon_task = task_spawn_cmd("commander", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 40, + 3000, + commander_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + warnx("\tcommander is running\n"); + + } else { + warnx("\tcommander not started\n"); + } + + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +void usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + + fprintf(stderr, "usage: daemon {start|stop|status} [-p ]\n\n"); + exit(1); +} void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed) { @@ -456,116 +515,6 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } -/* - * Provides a coarse estimate of remaining battery power. - * - * The estimate is very basic and based on decharging voltage curves. - * - * @return the estimated remaining capacity in 0..1 - */ -float battery_remaining_estimate_voltage(float voltage); - -PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f); -PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f); -PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3); - -float battery_remaining_estimate_voltage(float voltage) -{ - float ret = 0; - static param_t bat_volt_empty; - static param_t bat_volt_full; - static param_t bat_n_cells; - static bool initialized = false; - static unsigned int counter = 0; - static float ncells = 3; - // XXX change cells to int (and param to INT32) - - if (!initialized) { - bat_volt_empty = param_find("BAT_V_EMPTY"); - bat_volt_full = param_find("BAT_V_FULL"); - bat_n_cells = param_find("BAT_N_CELLS"); - initialized = true; - } - - static float chemistry_voltage_empty = 3.2f; - static float chemistry_voltage_full = 4.05f; - - if (counter % 100 == 0) { - param_get(bat_volt_empty, &chemistry_voltage_empty); - param_get(bat_volt_full, &chemistry_voltage_full); - param_get(bat_n_cells, &ncells); - } - - counter++; - - ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty)); - - /* limit to sane values */ - ret = (ret < 0) ? 0 : ret; - ret = (ret > 1) ? 1 : ret; - return ret; -} - -void usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - - fprintf(stderr, "usage: daemon {start|stop|status} [-p ]\n\n"); - exit(1); -} - -/** - * The daemon app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ -int commander_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - warnx("commander already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - daemon_task = task_spawn_cmd("commander", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 40, - 3000, - commander_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - warnx("\tcommander is running\n"); - - } else { - warnx("\tcommander not started\n"); - } - - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - int commander_thread_main(int argc, char *argv[]) { /* not yet initialized */ diff --git a/src/modules/commander/commander_helper.c b/src/modules/commander/commander_helper.c index fb5c478850..199f73e6c8 100644 --- a/src/modules/commander/commander_helper.c +++ b/src/modules/commander/commander_helper.c @@ -49,6 +49,7 @@ #include #include #include +#include #include #include #include @@ -172,4 +173,46 @@ int led_on(int led) int led_off(int led) { return ioctl(leds, LED_OFF, led); +} + + +PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f); +PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f); +PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3); + +float battery_remaining_estimate_voltage(float voltage) +{ + float ret = 0; + static param_t bat_volt_empty; + static param_t bat_volt_full; + static param_t bat_n_cells; + static bool initialized = false; + static unsigned int counter = 0; + static float ncells = 3; + // XXX change cells to int (and param to INT32) + + if (!initialized) { + bat_volt_empty = param_find("BAT_V_EMPTY"); + bat_volt_full = param_find("BAT_V_FULL"); + bat_n_cells = param_find("BAT_N_CELLS"); + initialized = true; + } + + static float chemistry_voltage_empty = 3.2f; + static float chemistry_voltage_full = 4.05f; + + if (counter % 100 == 0) { + param_get(bat_volt_empty, &chemistry_voltage_empty); + param_get(bat_volt_full, &chemistry_voltage_full); + param_get(bat_n_cells, &ncells); + } + + counter++; + + ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty)); + + /* limit to sane values */ + ret = (ret < 0) ? 0 : ret; + ret = (ret > 1) ? 1 : ret; + return ret; } \ No newline at end of file diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index 71a257c86d..c621b98232 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -68,4 +68,13 @@ int led_toggle(int led); int led_on(int led); int led_off(int led); +/** + * Provides a coarse estimate of remaining battery power. + * + * The estimate is very basic and based on decharging voltage curves. + * + * @return the estimated remaining capacity in 0..1 + */ +float battery_remaining_estimate_voltage(float voltage); + #endif /* COMMANDER_HELPER_H_ */ From 6dc3fcd1ad108bc830231c1da50982e18eb7f799 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 16 Jul 2013 10:05:51 +0200 Subject: [PATCH 261/486] Some more commander cleanup, param update handling code was doubled --- src/modules/commander/commander.c | 79 ++++++++++--------------------- 1 file changed, 24 insertions(+), 55 deletions(-) diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index d7bf6ac57f..b457d98a18 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -39,11 +39,6 @@ * @file commander.c * Main system state machine implementation. * - * @author Petri Tanskanen - * @author Lorenz Meier - * @author Thomas Gubler - * @author Julian Oes - * */ #include "commander.h" @@ -656,9 +651,23 @@ int commander_thread_main(int argc, char *argv[]) uint16_t stick_off_counter = 0; uint16_t stick_on_counter = 0; - /* XXX what exactly is this for? */ + /* To remember when last notification was sent */ uint64_t last_print_time = 0; + float voltage_previous = 0.0f; + + uint64_t last_idle_time = 0; + + uint64_t start_time = 0; + + /* XXX use this! */ + //uint64_t failsave_ll_start_time = 0; + + bool state_changed = true; + bool param_init_forced = true; + + bool new_data = false; + /* Subscribe to safety topic */ int safety_sub = orb_subscribe(ORB_ID(safety)); struct safety_s safety; @@ -702,6 +711,7 @@ int commander_thread_main(int argc, char *argv[]) struct sensor_combined_s sensors; memset(&sensors, 0, sizeof(sensors)); + /* Subscribe to differential pressure topic */ int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); struct differential_pressure_s diff_pres; memset(&diff_pres, 0, sizeof(diff_pres)); @@ -717,7 +727,7 @@ int commander_thread_main(int argc, char *argv[]) struct parameter_update_s param_changed; memset(¶m_changed, 0, sizeof(param_changed)); - /* subscribe to battery topic */ + /* Subscribe to battery topic */ int battery_sub = orb_subscribe(ORB_ID(battery_status)); struct battery_status_s battery; memset(&battery, 0, sizeof(battery)); @@ -728,28 +738,14 @@ int commander_thread_main(int argc, char *argv[]) struct subsystem_info_s info; memset(&info, 0, sizeof(info)); - // uint8_t vehicle_state_previous = current_status.state_machine; - float voltage_previous = 0.0f; - - uint64_t last_idle_time = 0; - /* now initialized */ commander_initialized = true; thread_running = true; - uint64_t start_time = hrt_absolute_time(); - - /* XXX use this! */ - //uint64_t failsave_ll_start_time = 0; - - bool state_changed = true; - bool param_init_forced = true; + start_time = hrt_absolute_time(); while (!thread_should_exit) { - /* Get current values */ - bool new_data; - /* update parameters */ orb_check(param_changed_sub, &new_data); @@ -758,11 +754,10 @@ int commander_thread_main(int argc, char *argv[]) /* parameters changed */ orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); - /* update parameters */ if (!armed.armed) { if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { - warnx("failed setting new system type"); + warnx("failed getting new system type"); } /* disable manual override for all systems that rely on electronic stabilization */ @@ -816,37 +811,6 @@ int commander_thread_main(int argc, char *argv[]) /* handle it */ handle_command(status_pub, ¤t_status, &cmd, armed_pub, &armed); } - - - /* update parameters */ - orb_check(param_changed_sub, &new_data); - - if (new_data || param_init_forced) { - param_init_forced = false; - /* parameters changed */ - orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); - - /* update parameters */ - if (!armed.armed) { - if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { - warnx("failed setting new system type"); - } - - /* disable manual override for all systems that rely on electronic stabilization */ - if (current_status.system_type == VEHICLE_TYPE_QUADROTOR || - current_status.system_type == VEHICLE_TYPE_HEXAROTOR || - current_status.system_type == VEHICLE_TYPE_OCTOROTOR) { - current_status.flag_external_manual_override_ok = false; - - } else { - current_status.flag_external_manual_override_ok = true; - } - - /* check and update system / component ID */ - param_get(_param_system_id, &(current_status.system_id)); - param_get(_param_component_id, &(current_status.component_id)); - } - } /* update safety topic */ orb_check(safety_sub, &new_data); @@ -1664,11 +1628,16 @@ int commander_thread_main(int argc, char *argv[]) buzzer_deinit(); close(sp_man_sub); close(sp_offboard_sub); + close(local_position_sub); close(global_position_sub); + close(gps_sub); close(sensor_sub); close(safety_sub); close(cmd_sub); close(subsys_sub); + close(diff_pres_sub); + close(param_changed_sub); + close(battery_sub); warnx("exiting"); fflush(stdout); From 6902177b997906dc0003adc9e8210d1901f3dafc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 16 Jul 2013 12:45:43 +0200 Subject: [PATCH 262/486] Default to 2000 dps for L3GD20 --- src/drivers/l3gd20/l3gd20.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index f474818233..28d6397c95 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -334,10 +334,10 @@ L3GD20::init() write_reg(ADDR_CTRL_REG4, REG4_BDU); write_reg(ADDR_CTRL_REG5, 0); - write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */ - write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_STREAM_MODE); /* Enable FIFO, old data is overwritten */ + write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */ + write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_STREAM_MODE); /* Enable FIFO, old data is overwritten */ - set_range(500); /* default to 500dps */ + set_range(2000); /* default to 2000dps */ set_samplerate(0); /* max sample rate */ ret = OK; From c8aca814ca4bff633dfd4a8341c08a2d4b8074fa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 16 Jul 2013 12:46:23 +0200 Subject: [PATCH 263/486] Improved comments --- src/drivers/l3gd20/l3gd20.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 28d6397c95..a9af6711b1 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -351,7 +351,7 @@ L3GD20::probe() /* read dummy value to void to clear SPI statemachine on sensor */ (void)read_reg(ADDR_WHO_AM_I); - /* verify that the device is attached and functioning */ + /* verify that the device is attached and functioning, accept L3GD20 and L3GD20H */ if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM || read_reg(ADDR_WHO_AM_I) == WHO_I_AM_H) return OK; From 76edfa896b57782d7a4333bcf7a582952cb0fae4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 16 Jul 2013 13:24:02 +0200 Subject: [PATCH 264/486] Fixed disarming bug, use flag instead of mode switch --- src/modules/commander/commander.c | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index b457d98a18..144fda79ac 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -1387,8 +1387,7 @@ int commander_thread_main(int argc, char *argv[]) * Check if left stick is in lower left position --> switch to standby state. * Do this only for multirotors, not for fixed wing aircraft. */ - // printf("checking\n"); - if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { + if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.1f)) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { @@ -1396,7 +1395,7 @@ int commander_thread_main(int argc, char *argv[]) (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) ) { - if (current_status.mode_switch != MODE_SWITCH_MANUAL) { + if (control_mode.flag_control_position_enabled || control_mode.flag_control_velocity_enabled) { mavlink_log_critical(mavlink_fd, "DISARM DENY, go manual mode first"); tune_negative(); } else { @@ -1417,8 +1416,7 @@ int commander_thread_main(int argc, char *argv[]) } /* check if left stick is in lower right position and we're in manual --> arm */ - if (!control_mode.flag_control_position_enabled && !control_mode.flag_control_velocity_enabled && - sp_man.yaw > STICK_ON_OFF_LIMIT && + if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, armed_pub, &armed, mavlink_fd); From 39d88471896ac46c4aae475f7d6c73e44e7b5f25 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 16 Jul 2013 16:43:11 +0200 Subject: [PATCH 265/486] sercon is only used by APM now --- ROMFS/px4fmu_common/init.d/rcS | 41 +++++++++++++++++----------------- 1 file changed, 21 insertions(+), 20 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 22dec87cb0..498c93f288 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -42,30 +42,31 @@ then sh /fs/microsd/etc/rc.txt fi -# -# Check for USB host -# -if [ $USB != autoconnect ] -then - echo "[init] not connecting USB" -else - if sercon - then - echo "[init] USB interface connected" - else - if [ -f /dev/ttyACM0 ] - echo "[init] NSH via USB" - then - else - echo "[init] No USB connected" - fi - fi -fi - # if this is an APM build then there will be a rc.APM script # from an EXTERNAL_SCRIPTS build option if [ -f /etc/init.d/rc.APM ] then + + # + # Check for USB host + # + if [ $USB != autoconnect ] + then + echo "[init] not connecting USB" + else + if sercon + then + echo "[init] USB interface connected" + else + if [ -f /dev/ttyACM0 ] + echo "[init] NSH via USB" + then + else + echo "[init] No USB connected" + fi + fi + fi + echo Running rc.APM # if APM startup is successful then nsh will exit sh /etc/init.d/rc.APM From bcdedd9a35a5b9ebf3851a0d472adab8d3e7edac Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 16 Jul 2013 18:55:32 +0200 Subject: [PATCH 266/486] Changed location of lots of flags and conditions, needs testing and more work --- src/drivers/blinkm/blinkm.cpp | 2 +- src/drivers/px4io/px4io.cpp | 18 +-- .../attitude_estimator_ekf_main.cpp | 14 +- .../attitude_estimator_so3_comp_main.cpp | 14 +- src/modules/commander/commander.c | 122 ++++++++---------- src/modules/commander/state_machine_helper.c | 11 +- src/modules/commander/state_machine_helper.h | 2 +- src/modules/controllib/fixedwing.cpp | 24 ++-- .../fixedwing_backside_main.cpp | 6 +- src/modules/mavlink/mavlink.c | 15 ++- src/modules/mavlink/orb_listener.c | 5 +- src/modules/mavlink_onboard/mavlink.c | 4 +- src/modules/sdlog2/sdlog2.c | 4 +- src/modules/sensors/sensors.cpp | 32 ++--- .../uORB/topics/vehicle_control_mode.h | 3 - src/modules/uORB/topics/vehicle_status.h | 37 ++---- 16 files changed, 149 insertions(+), 164 deletions(-) diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index e83a87aa93..9bb020a6b4 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -536,7 +536,7 @@ BlinkM::led() /* looking for lipo cells that are connected */ printf(" checking cells\n"); for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) { - if(vehicle_status_raw.voltage_battery < num_of_cells * MAX_CELL_VOLTAGE) break; + if(vehicle_status_raw.battery_voltage < num_of_cells * MAX_CELL_VOLTAGE) break; } printf(" cells found:%d\n", num_of_cells); diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index ea732eccd1..827b0bb009 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -77,7 +77,7 @@ #include #include #include -#include +#include #include #include #include @@ -180,7 +180,7 @@ private: /* subscribed topics */ int _t_actuators; ///< actuator controls topic int _t_actuator_armed; ///< system armed control topic - int _t_vstatus; ///< system / vehicle status + int _t_vehicle_control_mode; ///< vehicle control mode topic int _t_param; ///< parameter update topic /* advertised topics */ @@ -362,7 +362,7 @@ PX4IO::PX4IO() : _alarms(0), _t_actuators(-1), _t_actuator_armed(-1), - _t_vstatus(-1), + _t_vehicle_control_mode(-1), _t_param(-1), _to_input_rc(0), _to_actuators_effective(0), @@ -647,8 +647,8 @@ PX4IO::task_main() _t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed)); orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */ - _t_vstatus = orb_subscribe(ORB_ID(vehicle_status)); - orb_set_interval(_t_vstatus, 200); /* 5Hz update rate max. */ + _t_vehicle_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode)); + orb_set_interval(_t_vehicle_control_mode, 200); /* 5Hz update rate max. */ _t_param = orb_subscribe(ORB_ID(parameter_update)); orb_set_interval(_t_param, 500); /* 2Hz update rate max. */ @@ -659,7 +659,7 @@ PX4IO::task_main() fds[0].events = POLLIN; fds[1].fd = _t_actuator_armed; fds[1].events = POLLIN; - fds[2].fd = _t_vstatus; + fds[2].fd = _t_vehicle_control_mode; fds[2].events = POLLIN; fds[3].fd = _t_param; fds[3].events = POLLIN; @@ -834,10 +834,10 @@ int PX4IO::io_set_arming_state() { actuator_armed_s armed; ///< system armed state - vehicle_status_s vstatus; ///< overall system state + vehicle_control_mode_s control_mode; ///< vehicle_control_mode orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &armed); - orb_copy(ORB_ID(vehicle_status), _t_vstatus, &vstatus); + orb_copy(ORB_ID(vehicle_control_mode), _t_vehicle_control_mode, &control_mode); uint16_t set = 0; uint16_t clear = 0; @@ -853,7 +853,7 @@ PX4IO::io_set_arming_state() clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; } - if (vstatus.flag_external_manual_override_ok) { + if (control_mode.flag_external_manual_override_ok) { set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; } else { clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index d8b40ac3b5..d1b941ffe2 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -57,7 +57,7 @@ #include #include #include -#include +#include #include #include @@ -214,8 +214,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds memset(&raw, 0, sizeof(raw)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); - struct vehicle_status_s state; - memset(&state, 0, sizeof(state)); + struct vehicle_control_mode_s control_mode; + memset(&control_mode, 0, sizeof(control_mode)); uint64_t last_data = 0; uint64_t last_measurement = 0; @@ -228,8 +228,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* subscribe to param changes */ int sub_params = orb_subscribe(ORB_ID(parameter_update)); - /* subscribe to system state*/ - int sub_state = orb_subscribe(ORB_ID(vehicle_status)); + /* subscribe to control mode*/ + int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode)); /* advertise attitude */ orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); @@ -281,9 +281,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* XXX this is seriously bad - should be an emergency */ } else if (ret == 0) { /* check if we're in HIL - not getting sensor data is fine then */ - orb_copy(ORB_ID(vehicle_status), sub_state, &state); + orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode); - if (!state.flag_hil_enabled) { + if (!control_mode.flag_system_hil_enabled) { fprintf(stderr, "[att ekf] WARNING: Not getting sensors - sensor app running?\n"); } diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp index 3ca50fb39c..2a06f1628b 100755 --- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp +++ b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp @@ -35,7 +35,7 @@ #include #include #include -#include +#include #include #include @@ -545,8 +545,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); - struct vehicle_status_s state; - memset(&state, 0, sizeof(state)); + struct vehicle_control_mode_s control_mode; + memset(&control_mode, 0, sizeof(control_mode)); uint64_t last_data = 0; uint64_t last_measurement = 0; @@ -559,8 +559,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* subscribe to param changes */ int sub_params = orb_subscribe(ORB_ID(parameter_update)); - /* subscribe to system state*/ - int sub_state = orb_subscribe(ORB_ID(vehicle_status)); + /* subscribe to control mode */ + int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode)); /* advertise attitude */ orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); @@ -610,9 +610,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* XXX this is seriously bad - should be an emergency */ } else if (ret == 0) { /* check if we're in HIL - not getting sensor data is fine then */ - orb_copy(ORB_ID(vehicle_status), sub_state, &state); + orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode); - if (!state.flag_hil_enabled) { + if (!control_mode.flag_system_hil_enabled) { fprintf(stderr, "[att so3_comp] WARNING: Not getting sensors - sensor app running?\n"); } diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index 144fda79ac..c4dc495f6a 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -171,7 +171,7 @@ void usage(const char *reason); /** * React to commands that are sent e.g. from the mavlink module. */ -void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed); +void handle_command(int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed); /** * Mainloop of commander. @@ -236,7 +236,7 @@ void usage(const char *reason) exit(1); } -void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed) +void handle_command(int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed) { /* result of the command */ uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; @@ -248,7 +248,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request to activate HIL */ if ((int)cmd->param1 & VEHICLE_MODE_FLAG_HIL_ENABLED) { - if (OK == hil_state_transition(status_pub, current_vehicle_status, mavlink_fd, HIL_STATE_ON)) { + if (OK == hil_state_transition(HIL_STATE_ON, status_pub, current_status, control_mode_pub, current_control_mode, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -256,13 +256,13 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } if ((int)cmd->param1 & VEHICLE_MODE_FLAG_SAFETY_ARMED) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; } } else { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -275,14 +275,14 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request to arm */ if ((int)cmd->param1 == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; } /* request to disarm */ } else if ((int)cmd->param1 == 0) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -295,7 +295,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request for an autopilot reboot */ if ((int)cmd->param1 == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_REBOOT, armed_pub, armed, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_REBOOT, armed_pub, armed, mavlink_fd)) { /* reboot is executed later, after positive tune is sent */ result = VEHICLE_CMD_RESULT_ACCEPTED; tune_positive(); @@ -346,7 +346,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION; @@ -365,7 +365,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION; @@ -426,7 +426,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION; @@ -445,7 +445,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION; @@ -580,7 +580,7 @@ int commander_thread_main(int argc, char *argv[]) current_status.offboard_control_signal_lost = true; /* allow manual override initially */ - current_status.flag_external_manual_override_ok = true; + control_mode.flag_external_manual_override_ok = true; /* flag position info as bad, do not allow auto mode */ // current_status.flag_vector_flight_mode_ok = false; @@ -637,32 +637,24 @@ int commander_thread_main(int argc, char *argv[]) pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL); /* Start monitoring loop */ - uint16_t counter = 0; - - /* Initialize to 0.0V */ - float battery_voltage = 0.0f; - bool battery_voltage_valid = false; - bool low_battery_voltage_actions_done = false; - bool critical_battery_voltage_actions_done = false; - uint8_t low_voltage_counter = 0; - uint16_t critical_voltage_counter = 0; - float bat_remain = 1.0f; - - uint16_t stick_off_counter = 0; - uint16_t stick_on_counter = 0; + unsigned counter = 0; + unsigned low_voltage_counter = 0; + unsigned critical_voltage_counter = 0; + unsigned stick_off_counter = 0; + unsigned stick_on_counter = 0; /* To remember when last notification was sent */ uint64_t last_print_time = 0; float voltage_previous = 0.0f; + bool low_battery_voltage_actions_done; + bool critical_battery_voltage_actions_done; + uint64_t last_idle_time = 0; uint64_t start_time = 0; - /* XXX use this! */ - //uint64_t failsave_ll_start_time = 0; - bool state_changed = true; bool param_init_forced = true; @@ -764,10 +756,10 @@ int commander_thread_main(int argc, char *argv[]) if (current_status.system_type == VEHICLE_TYPE_QUADROTOR || current_status.system_type == VEHICLE_TYPE_HEXAROTOR || current_status.system_type == VEHICLE_TYPE_OCTOROTOR) { - current_status.flag_external_manual_override_ok = false; + control_mode.flag_external_manual_override_ok = false; } else { - current_status.flag_external_manual_override_ok = true; + control_mode.flag_external_manual_override_ok = true; } /* check and update system / component ID */ @@ -809,7 +801,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - handle_command(status_pub, ¤t_status, &cmd, armed_pub, &armed); + handle_command(status_pub, ¤t_status, control_mode_pub, &control_mode, &cmd, armed_pub, &armed); } /* update safety topic */ @@ -848,16 +840,20 @@ int commander_thread_main(int argc, char *argv[]) orb_check(battery_sub, &new_data); if (new_data) { orb_copy(ORB_ID(battery_status), battery_sub, &battery); - battery_voltage = battery.voltage_v; - battery_voltage_valid = true; + current_status.battery_voltage = battery.voltage_v; + current_status.condition_battery_voltage_valid = true; /* * Only update battery voltage estimate if system has * been running for two and a half seconds. */ - if (hrt_absolute_time() - start_time > 2500000) { - bat_remain = battery_remaining_estimate_voltage(battery_voltage); - } + + } + + if (hrt_absolute_time() - start_time > 2500000 && current_status.condition_battery_voltage_valid) { + current_status.battery_remaining = battery_remaining_estimate_voltage(current_status.battery_voltage); + } else { + current_status.battery_voltage = 0.0f; } /* update subsystem */ @@ -897,14 +893,15 @@ int commander_thread_main(int argc, char *argv[]) if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) { /* XXX if armed */ - if ((false /*current_status.state_machine == SYSTEM_STATE_GROUND_READY || - current_status.state_machine == SYSTEM_STATE_AUTO || - current_status.state_machine == SYSTEM_STATE_MANUAL*/)) { + if (armed.armed) { /* armed, solid */ led_on(LED_AMBER); - } else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - /* not armed */ + } else if (armed.ready_to_arm && (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0)) { + /* ready to arm */ + led_toggle(LED_AMBER); + } else if (counter % (100000 / COMMANDER_MONITORING_INTERVAL) == 0) { + /* not ready to arm, something is wrong */ led_toggle(LED_AMBER); } @@ -926,15 +923,16 @@ int commander_thread_main(int argc, char *argv[]) led_off(LED_BLUE); } - /* toggle GPS led at 5 Hz in HIL mode */ - if (current_status.flag_hil_enabled) { - /* hil enabled */ - led_toggle(LED_BLUE); - } else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) { - /* toggle arming (red) at 5 Hz on low battery or error */ - led_toggle(LED_AMBER); - } + // /* toggle GPS led at 5 Hz in HIL mode */ + // if (current_status.flag_hil_enabled) { + // /* hil enabled */ + // led_toggle(LED_BLUE); + + // } else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) { + // /* toggle arming (red) at 5 Hz on low battery or error */ + // led_toggle(LED_AMBER); + // } } @@ -948,35 +946,29 @@ int commander_thread_main(int argc, char *argv[]) last_idle_time = system_load.tasks[0].total_runtime; } - /* Check battery voltage */ - /* write to sys_status */ - if (battery_voltage_valid) { - current_status.voltage_battery = battery_voltage; - - } else { - current_status.voltage_battery = 0.0f; - } + /* if battery voltage is getting lower, warn using buzzer, etc. */ - if (battery_voltage_valid && (bat_remain < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS + if (current_status.condition_battery_voltage_valid && (current_status.battery_remaining < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { low_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] WARNING! LOW BATTERY!"); current_status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING; + tune_low_bat(); } low_voltage_counter++; } /* Critical, this is rather an emergency, change state machine */ - else if (battery_voltage_valid && (bat_remain < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) { + else if (current_status.condition_battery_voltage_valid && (current_status.battery_remaining < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) { if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { critical_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!"); current_status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; - // XXX implement this - // state_machine_emergency(status_pub, ¤t_status, mavlink_fd); + tune_critical_bat(); + // XXX implement state change here } critical_voltage_counter++; @@ -1585,7 +1577,7 @@ int commander_thread_main(int argc, char *argv[]) /* Store old modes to detect and act on state transitions */ - voltage_previous = current_status.voltage_battery; + voltage_previous = current_status.battery_voltage; /* play tone according to evaluation result */ @@ -1595,10 +1587,10 @@ int commander_thread_main(int argc, char *argv[]) arm_tune_played = true; /* Trigger audio event for low battery */ - } else if (bat_remain < 0.1f && battery_voltage_valid) { + } else if (current_status.battery_remaining < 0.1f && current_status.condition_battery_voltage_valid) { if (tune_critical_bat() == OK) battery_tune_played = true; - } else if (bat_remain < 0.2f && battery_voltage_valid) { + } else if (current_status.battery_remaining < 0.2f && current_status.condition_battery_voltage_valid) { if (tune_low_bat() == OK) battery_tune_played = true; } else if(battery_tune_played) { diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index 0b241d108d..792ead8f3c 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -510,7 +510,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /** * Transition from one hil state to another */ -int hil_state_transition(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state) +int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd) { bool valid_transition = false; int ret = ERROR; @@ -530,7 +530,7 @@ int hil_state_transition(int status_pub, struct vehicle_status_s *current_status if (current_status->arming_state == ARMING_STATE_INIT || current_status->arming_state == ARMING_STATE_STANDBY) { - current_status->flag_hil_enabled = false; + current_control_mode->flag_system_hil_enabled = false; mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); valid_transition = true; } @@ -541,7 +541,7 @@ int hil_state_transition(int status_pub, struct vehicle_status_s *current_status if (current_status->arming_state == ARMING_STATE_INIT || current_status->arming_state == ARMING_STATE_STANDBY) { - current_status->flag_hil_enabled = true; + current_control_mode->flag_system_hil_enabled = true; mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); valid_transition = true; } @@ -558,8 +558,11 @@ int hil_state_transition(int status_pub, struct vehicle_status_s *current_status current_status->counter++; current_status->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, current_status); + + current_control_mode->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, current_control_mode); + ret = OK; } else { mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index e591d2fef9..75fdd224c4 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -54,6 +54,6 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, int control_mode_pub, struct vehicle_control_mode_s *control_mode, const int mavlink_fd); -int hil_state_transition(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state); +int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd); #endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/src/modules/controllib/fixedwing.cpp b/src/modules/controllib/fixedwing.cpp index 9435959e9b..0cfcfd51d1 100644 --- a/src/modules/controllib/fixedwing.cpp +++ b/src/modules/controllib/fixedwing.cpp @@ -39,6 +39,7 @@ #include "fixedwing.hpp" +#if 0 namespace control { @@ -277,11 +278,12 @@ void BlockMultiModeBacksideAutopilot::update() // This is not a hack, but a design choice. /* do not limit in HIL */ - if (!_status.flag_hil_enabled) { - /* limit to value of manual throttle */ - _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? - _actuators.control[CH_THR] : _manual.throttle; - } +#warning fix this + // if (!_status.flag_hil_enabled) { + // /* limit to value of manual throttle */ + // _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? + // _actuators.control[CH_THR] : _manual.throttle; + // } } else if (_status.navigation_state == NAVIGATION_STATE_MANUAL) { @@ -343,11 +345,12 @@ void BlockMultiModeBacksideAutopilot::update() // This is not a hack, but a design choice. /* do not limit in HIL */ - if (!_status.flag_hil_enabled) { - /* limit to value of manual throttle */ - _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? - _actuators.control[CH_THR] : _manual.throttle; - } +#warning fix this + // if (!_status.flag_hil_enabled) { + // /* limit to value of manual throttle */ + // _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? + // _actuators.control[CH_THR] : _manual.throttle; + // } #warning fix this // } @@ -382,3 +385,4 @@ BlockMultiModeBacksideAutopilot::~BlockMultiModeBacksideAutopilot() } // namespace control +#endif \ No newline at end of file diff --git a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp index 4803a526eb..677a867712 100644 --- a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp +++ b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp @@ -150,12 +150,14 @@ int control_demo_thread_main(int argc, char *argv[]) using namespace control; - fixedwing::BlockMultiModeBacksideAutopilot autopilot(NULL, "FWB"); +#warning fix this +// fixedwing::BlockMultiModeBacksideAutopilot autopilot(NULL, "FWB"); thread_running = true; while (!thread_should_exit) { - autopilot.update(); +#warning fix this +// autopilot.update(); } warnx("exiting."); diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index a2758a45c7..5f683c4437 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -191,7 +191,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) **/ /* HIL */ - if (v_status.flag_hil_enabled) { + if (v_status.hil_state == HIL_STATE_ON) { *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; } @@ -234,11 +234,11 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) **/ /* set calibration state */ - if (v_status.flag_preflight_calibration) { + if (v_status.preflight_calibration) { *mavlink_state = MAV_STATE_CALIBRATING; - } else if (v_status.flag_system_emergency) { + } else if (v_status.system_emergency) { *mavlink_state = MAV_STATE_EMERGENCY; @@ -677,7 +677,10 @@ int mavlink_thread_main(int argc, char *argv[]) mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state); /* switch HIL mode if required */ - set_hil_on_off(v_status.flag_hil_enabled); + if (v_status.hil_state == HIL_STATE_ON) + set_hil_on_off(true); + else if (v_status.hil_state == HIL_STATE_OFF) + set_hil_on_off(false); /* send status (values already copied in the section above) */ mavlink_msg_sys_status_send(chan, @@ -685,8 +688,8 @@ int mavlink_thread_main(int argc, char *argv[]) v_status.onboard_control_sensors_enabled, v_status.onboard_control_sensors_health, v_status.load, - v_status.voltage_battery * 1000.0f, - v_status.current_battery * 1000.0f, + v_status.battery_voltage * 1000.0f, + v_status.battery_current * 1000.0f, v_status.battery_remaining, v_status.drop_rate_comm, v_status.errors_comm, diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 57a5d5dd50..d088d421ed 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -272,7 +272,10 @@ l_vehicle_status(const struct listener *l) orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); /* enable or disable HIL */ - set_hil_on_off(v_status.flag_hil_enabled); + if (v_status.hil_state == HIL_STATE_ON) + set_hil_on_off(true); + else if (v_status.hil_state == HIL_STATE_OFF) + set_hil_on_off(false); /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c index 9ed2c6c12e..c5dbd00dd9 100644 --- a/src/modules/mavlink_onboard/mavlink.c +++ b/src/modules/mavlink_onboard/mavlink.c @@ -449,8 +449,8 @@ int mavlink_thread_main(int argc, char *argv[]) v_status.onboard_control_sensors_enabled, v_status.onboard_control_sensors_health, v_status.load, - v_status.voltage_battery * 1000.0f, - v_status.current_battery * 1000.0f, + v_status.battery_voltage * 1000.0f, + v_status.battery_current * 1000.0f, v_status.battery_remaining, v_status.drop_rate_comm, v_status.errors_comm, diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index b20d38b5ee..ab39830192 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -945,8 +945,8 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_STAT.manual_control_mode = 0; log_msg.body.log_STAT.manual_sas_mode = 0; log_msg.body.log_STAT.armed = (unsigned char) buf_armed.armed; /* XXX fmu armed correct? */ - log_msg.body.log_STAT.battery_voltage = buf_status.voltage_battery; - log_msg.body.log_STAT.battery_current = buf_status.current_battery; + log_msg.body.log_STAT.battery_voltage = buf_status.battery_voltage; + log_msg.body.log_STAT.battery_current = buf_status.battery_current; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; log_msg.body.log_STAT.battery_warning = (unsigned char) buf_status.battery_warning; LOGBUFFER_WRITE_AND_COUNT(STAT); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index acc0c2717b..5e334638f4 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -73,7 +73,7 @@ #include #include #include -#include +#include #include #include #include @@ -165,7 +165,7 @@ private: int _baro_sub; /**< raw baro data subscription */ int _airspeed_sub; /**< airspeed subscription */ int _diff_pres_sub; /**< raw differential pressure subscription */ - int _vstatus_sub; /**< vehicle status subscription */ + int _vcontrol_mode_sub; /**< vehicle control mode subscription */ int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ @@ -352,9 +352,9 @@ private: void diff_pres_poll(struct sensor_combined_s &raw); /** - * Check for changes in vehicle status. + * Check for changes in vehicle control mode. */ - void vehicle_status_poll(); + void vehicle_control_mode_poll(); /** * Check for changes in parameters. @@ -411,7 +411,7 @@ Sensors::Sensors() : _mag_sub(-1), _rc_sub(-1), _baro_sub(-1), - _vstatus_sub(-1), + _vcontrol_mode_sub(-1), _params_sub(-1), _manual_control_sub(-1), @@ -941,21 +941,21 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) } void -Sensors::vehicle_status_poll() +Sensors::vehicle_control_mode_poll() { - struct vehicle_status_s vstatus; - bool vstatus_updated; + struct vehicle_control_mode_s vcontrol_mode; + bool vcontrol_mode_updated; - /* Check HIL state if vehicle status has changed */ - orb_check(_vstatus_sub, &vstatus_updated); + /* Check HIL state if vehicle control mode has changed */ + orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated); - if (vstatus_updated) { + if (vcontrol_mode_updated) { - orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &vstatus); + orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &vcontrol_mode); /* switching from non-HIL to HIL mode */ //printf("[sensors] Vehicle mode: %i \t AND: %i, HIL: %i\n", vstatus.mode, vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED, hil_enabled); - if (vstatus.flag_hil_enabled && !_hil_enabled) { + if (vcontrol_mode.flag_system_hil_enabled && !_hil_enabled) { _hil_enabled = true; _publishing = false; @@ -1348,12 +1348,12 @@ Sensors::task_main() _rc_sub = orb_subscribe(ORB_ID(input_rc)); _baro_sub = orb_subscribe(ORB_ID(sensor_baro)); _diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); - _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); /* rate limit vehicle status updates to 5Hz */ - orb_set_interval(_vstatus_sub, 200); + orb_set_interval(_vcontrol_mode_sub, 200); /* * do advertisements @@ -1406,7 +1406,7 @@ Sensors::task_main() perf_begin(_loop_perf); /* check vehicle status for changes to publication state */ - vehicle_status_poll(); + vehicle_control_mode_poll(); /* check parameters for updates */ parameter_update_poll(); diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 02bf5568ab..8481a624fd 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -66,9 +66,6 @@ struct vehicle_control_mode_s bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ - bool flag_system_emergency; - bool flag_preflight_calibration; - // XXX needs yet to be set by state machine helper bool flag_system_hil_enabled; diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 2bcd57f4bb..ec08a6c13a 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -177,14 +177,11 @@ struct vehicle_status_s int32_t system_id; /**< system id, inspired by MAVLink's system ID field */ int32_t component_id; /**< subsystem / component id, inspired by MAVLink's component ID field */ - /* system flags - these represent the state predicates */ - mode_switch_pos_t mode_switch; return_switch_pos_t return_switch; mission_switch_pos_t mission_switch; - - bool condition_system_emergency; + bool condition_battery_voltage_valid; bool condition_system_in_air_restore; /**< true if we can restore in mid air */ bool condition_system_sensors_initialized; bool condition_system_returned_to_home; @@ -195,28 +192,6 @@ struct vehicle_status_s bool condition_local_position_valid; bool condition_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */ - // bool condition_auto_flight_mode_ok; /**< conditions of auto flight mode apply plus a valid takeoff position lock has been aquired */ - bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ - - bool flag_hil_enabled; /**< true if hardware in the loop simulation is enabled */ - //bool flag_armed; /**< true is motors / actuators are armed */ - //bool flag_safety_off; /**< true if safety is off */ - bool flag_system_emergency; - bool flag_preflight_calibration; - - // bool flag_control_manual_enabled; /**< true if manual input is mixed in */ - // bool flag_control_offboard_enabled; /**< true if offboard control input is on */ - // bool flag_auto_enabled; - // bool flag_control_rates_enabled; /**< true if rates are stabilized */ - // bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ - // bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ - // bool flag_control_position_enabled; /**< true if position is controlled */ - - // bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */ - // bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */ - // bool flag_preflight_accel_calibration; - // bool flag_preflight_airspeed_calibration; - bool rc_signal_found_once; bool rc_signal_lost; /**< true if RC reception is terminally lost */ bool rc_signal_cutting_off; /**< true if RC reception is weak / cutting off */ @@ -230,14 +205,20 @@ struct vehicle_status_s bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ bool failsave_highlevel; + bool preflight_calibration; + + bool system_emergency; + /* see SYS_STATUS mavlink message for the following */ uint32_t onboard_control_sensors_present; uint32_t onboard_control_sensors_enabled; uint32_t onboard_control_sensors_health; + float load; - float voltage_battery; - float current_battery; + float battery_voltage; + float battery_current; float battery_remaining; + enum VEHICLE_BATTERY_WARNING battery_warning; /**< current battery warning mode, as defined by VEHICLE_BATTERY_WARNING enum */ uint16_t drop_rate_comm; uint16_t errors_comm; From 35a519a2ea36f6ae69a2084428cdff6ed91e7b07 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 16 Jul 2013 20:36:50 +0200 Subject: [PATCH 267/486] Fixed sensor driver name --- ROMFS/px4fmu_common/init.d/rc.sensors | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 62c7184b85..be481aa8d7 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -25,7 +25,7 @@ then else echo "using L3GD20 and LSM303D" l3gd20 start - lsm303 start + lsm303d start fi # From e86e2a093861e51fde836f8cd383b09536ca0542 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 17 Jul 2013 07:57:02 +0200 Subject: [PATCH 268/486] Add additional file name options --- src/drivers/px4io/px4io.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 8f8555f1f3..1e00f9668c 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1890,7 +1890,7 @@ px4io_main(int argc, char *argv[]) } PX4IO_Uploader *up; - const char *fn[3]; + const char *fn[5]; /* work out what we're uploading... */ if (argc > 2) { @@ -1900,7 +1900,9 @@ px4io_main(int argc, char *argv[]) } else { fn[0] = "/fs/microsd/px4io.bin"; fn[1] = "/etc/px4io.bin"; - fn[2] = nullptr; + fn[2] = "/fs/microsd/px4io2.bin"; + fn[3] = "/etc/px4io2.bin"; + fn[4] = nullptr; } up = new PX4IO_Uploader; From e266f6d425eb606e35509181331d0286713a4a34 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 17 Jul 2013 08:37:02 +0200 Subject: [PATCH 269/486] WIP on microSD support --- nuttx-configs/px4fmu-v2/nsh/defconfig | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index efbb883a5b..b068b1de20 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -302,7 +302,23 @@ CONFIG_STM32_I2CTIMEOTICKS=500 # # SDIO Configuration # -CONFIG_SDIO_PRI=128 + +# +# Maintain legacy build behavior (revisit) +# + +CONFIG_MMCSD=y +#CONFIG_MMCSD_SPI=y +CONFIG_MMCSD_SDIO=y +CONFIG_MTD=y + +# +# STM32 SDIO-based MMC/SD driver +# +CONFIG_SDIO_DMA=y +CONFIG_MMCSD_MULTIBLOCK_DISABLE=y +CONFIG_MMCSD_MMCSUPPORT=n +CONFIG_MMCSD_HAVECARDDETECT=n # # USB Host Configuration @@ -361,6 +377,7 @@ CONFIG_ARCH_BOARD="px4fmu-v2" # # Common Board Options # +CONFIG_NSH_MMCSDSLOTNO=0 CONFIG_NSH_MMCSDMINOR=0 # From 72f58829c31c831d98106ceb443c97925fcb9de5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 17 Jul 2013 08:49:15 +0200 Subject: [PATCH 270/486] Fixed microSD, operational fine, will need more work with CCM SRAM. --- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index b068b1de20..839d476daf 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -253,7 +253,7 @@ CONFIG_STM32_JTAG_SW_ENABLE=y CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y # CONFIG_STM32_FORCEPOWER is not set # CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set -# CONFIG_STM32_CCMEXCLUDE is not set +CONFIG_STM32_CCMEXCLUDE=y CONFIG_STM32_DMACAPABLE=y # CONFIG_STM32_TIM1_PWM is not set # CONFIG_STM32_TIM3_PWM is not set From c4248c055f098e775e57a7f0ea6ffda49e930b01 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 17 Jul 2013 19:54:47 +0400 Subject: [PATCH 271/486] position_estimator_inav: minor fixes --- .../position_estimator_inav/position_estimator_inav_main.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 3b0fbd7823..a9bc09e0d2 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -115,6 +115,7 @@ int position_estimator_inav_main(int argc, char *argv[]) exit(0); } + verbose_mode = false; if (argc > 1) if (!strcmp(argv[2], "-v")) verbose_mode = true; @@ -379,10 +380,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } - accel_NED[2] += CONSTANTS_ONE_G; accel_corr[0] = accel_NED[0] - x_est[2]; accel_corr[1] = accel_NED[1] - y_est[2]; - accel_corr[2] = accel_NED[2] - z_est[2]; + accel_corr[2] = accel_NED[2] + CONSTANTS_ONE_G - z_est[2]; } else { memset(accel_corr, 0, sizeof(accel_corr)); From 5679a1b2b17085ab4aafee6b97790c340785a6a6 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 17 Jul 2013 09:19:17 -0700 Subject: [PATCH 272/486] Fix handling of register read operation errors. --- src/drivers/px4io/px4io.cpp | 30 +++++++++++++++++------------- 1 file changed, 17 insertions(+), 13 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 1e00f9668c..b019c4fc50 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -256,7 +256,7 @@ private: * @param offset Register offset to start writing at. * @param values Pointer to array of values to write. * @param num_values The number of values to write. - * @return Zero if all values were successfully written. + * @return OK if all values were successfully written. */ int io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values); @@ -266,7 +266,7 @@ private: * @param page Register page to write to. * @param offset Register offset to write to. * @param value Value to write. - * @return Zero if the value was written successfully. + * @return OK if the value was written successfully. */ int io_reg_set(uint8_t page, uint8_t offset, const uint16_t value); @@ -277,7 +277,7 @@ private: * @param offset Register offset to start reading from. * @param values Pointer to array where values should be stored. * @param num_values The number of values to read. - * @return Zero if all values were successfully read. + * @return OK if all values were successfully read. */ int io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values); @@ -1149,9 +1149,11 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned } int ret = _interface->write((page << 8) | offset, (void *)values, num_values); - if (ret != num_values) + if (ret != num_values) { debug("io_reg_set(%u,%u,%u): error %d", page, offset, num_values, ret); - return ret; + return -1; + } + return OK; } int @@ -1169,10 +1171,12 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_v return -EINVAL; } - int ret = _interface->read((page << 8) | offset, reinterpret_cast(values), num_values); - if (ret != num_values) + int ret = _interface->read((page << 8) | offset, reinterpret_cast(values), num_values); + if (ret != num_values) { debug("io_reg_get(%u,%u,%u): data error %d", page, offset, num_values, ret); - return ret; + return -1; + } + return OK; } uint32_t @@ -1180,7 +1184,7 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset) { uint16_t value; - if (io_reg_get(page, offset, &value, 1)) + if (io_reg_get(page, offset, &value, 1) != OK) return _io_reg_get_error; return value; @@ -1193,7 +1197,7 @@ PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t uint16_t value; ret = io_reg_get(page, offset, &value, 1); - if (ret) + if (ret != OK) return ret; value &= ~clearbits; value |= setbits; @@ -1500,9 +1504,9 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0); - uint32_t value = io_reg_get(PX4IO_PAGE_PWM_INFO, PX4IO_RATE_MAP_BASE + channel); - - *(uint32_t *)arg = value; + *(uint32_t *)arg = io_reg_get(PX4IO_PAGE_PWM_INFO, PX4IO_RATE_MAP_BASE + channel); + if (*(uint32_t *)arg == _io_reg_get_error) + ret = -EIO; break; } From 17366c4b0d2bbcb3d0705bac1a7e4bc737e0bf40 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 17 Jul 2013 22:22:51 +0400 Subject: [PATCH 273/486] multirotor_pos_control: fixes for AUTO mode, minor cleanup --- src/modules/multirotor_pos_control/multirotor_pos_control.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 4bae7efa4a..e5a3f3e548 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -333,21 +333,21 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) local_pos_sp.z = local_pos.home_alt - global_pos_sp.altitude; } - warnx("new setpoint: lat = %.10f, lon = %.10f, x = %.2f, y = %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); + warnx("new setpoint: lat = %.10f, lon = %.10f, x = %.2f, y = %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); mavlink_log_info(mavlink_fd, "new setpoint: %.7f, %.7f, %.2f, %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); /* publish local position setpoint as projection of global position setpoint */ orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); } } - if (reset_sp_alt) { + if (status.flag_control_manual_enabled && reset_sp_alt) { reset_sp_alt = false; local_pos_sp.z = local_pos.z; z_vel_pid.integral = -manual.throttle; // thrust PID uses Z downside mavlink_log_info(mavlink_fd, "reset alt setpoint: z = %.2f, throttle = %.2f", local_pos_sp.z, manual.throttle); } - if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE && reset_sp_pos) { + if (status.flag_control_manual_enabled && status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE && reset_sp_pos) { reset_sp_pos = false; local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; From 68b7e0315568efc91a067e161d134b00c1c7e132 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 17 Jul 2013 22:27:22 +0400 Subject: [PATCH 274/486] sdlog2: copyright fix --- src/modules/sdlog2/sdlog2.c | 4 ++-- src/modules/sdlog2/sdlog2_messages.h | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 6641d88f32..1d34daf588 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -2,7 +2,7 @@ * * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. * Author: Lorenz Meier - * Anton Babushkin + * Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -40,7 +40,7 @@ * does the heavy SD I/O in a low-priority worker thread. * * @author Lorenz Meier - * @author Anton Babushkin + * @author Anton Babushkin */ #include diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 06e640b5b3..95d19d955e 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -1,7 +1,7 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin + * Author: Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,7 +37,7 @@ * * Log messages and structures definition. * - * @author Anton Babushkin + * @author Anton Babushkin */ #ifndef SDLOG2_MESSAGES_H_ From 17445b0fbb57708816a535b87611f439d6e63c31 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 17 Jul 2013 22:54:05 +0200 Subject: [PATCH 275/486] Added led support to FMUv2 --- makefiles/config_px4fmu-v2_default.mk | 1 + src/drivers/boards/px4fmu/px4fmu_init.c | 2 +- src/drivers/boards/px4fmuv2/module.mk | 3 +- src/drivers/boards/px4fmuv2/px4fmu2_led.c | 85 +++++++++++++++++++++++ src/drivers/boards/px4fmuv2/px4fmu_init.c | 5 +- 5 files changed, 91 insertions(+), 5 deletions(-) create mode 100644 src/drivers/boards/px4fmuv2/px4fmu2_led.c diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 8e64bd754b..73dc8bd9d4 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -14,6 +14,7 @@ MODULES += drivers/device MODULES += drivers/stm32 MODULES += drivers/stm32/adc MODULES += drivers/stm32/tone_alarm +MODULES += drivers/led MODULES += drivers/px4fmu MODULES += drivers/px4io MODULES += drivers/boards/px4fmuv2 diff --git a/src/drivers/boards/px4fmu/px4fmu_init.c b/src/drivers/boards/px4fmu/px4fmu_init.c index 212a92cfa8..36af2298c2 100644 --- a/src/drivers/boards/px4fmu/px4fmu_init.c +++ b/src/drivers/boards/px4fmu/px4fmu_init.c @@ -194,7 +194,7 @@ __EXPORT int nsh_archinitialize(void) /* initial LED state */ drv_led_start(); led_off(LED_AMBER); - led_on(LED_BLUE); + led_off(LED_BLUE); /* Configure SPI-based devices */ diff --git a/src/drivers/boards/px4fmuv2/module.mk b/src/drivers/boards/px4fmuv2/module.mk index eb8841e4d1..b405165e1a 100644 --- a/src/drivers/boards/px4fmuv2/module.mk +++ b/src/drivers/boards/px4fmuv2/module.mk @@ -6,4 +6,5 @@ SRCS = px4fmu_can.c \ px4fmu_init.c \ px4fmu_pwm_servo.c \ px4fmu_spi.c \ - px4fmu_usb.c + px4fmu_usb.c \ + px4fmu2_led.c diff --git a/src/drivers/boards/px4fmuv2/px4fmu2_led.c b/src/drivers/boards/px4fmuv2/px4fmu2_led.c new file mode 100644 index 0000000000..bbf29521bd --- /dev/null +++ b/src/drivers/boards/px4fmuv2/px4fmu2_led.c @@ -0,0 +1,85 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 px4fmu2_led.c + * + * PX4FMU LED backend. + */ + +#include + +#include + +#include "stm32.h" +#include "px4fmu_internal.h" + +#include + +/* + * Ideally we'd be able to get these from up_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(); +extern void led_on(int led); +extern void led_off(int led); +__END_DECLS + +__EXPORT void led_init() +{ + /* Configure LED1 GPIO for output */ + + stm32_configgpio(GPIO_LED1); +} + +__EXPORT void led_on(int led) +{ + if (led == 0) + { + /* Pull down to switch on */ + stm32_gpiowrite(GPIO_LED1, false); + } +} + +__EXPORT void led_off(int led) +{ + if (led == 0) + { + /* Pull up to switch off */ + stm32_gpiowrite(GPIO_LED1, true); + } +} diff --git a/src/drivers/boards/px4fmuv2/px4fmu_init.c b/src/drivers/boards/px4fmuv2/px4fmu_init.c index 57f2812b87..c491116324 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu_init.c +++ b/src/drivers/boards/px4fmuv2/px4fmu_init.c @@ -192,9 +192,8 @@ __EXPORT int nsh_archinitialize(void) (hrt_callout)stm32_serial_dma_poll, NULL); - // initial LED state - // XXX need to make this work again -// drv_led_start(); + /* initial LED state */ + drv_led_start(); up_ledoff(LED_AMBER); /* Configure SPI-based devices */ From a19e0f2f9c5678b3e1ae56fd48ec7c95eed36ba7 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 18 Jul 2013 09:45:27 +0200 Subject: [PATCH 276/486] Small fix for make distclean, Linux find doesn't seem to know the -depth n argument --- Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Makefile b/Makefile index 9905f8a63c..a703bef4cf 100644 --- a/Makefile +++ b/Makefile @@ -174,7 +174,7 @@ clean: distclean: clean $(Q) $(REMOVE) $(ARCHIVE_DIR)*.export $(Q) make -C $(NUTTX_SRC) -r $(MQUIET) distclean - $(Q) (cd $(NUTTX_SRC)/configs && find . -type l -depth 1 -delete) + $(Q) (cd $(NUTTX_SRC)/configs && find . -maxdepth 1 -type l -delete) # # Print some help text From 349c9624694ff0d17d10523470ff62b34356207e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 18 Jul 2013 10:01:43 +0200 Subject: [PATCH 277/486] Compiling / executing WIP on leds, leds not operational yet --- nuttx-configs/px4fmu-v2/nsh/defconfig | 47 +++++++++++++++++------ src/drivers/boards/px4fmuv2/module.mk | 2 +- src/drivers/boards/px4fmuv2/px4fmu2_led.c | 2 +- src/drivers/led/led.cpp | 4 +- 4 files changed, 39 insertions(+), 16 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 839d476daf..70500e4b0a 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -242,6 +242,9 @@ CONFIG_STM32_ADC=y CONFIG_STM32_SPI=y CONFIG_STM32_I2C=y +CONFIG_ARCH_HAVE_LEDS=y +# CONFIG_ARCH_LEDS is not set + # # Alternate Pin Mapping # @@ -274,9 +277,9 @@ CONFIG_USART1_RXDMA=y # CONFIG_USART2_RS485 is not set CONFIG_USART2_RXDMA=y # CONFIG_USART3_RS485 is not set -CONFIG_USART3_RXDMA=y +# CONFIG_USART3_RXDMA is not set # CONFIG_UART4_RS485 is not set -CONFIG_UART4_RXDMA=y +# CONFIG_UART4_RXDMA is not set # CONFIG_UART5_RXDMA is not set # CONFIG_USART6_RS485 is not set # CONFIG_USART6_RXDMA is not set @@ -284,6 +287,26 @@ CONFIG_UART4_RXDMA=y CONFIG_USART8_RXDMA=y CONFIG_STM32_USART_SINGLEWIRE=y + +# Previous: +## CONFIG_USART1_RS485 is not set +#CONFIG_USART1_RXDMA=y +## CONFIG_USART2_RS485 is not set +#CONFIG_USART2_RXDMA=y +## CONFIG_USART3_RS485 is not set +#CONFIG_USART3_RXDMA=y +## CONFIG_UART4_RS485 is not set +#CONFIG_UART4_RXDMA=y +## CONFIG_UART5_RXDMA is not set +## CONFIG_USART6_RS485 is not set +## CONFIG_USART6_RXDMA is not set +## CONFIG_USART7_RXDMA is not set +#CONFIG_USART8_RXDMA=y +#CONFIG_STM32_USART_SINGLEWIRE=y + + + + # # SPI Configuration # @@ -525,8 +548,8 @@ CONFIG_NO_SERIAL_CONSOLE=y # # USART1 Configuration # -CONFIG_USART1_RXBUFSIZE=16 -CONFIG_USART1_TXBUFSIZE=16 +CONFIG_USART1_RXBUFSIZE=512 +CONFIG_USART1_TXBUFSIZE=512 CONFIG_USART1_BAUD=115200 CONFIG_USART1_BITS=8 CONFIG_USART1_PARITY=0 @@ -551,7 +574,7 @@ CONFIG_USART2_OFLOWCONTROL=y # CONFIG_USART3_RXBUFSIZE=512 CONFIG_USART3_TXBUFSIZE=512 -CONFIG_USART3_BAUD=115200 +CONFIG_USART3_BAUD=57600 CONFIG_USART3_BITS=8 CONFIG_USART3_PARITY=0 CONFIG_USART3_2STOP=0 @@ -573,9 +596,9 @@ CONFIG_UART4_2STOP=0 # # USART6 Configuration # -CONFIG_USART6_RXBUFSIZE=16 -CONFIG_USART6_TXBUFSIZE=16 -CONFIG_USART6_BAUD=115200 +CONFIG_USART6_RXBUFSIZE=256 +CONFIG_USART6_TXBUFSIZE=256 +CONFIG_USART6_BAUD=57600 CONFIG_USART6_BITS=8 CONFIG_USART6_PARITY=0 CONFIG_USART6_2STOP=0 @@ -585,8 +608,8 @@ CONFIG_USART6_2STOP=0 # # UART7 Configuration # -CONFIG_UART7_RXBUFSIZE=128 -CONFIG_UART7_TXBUFSIZE=128 +CONFIG_UART7_RXBUFSIZE=256 +CONFIG_UART7_TXBUFSIZE=256 CONFIG_UART7_BAUD=57600 CONFIG_UART7_BITS=8 CONFIG_UART7_PARITY=0 @@ -597,8 +620,8 @@ CONFIG_UART7_2STOP=0 # # UART8 Configuration # -CONFIG_UART8_RXBUFSIZE=128 -CONFIG_UART8_TXBUFSIZE=128 +CONFIG_UART8_RXBUFSIZE=256 +CONFIG_UART8_TXBUFSIZE=256 CONFIG_UART8_BAUD=57600 CONFIG_UART8_BITS=8 CONFIG_UART8_PARITY=0 diff --git a/src/drivers/boards/px4fmuv2/module.mk b/src/drivers/boards/px4fmuv2/module.mk index b405165e1a..99d37eecac 100644 --- a/src/drivers/boards/px4fmuv2/module.mk +++ b/src/drivers/boards/px4fmuv2/module.mk @@ -3,7 +3,7 @@ # SRCS = px4fmu_can.c \ - px4fmu_init.c \ + px4fmu2_init.c \ px4fmu_pwm_servo.c \ px4fmu_spi.c \ px4fmu_usb.c \ diff --git a/src/drivers/boards/px4fmuv2/px4fmu2_led.c b/src/drivers/boards/px4fmuv2/px4fmu2_led.c index bbf29521bd..85177df56b 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu2_led.c +++ b/src/drivers/boards/px4fmuv2/px4fmu2_led.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/drivers/led/led.cpp b/src/drivers/led/led.cpp index 04b565358c..27e43b2454 100644 --- a/src/drivers/led/led.cpp +++ b/src/drivers/led/led.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -110,7 +110,7 @@ LED *gLED; } void -drv_led_start() +drv_led_start(void) { if (gLED == nullptr) { gLED = new LED; From d84824dd4f1d076894df82034868c4655cf7c530 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 18 Jul 2013 10:35:36 +0200 Subject: [PATCH 278/486] Compile fixes --- src/drivers/boards/px4fmuv2/{px4fmu_init.c => px4fmu2_init.c} | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) rename src/drivers/boards/px4fmuv2/{px4fmu_init.c => px4fmu2_init.c} (99%) diff --git a/src/drivers/boards/px4fmuv2/px4fmu_init.c b/src/drivers/boards/px4fmuv2/px4fmu2_init.c similarity index 99% rename from src/drivers/boards/px4fmuv2/px4fmu_init.c rename to src/drivers/boards/px4fmuv2/px4fmu2_init.c index c491116324..535e075b22 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu_init.c +++ b/src/drivers/boards/px4fmuv2/px4fmu2_init.c @@ -193,7 +193,7 @@ __EXPORT int nsh_archinitialize(void) NULL); /* initial LED state */ - drv_led_start(); + //drv_led_start(); up_ledoff(LED_AMBER); /* Configure SPI-based devices */ From 3bea32af8d41585976b78c6dad2701df4180659f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 18 Jul 2013 13:23:14 +0200 Subject: [PATCH 279/486] Add HRT and PPM again in IO defconfig --- nuttx-configs/px4io-v2/nsh/defconfig | 22 ++++++++++++++++++++++ nuttx-configs/px4io-v2/test/defconfig | 22 ++++++++++++++++++++++ 2 files changed, 44 insertions(+) diff --git a/nuttx-configs/px4io-v2/nsh/defconfig b/nuttx-configs/px4io-v2/nsh/defconfig index 846ea8fb9f..4111e70eba 100755 --- a/nuttx-configs/px4io-v2/nsh/defconfig +++ b/nuttx-configs/px4io-v2/nsh/defconfig @@ -195,6 +195,28 @@ SERIAL_HAVE_CONSOLE_DMA=y CONFIG_USART2_RXDMA=n CONFIG_USART3_RXDMA=y +# +# PX4IO specific driver settings +# +# CONFIG_HRT_TIMER +# Enables the high-resolution timer. The board definition must +# set HRT_TIMER and HRT_TIMER_CHANNEL to the timer and capture/ +# compare channels to be used. +# CONFIG_HRT_PPM +# Enables R/C PPM input using the HRT. The board definition must +# set HRT_PPM_CHANNEL to the timer capture/compare channel to be +# used, and define GPIO_PPM_IN to configure the appropriate timer +# GPIO. +# CONFIG_PWM_SERVO +# Enables the PWM servo driver. The driver configuration must be +# supplied by the board support at initialisation time. +# Note that USART2 must be disabled on the PX4 board for this to +# be available. +# +# +CONFIG_HRT_TIMER=y +CONFIG_HRT_PPM=y + # # General build options # diff --git a/nuttx-configs/px4io-v2/test/defconfig b/nuttx-configs/px4io-v2/test/defconfig index 19dfefdf84..132c40d806 100755 --- a/nuttx-configs/px4io-v2/test/defconfig +++ b/nuttx-configs/px4io-v2/test/defconfig @@ -187,6 +187,28 @@ CONFIG_USART1_2STOP=0 CONFIG_USART2_2STOP=0 CONFIG_USART3_2STOP=0 +# +# PX4IO specific driver settings +# +# CONFIG_HRT_TIMER +# Enables the high-resolution timer. The board definition must +# set HRT_TIMER and HRT_TIMER_CHANNEL to the timer and capture/ +# compare channels to be used. +# CONFIG_HRT_PPM +# Enables R/C PPM input using the HRT. The board definition must +# set HRT_PPM_CHANNEL to the timer capture/compare channel to be +# used, and define GPIO_PPM_IN to configure the appropriate timer +# GPIO. +# CONFIG_PWM_SERVO +# Enables the PWM servo driver. The driver configuration must be +# supplied by the board support at initialisation time. +# Note that USART2 must be disabled on the PX4 board for this to +# be available. +# +# +CONFIG_HRT_TIMER=y +CONFIG_HRT_PPM=y + # # General build options # From f5f7b3f6dd1c0d48ffb0aa8c78435715a4ce3f5e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 18 Jul 2013 14:42:08 +0200 Subject: [PATCH 280/486] Added scale ioctl to LSM303D driver --- src/drivers/lsm303d/lsm303d.cpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index ba7316e557..af7746d140 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -705,6 +705,23 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) else return -EINVAL; + case ACCELIOCSSCALE: + { + /* copy scale, but only if off by a few percent */ + struct accel_scale *s = (struct accel_scale *) arg; + float sum = s->x_scale + s->y_scale + s->z_scale; + if (sum > 2.0f && sum < 4.0f) { + memcpy(&_accel_scale, s, sizeof(_accel_scale)); + return OK; + } else { + return -EINVAL; + } + } + + case ACCELIOCGSCALE: + /* copy scale out */ + memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale)); + return OK; default: /* give it to the superclass */ From eb28f5996f1e102939d9ebb3a562641137ee419b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 18 Jul 2013 15:21:39 +0200 Subject: [PATCH 281/486] Some lost LSM303D fixes for range and scale --- src/drivers/lsm303d/lsm303d.cpp | 68 ++++++++++++++++++++------------- 1 file changed, 42 insertions(+), 26 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index af7746d140..14b3d6ab85 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -156,11 +156,11 @@ static const int ERROR = -1; #define REG5_RATE_100HZ_M ((1<<4) | (0<<3) | (1<<2)) #define REG5_RATE_DO_NOT_USE_M ((1<<4) | (1<<3) | (0<<2)) -#define REG6_FULL_SCALE_BITS_M ((1<<7) | (1<<6)) -#define REG6_FULL_SCALE_2GA_M ((0<<7) | (0<<6)) -#define REG6_FULL_SCALE_4GA_M ((0<<7) | (1<<6)) -#define REG6_FULL_SCALE_8GA_M ((1<<7) | (0<<6)) -#define REG6_FULL_SCALE_12GA_M ((1<<7) | (1<<6)) +#define REG6_FULL_SCALE_BITS_M ((1<<6) | (1<<5)) +#define REG6_FULL_SCALE_2GA_M ((0<<6) | (0<<5)) +#define REG6_FULL_SCALE_4GA_M ((0<<6) | (1<<5)) +#define REG6_FULL_SCALE_8GA_M ((1<<6) | (0<<5)) +#define REG6_FULL_SCALE_12GA_M ((1<<6) | (1<<5)) #define REG7_CONT_MODE_M ((0<<1) | (0<<0)) @@ -218,6 +218,8 @@ private: float _accel_range_m_s2; orb_advert_t _accel_topic; + unsigned _current_samplerate; + unsigned _num_mag_reports; volatile unsigned _next_mag_report; volatile unsigned _oldest_mag_report; @@ -405,8 +407,6 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : // enable debug() calls _debug_enabled = true; - _mag_range_scale = 12.0f/32767.0f; - // default scale factors _accel_scale.x_offset = 0; _accel_scale.x_scale = 1.0f; @@ -873,37 +873,43 @@ LSM303D::set_range(unsigned max_g) { uint8_t setbits = 0; uint8_t clearbits = REG2_FULL_SCALE_BITS_A; - float new_range = 0.0f; + float new_range_g = 0.0f; + float new_scale_g_digit = 0.0f; if (max_g == 0) max_g = 16; if (max_g <= 2) { - new_range = 2.0f; + new_range_g = 2.0f; setbits |= REG2_FULL_SCALE_2G_A; + new_scale_g_digit = 0.061e-3f; } else if (max_g <= 4) { - new_range = 4.0f; + new_range_g = 4.0f; setbits |= REG2_FULL_SCALE_4G_A; + new_scale_g_digit = 0.122e-3f; } else if (max_g <= 6) { - new_range = 6.0f; + new_range_g = 6.0f; setbits |= REG2_FULL_SCALE_6G_A; + new_scale_g_digit = 0.183e-3f; } else if (max_g <= 8) { - new_range = 8.0f; + new_range_g = 8.0f; setbits |= REG2_FULL_SCALE_8G_A; + new_scale_g_digit = 0.244e-3f; } else if (max_g <= 16) { - new_range = 16.0f; + new_range_g = 16.0f; setbits |= REG2_FULL_SCALE_16G_A; + new_scale_g_digit = 0.732e-3f; } else { return -EINVAL; } - _accel_range_m_s2 = new_range * 9.80665f; - _accel_range_scale = _accel_range_m_s2 / 32768.0f; + _accel_range_m_s2 = new_range_g * 9.80665f; + _accel_range_scale = new_scale_g_digit * 9.80665f; modify_reg(ADDR_CTRL_REG2, clearbits, setbits); @@ -916,6 +922,7 @@ LSM303D::mag_set_range(unsigned max_ga) uint8_t setbits = 0; uint8_t clearbits = REG6_FULL_SCALE_BITS_M; float new_range = 0.0f; + float new_scale_ga_digit = 0.0f; if (max_ga == 0) max_ga = 12; @@ -923,25 +930,29 @@ LSM303D::mag_set_range(unsigned max_ga) if (max_ga <= 2) { new_range = 2.0f; setbits |= REG6_FULL_SCALE_2GA_M; + new_scale_ga_digit = 0.080e-3f; } else if (max_ga <= 4) { new_range = 4.0f; setbits |= REG6_FULL_SCALE_4GA_M; + new_scale_ga_digit = 0.160e-3f; } else if (max_ga <= 8) { new_range = 8.0f; setbits |= REG6_FULL_SCALE_8GA_M; + new_scale_ga_digit = 0.320e-3f; } else if (max_ga <= 12) { new_range = 12.0f; setbits |= REG6_FULL_SCALE_12GA_M; + new_scale_ga_digit = 0.479e-3f; } else { return -EINVAL; } _mag_range_ga = new_range; - _mag_range_scale = _mag_range_ga / 32768.0f; + _mag_range_scale = new_scale_ga_digit; modify_reg(ADDR_CTRL_REG6, clearbits, setbits); @@ -1008,18 +1019,23 @@ LSM303D::set_samplerate(unsigned frequency) if (frequency <= 100) { setbits |= REG1_RATE_100HZ_A; + _current_samplerate = 100; } else if (frequency <= 200) { setbits |= REG1_RATE_200HZ_A; + _current_samplerate = 200; } else if (frequency <= 400) { setbits |= REG1_RATE_400HZ_A; + _current_samplerate = 400; } else if (frequency <= 800) { setbits |= REG1_RATE_800HZ_A; + _current_samplerate = 800; } else if (frequency <= 1600) { setbits |= REG1_RATE_1600HZ_A; + _current_samplerate = 1600; } else { return -EINVAL; @@ -1358,7 +1374,7 @@ void test() { int fd_accel = -1; - struct accel_report a_report; + struct accel_report accel_report; ssize_t sz; int filter_bandwidth; @@ -1369,20 +1385,20 @@ test() err(1, "%s open failed", ACCEL_DEVICE_PATH); /* do a simple demand read */ - sz = read(fd_accel, &a_report, sizeof(a_report)); + sz = read(fd_accel, &accel_report, sizeof(accel_report)); - if (sz != sizeof(a_report)) + if (sz != sizeof(accel_report)) err(1, "immediate read failed"); - warnx("accel x: \t% 9.5f\tm/s^2", (double)a_report.x); - warnx("accel y: \t% 9.5f\tm/s^2", (double)a_report.y); - warnx("accel z: \t% 9.5f\tm/s^2", (double)a_report.z); - warnx("accel x: \t%d\traw", (int)a_report.x_raw); - warnx("accel y: \t%d\traw", (int)a_report.y_raw); - warnx("accel z: \t%d\traw", (int)a_report.z_raw); + warnx("accel x: \t% 9.5f\tm/s^2", (double)accel_report.x); + warnx("accel y: \t% 9.5f\tm/s^2", (double)accel_report.y); + warnx("accel z: \t% 9.5f\tm/s^2", (double)accel_report.z); + warnx("accel x: \t%d\traw", (int)accel_report.x_raw); + warnx("accel y: \t%d\traw", (int)accel_report.y_raw); + warnx("accel z: \t%d\traw", (int)accel_report.z_raw); - warnx("accel range: %8.4f m/s^2", (double)a_report.range_m_s2); + warnx("accel range: %8.4f m/s^2", (double)accel_report.range_m_s2); if (ERROR == (filter_bandwidth = ioctl(fd_accel, ACCELIOCGLOWPASS, 0))) warnx("accel antialias filter bandwidth: fail"); else From f4df4a4e081d9eaaa5dbeef013fa6320b0cea3f7 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 18 Jul 2013 15:30:39 +0200 Subject: [PATCH 282/486] Forgot to add current samplerate to constructor --- src/drivers/lsm303d/lsm303d.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 14b3d6ab85..b157d74c6e 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -395,6 +395,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _accel_range_scale(0.0f), _accel_range_m_s2(0.0f), _accel_topic(-1), + _current_samplerate(0), _num_mag_reports(0), _next_mag_report(0), _oldest_mag_report(0), From da152e148d471ded29857e29040f33c7356c9050 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 18 Jul 2013 16:15:43 +0200 Subject: [PATCH 283/486] Added iirFilter to LSM303D --- makefiles/config_px4fmu-v1_default.mk | 1 - src/drivers/lsm303d/iirFilter.c | 255 ++++++++++++++++++++++++++ src/drivers/lsm303d/iirFilter.h | 59 ++++++ src/drivers/lsm303d/lsm303d.cpp | 32 +++- src/drivers/lsm303d/module.mk | 3 +- 5 files changed, 345 insertions(+), 5 deletions(-) create mode 100644 src/drivers/lsm303d/iirFilter.c create mode 100644 src/drivers/lsm303d/iirFilter.h diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index d9a699da76..74be1cd23f 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -18,7 +18,6 @@ MODULES += drivers/led MODULES += drivers/px4io MODULES += drivers/px4fmu MODULES += drivers/boards/px4fmu -MODULES += drivers/lsm303d MODULES += drivers/ardrone_interface MODULES += drivers/l3gd20 MODULES += drivers/bma180 diff --git a/src/drivers/lsm303d/iirFilter.c b/src/drivers/lsm303d/iirFilter.c new file mode 100644 index 0000000000..8311f14d6d --- /dev/null +++ b/src/drivers/lsm303d/iirFilter.c @@ -0,0 +1,255 @@ +#include "math.h" +#include "string.h" +#include "iirFilter.h" + +/////////////////////////////////////////////////////////////////////////////// +// Internal function prototypes + +int btZpgcToZpgd(const TF_ZPG_t *pkZpgc, double Ts, TF_ZPG_t *pZpgd); + +int btDifcToZpgd(const TF_DIF_t *pkDifc, double Ts, TF_ZPG_t *pZpgd); + +int tPolydToFil(const TF_POLY_t *pkPolyd, FIL_T *pFilt); + +int tZpgxToPolyx(const TF_ZPG_t *pkZpg, TF_POLY_t *pPoly); + +/////////////////////////////////////////////////////////////////////////////// +// external functions + +int testFunction() +{ + printf("TEST\n"); + return 1; +} + +int initFilter(const TF_DIF_t *pDifc, double Ts, FIL_T *pFilt) +{ + TF_POLY_t polyd; + TF_ZPG_t zpgd; + + memset(pFilt, 0, sizeof(FIL_T)); + + // perform bilinear transform with frequency pre warping + btDifcToZpgd(pDifc, Ts, &zpgd); + + // calculate polynom + tZpgxToPolyx(&zpgd, &polyd); + + // set the filter parameters + tPolydToFil(&polyd, pFilt); + + return 1; +} + +// run filter using inp, return output of the filter +float updateFilter(FIL_T *pFilt, float inp) +{ + float outp = 0; + int idx; // index used for different things + int i; // loop variable + + // Store the input to the input array + idx = pFilt->inpCnt % MAX_LENGTH; + pFilt->inpData[idx] = inp; + + // calculate numData * inpData + for (i = 0; i < pFilt->numLength; i++) + { + // index of inp array + idx = (pFilt->inpCnt + i - pFilt->numLength + 1) % MAX_LENGTH; + outp += pFilt->numData[i] * pFilt->inpData[idx]; + } + + // calculate denData * outData + for (i = 0; i < pFilt->denLength; i++) + { + // index of inp array + idx = (pFilt->inpCnt + i - pFilt->denLength) % MAX_LENGTH; + outp -= pFilt->denData[i] * pFilt->outData[idx]; + } + + // store the ouput data to the output array + idx = pFilt->inpCnt % MAX_LENGTH; + pFilt->outData[idx] = outp; + + pFilt->inpCnt += 1; + + return outp; +} + +/////////////////////////////////////////////////////////////////////////////// +// Internal functions + +int tPolydToFil(const TF_POLY_t *pkPolyd, FIL_T *pFilt) +{ + double gain; + int i; + + if (pkPolyd->Ts < 0) + { + return 0; + } + + // initialize filter to 0 + memset(pFilt, 0, sizeof(FIL_T)); + + gain = pkPolyd->denData[pkPolyd->denLength - 1]; + pFilt->Ts = pkPolyd->Ts; + + pFilt->denLength = pkPolyd->denLength - 1; + pFilt->numLength = pkPolyd->numLength; + + for (i = 0; i < pkPolyd->numLength; i++) + { + pFilt->numData[i] = pkPolyd->numData[i] / gain; + } + + for (i = 0; i < (pkPolyd->denLength - 1); i++) + { + pFilt->denData[i] = pkPolyd->denData[i] / gain; + } +} + +// bilinear transformation of poles and zeros +int btDifcToZpgd(const TF_DIF_t *pkDifc, + double Ts, + TF_ZPG_t *pZpgd) +{ + TF_ZPG_t zpgc; + int totDiff; + int i; + + zpgc.zerosLength = 0; + zpgc.polesLength = 0; + zpgc.gain = pkDifc->gain; + zpgc.Ts = pkDifc->Ts; + + // Total number of differentiators / integerators + // positive diff, negative integ, 0 for no int/diff + totDiff = pkDifc->numDiff - pkDifc->numInt + pkDifc->highpassLength; + + while (zpgc.zerosLength < totDiff) + { + zpgc.zerosData[zpgc.zerosLength] = 0; + zpgc.zerosLength++; + } + while (zpgc.polesLength < -totDiff) + { + zpgc.polesData[zpgc.polesLength] = 0; + zpgc.polesLength++; + } + + // The next step is to calculate the poles + // This has to be done for the highpass and lowpass filters + // As we are using bilinear transformation there will be frequency + // warping which has to be accounted for + for (i = 0; i < pkDifc->lowpassLength; i++) + { + // pre-warping: + double freq = 2.0 / Ts * tan(pkDifc->lowpassData[i] * 2.0 * M_PI * Ts / 2.0); + // calculate pole: + zpgc.polesData[zpgc.polesLength] = -freq; + // adjust gain such that lp has gain = 1 for low frequencies + zpgc.gain *= freq; + zpgc.polesLength++; + } + for (i = 0; i < pkDifc->highpassLength; i++) + { + // pre-warping + double freq = 2.0 / Ts * tan(pkDifc->highpassData[i] * 2.0 * M_PI * Ts / 2.0); + // calculate pole: + zpgc.polesData[zpgc.polesLength] = -freq; + // gain does not have to be adjusted + zpgc.polesLength++; + } + + return btZpgcToZpgd(&zpgc, Ts, pZpgd); +} + +// bilinear transformation of poles and zeros +int btZpgcToZpgd(const TF_ZPG_t *pkZpgc, + double Ts, + TF_ZPG_t *pZpgd) +{ + int i; + + // init digital gain + pZpgd->gain = pkZpgc->gain; + + // transform the poles + pZpgd->polesLength = pkZpgc->polesLength; + for (i = 0; i < pkZpgc->polesLength; i++) + { + pZpgd->polesData[i] = (2.0 / Ts + pkZpgc->polesData[i]) / (2.0 / Ts - pkZpgc->polesData[i]); + pZpgd->gain /= (2.0 / Ts - pkZpgc->polesData[i]); + } + + // transform the zeros + pZpgd->zerosLength = pkZpgc->zerosLength; + for (i = 0; i < pkZpgc->zerosLength; i++) + { + pZpgd->zerosData[i] = (2.0 / Ts + pkZpgc->zerosData[i]) / (2.0 / Ts + pkZpgc->zerosData[i]); + pZpgd->gain *= (2.0 / Ts - pkZpgc->zerosData[i]); + } + + // if we don't have the same number of poles as zeros we have to add + // poles or zeros due to the bilinear transformation + while (pZpgd->zerosLength < pZpgd->polesLength) + { + pZpgd->zerosData[pZpgd->zerosLength] = -1.0; + pZpgd->zerosLength++; + } + while (pZpgd->zerosLength > pZpgd->polesLength) + { + pZpgd->polesData[pZpgd->polesLength] = -1.0; + pZpgd->polesLength++; + } + + pZpgd->Ts = Ts; + + return 1; +} + +// calculate polynom from zero, pole, gain form +int tZpgxToPolyx(const TF_ZPG_t *pkZpg, TF_POLY_t *pPoly) +{ + int i, j; // counter variable + double tmp0, tmp1, gain; + + // copy Ts + pPoly->Ts = pkZpg->Ts; + + // calculate denominator polynom + pPoly->denLength = 1; + pPoly->denData[0] = 1.0; + for (i = 0; i < pkZpg->polesLength; i++) + { + // init temporary variable + tmp0 = 0.0; + // increase the polynom by 1 + pPoly->denData[pPoly->denLength] = 0; + pPoly->denLength++; + for (j = 0; j < pPoly->denLength; j++) + { + tmp1 = pPoly->denData[j]; + pPoly->denData[j] = tmp1 * -pkZpg->polesData[i] + tmp0; + tmp0 = tmp1; + } + } + + // calculate numerator polynom + pPoly->numLength = 1; + pPoly->numData[0] = pkZpg->gain; + for (i = 0; i < pkZpg->zerosLength; i++) + { + tmp0 = 0.0; + pPoly->numData[pPoly->numLength] = 0; + pPoly->numLength++; + for (j = 0; j < pPoly->numLength; j++) + { + tmp1 = pPoly->numData[j]; + pPoly->numData[j] = tmp1 * -pkZpg->zerosData[i] + tmp0; + tmp0 = tmp1; + } + } +} diff --git a/src/drivers/lsm303d/iirFilter.h b/src/drivers/lsm303d/iirFilter.h new file mode 100644 index 0000000000..ab4223a8ed --- /dev/null +++ b/src/drivers/lsm303d/iirFilter.h @@ -0,0 +1,59 @@ +#ifndef IIRFILTER__H__ +#define IIRFILTER__H__ + +__BEGIN_DECLS + +#define MAX_LENGTH 4 + +typedef struct FILTER_s +{ + float denData[MAX_LENGTH]; + float numData[MAX_LENGTH]; + int denLength; + int numLength; + float Ts; + float inpData[MAX_LENGTH]; + float outData[MAX_LENGTH]; + unsigned int inpCnt; +} FIL_T; + +typedef struct TF_ZPG_s +{ + int zerosLength; + double zerosData[MAX_LENGTH + 1]; + int polesLength; + double polesData[MAX_LENGTH + 1]; + double gain; + double Ts; +} TF_ZPG_t; + +typedef struct TF_POLY_s +{ + int numLength; + double numData[MAX_LENGTH]; + int denLength; + double denData[MAX_LENGTH]; + double Ts; +} TF_POLY_t; + +typedef struct TF_DIF_s +{ + int numInt; + int numDiff; + int lowpassLength; + int highpassLength; + double lowpassData[MAX_LENGTH]; + int highpassData[MAX_LENGTH]; + double gain; + double Ts; +} TF_DIF_t; + +__EXPORT int testFunction(void); + +__EXPORT float updateFilter(FIL_T *pFilt, float inp); + +__EXPORT int initFilter(const TF_DIF_t *pDifc, double Ts, FIL_T *pFilt); + +__END_DECLS + +#endif diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index b157d74c6e..80d777826e 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -65,6 +65,7 @@ #include #include +#include "iirFilter.h" /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -220,6 +221,10 @@ private: unsigned _current_samplerate; + FIL_T _filter_x; + FIL_T _filter_y; + FIL_T _filter_z; + unsigned _num_mag_reports; volatile unsigned _next_mag_report; volatile unsigned _oldest_mag_report; @@ -489,6 +494,22 @@ LSM303D::init() set_antialias_filter_bandwidth(50); /* available bandwidths: 50, 194, 362 or 773 Hz */ set_samplerate(400); /* max sample rate */ + /* initiate filter */ + TF_DIF_t tf_filter; + tf_filter.numInt = 0; + tf_filter.numDiff = 0; + tf_filter.lowpassLength = 2; + tf_filter.highpassLength = 0; + tf_filter.lowpassData[0] = 10; + tf_filter.lowpassData[1] = 10; + //tf_filter.highpassData[0] = ; + tf_filter.gain = 1; + tf_filter.Ts = 1/_current_samplerate; + + initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_x); + initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_y); + initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_z); + mag_set_range(4); /* XXX: take highest sensor range of 12GA? */ mag_set_samplerate(100); @@ -1161,9 +1182,14 @@ LSM303D::measure() accel_report->y_raw = raw_accel_report.y; accel_report->z_raw = raw_accel_report.z; - accel_report->x = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - accel_report->y = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - accel_report->z = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + float x_in_new = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + float y_in_new = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + float z_in_new = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + + accel_report->x = updateFilter(&_filter_x, x_in_new); + accel_report->y = updateFilter(&_filter_y, y_in_new); + accel_report->z = updateFilter(&_filter_z, z_in_new); + accel_report->scaling = _accel_range_scale; accel_report->range_m_s2 = _accel_range_m_s2; diff --git a/src/drivers/lsm303d/module.mk b/src/drivers/lsm303d/module.mk index e93b1e3318..8fd5679c9f 100644 --- a/src/drivers/lsm303d/module.mk +++ b/src/drivers/lsm303d/module.mk @@ -3,4 +3,5 @@ # MODULE_COMMAND = lsm303d -SRCS = lsm303d.cpp +SRCS = lsm303d.cpp \ + iirFilter.c From e309b9ab4a633065f06a0f0c66542df84e6147d5 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 18 Jul 2013 19:45:44 +0200 Subject: [PATCH 284/486] Disable IIR filter for now --- src/drivers/lsm303d/lsm303d.cpp | 52 ++++++++++++++++++--------------- 1 file changed, 28 insertions(+), 24 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 80d777826e..a9a9bb69fd 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -221,9 +221,9 @@ private: unsigned _current_samplerate; - FIL_T _filter_x; - FIL_T _filter_y; - FIL_T _filter_z; +// FIL_T _filter_x; +// FIL_T _filter_y; +// FIL_T _filter_z; unsigned _num_mag_reports; volatile unsigned _next_mag_report; @@ -495,20 +495,20 @@ LSM303D::init() set_samplerate(400); /* max sample rate */ /* initiate filter */ - TF_DIF_t tf_filter; - tf_filter.numInt = 0; - tf_filter.numDiff = 0; - tf_filter.lowpassLength = 2; - tf_filter.highpassLength = 0; - tf_filter.lowpassData[0] = 10; - tf_filter.lowpassData[1] = 10; - //tf_filter.highpassData[0] = ; - tf_filter.gain = 1; - tf_filter.Ts = 1/_current_samplerate; - - initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_x); - initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_y); - initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_z); +// TF_DIF_t tf_filter; +// tf_filter.numInt = 0; +// tf_filter.numDiff = 0; +// tf_filter.lowpassLength = 2; +// tf_filter.highpassLength = 0; +// tf_filter.lowpassData[0] = 10; +// tf_filter.lowpassData[1] = 10; +// //tf_filter.highpassData[0] = ; +// tf_filter.gain = 1; +// tf_filter.Ts = 1/_current_samplerate; +// +// initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_x); +// initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_y); +// initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_z); mag_set_range(4); /* XXX: take highest sensor range of 12GA? */ mag_set_samplerate(100); @@ -1182,13 +1182,17 @@ LSM303D::measure() accel_report->y_raw = raw_accel_report.y; accel_report->z_raw = raw_accel_report.z; - float x_in_new = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - float y_in_new = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - float z_in_new = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; - - accel_report->x = updateFilter(&_filter_x, x_in_new); - accel_report->y = updateFilter(&_filter_y, y_in_new); - accel_report->z = updateFilter(&_filter_z, z_in_new); +// float x_in_new = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; +// float y_in_new = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; +// float z_in_new = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; +// +// accel_report->x = updateFilter(&_filter_x, x_in_new); +// accel_report->y = updateFilter(&_filter_y, y_in_new); +// accel_report->z = updateFilter(&_filter_z, z_in_new); + + accel_report->x = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + accel_report->y = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + accel_report->z = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; accel_report->scaling = _accel_range_scale; accel_report->range_m_s2 = _accel_range_m_s2; From 374d204b9223b5b3c33ec1fd48a120a9984160c2 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 18 Jul 2013 19:48:15 +0200 Subject: [PATCH 285/486] Enable BDU instead of CONT mode --- src/drivers/lsm303d/lsm303d.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index a9a9bb69fd..844cc3884f 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -125,7 +125,7 @@ static const int ERROR = -1; #define REG1_RATE_800HZ_A ((1<<7) | (0<<6) | (0<<5) | (1<<4)) #define REG1_RATE_1600HZ_A ((1<<7) | (0<<6) | (1<<5) | (0<<4)) -#define REG1_CONT_UPDATE_A (0<<3) +#define REG1_BDU_UPDATE (1<<3) #define REG1_Z_ENABLE_A (1<<2) #define REG1_Y_ENABLE_A (1<<1) #define REG1_X_ENABLE_A (1<<0) @@ -482,7 +482,7 @@ LSM303D::init() _mag_topic = orb_advertise(ORB_ID(sensor_mag), &_mag_reports[0]); /* enable accel, XXX do this with an ioctl? */ - write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A); + write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE); /* enable mag, XXX do this with an ioctl? */ write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); From da1bf69ce249f22df16e7ccb21d17c08bcbbbedf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 19 Jul 2013 13:07:51 +0200 Subject: [PATCH 286/486] Added gyro scale estimation (but param is NOT written) --- src/modules/commander/gyro_calibration.c | 80 ++++++++++++++++++++++++ 1 file changed, 80 insertions(+) diff --git a/src/modules/commander/gyro_calibration.c b/src/modules/commander/gyro_calibration.c index f452910c9a..865f74ab44 100644 --- a/src/modules/commander/gyro_calibration.c +++ b/src/modules/commander/gyro_calibration.c @@ -104,6 +104,86 @@ void do_gyro_calibration(int mavlink_fd) gyro_offset[1] = gyro_offset[1] / calibration_count; gyro_offset[2] = gyro_offset[2] / calibration_count; + + + + /*** --- SCALING --- ***/ + + mavlink_log_info(mavlink_fd, "offset calibration finished. Rotate for scale 130x"); + mavlink_log_info(mavlink_fd, "or do not rotate and wait for 5 seconds to skip."); + warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip."); + + unsigned rotations_count = 30; + float gyro_integral = 0.0f; + float baseline_integral = 0.0f; + + // XXX change to mag topic + orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); + + float mag_last = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]); + if (mag_last > M_PI_F) mag_last -= 2*M_PI_F; + if (mag_last < -M_PI_F) mag_last += 2*M_PI_F; + + + uint64_t last_time = hrt_absolute_time(); + uint64_t start_time = hrt_absolute_time(); + + while ((int)fabsf(baseline_integral / (2.0f * M_PI_F)) < rotations_count) { + + /* abort this loop if not rotated more than 180 degrees within 5 seconds */ + if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f) + && (hrt_absolute_time() - start_time > 5 * 1e6)) + break; + + /* wait blocking for new data */ + struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + + float dt_ms = (hrt_absolute_time() - last_time) / 1e3f; + last_time = hrt_absolute_time(); + + orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); + + // XXX this is just a proof of concept and needs world / body + // transformation and more + + //math::Vector2f magNav(raw.magnetometer_ga); + + // calculate error between estimate and measurement + // apply declination correction for true heading as well. + //float mag = -atan2f(magNav(1),magNav(0)); + float mag = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]); + if (mag > M_PI_F) mag -= 2*M_PI_F; + if (mag < -M_PI_F) mag += 2*M_PI_F; + + float diff = mag - mag_last; + + if (diff > M_PI_F) diff -= 2*M_PI_F; + if (diff < -M_PI_F) diff += 2*M_PI_F; + + baseline_integral += diff; + mag_last = mag; + // Jump through some timing scale hoops to avoid + // operating near the 1e6/1e8 max sane resolution of float. + gyro_integral += (raw.gyro_rad_s[2] * dt_ms) / 1e3f; + + warnx("dbg: b: %6.4f, g: %6.4f", baseline_integral, gyro_integral); + + // } else if (poll_ret == 0) { + // /* any poll failure for 1s is a reason to abort */ + // mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); + // return; + } + } + + float gyro_scale = baseline_integral / gyro_integral; + warnx("gyro scale: yaw (z): %6.4f", gyro_scale); + mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", gyro_scale); + + if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) { if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0])) From c93e743374731ec06020ba6919c6d48594d4a58c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 19 Jul 2013 17:47:32 +0200 Subject: [PATCH 287/486] Changed the MS5611 from the workq to hrt_call_every implementation, this seems to solve the SPI chip select overlaps --- src/drivers/ms5611/ms5611.cpp | 138 +++++++++++++--------------------- 1 file changed, 54 insertions(+), 84 deletions(-) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index d9268c0b30..4524160355 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -111,8 +111,9 @@ protected: ms5611::prom_s _prom; - struct work_s _work; - unsigned _measure_ticks; + struct hrt_call _baro_call; + + unsigned _call_baro_interval; unsigned _num_reports; volatile unsigned _next_report; @@ -143,12 +144,12 @@ protected: * @note This function is called at open and error time. It might make sense * to make it more aggressive about resetting the bus in case of errors. */ - void start_cycle(); + void start(); /** * Stop the automatic measurement state machine. */ - void stop_cycle(); + void stop(); /** * Perform a poll cycle; collect from the previous measurement @@ -166,12 +167,11 @@ protected: void cycle(); /** - * Static trampoline from the workq context; because we don't have a - * generic workq wrapper yet. + * Static trampoline; XXX is this needed? * * @param arg Instance pointer for the driver that is polling. */ - static void cycle_trampoline(void *arg); + void cycle_trampoline(void *arg); /** * Issue a measurement command for the current state. @@ -184,6 +184,7 @@ protected: * Collect the result of the most recent measurement. */ virtual int collect(); + }; /* @@ -195,7 +196,7 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) : CDev("MS5611", BARO_DEVICE_PATH), _interface(interface), _prom(prom_buf.s), - _measure_ticks(0), + _call_baro_interval(0), _num_reports(0), _next_report(0), _oldest_report(0), @@ -212,14 +213,13 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) : _comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows")) { - // work_cancel in stop_cycle called from the dtor will explode if we don't do this... - memset(&_work, 0, sizeof(_work)); + } MS5611::~MS5611() { /* make sure we are truly inactive */ - stop_cycle(); + stop(); /* free any existing reports */ if (_reports != nullptr) @@ -277,7 +277,7 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) return -ENOSPC; /* if automatic measurement is enabled */ - if (_measure_ticks > 0) { + if (_call_baro_interval > 0) { /* * While there is space in the caller's buffer, and reports, copy them. @@ -298,41 +298,13 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) /* manual measurement - run one conversion */ /* XXX really it'd be nice to lock against other readers here */ - do { - _measure_phase = 0; - _oldest_report = _next_report = 0; + _measure_phase = 0; + _oldest_report = _next_report = 0; + measure(); - /* do temperature first */ - if (OK != measure()) { - ret = -EIO; - break; - } - - usleep(MS5611_CONVERSION_INTERVAL); - - if (OK != collect()) { - ret = -EIO; - break; - } - - /* now do a pressure measurement */ - if (OK != measure()) { - ret = -EIO; - break; - } - - usleep(MS5611_CONVERSION_INTERVAL); - - if (OK != collect()) { - ret = -EIO; - break; - } - - /* state machine will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); - - } while (0); + /* state machine will have generated a report, copy it out */ + memcpy(buffer, _reports, sizeof(*_reports)); + ret = sizeof(*_reports); return ret; } @@ -347,8 +319,8 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: - stop_cycle(); - _measure_ticks = 0; + stop(); + _call_baro_interval = 0; return OK; /* external signalling not supported */ @@ -362,14 +334,14 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: { /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); + bool want_start = (_call_baro_interval == 0); /* set interval for next measurement to minimum legal value */ - _measure_ticks = USEC2TICK(MS5611_CONVERSION_INTERVAL); + _baro_call.period = _call_baro_interval = USEC2TICK(MS5611_CONVERSION_INTERVAL); /* if we need to start the poll state machine, do it */ if (want_start) - start_cycle(); + start(); return OK; } @@ -377,7 +349,7 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); + bool want_start = (_call_baro_interval == 0); /* convert hz to tick interval via microseconds */ unsigned ticks = USEC2TICK(1000000 / arg); @@ -387,11 +359,11 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; /* update interval for next measurement */ - _measure_ticks = ticks; + _baro_call.period = _call_baro_interval = ticks; /* if we need to start the poll state machine, do it */ if (want_start) - start_cycle(); + start(); return OK; } @@ -399,10 +371,10 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_measure_ticks == 0) + if (_call_baro_interval == 0) return SENSOR_POLLRATE_MANUAL; - return (1000 / _measure_ticks); + return (1000 / _call_baro_interval); case SENSORIOCSQUEUEDEPTH: { /* add one to account for the sentinel in the ring */ @@ -419,11 +391,11 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) return -ENOMEM; /* reset the measurement state machine with the new buffer, free the old */ - stop_cycle(); + stop(); delete[] _reports; _num_reports = arg; _reports = buf; - start_cycle(); + start(); return OK; } @@ -457,30 +429,32 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) } void -MS5611::start_cycle() +MS5611::start() { + /* make sure we are stopped first */ + stop(); - /* reset the report ring and state machine */ - _collect_phase = false; - _measure_phase = 0; + /* reset the report ring */ _oldest_report = _next_report = 0; + /* reset to first measure phase */ + _measure_phase = 0; /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, 1); + hrt_call_every(&_baro_call, 1000, _call_baro_interval, (hrt_callout)&MS5611::cycle_trampoline, this); } void -MS5611::stop_cycle() +MS5611::stop() { - work_cancel(HPWORK, &_work); + hrt_cancel(&_baro_call); } void MS5611::cycle_trampoline(void *arg) { - MS5611 *dev = reinterpret_cast(arg); + MS5611 *dev = (MS5611 *)arg; - dev->cycle(); + cycle(); } void @@ -504,7 +478,7 @@ MS5611::cycle() //log("collection error %d", ret); } /* reset the collection state machine and try again */ - start_cycle(); + start(); return; } @@ -517,14 +491,16 @@ MS5611::cycle() * doing pressure measurements at something close to the desired rate. */ if ((_measure_phase != 0) && - (_measure_ticks > USEC2TICK(MS5611_CONVERSION_INTERVAL))) { + (_call_baro_interval > USEC2TICK(MS5611_CONVERSION_INTERVAL))) { - /* schedule a fresh cycle call when we are ready to measure again */ - work_queue(HPWORK, - &_work, - (worker_t)&MS5611::cycle_trampoline, - this, - _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL)); + // XXX maybe do something appropriate as well + +// /* schedule a fresh cycle call when we are ready to measure again */ +// work_queue(HPWORK, +// &_work, +// (worker_t)&MS5611::cycle_trampoline, +// this, +// _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL)); return; } @@ -535,19 +511,12 @@ MS5611::cycle() if (ret != OK) { //log("measure error %d", ret); /* reset the collection state machine and try again */ - start_cycle(); + start(); return; } /* next phase is collection */ _collect_phase = true; - - /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, - &_work, - (worker_t)&MS5611::cycle_trampoline, - this, - USEC2TICK(MS5611_CONVERSION_INTERVAL)); } int @@ -696,13 +665,14 @@ MS5611::collect() return OK; } + void MS5611::print_info() { perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); perf_print_counter(_buffer_overflows); - printf("poll interval: %u ticks\n", _measure_ticks); + printf("poll interval: %u ticks\n", _call_baro_interval); printf("report queue: %u (%u/%u @ %p)\n", _num_reports, _oldest_report, _next_report, _reports); printf("TEMP: %d\n", _TEMP); From 094ff1261aa4a651e898c50d4451d283cb899a72 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 19 Jul 2013 18:30:01 +0200 Subject: [PATCH 288/486] Corrected the interval of the MS5611 --- src/drivers/ms5611/ms5611.cpp | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 4524160355..122cf0cec3 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -336,8 +336,9 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) /* do we need to start internal polling? */ bool want_start = (_call_baro_interval == 0); - /* set interval for next measurement to minimum legal value */ - _baro_call.period = _call_baro_interval = USEC2TICK(MS5611_CONVERSION_INTERVAL); + /* set interval to minimum legal value */ + _baro_call.period = _call_baro_interval = MS5611_CONVERSION_INTERVAL; + /* if we need to start the poll state machine, do it */ if (want_start) @@ -351,15 +352,12 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) /* do we need to start internal polling? */ bool want_start = (_call_baro_interval == 0); - /* convert hz to tick interval via microseconds */ - unsigned ticks = USEC2TICK(1000000 / arg); - /* check against maximum rate */ - if (ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL)) + if (1000000/arg < MS5611_CONVERSION_INTERVAL) return -EINVAL; - /* update interval for next measurement */ - _baro_call.period = _call_baro_interval = ticks; + /* update interval */ + _baro_call.period = _call_baro_interval = 1000000/arg; /* if we need to start the poll state machine, do it */ if (want_start) From 1dac58571eadf528e949c4e170de1edf61be2982 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 19 Jul 2013 22:40:45 +0400 Subject: [PATCH 289/486] multirotor_pos_control: minor cleanup --- src/modules/multirotor_pos_control/multirotor_pos_control.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index e5a3f3e548..b49186ee92 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -95,7 +95,7 @@ static void usage(const char *reason) if (reason) fprintf(stderr, "%s\n", reason); - fprintf(stderr, "usage: deamon {start|stop|status} [-p ]\n\n"); + fprintf(stderr, "usage: multirotor_pos_control {start|stop|status}\n\n"); exit(1); } @@ -314,7 +314,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) double lat_home = local_pos.home_lat * 1e-7; double lon_home = local_pos.home_lon * 1e-7; map_projection_init(lat_home, lon_home); - warnx("local pos home: lat = %.10f, lon = %.10f", lat_home, lon_home); + warnx("local pos home: lat = %.10f, lon = %.10f", lat_home, lon_home); mavlink_log_info(mavlink_fd, "local pos home: %.7f, %.7f", lat_home, lon_home); } From 68ab69de010adbe37b37558824ca013ecd0d569a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 20 Jul 2013 13:47:51 +0200 Subject: [PATCH 290/486] moved commander to C++, preparation for better gyro scale calibration respecting the current attitude for more accurate results --- ...ration.c => accelerometer_calibration.cpp} | 59 +++++++++------- ...calibration.c => airspeed_calibration.cpp} | 6 +- ...aro_calibration.c => baro_calibration.cpp} | 2 +- ...on_routines.c => calibration_routines.cpp} | 2 +- .../commander/{commander.c => commander.cpp} | 19 +++-- ...ommander_helper.c => commander_helper.cpp} | 13 ++-- .../{commander.h => commander_params.c} | 28 ++++---- ...yro_calibration.c => gyro_calibration.cpp} | 70 ++++++++++++++++--- ...{mag_calibration.c => mag_calibration.cpp} | 6 +- src/modules/commander/module.mk | 21 +++--- .../{rc_calibration.c => rc_calibration.cpp} | 2 +- ...hine_helper.c => state_machine_helper.cpp} | 7 +- src/modules/systemlib/systemlib.h | 7 +- 13 files changed, 156 insertions(+), 86 deletions(-) rename src/modules/commander/{accelerometer_calibration.c => accelerometer_calibration.cpp} (91%) rename src/modules/commander/{airspeed_calibration.c => airspeed_calibration.cpp} (97%) rename src/modules/commander/{baro_calibration.c => baro_calibration.cpp} (98%) rename src/modules/commander/{calibration_routines.c => calibration_routines.cpp} (99%) rename src/modules/commander/{commander.c => commander.cpp} (99%) rename src/modules/commander/{commander_helper.c => commander_helper.cpp} (97%) rename src/modules/commander/{commander.h => commander_params.c} (76%) rename src/modules/commander/{gyro_calibration.c => gyro_calibration.cpp} (83%) rename src/modules/commander/{mag_calibration.c => mag_calibration.cpp} (98%) rename src/modules/commander/{rc_calibration.c => rc_calibration.cpp} (99%) rename src/modules/commander/{state_machine_helper.c => state_machine_helper.cpp} (99%) diff --git a/src/modules/commander/accelerometer_calibration.c b/src/modules/commander/accelerometer_calibration.cpp similarity index 91% rename from src/modules/commander/accelerometer_calibration.c rename to src/modules/commander/accelerometer_calibration.cpp index bc9d24956f..df92d51d20 100644 --- a/src/modules/commander/accelerometer_calibration.c +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -33,7 +33,7 @@ ****************************************************************************/ /** - * @file accelerometer_calibration.c + * @file accelerometer_calibration.cpp * * Implementation of accelerometer calibration. * @@ -113,8 +113,6 @@ #include #include #include - - #include #include #include @@ -123,6 +121,12 @@ #include #include +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]); int detect_orientation(int mavlink_fd, int sub_sensor_combined); int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num); @@ -229,7 +233,10 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float sprintf(str, "meas started: %s", orientation_strs[orient]); mavlink_log_info(mavlink_fd, str); read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num); - str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient], accel_ref[orient][0], accel_ref[orient][1], accel_ref[orient][2]); + str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient], + (double)accel_ref[orient][0], + (double)accel_ref[orient][1], + (double)accel_ref[orient][2]); mavlink_log_info(mavlink_fd, str); data_collected[orient] = true; tune_neutral(); @@ -274,7 +281,9 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { float accel_err_thr = 5.0f; /* still time required in us */ int64_t still_time = 2000000; - struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; + struct pollfd fds[1]; + fds[0].fd = sub_sensor_combined; + fds[0].events = POLLIN; hrt_abstime t_start = hrt_absolute_time(); /* set timeout to 30s */ @@ -342,29 +351,29 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { } } - if ( fabs(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr && - fabs(accel_ema[1]) < accel_err_thr && - fabs(accel_ema[2]) < accel_err_thr ) + if ( fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr ) return 0; // [ g, 0, 0 ] - if ( fabs(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr && - fabs(accel_ema[1]) < accel_err_thr && - fabs(accel_ema[2]) < accel_err_thr ) + if ( fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr ) return 1; // [ -g, 0, 0 ] - if ( fabs(accel_ema[0]) < accel_err_thr && - fabs(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr && - fabs(accel_ema[2]) < accel_err_thr ) + if ( fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr ) return 2; // [ 0, g, 0 ] - if ( fabs(accel_ema[0]) < accel_err_thr && - fabs(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr && - fabs(accel_ema[2]) < accel_err_thr ) + if ( fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr ) return 3; // [ 0, -g, 0 ] - if ( fabs(accel_ema[0]) < accel_err_thr && - fabs(accel_ema[1]) < accel_err_thr && - fabs(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr ) + if ( fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr ) return 4; // [ 0, 0, g ] - if ( fabs(accel_ema[0]) < accel_err_thr && - fabs(accel_ema[1]) < accel_err_thr && - fabs(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr ) + if ( fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr ) return 5; // [ 0, 0, -g ] mavlink_log_info(mavlink_fd, "ERROR: invalid orientation"); @@ -376,7 +385,9 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { * Read specified number of accelerometer samples, calculate average and dispersion. */ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num) { - struct pollfd fds[1] = { { .fd = sensor_combined_sub, .events = POLLIN } }; + struct pollfd fds[1]; + fds[0].fd = sensor_combined_sub; + fds[0].events = POLLIN; int count = 0; float accel_sum[3] = { 0.0f, 0.0f, 0.0f }; diff --git a/src/modules/commander/airspeed_calibration.c b/src/modules/commander/airspeed_calibration.cpp similarity index 97% rename from src/modules/commander/airspeed_calibration.c rename to src/modules/commander/airspeed_calibration.cpp index feaf11aee7..df08292e32 100644 --- a/src/modules/commander/airspeed_calibration.c +++ b/src/modules/commander/airspeed_calibration.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file airspeed_calibration.c + * @file airspeed_calibration.cpp * Airspeed sensor calibration routine */ @@ -65,7 +65,9 @@ void do_airspeed_calibration(int mavlink_fd) while (calibration_counter < calibration_count) { /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = diff_pres_sub, .events = POLLIN } }; + struct pollfd fds[1]; + fds[0].fd = diff_pres_sub; + fds[0].events = POLLIN; int poll_ret = poll(fds, 1, 1000); diff --git a/src/modules/commander/baro_calibration.c b/src/modules/commander/baro_calibration.cpp similarity index 98% rename from src/modules/commander/baro_calibration.c rename to src/modules/commander/baro_calibration.cpp index a705947948..d7515b3d9b 100644 --- a/src/modules/commander/baro_calibration.c +++ b/src/modules/commander/baro_calibration.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file baro_calibration.c + * @file baro_calibration.cpp * Barometer calibration routine */ diff --git a/src/modules/commander/calibration_routines.c b/src/modules/commander/calibration_routines.cpp similarity index 99% rename from src/modules/commander/calibration_routines.c rename to src/modules/commander/calibration_routines.cpp index 799cd42697..be38ea1046 100644 --- a/src/modules/commander/calibration_routines.c +++ b/src/modules/commander/calibration_routines.cpp @@ -33,7 +33,7 @@ ****************************************************************************/ /** - * @file calibration_routines.c + * @file calibration_routines.cpp * Calibration routines implementations. * * @author Lorenz Meier diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.cpp similarity index 99% rename from src/modules/commander/commander.c rename to src/modules/commander/commander.cpp index c4dc495f6a..253b6295f1 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.cpp @@ -36,13 +36,11 @@ ****************************************************************************/ /** - * @file commander.c + * @file commander.cpp * Main system state machine implementation. * */ -#include "commander.h" - #include #include #include @@ -97,12 +95,11 @@ #include "rc_calibration.h" #include "airspeed_calibration.h" -PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */ -//PARAM_DEFINE_INT32(SYS_FAILSAVE_HL, 0); /**< Go into high-level failsafe after 0 ms */ -PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); -PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); -PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); - +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; extern struct system_load_s system_load; @@ -160,8 +157,10 @@ enum { * * The actual stack size should be set in the call * to task_create(). + * + * @ingroup apps */ -__EXPORT int commander_main(int argc, char *argv[]); +extern "C" __EXPORT int commander_main(int argc, char *argv[]); /** * Print the correct usage. diff --git a/src/modules/commander/commander_helper.c b/src/modules/commander/commander_helper.cpp similarity index 97% rename from src/modules/commander/commander_helper.c rename to src/modules/commander/commander_helper.cpp index 199f73e6c8..9427bd8925 100644 --- a/src/modules/commander/commander_helper.c +++ b/src/modules/commander/commander_helper.cpp @@ -34,7 +34,7 @@ ****************************************************************************/ /** - * @file commander_helper.c + * @file commander_helper.cpp * Commander helper functions implementations */ @@ -56,6 +56,12 @@ #include "commander_helper.h" +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + bool is_multirotor(const struct vehicle_status_s *current_status) { return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || @@ -175,11 +181,6 @@ int led_off(int led) return ioctl(leds, LED_OFF, led); } - -PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f); -PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f); -PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3); - float battery_remaining_estimate_voltage(float voltage) { float ret = 0; diff --git a/src/modules/commander/commander.h b/src/modules/commander/commander_params.c similarity index 76% rename from src/modules/commander/commander.h rename to src/modules/commander/commander_params.c index 6e57c0ba5f..6832fc5ce3 100644 --- a/src/modules/commander/commander.h +++ b/src/modules/commander/commander_params.c @@ -1,10 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Petri Tanskanen - * Lorenz Meier - * Thomas Gubler - * Julian Oes + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,19 +33,22 @@ ****************************************************************************/ /** - * @file commander.h - * Main system state machine definition. + * @file commander_params.c + * + * Parameters defined by the sensors task. * - * @author Petri Tanskanen * @author Lorenz Meier * @author Thomas Gubler * @author Julian Oes - * */ -#ifndef COMMANDER_H_ -#define COMMANDER_H_ +#include +#include - - -#endif /* COMMANDER_H_ */ +PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */ +PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); +PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); +PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); +PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f); +PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f); +PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3); diff --git a/src/modules/commander/gyro_calibration.c b/src/modules/commander/gyro_calibration.cpp similarity index 83% rename from src/modules/commander/gyro_calibration.c rename to src/modules/commander/gyro_calibration.cpp index 865f74ab44..9e6909db07 100644 --- a/src/modules/commander/gyro_calibration.c +++ b/src/modules/commander/gyro_calibration.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file gyro_calibration.c + * @file gyro_calibration.cpp * Gyroscope calibration routine */ @@ -82,7 +82,9 @@ void do_gyro_calibration(int mavlink_fd) while (calibration_counter < calibration_count) { /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; + struct pollfd fds[1]; + fds[0].fd = sub_sensor_combined; + fds[0].events = POLLIN; int poll_ret = poll(fds, 1, 1000); @@ -105,6 +107,49 @@ void do_gyro_calibration(int mavlink_fd) gyro_offset[2] = gyro_offset[2] / calibration_count; + if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) { + + if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0])) + || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1])) + || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) { + mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!"); + } + + /* set offsets to actual value */ + fd = open(GYRO_DEVICE_PATH, 0); + struct gyro_scale gscale = { + gyro_offset[0], + 1.0f, + gyro_offset[1], + 1.0f, + gyro_offset[2], + 1.0f, + }; + + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) + warn("WARNING: failed to set scale / offsets for gyro"); + + close(fd); + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warnx("WARNING: auto-save of params to storage failed"); + mavlink_log_critical(mavlink_fd, "gyro store failed"); + // XXX negative tune + return; + } + + mavlink_log_info(mavlink_fd, "gyro calibration done"); + + tune_positive(); + /* third beep by cal end routine */ + + } else { + mavlink_log_info(mavlink_fd, "offset cal FAILED (NaN)"); + return; + } /*** --- SCALING --- ***/ @@ -136,7 +181,9 @@ void do_gyro_calibration(int mavlink_fd) break; /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; + struct pollfd fds[1]; + fds[0].fd = sub_sensor_combined; + fds[0].events = POLLIN; int poll_ret = poll(fds, 1, 1000); @@ -180,27 +227,28 @@ void do_gyro_calibration(int mavlink_fd) } float gyro_scale = baseline_integral / gyro_integral; + float gyro_scales[] = { gyro_scale, gyro_scale, gyro_scale }; warnx("gyro scale: yaw (z): %6.4f", gyro_scale); mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", gyro_scale); - if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) { + if (isfinite(gyro_scales[0]) && isfinite(gyro_scales[1]) && isfinite(gyro_scales[2])) { - if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0])) - || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1])) - || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) { - mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!"); + if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scales[0])) + || param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scales[1])) + || param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scales[2]))) { + mavlink_log_critical(mavlink_fd, "Setting gyro scale failed!"); } /* set offsets to actual value */ fd = open(GYRO_DEVICE_PATH, 0); struct gyro_scale gscale = { gyro_offset[0], - 1.0f, + gyro_scales[0], gyro_offset[1], - 1.0f, + gyro_scales[1], gyro_offset[2], - 1.0f, + gyro_scales[2], }; if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) diff --git a/src/modules/commander/mag_calibration.c b/src/modules/commander/mag_calibration.cpp similarity index 98% rename from src/modules/commander/mag_calibration.c rename to src/modules/commander/mag_calibration.cpp index dbd31a7e75..9a25103f8c 100644 --- a/src/modules/commander/mag_calibration.c +++ b/src/modules/commander/mag_calibration.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file mag_calibration.c + * @file mag_calibration.cpp * Magnetometer calibration routine */ @@ -120,7 +120,9 @@ void do_mag_calibration(int mavlink_fd) calibration_counter < calibration_maxcount) { /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = sub_mag, .events = POLLIN } }; + struct pollfd fds[1]; + fds[0].fd = sub_mag; + fds[0].events = POLLIN; /* user guidance */ if (hrt_absolute_time() >= axis_deadline && diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index fef8e366b2..91d75121eb 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -36,14 +36,15 @@ # MODULE_COMMAND = commander -SRCS = commander.c \ - state_machine_helper.c \ - commander_helper.c \ - calibration_routines.c \ - accelerometer_calibration.c \ - gyro_calibration.c \ - mag_calibration.c \ - baro_calibration.c \ - rc_calibration.c \ - airspeed_calibration.c +SRCS = commander.cpp \ + commander_params.c \ + state_machine_helper.cpp \ + commander_helper.cpp \ + calibration_routines.cpp \ + accelerometer_calibration.cpp \ + gyro_calibration.cpp \ + mag_calibration.cpp \ + baro_calibration.cpp \ + rc_calibration.cpp \ + airspeed_calibration.cpp diff --git a/src/modules/commander/rc_calibration.c b/src/modules/commander/rc_calibration.cpp similarity index 99% rename from src/modules/commander/rc_calibration.c rename to src/modules/commander/rc_calibration.cpp index a21d3f558b..0de411713b 100644 --- a/src/modules/commander/rc_calibration.c +++ b/src/modules/commander/rc_calibration.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file rc_calibration.c + * @file rc_calibration.cpp * Remote Control calibration routine */ diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.cpp similarity index 99% rename from src/modules/commander/state_machine_helper.c rename to src/modules/commander/state_machine_helper.cpp index 792ead8f3c..3cf60a99ac 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.cpp @@ -34,7 +34,7 @@ ****************************************************************************/ /** - * @file state_machine_helper.c + * @file state_machine_helper.cpp * State machine helper functions implementations */ @@ -56,6 +56,11 @@ #include "state_machine_helper.h" #include "commander_helper.h" +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int armed_pub, struct actuator_armed_s *armed, const int mavlink_fd) { diff --git a/src/modules/systemlib/systemlib.h b/src/modules/systemlib/systemlib.h index 0194b5e529..356215b0e2 100644 --- a/src/modules/systemlib/systemlib.h +++ b/src/modules/systemlib/systemlib.h @@ -42,11 +42,12 @@ #include #include -/** Reboots the board */ -extern void up_systemreset(void) noreturn_function; - __BEGIN_DECLS +/** Reboots the board */ +//extern void up_systemreset(void) noreturn_function; +#include <../arch/common/up_internal.h> + /** Sends SIGUSR1 to all processes */ __EXPORT void killall(void); From eb52eae153bc83144ddb5d6d03fdbb22aa0c9203 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 20 Jul 2013 13:48:19 +0200 Subject: [PATCH 291/486] Code style for BlinkM --- src/drivers/blinkm/blinkm.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 9bb020a6b4..aa94be4938 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -866,11 +866,13 @@ BlinkM::get_firmware_version(uint8_t version[2]) return transfer(&msg, sizeof(msg), version, 2); } +void blinkm_usage(); + void blinkm_usage() { - fprintf(stderr, "missing command: try 'start', 'systemstate', 'ledoff', 'list' or a script name {options}\n"); - fprintf(stderr, "options:\n"); - fprintf(stderr, "\t-b --bus i2cbus (3)\n"); - fprintf(stderr, "\t-a --blinkmaddr blinkmaddr (9)\n"); + warnx("missing command: try 'start', 'systemstate', 'ledoff', 'list' or a script name {options}"); + warnx("options:"); + warnx("\t-b --bus i2cbus (3)"); + warnx("\t-a --blinkmaddr blinkmaddr (9)"); } int From cfbdf7c9037c948e869fc805a79fb92e5441e15a Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 20 Jul 2013 18:51:25 -0700 Subject: [PATCH 292/486] Revert "Corrected the interval of the MS5611" This reverts commit 094ff1261aa4a651e898c50d4451d283cb899a72. --- src/drivers/ms5611/ms5611.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 122cf0cec3..4524160355 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -336,9 +336,8 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) /* do we need to start internal polling? */ bool want_start = (_call_baro_interval == 0); - /* set interval to minimum legal value */ - _baro_call.period = _call_baro_interval = MS5611_CONVERSION_INTERVAL; - + /* set interval for next measurement to minimum legal value */ + _baro_call.period = _call_baro_interval = USEC2TICK(MS5611_CONVERSION_INTERVAL); /* if we need to start the poll state machine, do it */ if (want_start) @@ -352,12 +351,15 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) /* do we need to start internal polling? */ bool want_start = (_call_baro_interval == 0); + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); + /* check against maximum rate */ - if (1000000/arg < MS5611_CONVERSION_INTERVAL) + if (ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL)) return -EINVAL; - /* update interval */ - _baro_call.period = _call_baro_interval = 1000000/arg; + /* update interval for next measurement */ + _baro_call.period = _call_baro_interval = ticks; /* if we need to start the poll state machine, do it */ if (want_start) From f6daafba03fab76bac41fbaac60ea8c4d324c65b Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 20 Jul 2013 18:51:46 -0700 Subject: [PATCH 293/486] Revert "Changed the MS5611 from the workq to hrt_call_every implementation, this seems to solve the SPI chip select overlaps" This reverts commit c93e743374731ec06020ba6919c6d48594d4a58c. --- src/drivers/ms5611/ms5611.cpp | 138 +++++++++++++++++++++------------- 1 file changed, 84 insertions(+), 54 deletions(-) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 4524160355..d9268c0b30 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -111,9 +111,8 @@ protected: ms5611::prom_s _prom; - struct hrt_call _baro_call; - - unsigned _call_baro_interval; + struct work_s _work; + unsigned _measure_ticks; unsigned _num_reports; volatile unsigned _next_report; @@ -144,12 +143,12 @@ protected: * @note This function is called at open and error time. It might make sense * to make it more aggressive about resetting the bus in case of errors. */ - void start(); + void start_cycle(); /** * Stop the automatic measurement state machine. */ - void stop(); + void stop_cycle(); /** * Perform a poll cycle; collect from the previous measurement @@ -167,11 +166,12 @@ protected: void cycle(); /** - * Static trampoline; XXX is this needed? + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. * * @param arg Instance pointer for the driver that is polling. */ - void cycle_trampoline(void *arg); + static void cycle_trampoline(void *arg); /** * Issue a measurement command for the current state. @@ -184,7 +184,6 @@ protected: * Collect the result of the most recent measurement. */ virtual int collect(); - }; /* @@ -196,7 +195,7 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) : CDev("MS5611", BARO_DEVICE_PATH), _interface(interface), _prom(prom_buf.s), - _call_baro_interval(0), + _measure_ticks(0), _num_reports(0), _next_report(0), _oldest_report(0), @@ -213,13 +212,14 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) : _comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows")) { - + // work_cancel in stop_cycle called from the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); } MS5611::~MS5611() { /* make sure we are truly inactive */ - stop(); + stop_cycle(); /* free any existing reports */ if (_reports != nullptr) @@ -277,7 +277,7 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) return -ENOSPC; /* if automatic measurement is enabled */ - if (_call_baro_interval > 0) { + if (_measure_ticks > 0) { /* * While there is space in the caller's buffer, and reports, copy them. @@ -298,13 +298,41 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) /* manual measurement - run one conversion */ /* XXX really it'd be nice to lock against other readers here */ - _measure_phase = 0; - _oldest_report = _next_report = 0; - measure(); + do { + _measure_phase = 0; + _oldest_report = _next_report = 0; - /* state machine will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); + /* do temperature first */ + if (OK != measure()) { + ret = -EIO; + break; + } + + usleep(MS5611_CONVERSION_INTERVAL); + + if (OK != collect()) { + ret = -EIO; + break; + } + + /* now do a pressure measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } + + usleep(MS5611_CONVERSION_INTERVAL); + + if (OK != collect()) { + ret = -EIO; + break; + } + + /* state machine will have generated a report, copy it out */ + memcpy(buffer, _reports, sizeof(*_reports)); + ret = sizeof(*_reports); + + } while (0); return ret; } @@ -319,8 +347,8 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: - stop(); - _call_baro_interval = 0; + stop_cycle(); + _measure_ticks = 0; return OK; /* external signalling not supported */ @@ -334,14 +362,14 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: { /* do we need to start internal polling? */ - bool want_start = (_call_baro_interval == 0); + bool want_start = (_measure_ticks == 0); /* set interval for next measurement to minimum legal value */ - _baro_call.period = _call_baro_interval = USEC2TICK(MS5611_CONVERSION_INTERVAL); + _measure_ticks = USEC2TICK(MS5611_CONVERSION_INTERVAL); /* if we need to start the poll state machine, do it */ if (want_start) - start(); + start_cycle(); return OK; } @@ -349,7 +377,7 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ - bool want_start = (_call_baro_interval == 0); + bool want_start = (_measure_ticks == 0); /* convert hz to tick interval via microseconds */ unsigned ticks = USEC2TICK(1000000 / arg); @@ -359,11 +387,11 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; /* update interval for next measurement */ - _baro_call.period = _call_baro_interval = ticks; + _measure_ticks = ticks; /* if we need to start the poll state machine, do it */ if (want_start) - start(); + start_cycle(); return OK; } @@ -371,10 +399,10 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_call_baro_interval == 0) + if (_measure_ticks == 0) return SENSOR_POLLRATE_MANUAL; - return (1000 / _call_baro_interval); + return (1000 / _measure_ticks); case SENSORIOCSQUEUEDEPTH: { /* add one to account for the sentinel in the ring */ @@ -391,11 +419,11 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) return -ENOMEM; /* reset the measurement state machine with the new buffer, free the old */ - stop(); + stop_cycle(); delete[] _reports; _num_reports = arg; _reports = buf; - start(); + start_cycle(); return OK; } @@ -429,32 +457,30 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) } void -MS5611::start() +MS5611::start_cycle() { - /* make sure we are stopped first */ - stop(); - /* reset the report ring */ - _oldest_report = _next_report = 0; - /* reset to first measure phase */ + /* reset the report ring and state machine */ + _collect_phase = false; _measure_phase = 0; + _oldest_report = _next_report = 0; /* schedule a cycle to start things */ - hrt_call_every(&_baro_call, 1000, _call_baro_interval, (hrt_callout)&MS5611::cycle_trampoline, this); + work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, 1); } void -MS5611::stop() +MS5611::stop_cycle() { - hrt_cancel(&_baro_call); + work_cancel(HPWORK, &_work); } void MS5611::cycle_trampoline(void *arg) { - MS5611 *dev = (MS5611 *)arg; + MS5611 *dev = reinterpret_cast(arg); - cycle(); + dev->cycle(); } void @@ -478,7 +504,7 @@ MS5611::cycle() //log("collection error %d", ret); } /* reset the collection state machine and try again */ - start(); + start_cycle(); return; } @@ -491,16 +517,14 @@ MS5611::cycle() * doing pressure measurements at something close to the desired rate. */ if ((_measure_phase != 0) && - (_call_baro_interval > USEC2TICK(MS5611_CONVERSION_INTERVAL))) { + (_measure_ticks > USEC2TICK(MS5611_CONVERSION_INTERVAL))) { - // XXX maybe do something appropriate as well - -// /* schedule a fresh cycle call when we are ready to measure again */ -// work_queue(HPWORK, -// &_work, -// (worker_t)&MS5611::cycle_trampoline, -// this, -// _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL)); + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&MS5611::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL)); return; } @@ -511,12 +535,19 @@ MS5611::cycle() if (ret != OK) { //log("measure error %d", ret); /* reset the collection state machine and try again */ - start(); + start_cycle(); return; } /* next phase is collection */ _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&MS5611::cycle_trampoline, + this, + USEC2TICK(MS5611_CONVERSION_INTERVAL)); } int @@ -665,14 +696,13 @@ MS5611::collect() return OK; } - void MS5611::print_info() { perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); perf_print_counter(_buffer_overflows); - printf("poll interval: %u ticks\n", _call_baro_interval); + printf("poll interval: %u ticks\n", _measure_ticks); printf("report queue: %u (%u/%u @ %p)\n", _num_reports, _oldest_report, _next_report, _reports); printf("TEMP: %d\n", _TEMP); From 31ded04c80f68bd071840770d49eb93d2d9c9fe2 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 20 Jul 2013 19:09:59 -0700 Subject: [PATCH 294/486] Disable interrupts during ms5611 SPI transactions to prevent interruption by sensor drivers polling other sensors from interrupt context. --- src/drivers/ms5611/ms5611.cpp | 148 ++++++++++++------------------ src/drivers/ms5611/ms5611_spi.cpp | 28 +++++- 2 files changed, 82 insertions(+), 94 deletions(-) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index d9268c0b30..122cf0cec3 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -111,8 +111,9 @@ protected: ms5611::prom_s _prom; - struct work_s _work; - unsigned _measure_ticks; + struct hrt_call _baro_call; + + unsigned _call_baro_interval; unsigned _num_reports; volatile unsigned _next_report; @@ -143,12 +144,12 @@ protected: * @note This function is called at open and error time. It might make sense * to make it more aggressive about resetting the bus in case of errors. */ - void start_cycle(); + void start(); /** * Stop the automatic measurement state machine. */ - void stop_cycle(); + void stop(); /** * Perform a poll cycle; collect from the previous measurement @@ -166,12 +167,11 @@ protected: void cycle(); /** - * Static trampoline from the workq context; because we don't have a - * generic workq wrapper yet. + * Static trampoline; XXX is this needed? * * @param arg Instance pointer for the driver that is polling. */ - static void cycle_trampoline(void *arg); + void cycle_trampoline(void *arg); /** * Issue a measurement command for the current state. @@ -184,6 +184,7 @@ protected: * Collect the result of the most recent measurement. */ virtual int collect(); + }; /* @@ -195,7 +196,7 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) : CDev("MS5611", BARO_DEVICE_PATH), _interface(interface), _prom(prom_buf.s), - _measure_ticks(0), + _call_baro_interval(0), _num_reports(0), _next_report(0), _oldest_report(0), @@ -212,14 +213,13 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) : _comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows")) { - // work_cancel in stop_cycle called from the dtor will explode if we don't do this... - memset(&_work, 0, sizeof(_work)); + } MS5611::~MS5611() { /* make sure we are truly inactive */ - stop_cycle(); + stop(); /* free any existing reports */ if (_reports != nullptr) @@ -277,7 +277,7 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) return -ENOSPC; /* if automatic measurement is enabled */ - if (_measure_ticks > 0) { + if (_call_baro_interval > 0) { /* * While there is space in the caller's buffer, and reports, copy them. @@ -298,41 +298,13 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) /* manual measurement - run one conversion */ /* XXX really it'd be nice to lock against other readers here */ - do { - _measure_phase = 0; - _oldest_report = _next_report = 0; + _measure_phase = 0; + _oldest_report = _next_report = 0; + measure(); - /* do temperature first */ - if (OK != measure()) { - ret = -EIO; - break; - } - - usleep(MS5611_CONVERSION_INTERVAL); - - if (OK != collect()) { - ret = -EIO; - break; - } - - /* now do a pressure measurement */ - if (OK != measure()) { - ret = -EIO; - break; - } - - usleep(MS5611_CONVERSION_INTERVAL); - - if (OK != collect()) { - ret = -EIO; - break; - } - - /* state machine will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); - - } while (0); + /* state machine will have generated a report, copy it out */ + memcpy(buffer, _reports, sizeof(*_reports)); + ret = sizeof(*_reports); return ret; } @@ -347,8 +319,8 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: - stop_cycle(); - _measure_ticks = 0; + stop(); + _call_baro_interval = 0; return OK; /* external signalling not supported */ @@ -362,14 +334,15 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: { /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); + bool want_start = (_call_baro_interval == 0); + + /* set interval to minimum legal value */ + _baro_call.period = _call_baro_interval = MS5611_CONVERSION_INTERVAL; - /* set interval for next measurement to minimum legal value */ - _measure_ticks = USEC2TICK(MS5611_CONVERSION_INTERVAL); /* if we need to start the poll state machine, do it */ if (want_start) - start_cycle(); + start(); return OK; } @@ -377,21 +350,18 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); - - /* convert hz to tick interval via microseconds */ - unsigned ticks = USEC2TICK(1000000 / arg); + bool want_start = (_call_baro_interval == 0); /* check against maximum rate */ - if (ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL)) + if (1000000/arg < MS5611_CONVERSION_INTERVAL) return -EINVAL; - /* update interval for next measurement */ - _measure_ticks = ticks; + /* update interval */ + _baro_call.period = _call_baro_interval = 1000000/arg; /* if we need to start the poll state machine, do it */ if (want_start) - start_cycle(); + start(); return OK; } @@ -399,10 +369,10 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_measure_ticks == 0) + if (_call_baro_interval == 0) return SENSOR_POLLRATE_MANUAL; - return (1000 / _measure_ticks); + return (1000 / _call_baro_interval); case SENSORIOCSQUEUEDEPTH: { /* add one to account for the sentinel in the ring */ @@ -419,11 +389,11 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) return -ENOMEM; /* reset the measurement state machine with the new buffer, free the old */ - stop_cycle(); + stop(); delete[] _reports; _num_reports = arg; _reports = buf; - start_cycle(); + start(); return OK; } @@ -457,30 +427,32 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) } void -MS5611::start_cycle() +MS5611::start() { + /* make sure we are stopped first */ + stop(); - /* reset the report ring and state machine */ - _collect_phase = false; - _measure_phase = 0; + /* reset the report ring */ _oldest_report = _next_report = 0; + /* reset to first measure phase */ + _measure_phase = 0; /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, 1); + hrt_call_every(&_baro_call, 1000, _call_baro_interval, (hrt_callout)&MS5611::cycle_trampoline, this); } void -MS5611::stop_cycle() +MS5611::stop() { - work_cancel(HPWORK, &_work); + hrt_cancel(&_baro_call); } void MS5611::cycle_trampoline(void *arg) { - MS5611 *dev = reinterpret_cast(arg); + MS5611 *dev = (MS5611 *)arg; - dev->cycle(); + cycle(); } void @@ -504,7 +476,7 @@ MS5611::cycle() //log("collection error %d", ret); } /* reset the collection state machine and try again */ - start_cycle(); + start(); return; } @@ -517,14 +489,16 @@ MS5611::cycle() * doing pressure measurements at something close to the desired rate. */ if ((_measure_phase != 0) && - (_measure_ticks > USEC2TICK(MS5611_CONVERSION_INTERVAL))) { + (_call_baro_interval > USEC2TICK(MS5611_CONVERSION_INTERVAL))) { - /* schedule a fresh cycle call when we are ready to measure again */ - work_queue(HPWORK, - &_work, - (worker_t)&MS5611::cycle_trampoline, - this, - _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL)); + // XXX maybe do something appropriate as well + +// /* schedule a fresh cycle call when we are ready to measure again */ +// work_queue(HPWORK, +// &_work, +// (worker_t)&MS5611::cycle_trampoline, +// this, +// _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL)); return; } @@ -535,19 +509,12 @@ MS5611::cycle() if (ret != OK) { //log("measure error %d", ret); /* reset the collection state machine and try again */ - start_cycle(); + start(); return; } /* next phase is collection */ _collect_phase = true; - - /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, - &_work, - (worker_t)&MS5611::cycle_trampoline, - this, - USEC2TICK(MS5611_CONVERSION_INTERVAL)); } int @@ -696,13 +663,14 @@ MS5611::collect() return OK; } + void MS5611::print_info() { perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); perf_print_counter(_buffer_overflows); - printf("poll interval: %u ticks\n", _measure_ticks); + printf("poll interval: %u ticks\n", _call_baro_interval); printf("report queue: %u (%u/%u @ %p)\n", _num_reports, _oldest_report, _next_report, _reports); printf("TEMP: %d\n", _TEMP); diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 156832a618..1ea6989224 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -103,6 +103,14 @@ private: * @param reg The register to read. */ uint16_t _reg16(unsigned reg); + + /** + * Wrapper around transfer() that prevents interrupt-context transfers + * from pre-empting us. The sensor may (does) share a bus with sensors + * that are polled from interrupt context (or we may be pre-empted) + * so we need to guarantee that transfers complete without interruption. + */ + int _transfer(uint8_t *send, uint8_t *recv, unsigned len); }; device::Device * @@ -161,7 +169,7 @@ MS5611_SPI::read(unsigned offset, void *data, unsigned count) /* read the most recent measurement */ buf[0] = 0 | DIR_WRITE; - int ret = transfer(&buf[0], &buf[0], sizeof(buf)); + int ret = _transfer(&buf[0], &buf[0], sizeof(buf)); if (ret == OK) { /* fetch the raw value */ @@ -206,7 +214,7 @@ MS5611_SPI::_reset() { uint8_t cmd = ADDR_RESET_CMD | DIR_WRITE; - return transfer(&cmd, nullptr, 1); + return _transfer(&cmd, nullptr, 1); } int @@ -214,7 +222,7 @@ MS5611_SPI::_measure(unsigned addr) { uint8_t cmd = addr | DIR_WRITE; - return transfer(&cmd, nullptr, 1); + return _transfer(&cmd, nullptr, 1); } @@ -244,9 +252,21 @@ MS5611_SPI::_reg16(unsigned reg) cmd[0] = reg | DIR_READ; - transfer(cmd, cmd, sizeof(cmd)); + _transfer(cmd, cmd, sizeof(cmd)); return (uint16_t)(cmd[1] << 8) | cmd[2]; } +int +MS5611_SPI::_transfer(uint8_t *send, uint8_t *recv, unsigned len) +{ + irqstate_t flags = irqsave(); + + int ret = transfer(send, recv, len); + + irqrestore(flags); + + return ret; +} + #endif /* PX4_SPIDEV_BARO */ From 98a4345410e91a1e3966b3364f487652af210d03 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 21 Jul 2013 22:42:45 +0400 Subject: [PATCH 295/486] multirotor_pos_control: some refactoring and cleanup, attitude-thrust correction moved to thrust_pid --- .../multirotor_pos_control.c | 39 +++++------- .../multirotor_pos_control_params.c | 6 +- .../multirotor_pos_control_params.h | 4 +- .../multirotor_pos_control/thrust_pid.c | 61 ++++++++++--------- .../multirotor_pos_control/thrust_pid.h | 2 +- 5 files changed, 54 insertions(+), 58 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index b49186ee92..6252178d11 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -235,7 +235,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) for (int i = 0; i < 2; i++) { pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f); - pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.slope_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); + pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); } pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f); @@ -259,7 +259,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) for (int i = 0; i < 2; i++) { pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f); - pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.slope_max); + pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max); } pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max); @@ -416,12 +416,12 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } /* run position & altitude controllers, calculate velocity setpoint */ - global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz, dt); + global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2]; if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) { /* calculate velocity set point in NED frame */ - global_vel_sp.vx = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx, dt); - global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy, dt); + global_vel_sp.vx = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx - sp_move_rate[0], dt) + sp_move_rate[0]; + global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy - sp_move_rate[1], dt) + sp_move_rate[1]; /* limit horizontal speed */ float xy_vel_sp_norm = norm(global_vel_sp.vx, global_vel_sp.vy) / params.xy_vel_max; @@ -439,9 +439,9 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* publish new velocity setpoint */ orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp); - /* run velocity controllers, calculate thrust vector */ + /* run velocity controllers, calculate thrust vector with attitude-thrust compensation */ float thrust_sp[3] = { 0.0f, 0.0f, 0.0f }; - thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt); + thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]); if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) { /* calculate velocity set point in NED frame */ @@ -452,33 +452,24 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* thrust_vector now contains desired acceleration (but not in m/s^2) in NED frame */ /* limit horizontal part of thrust */ float thrust_xy_dir = atan2f(thrust_sp[1], thrust_sp[0]); - float thrust_xy_norm = norm(thrust_sp[0], thrust_sp[1]); + /* assuming that vertical component of thrust is g, + * horizontal component = g * tan(alpha) */ + float tilt = atanf(norm(thrust_sp[0], thrust_sp[1])); - if (thrust_xy_norm > params.slope_max) { - thrust_xy_norm = params.slope_max; + if (tilt > params.tilt_max) { + tilt = params.tilt_max; } - /* use approximation: slope ~ sin(slope) = force */ /* convert direction to body frame */ thrust_xy_dir -= att.yaw; if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) { /* calculate roll and pitch */ - att_sp.roll_body = sinf(thrust_xy_dir) * thrust_xy_norm; - att_sp.pitch_body = -cosf(thrust_xy_dir) * thrust_xy_norm / cosf(att_sp.roll_body); // reverse pitch + att_sp.roll_body = sinf(thrust_xy_dir) * tilt; + att_sp.pitch_body = -cosf(thrust_xy_dir) * tilt / cosf(att_sp.roll_body); } - /* attitude-thrust compensation */ - float att_comp; - - if (att.R[2][2] > 0.8f) - att_comp = 1.0f / att.R[2][2]; - else if (att.R[2][2] > 0.0f) - att_comp = ((1.0f / 0.8f - 1.0f) / 0.8f) * att.R[2][2] + 1.0f; - else - att_comp = 1.0f; - - att_sp.thrust = -thrust_sp[2] * att_comp; + att_sp.thrust = -thrust_sp[2]; att_sp.timestamp = hrt_absolute_time(); if (status.flag_control_manual_enabled) { diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c index f8a982c6c0..1094fd23b7 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -56,7 +56,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.2f); PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.0f); PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.0f); PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 10.0f); -PARAM_DEFINE_FLOAT(MPC_SLOPE_MAX, 0.5f); +PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 0.5f); int parameters_init(struct multirotor_position_control_param_handles *h) { @@ -74,7 +74,7 @@ int parameters_init(struct multirotor_position_control_param_handles *h) h->xy_vel_i = param_find("MPC_XY_VEL_I"); h->xy_vel_d = param_find("MPC_XY_VEL_D"); h->xy_vel_max = param_find("MPC_XY_VEL_MAX"); - h->slope_max = param_find("MPC_SLOPE_MAX"); + h->tilt_max = param_find("MPC_TILT_MAX"); h->rc_scale_pitch = param_find("RC_SCALE_PITCH"); h->rc_scale_roll = param_find("RC_SCALE_ROLL"); @@ -99,7 +99,7 @@ int parameters_update(const struct multirotor_position_control_param_handles *h, param_get(h->xy_vel_i, &(p->xy_vel_i)); param_get(h->xy_vel_d, &(p->xy_vel_d)); param_get(h->xy_vel_max, &(p->xy_vel_max)); - param_get(h->slope_max, &(p->slope_max)); + param_get(h->tilt_max, &(p->tilt_max)); param_get(h->rc_scale_pitch, &(p->rc_scale_pitch)); param_get(h->rc_scale_roll, &(p->rc_scale_roll)); diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h index e9b1f5c390..14a3de2e59 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h @@ -56,7 +56,7 @@ struct multirotor_position_control_params { float xy_vel_i; float xy_vel_d; float xy_vel_max; - float slope_max; + float tilt_max; float rc_scale_pitch; float rc_scale_roll; @@ -78,7 +78,7 @@ struct multirotor_position_control_param_handles { param_t xy_vel_i; param_t xy_vel_d; param_t xy_vel_max; - param_t slope_max; + param_t tilt_max; param_t rc_scale_pitch; param_t rc_scale_roll; diff --git a/src/modules/multirotor_pos_control/thrust_pid.c b/src/modules/multirotor_pos_control/thrust_pid.c index 89efe1334e..51dcd7df24 100644 --- a/src/modules/multirotor_pos_control/thrust_pid.c +++ b/src/modules/multirotor_pos_control/thrust_pid.c @@ -1,11 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. - * Author: Laurens Mackay - * Tobias Naegeli - * Martin Rutschmann - * Anton Babushkin - * Julian Oes + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -39,13 +35,9 @@ /** * @file thrust_pid.c * - * Implementation of generic PID control interface. + * Implementation of thrust control PID. * - * @author Laurens Mackay - * @author Tobias Naegeli - * @author Martin Rutschmann * @author Anton Babushkin - * @author Julian Oes */ #include "thrust_pid.h" @@ -108,16 +100,18 @@ __EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, fl return ret; } -__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt) +__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22) { /* Alternative integral component calculation - error = setpoint - actual_position - integral = integral + (Ki*error*dt) - derivative = (error - previous_error)/dt - output = (Kp*error) + integral + (Kd*derivative) - previous_error = error - wait(dt) - goto start + * + * start: + * error = setpoint - current_value + * integral = integral + (Ki * error * dt) + * derivative = (error - previous_error) / dt + * previous_error = error + * output = (Kp * error) + integral + (Kd * derivative) + * wait(dt) + * goto start */ if (!isfinite(sp) || !isfinite(val) || !isfinite(dt)) { @@ -147,21 +141,32 @@ __EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, floa d = 0.0f; } - // Calculate the error integral and check for saturation + /* calculate the error integral */ i = pid->integral + (pid->ki * error * dt); - float output = (error * pid->kp) + i + (d * pid->kd); + /* attitude-thrust compensation + * r22 is (2, 2) componet of rotation matrix for current attitude */ + float att_comp; + + if (r22 > 0.8f) + att_comp = 1.0f / r22; + else if (r22 > 0.0f) + att_comp = ((1.0f / 0.8f - 1.0f) / 0.8f) * r22 + 1.0f; + else + att_comp = 1.0f; + + /* calculate output */ + float output = ((error * pid->kp) + i + (d * pid->kd)) * att_comp; + + /* check for saturation */ if (output < pid->limit_min || output > pid->limit_max) { - i = pid->integral; // If saturated then do not update integral value - // recalculate output with old integral - output = (error * pid->kp) + i + (d * pid->kd); + /* saturated, recalculate output with old integral */ + output = (error * pid->kp) + pid->integral + (d * pid->kd); } else { - if (!isfinite(i)) { - i = 0.0f; + if (isfinite(i)) { + pid->integral = i; } - - pid->integral = i; } if (isfinite(output)) { diff --git a/src/modules/multirotor_pos_control/thrust_pid.h b/src/modules/multirotor_pos_control/thrust_pid.h index 65ee33c51c..551c032fe2 100644 --- a/src/modules/multirotor_pos_control/thrust_pid.h +++ b/src/modules/multirotor_pos_control/thrust_pid.h @@ -68,7 +68,7 @@ typedef struct { __EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min); __EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max); -__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt); +__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22); __END_DECLS From bd50092dd29a83dd5044fc3027e20be91b3275e5 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 21 Jul 2013 22:44:46 +0400 Subject: [PATCH 296/486] position_estimator_inav: accelerometer bias estimation for Z, default weights for GPS updated --- .../position_estimator_inav_main.c | 20 +++++++++++++------ .../position_estimator_inav_params.c | 9 ++++++--- .../position_estimator_inav_params.h | 2 ++ 3 files changed, 22 insertions(+), 9 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index a9bc09e0d2..42b5fcb615 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -167,9 +167,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) int baro_init_num = 200; float baro_alt0 = 0.0f; /* to determine while start up */ - double lat_current = 0.0; //[°]] --> 47.0 - double lon_current = 0.0; //[°]] -->8.5 - double alt_current = 0.0; //[m] above MSL + double lat_current = 0.0f; //[°] --> 47.0 + double lon_current = 0.0f; //[°] --> 8.5 + double alt_current = 0.0f; //[m] above MSL uint32_t accel_counter = 0; uint32_t baro_counter = 0; @@ -302,8 +302,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) hrt_abstime pub_last = hrt_absolute_time(); uint32_t pub_interval = 4000; // limit publish rate to 250 Hz + /* acceleration in NED frame */ + float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G }; + /* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */ float accel_corr[] = { 0.0f, 0.0f, 0.0f }; // N E D + float accel_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame float baro_corr = 0.0f; // D float gps_corr[2][2] = { { 0.0f, 0.0f }, // N (pos, vel) @@ -369,9 +373,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (sensor.accelerometer_counter > accel_counter) { if (att.R_valid) { + /* correct accel bias, now only for Z */ + sensor.accelerometer_m_s2[2] -= accel_bias[2]; /* transform acceleration vector from body frame to NED frame */ - float accel_NED[3]; - for (int i = 0; i < 3; i++) { accel_NED[i] = 0.0f; @@ -425,7 +429,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.home_timestamp = hrt_absolute_time(); z_est[0] += sonar_corr; sonar_corr = 0.0f; - sonar_corr_filtered = 0.0; + sonar_corr_filtered = 0.0f; } } } @@ -470,6 +474,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; t_prev = t; + /* accel bias correction, now only for Z + * not strictly correct, but stable and works */ + accel_bias[2] += (accel_NED[2] + CONSTANTS_ONE_G) * params.w_acc_bias * dt; + /* inertial filter prediction for altitude */ inertial_filter_predict(dt, z_est); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index c90c611a77..70248b3b72 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -44,10 +44,11 @@ PARAM_DEFINE_INT32(INAV_USE_GPS, 1); PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 1.0f); PARAM_DEFINE_FLOAT(INAV_W_ALT_ACC, 50.0f); PARAM_DEFINE_FLOAT(INAV_W_ALT_SONAR, 3.0f); -PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_P, 4.0f); -PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 0.0f); +PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_P, 1.0f); +PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 2.0f); PARAM_DEFINE_FLOAT(INAV_W_POS_ACC, 10.0f); -PARAM_DEFINE_FLOAT(INAV_W_POS_FLOW, 10.0f); +PARAM_DEFINE_FLOAT(INAV_W_POS_FLOW, 0.0f); +PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.0f); PARAM_DEFINE_FLOAT(INAV_FLOW_K, 1.0f); PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.02f); PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); @@ -62,6 +63,7 @@ int parameters_init(struct position_estimator_inav_param_handles *h) h->w_pos_gps_v = param_find("INAV_W_POS_GPS_V"); h->w_pos_acc = param_find("INAV_W_POS_ACC"); h->w_pos_flow = param_find("INAV_W_POS_FLOW"); + h->w_acc_bias = param_find("INAV_W_ACC_BIAS"); h->flow_k = param_find("INAV_FLOW_K"); h->sonar_filt = param_find("INAV_SONAR_FILT"); h->sonar_err = param_find("INAV_SONAR_ERR"); @@ -79,6 +81,7 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->w_pos_gps_v, &(p->w_pos_gps_v)); param_get(h->w_pos_acc, &(p->w_pos_acc)); param_get(h->w_pos_flow, &(p->w_pos_flow)); + param_get(h->w_acc_bias, &(p->w_acc_bias)); param_get(h->flow_k, &(p->flow_k)); param_get(h->sonar_filt, &(p->sonar_filt)); param_get(h->sonar_err, &(p->sonar_err)); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index cca172b5d6..1e70a3c48a 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -49,6 +49,7 @@ struct position_estimator_inav_params { float w_pos_gps_v; float w_pos_acc; float w_pos_flow; + float w_acc_bias; float flow_k; float sonar_filt; float sonar_err; @@ -63,6 +64,7 @@ struct position_estimator_inav_param_handles { param_t w_pos_gps_v; param_t w_pos_acc; param_t w_pos_flow; + param_t w_acc_bias; param_t flow_k; param_t sonar_filt; param_t sonar_err; From 963abd66badf71a925f80e12312c429d64999424 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 22 Jul 2013 21:48:21 +0400 Subject: [PATCH 297/486] commander: WIP, SEATBELT renamed to ASSISTED, added SIMPLE mode, added ASSISTED switch, some cleanup and reorganizing --- src/modules/commander/commander.cpp | 190 ++++++++++-------- .../commander/state_machine_helper.cpp | 66 ++++-- src/modules/mavlink/mavlink.c | 12 +- .../uORB/topics/manual_control_setpoint.h | 1 + src/modules/uORB/topics/vehicle_status.h | 17 +- 5 files changed, 178 insertions(+), 108 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 253b6295f1..25c5a84ea8 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1005,17 +1005,15 @@ int commander_thread_main(int argc, char *argv[]) /* check for global or local position updates, set a timeout of 2s */ - if (hrt_absolute_time() - last_global_position_time < 2000000 && hrt_absolute_time() > 2000000) { + if (hrt_absolute_time() - last_global_position_time < 2000000 && hrt_absolute_time() > 2000000 && global_position.valid) { current_status.condition_global_position_valid = true; - // XXX check for controller status and home position as well } else { current_status.condition_global_position_valid = false; } - if (hrt_absolute_time() - last_local_position_time < 2000000 && hrt_absolute_time() > 2000000) { + if (hrt_absolute_time() - last_local_position_time < 2000000 && hrt_absolute_time() > 2000000 && local_position.valid) { current_status.condition_local_position_valid = true; - // XXX check for controller status and home position as well } else { current_status.condition_local_position_valid = false; @@ -1154,7 +1152,7 @@ int commander_thread_main(int argc, char *argv[]) } else { /* center stick position, set seatbelt/simple control */ - current_status.mode_switch = MODE_SWITCH_SEATBELT; + current_status.mode_switch = MODE_SWITCH_ASSISTED; } // warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode); @@ -1167,21 +1165,32 @@ int commander_thread_main(int argc, char *argv[]) /* this switch is not properly mapped, set default */ current_status.return_switch = RETURN_SWITCH_NONE; - } else if (sp_man.return_switch < -STICK_ON_OFF_LIMIT) { - - /* bottom stick position, set altitude hold */ - current_status.return_switch = RETURN_SWITCH_NONE; - } else if (sp_man.return_switch > STICK_ON_OFF_LIMIT) { /* top stick position */ current_status.return_switch = RETURN_SWITCH_RETURN; } else { - /* center stick position, set default */ + /* center or bottom stick position, set default */ current_status.return_switch = RETURN_SWITCH_NONE; } + /* check assisted switch */ + if (!isfinite(sp_man.assisted_switch)) { + + /* this switch is not properly mapped, set default */ + current_status.assisted_switch = ASSISTED_SWITCH_SEATBELT; + + } else if (sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { + + /* top stick position */ + current_status.assisted_switch = ASSISTED_SWITCH_SIMPLE; + + } else { + /* center or bottom stick position, set default */ + current_status.assisted_switch = ASSISTED_SWITCH_SEATBELT; + } + /* check mission switch */ if (!isfinite(sp_man.mission_switch)) { @@ -1219,10 +1228,10 @@ int commander_thread_main(int argc, char *argv[]) warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); } - /* Try seatbelt or fallback to manual */ - } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT) { + /* Try assisted or fallback to manual */ + } else if (current_status.mode_switch == MODE_SWITCH_ASSISTED) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen @@ -1235,7 +1244,7 @@ int commander_thread_main(int argc, char *argv[]) if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { // first fallback to SEATBELT_STANDY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { // or fallback to MANUAL_STANDBY if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen @@ -1250,89 +1259,104 @@ int commander_thread_main(int argc, char *argv[]) /* evaluate the navigation state when armed */ case ARMING_STATE_ARMED: - /* Always accept manual mode */ if (current_status.mode_switch == MODE_SWITCH_MANUAL) { - + /* MANUAL */ if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } - /* SEATBELT_STANDBY (fallback: MANUAL) */ - } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT - && current_status.return_switch == RETURN_SWITCH_NONE) { - - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - - /* SEATBELT_DESCENT (fallback: MANUAL) */ - } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT - && current_status.return_switch == RETURN_SWITCH_RETURN) { - - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - - /* AUTO_LOITER (fallback: SEATBELT, MANUAL) */ - } else if (current_status.mode_switch == MODE_SWITCH_AUTO - && current_status.return_switch == RETURN_SWITCH_NONE - && current_status.mission_switch == MISSION_SWITCH_NONE) { - - /* we might come from the disarmed state AUTO_STANDBY */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_READY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL_STANDBY + } else if (current_status.mode_switch == MODE_SWITCH_ASSISTED) { + /* ASSISTED */ + if (current_status.return_switch == RETURN_SWITCH_RETURN) { + /* ASSISTED_DESCENT */ + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); } } - /* or from some other armed state like SEATBELT or MANUAL */ - } else if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_LOITER, control_mode_pub, &control_mode, mavlink_fd) != OK) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); + + } else { + if (current_status.assisted_switch == ASSISTED_SWITCH_SIMPLE) { + /* ASSISTED_SIMPLE */ + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SIMPLE, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to ASSISTED_SEATBELT + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } + } else { + /* ASSISTED_SEATBELT */ + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } } } } - /* AUTO_MISSION (fallback: SEATBELT, MANUAL) */ - } else if (current_status.mode_switch == MODE_SWITCH_AUTO - && current_status.return_switch == RETURN_SWITCH_NONE - && current_status.mission_switch == MISSION_SWITCH_MISSION) { - - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_MISSION, control_mode_pub, &control_mode, mavlink_fd) != OK) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); + } else if (current_status.mode_switch == MODE_SWITCH_AUTO) { + /* AUTO */ + if (current_status.return_switch == RETURN_SWITCH_RETURN) { + /* AUTO_RTL */ + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_RTL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to ASSISTED_DESCENT + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } } } - } - - /* AUTO_RTL (fallback: SEATBELT_DESCENT, MANUAL) */ - } else if (current_status.mode_switch == MODE_SWITCH_AUTO - && current_status.return_switch == RETURN_SWITCH_RETURN - && (current_status.mission_switch == MISSION_SWITCH_NONE || current_status.mission_switch == MISSION_SWITCH_MISSION)) { - - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_RTL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); + } else { + if (current_status.mission_switch == MISSION_SWITCH_MISSION) { + /* AUTO_MISSION */ + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_MISSION, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to ASSISTED_SEATBELT + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } + } else { + // TODO check this + if (current_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_status.navigation_state == NAVIGATION_STATE_AUTO_LAND) { + /* AUTO_READY */ + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_READY, control_mode_pub, &control_mode, mavlink_fd) != OK) { + /* AUTO_READY */ + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } + } else { + /* AUTO_LOITER */ + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_LOITER, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to ASSISTED_SEATBELT + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } } } } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 3cf60a99ac..60ab01536f 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -184,7 +184,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* transitions back to INIT are possible for calibration */ if (current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY) { ret = OK; @@ -200,7 +200,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* transitions from INIT and other STANDBY states as well as MANUAL are possible */ if (current_state->navigation_state == NAVIGATION_STATE_INIT - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { @@ -235,14 +235,15 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current } break; - case NAVIGATION_STATE_SEATBELT_STANDBY: + case NAVIGATION_STATE_ASSISTED_STANDBY: /* transitions from INIT and other STANDBY states as well as SEATBELT and SEATBELT_DESCENT are possible */ if (current_state->navigation_state == NAVIGATION_STATE_INIT || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT) { + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT) { /* need to be disarmed and have a position estimate */ if (current_state->arming_state != ARMING_STATE_STANDBY) { @@ -262,11 +263,12 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current } break; - case NAVIGATION_STATE_SEATBELT: + case NAVIGATION_STATE_ASSISTED_SEATBELT: /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/ - if (current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT + if (current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT || current_state->navigation_state == NAVIGATION_STATE_MANUAL || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER @@ -293,10 +295,43 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current } break; - case NAVIGATION_STATE_SEATBELT_DESCENT: + case NAVIGATION_STATE_ASSISTED_SIMPLE: + + /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/ + if (current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + + /* need to be armed and have a position estimate */ + if (current_state->arming_state != ARMING_STATE_ARMED) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: not armed"); + tune_negative(); + } else if (!current_state->condition_local_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate"); + tune_negative(); + } else { + ret = OK; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_ASSISTED_DESCENT: /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT and SEATBELT_STANDBY */ - if (current_state->navigation_state == NAVIGATION_STATE_SEATBELT + if (current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE || current_state->navigation_state == NAVIGATION_STATE_MANUAL || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER @@ -328,7 +363,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* transitions from INIT or from other STANDBY modes or from AUTO READY */ if (current_state->navigation_state == NAVIGATION_STATE_INIT || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) { /* need to be disarmed and have a position and home lock */ @@ -395,7 +430,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { /* need to have a position and home lock */ @@ -422,7 +458,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { /* need to have a mission ready */ @@ -446,7 +483,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { /* need to have a position and home lock */ diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index db8ee90678..ae8e168e18 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -221,9 +221,9 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; } - if (v_status.navigation_state == NAVIGATION_STATE_SEATBELT - || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT - || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY) { + if (v_status.navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT + || v_status.navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT + || v_status.navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY) { *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; } @@ -248,15 +248,15 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) || v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION || v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL || v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF - || v_status.navigation_state == NAVIGATION_STATE_SEATBELT - || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT + || v_status.navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT + || v_status.navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT || v_status.navigation_state == NAVIGATION_STATE_MANUAL) { *mavlink_state = MAV_STATE_ACTIVE; } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY // XXX correct? - || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || v_status.navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY || v_status.navigation_state == NAVIGATION_STATE_MANUAL_STANDBY) { *mavlink_state = MAV_STATE_STANDBY; diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index cfee81ea2b..eac6d6e986 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -58,6 +58,7 @@ struct manual_control_setpoint_s { float mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */ float return_switch; /**< land 2 position switch (mandatory): land, no effect */ + float assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ float mission_switch; /**< mission 2 position switch (optional): mission, loiter */ /** diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index ec08a6c13a..9b91e834e0 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -60,12 +60,13 @@ /* State Machine */ typedef enum { - NAVIGATION_STATE_INIT=0, + NAVIGATION_STATE_INIT = 0, NAVIGATION_STATE_MANUAL_STANDBY, NAVIGATION_STATE_MANUAL, - NAVIGATION_STATE_SEATBELT_STANDBY, - NAVIGATION_STATE_SEATBELT, - NAVIGATION_STATE_SEATBELT_DESCENT, + NAVIGATION_STATE_ASSISTED_STANDBY, + NAVIGATION_STATE_ASSISTED_SEATBELT, + NAVIGATION_STATE_ASSISTED_SIMPLE, + NAVIGATION_STATE_ASSISTED_DESCENT, NAVIGATION_STATE_AUTO_STANDBY, NAVIGATION_STATE_AUTO_READY, NAVIGATION_STATE_AUTO_TAKEOFF, @@ -98,7 +99,7 @@ typedef enum { typedef enum { MODE_SWITCH_MANUAL = 0, - MODE_SWITCH_SEATBELT, + MODE_SWITCH_ASSISTED, MODE_SWITCH_AUTO } mode_switch_pos_t; @@ -107,6 +108,11 @@ typedef enum { RETURN_SWITCH_RETURN } return_switch_pos_t; +typedef enum { + ASSISTED_SWITCH_SEATBELT = 0, + ASSISTED_SWITCH_SIMPLE +} assisted_switch_pos_t; + typedef enum { MISSION_SWITCH_NONE = 0, MISSION_SWITCH_MISSION @@ -179,6 +185,7 @@ struct vehicle_status_s mode_switch_pos_t mode_switch; return_switch_pos_t return_switch; + assisted_switch_pos_t assisted_switch; mission_switch_pos_t mission_switch; bool condition_battery_voltage_valid; From 55fd19f2813110d14d536943503851255c997b6f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 22 Jul 2013 22:37:10 +0400 Subject: [PATCH 298/486] sensors: ASSISTED switch channel added --- src/modules/sensors/sensor_params.c | 1 + src/modules/sensors/sensors.cpp | 14 +++++++++++++- src/modules/uORB/topics/rc_channels.h | 24 ++++++++++++++---------- 3 files changed, 28 insertions(+), 11 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 3ed9efc8b4..d0af9e17bf 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -167,6 +167,7 @@ PARAM_DEFINE_INT32(RC_MAP_YAW, 4); PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5); PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 6); +PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0); PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); //PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 89d5cd374c..b38dc8d890 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -208,6 +208,7 @@ private: int rc_map_mode_sw; int rc_map_return_sw; + int rc_map_assisted_sw; int rc_map_mission_sw; // int rc_map_offboard_ctrl_mode_sw; @@ -256,6 +257,7 @@ private: param_t rc_map_mode_sw; param_t rc_map_return_sw; + param_t rc_map_assisted_sw; param_t rc_map_mission_sw; // param_t rc_map_offboard_ctrl_mode_sw; @@ -464,6 +466,7 @@ Sensors::Sensors() : _parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS"); /* optional mode switches, not mapped per default */ + _parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW"); _parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSIO_SW"); // _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); @@ -617,6 +620,10 @@ Sensors::parameters_update() warnx("Failed getting return sw chan index"); } + if (param_get(_parameter_handles.rc_map_assisted_sw, &(_parameters.rc_map_assisted_sw)) != OK) { + warnx("Failed getting assisted sw chan index"); + } + if (param_get(_parameter_handles.rc_map_mission_sw, &(_parameters.rc_map_mission_sw)) != OK) { warnx("Failed getting mission sw chan index"); } @@ -673,6 +680,7 @@ Sensors::parameters_update() _rc.function[MODE] = _parameters.rc_map_mode_sw - 1; _rc.function[RETURN] = _parameters.rc_map_return_sw - 1; + _rc.function[ASSISTED] = _parameters.rc_map_assisted_sw - 1; _rc.function[MISSION] = _parameters.rc_map_mission_sw - 1; _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; @@ -1142,6 +1150,7 @@ Sensors::ppm_poll() manual_control.mode_switch = NAN; manual_control.return_switch = NAN; + manual_control.assisted_switch = NAN; manual_control.mission_switch = NAN; // manual_control.auto_offboard_input_switch = NAN; @@ -1249,7 +1258,10 @@ Sensors::ppm_poll() /* land switch input */ manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); - /* land switch input */ + /* assisted switch input */ + manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled); + + /* mission switch input */ manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); /* flaps */ diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index a0bb25af46..2167e44a2b 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -53,9 +53,12 @@ /** * The number of RC channel inputs supported. * Current (Q1/2013) radios support up to 18 channels, - * leaving at a sane value of 14. + * leaving at a sane value of 15. + * This number can be greater then number of RC channels, + * because single RC channel can be mapped to multiple + * functions, e.g. for various mode switches. */ -#define RC_CHANNELS_MAX 14 +#define RC_CHANNELS_MAX 15 /** * This defines the mapping of the RC functions. @@ -70,14 +73,15 @@ enum RC_CHANNELS_FUNCTION YAW = 3, MODE = 4, RETURN = 5, - MISSION = 6, - OFFBOARD_MODE = 7, - FLAPS = 8, - AUX_1 = 9, - AUX_2 = 10, - AUX_3 = 11, - AUX_4 = 12, - AUX_5 = 13, + ASSISTED = 6, + MISSION = 7, + OFFBOARD_MODE = 8, + FLAPS = 9, + AUX_1 = 10, + AUX_2 = 11, + AUX_3 = 12, + AUX_4 = 13, + AUX_5 = 14, RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */ }; From 7c1693a877d96ee1476dcb3a7f5cb5e8dfb27492 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 23 Jul 2013 14:56:12 +0400 Subject: [PATCH 299/486] commander: don't notify user about rejected disarm to not confuse, flag_control_altitude_enabled added, flags values fixed --- src/modules/commander/commander.cpp | 39 +++++++------------ .../commander/state_machine_helper.cpp | 24 +++++++++--- .../uORB/topics/vehicle_control_mode.h | 1 + 3 files changed, 33 insertions(+), 31 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 25c5a84ea8..1978d87824 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1398,30 +1398,18 @@ int commander_thread_main(int argc, char *argv[]) } } - /* - * Check if left stick is in lower left position --> switch to standby state. - * Do this only for multirotors, not for fixed wing aircraft. - */ - if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.1f)) { - + /* Check if left stick is in lower left position and we're in manual mode --> switch to standby state. + * Do this only for multirotors, not for fixed wing aircraft. + * TODO allow disarm when landed + */ + if (sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f && + control_mode.flag_control_manual_enabled && + ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - - if((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) - ) { - if (control_mode.flag_control_position_enabled || control_mode.flag_control_velocity_enabled) { - mavlink_log_critical(mavlink_fd, "DISARM DENY, go manual mode first"); - tune_negative(); - } else { - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); - tune_positive(); - } - - } else { - mavlink_log_critical(mavlink_fd, "DISARM not allowed"); - tune_negative(); - } + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); + tune_positive(); stick_off_counter = 0; } else { @@ -1430,9 +1418,8 @@ int commander_thread_main(int argc, char *argv[]) } } - /* check if left stick is in lower right position and we're in manual --> arm */ - if (sp_man.yaw > STICK_ON_OFF_LIMIT && - sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + /* check if left stick is in lower right position and we're in manual mode --> arm */ + if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, armed_pub, &armed, mavlink_fd); stick_on_counter = 0; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 60ab01536f..4a7aa66b70 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -192,6 +192,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = false; control_mode->flag_control_velocity_enabled = false; control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = false; control_mode->flag_control_manual_enabled = false; } break; @@ -214,6 +215,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = false; control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = false; control_mode->flag_control_manual_enabled = true; } } @@ -231,6 +233,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = false; control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = false; control_mode->flag_control_manual_enabled = true; } break; @@ -258,7 +261,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = false; - control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = true; } } break; @@ -290,7 +294,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = false; - control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = true; } } break; @@ -321,8 +326,9 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_rates_enabled = true; control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = true; } } break; @@ -353,7 +359,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = false; - control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = true; } } break; @@ -382,6 +389,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_manual_enabled = false; } } @@ -405,6 +413,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_manual_enabled = false; } } @@ -420,6 +429,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_manual_enabled = false; } break; @@ -447,6 +457,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_manual_enabled = false; } } @@ -472,6 +483,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_manual_enabled = false; } } @@ -500,6 +512,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_manual_enabled = false; } } @@ -524,6 +537,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_manual_enabled = false; } } diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 8481a624fd..d83eb45d9a 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -77,6 +77,7 @@ struct vehicle_control_mode_s bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ bool flag_control_position_enabled; /**< true if position is controlled */ + bool flag_control_altitude_enabled; /**< true if altitude is controlled */ bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ bool failsave_highlevel; /**< Set to true if high-level failsafe mode is enabled */ From 8dd5130d998f7609c8a5d457093e31320b3668fd Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 24 Jul 2013 18:20:04 +0400 Subject: [PATCH 300/486] position_estimator_inav, multirotor_pos_control: bugs fixed --- src/modules/multirotor_pos_control/multirotor_pos_control.c | 3 ++- .../position_estimator_inav/position_estimator_inav_main.c | 4 ++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 6252178d11..10f21c55de 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -259,7 +259,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) for (int i = 0; i < 2; i++) { pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f); - pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max); + // TODO 1000.0 is hotfix + pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1000.0f, params.tilt_max); } pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 42b5fcb615..d4b2f0e43c 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -453,8 +453,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) gps_corr[1][0] = gps_proj[1] - y_est[0]; if (gps.vel_ned_valid) { - gps_corr[0][1] = gps.vel_n_m_s; - gps_corr[1][1] = gps.vel_e_m_s; + gps_corr[0][1] = gps.vel_n_m_s - x_est[1]; + gps_corr[1][1] = gps.vel_e_m_s - y_est[1]; } else { gps_corr[0][1] = 0.0f; From d57d12818a3c79cc992ff70e765734e7145603d0 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 24 Jul 2013 22:51:27 -0700 Subject: [PATCH 301/486] Revert "Disable interrupts during ms5611 SPI transactions to prevent interruption by sensor drivers polling other sensors from interrupt context." This reverts commit 31ded04c80f68bd071840770d49eb93d2d9c9fe2. --- src/drivers/ms5611/ms5611.cpp | 148 ++++++++++++++++++------------ src/drivers/ms5611/ms5611_spi.cpp | 28 +----- 2 files changed, 94 insertions(+), 82 deletions(-) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 122cf0cec3..d9268c0b30 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -111,9 +111,8 @@ protected: ms5611::prom_s _prom; - struct hrt_call _baro_call; - - unsigned _call_baro_interval; + struct work_s _work; + unsigned _measure_ticks; unsigned _num_reports; volatile unsigned _next_report; @@ -144,12 +143,12 @@ protected: * @note This function is called at open and error time. It might make sense * to make it more aggressive about resetting the bus in case of errors. */ - void start(); + void start_cycle(); /** * Stop the automatic measurement state machine. */ - void stop(); + void stop_cycle(); /** * Perform a poll cycle; collect from the previous measurement @@ -167,11 +166,12 @@ protected: void cycle(); /** - * Static trampoline; XXX is this needed? + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. * * @param arg Instance pointer for the driver that is polling. */ - void cycle_trampoline(void *arg); + static void cycle_trampoline(void *arg); /** * Issue a measurement command for the current state. @@ -184,7 +184,6 @@ protected: * Collect the result of the most recent measurement. */ virtual int collect(); - }; /* @@ -196,7 +195,7 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) : CDev("MS5611", BARO_DEVICE_PATH), _interface(interface), _prom(prom_buf.s), - _call_baro_interval(0), + _measure_ticks(0), _num_reports(0), _next_report(0), _oldest_report(0), @@ -213,13 +212,14 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) : _comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows")) { - + // work_cancel in stop_cycle called from the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); } MS5611::~MS5611() { /* make sure we are truly inactive */ - stop(); + stop_cycle(); /* free any existing reports */ if (_reports != nullptr) @@ -277,7 +277,7 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) return -ENOSPC; /* if automatic measurement is enabled */ - if (_call_baro_interval > 0) { + if (_measure_ticks > 0) { /* * While there is space in the caller's buffer, and reports, copy them. @@ -298,13 +298,41 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) /* manual measurement - run one conversion */ /* XXX really it'd be nice to lock against other readers here */ - _measure_phase = 0; - _oldest_report = _next_report = 0; - measure(); + do { + _measure_phase = 0; + _oldest_report = _next_report = 0; - /* state machine will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); + /* do temperature first */ + if (OK != measure()) { + ret = -EIO; + break; + } + + usleep(MS5611_CONVERSION_INTERVAL); + + if (OK != collect()) { + ret = -EIO; + break; + } + + /* now do a pressure measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } + + usleep(MS5611_CONVERSION_INTERVAL); + + if (OK != collect()) { + ret = -EIO; + break; + } + + /* state machine will have generated a report, copy it out */ + memcpy(buffer, _reports, sizeof(*_reports)); + ret = sizeof(*_reports); + + } while (0); return ret; } @@ -319,8 +347,8 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: - stop(); - _call_baro_interval = 0; + stop_cycle(); + _measure_ticks = 0; return OK; /* external signalling not supported */ @@ -334,15 +362,14 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: { /* do we need to start internal polling? */ - bool want_start = (_call_baro_interval == 0); - - /* set interval to minimum legal value */ - _baro_call.period = _call_baro_interval = MS5611_CONVERSION_INTERVAL; + bool want_start = (_measure_ticks == 0); + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(MS5611_CONVERSION_INTERVAL); /* if we need to start the poll state machine, do it */ if (want_start) - start(); + start_cycle(); return OK; } @@ -350,18 +377,21 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ - bool want_start = (_call_baro_interval == 0); + bool want_start = (_measure_ticks == 0); + + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); /* check against maximum rate */ - if (1000000/arg < MS5611_CONVERSION_INTERVAL) + if (ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL)) return -EINVAL; - /* update interval */ - _baro_call.period = _call_baro_interval = 1000000/arg; + /* update interval for next measurement */ + _measure_ticks = ticks; /* if we need to start the poll state machine, do it */ if (want_start) - start(); + start_cycle(); return OK; } @@ -369,10 +399,10 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_call_baro_interval == 0) + if (_measure_ticks == 0) return SENSOR_POLLRATE_MANUAL; - return (1000 / _call_baro_interval); + return (1000 / _measure_ticks); case SENSORIOCSQUEUEDEPTH: { /* add one to account for the sentinel in the ring */ @@ -389,11 +419,11 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) return -ENOMEM; /* reset the measurement state machine with the new buffer, free the old */ - stop(); + stop_cycle(); delete[] _reports; _num_reports = arg; _reports = buf; - start(); + start_cycle(); return OK; } @@ -427,32 +457,30 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) } void -MS5611::start() +MS5611::start_cycle() { - /* make sure we are stopped first */ - stop(); - /* reset the report ring */ - _oldest_report = _next_report = 0; - /* reset to first measure phase */ + /* reset the report ring and state machine */ + _collect_phase = false; _measure_phase = 0; + _oldest_report = _next_report = 0; /* schedule a cycle to start things */ - hrt_call_every(&_baro_call, 1000, _call_baro_interval, (hrt_callout)&MS5611::cycle_trampoline, this); + work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, 1); } void -MS5611::stop() +MS5611::stop_cycle() { - hrt_cancel(&_baro_call); + work_cancel(HPWORK, &_work); } void MS5611::cycle_trampoline(void *arg) { - MS5611 *dev = (MS5611 *)arg; + MS5611 *dev = reinterpret_cast(arg); - cycle(); + dev->cycle(); } void @@ -476,7 +504,7 @@ MS5611::cycle() //log("collection error %d", ret); } /* reset the collection state machine and try again */ - start(); + start_cycle(); return; } @@ -489,16 +517,14 @@ MS5611::cycle() * doing pressure measurements at something close to the desired rate. */ if ((_measure_phase != 0) && - (_call_baro_interval > USEC2TICK(MS5611_CONVERSION_INTERVAL))) { + (_measure_ticks > USEC2TICK(MS5611_CONVERSION_INTERVAL))) { - // XXX maybe do something appropriate as well - -// /* schedule a fresh cycle call when we are ready to measure again */ -// work_queue(HPWORK, -// &_work, -// (worker_t)&MS5611::cycle_trampoline, -// this, -// _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL)); + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&MS5611::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL)); return; } @@ -509,12 +535,19 @@ MS5611::cycle() if (ret != OK) { //log("measure error %d", ret); /* reset the collection state machine and try again */ - start(); + start_cycle(); return; } /* next phase is collection */ _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&MS5611::cycle_trampoline, + this, + USEC2TICK(MS5611_CONVERSION_INTERVAL)); } int @@ -663,14 +696,13 @@ MS5611::collect() return OK; } - void MS5611::print_info() { perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); perf_print_counter(_buffer_overflows); - printf("poll interval: %u ticks\n", _call_baro_interval); + printf("poll interval: %u ticks\n", _measure_ticks); printf("report queue: %u (%u/%u @ %p)\n", _num_reports, _oldest_report, _next_report, _reports); printf("TEMP: %d\n", _TEMP); diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 1ea6989224..156832a618 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -103,14 +103,6 @@ private: * @param reg The register to read. */ uint16_t _reg16(unsigned reg); - - /** - * Wrapper around transfer() that prevents interrupt-context transfers - * from pre-empting us. The sensor may (does) share a bus with sensors - * that are polled from interrupt context (or we may be pre-empted) - * so we need to guarantee that transfers complete without interruption. - */ - int _transfer(uint8_t *send, uint8_t *recv, unsigned len); }; device::Device * @@ -169,7 +161,7 @@ MS5611_SPI::read(unsigned offset, void *data, unsigned count) /* read the most recent measurement */ buf[0] = 0 | DIR_WRITE; - int ret = _transfer(&buf[0], &buf[0], sizeof(buf)); + int ret = transfer(&buf[0], &buf[0], sizeof(buf)); if (ret == OK) { /* fetch the raw value */ @@ -214,7 +206,7 @@ MS5611_SPI::_reset() { uint8_t cmd = ADDR_RESET_CMD | DIR_WRITE; - return _transfer(&cmd, nullptr, 1); + return transfer(&cmd, nullptr, 1); } int @@ -222,7 +214,7 @@ MS5611_SPI::_measure(unsigned addr) { uint8_t cmd = addr | DIR_WRITE; - return _transfer(&cmd, nullptr, 1); + return transfer(&cmd, nullptr, 1); } @@ -252,21 +244,9 @@ MS5611_SPI::_reg16(unsigned reg) cmd[0] = reg | DIR_READ; - _transfer(cmd, cmd, sizeof(cmd)); + transfer(cmd, cmd, sizeof(cmd)); return (uint16_t)(cmd[1] << 8) | cmd[2]; } -int -MS5611_SPI::_transfer(uint8_t *send, uint8_t *recv, unsigned len) -{ - irqstate_t flags = irqsave(); - - int ret = transfer(send, recv, len); - - irqrestore(flags); - - return ret; -} - #endif /* PX4_SPIDEV_BARO */ From 33e33d71b8177fd3c1c9972e13d14184a5428e42 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 24 Jul 2013 22:53:10 -0700 Subject: [PATCH 302/486] Just the changes required to avoid SPI bus conflicts for MS5611 --- src/drivers/ms5611/ms5611_spi.cpp | 28 ++++++++++++++++++++++++---- 1 file changed, 24 insertions(+), 4 deletions(-) diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 156832a618..1ea6989224 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -103,6 +103,14 @@ private: * @param reg The register to read. */ uint16_t _reg16(unsigned reg); + + /** + * Wrapper around transfer() that prevents interrupt-context transfers + * from pre-empting us. The sensor may (does) share a bus with sensors + * that are polled from interrupt context (or we may be pre-empted) + * so we need to guarantee that transfers complete without interruption. + */ + int _transfer(uint8_t *send, uint8_t *recv, unsigned len); }; device::Device * @@ -161,7 +169,7 @@ MS5611_SPI::read(unsigned offset, void *data, unsigned count) /* read the most recent measurement */ buf[0] = 0 | DIR_WRITE; - int ret = transfer(&buf[0], &buf[0], sizeof(buf)); + int ret = _transfer(&buf[0], &buf[0], sizeof(buf)); if (ret == OK) { /* fetch the raw value */ @@ -206,7 +214,7 @@ MS5611_SPI::_reset() { uint8_t cmd = ADDR_RESET_CMD | DIR_WRITE; - return transfer(&cmd, nullptr, 1); + return _transfer(&cmd, nullptr, 1); } int @@ -214,7 +222,7 @@ MS5611_SPI::_measure(unsigned addr) { uint8_t cmd = addr | DIR_WRITE; - return transfer(&cmd, nullptr, 1); + return _transfer(&cmd, nullptr, 1); } @@ -244,9 +252,21 @@ MS5611_SPI::_reg16(unsigned reg) cmd[0] = reg | DIR_READ; - transfer(cmd, cmd, sizeof(cmd)); + _transfer(cmd, cmd, sizeof(cmd)); return (uint16_t)(cmd[1] << 8) | cmd[2]; } +int +MS5611_SPI::_transfer(uint8_t *send, uint8_t *recv, unsigned len) +{ + irqstate_t flags = irqsave(); + + int ret = transfer(send, recv, len); + + irqrestore(flags); + + return ret; +} + #endif /* PX4_SPIDEV_BARO */ From ffd14e1396fd216651c44615b06605f9e9f975e2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 25 Jul 2013 12:44:47 +0200 Subject: [PATCH 303/486] Updated F330 script, enabled amber led --- ROMFS/px4fmu_common/init.d/10_io_f330 | 15 +++++++-------- src/drivers/boards/px4fmuv2/px4fmu2_init.c | 4 ++-- src/drivers/boards/px4fmuv2/px4fmu2_led.c | 4 ++-- 3 files changed, 11 insertions(+), 12 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_io_f330 index 4107fab4fa..07e70993d3 100644 --- a/ROMFS/px4fmu_common/init.d/10_io_f330 +++ b/ROMFS/px4fmu_common/init.d/10_io_f330 @@ -15,20 +15,19 @@ then # Set all params here, then disable autoconfig param set SYS_AUTOCONFIG 0 - param set MC_ATTRATE_D 0.007 + param set MC_ATTRATE_D 0.005 param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.13 + param set MC_ATTRATE_P 0.1 param set MC_ATT_D 0.0 param set MC_ATT_I 0.0 - param set MC_ATT_P 7.0 - param set MC_POS_P 0.1 + param set MC_ATT_P 4.5 param set MC_RCLOSS_THR 0.0 param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.5 - param set MC_YAWPOS_P 1.0 + param set MC_YAWPOS_I 0.3 + param set MC_YAWPOS_P 0.6 param set MC_YAWRATE_D 0.0 param set MC_YAWRATE_I 0.0 - param set MC_YAWRATE_P 0.2 + param set MC_YAWRATE_P 0.1 param save /fs/microsd/params fi @@ -128,7 +127,7 @@ multirotor_att_control start # # Start logging # -sdlog2 start -r 20 -a -b 14 +sdlog2 start -r 20 -a -b 16 # # Start system state diff --git a/src/drivers/boards/px4fmuv2/px4fmu2_init.c b/src/drivers/boards/px4fmuv2/px4fmu2_init.c index 535e075b22..13829d5c41 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu2_init.c +++ b/src/drivers/boards/px4fmuv2/px4fmu2_init.c @@ -193,8 +193,8 @@ __EXPORT int nsh_archinitialize(void) NULL); /* initial LED state */ - //drv_led_start(); - up_ledoff(LED_AMBER); + drv_led_start(); + led_off(LED_AMBER); /* Configure SPI-based devices */ diff --git a/src/drivers/boards/px4fmuv2/px4fmu2_led.c b/src/drivers/boards/px4fmuv2/px4fmu2_led.c index 85177df56b..11a5c7211b 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu2_led.c +++ b/src/drivers/boards/px4fmuv2/px4fmu2_led.c @@ -68,7 +68,7 @@ __EXPORT void led_init() __EXPORT void led_on(int led) { - if (led == 0) + if (led == 1) { /* Pull down to switch on */ stm32_gpiowrite(GPIO_LED1, false); @@ -77,7 +77,7 @@ __EXPORT void led_on(int led) __EXPORT void led_off(int led) { - if (led == 0) + if (led == 1) { /* Pull up to switch off */ stm32_gpiowrite(GPIO_LED1, true); From a013fc5d0b28cd72f41a28c8229c2d7ab99965f4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 25 Jul 2013 20:05:45 +0400 Subject: [PATCH 304/486] multirotor_pos_control: limit xy velocity integral output to tilt_max / 2 --- .../multirotor_pos_control.c | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 10f21c55de..44d478a9e7 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -248,9 +248,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_status), state_sub, &status); /* check parameters at 1 Hz*/ - paramcheck_counter++; - - if (paramcheck_counter == 50) { + if (--paramcheck_counter <= 0) { + paramcheck_counter = 50; bool param_updated; orb_check(param_sub, ¶m_updated); @@ -259,15 +258,19 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) for (int i = 0; i < 2; i++) { pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f); - // TODO 1000.0 is hotfix - pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1000.0f, params.tilt_max); + /* use integral_limit_out = tilt_max / 2 */ + float i_limit; + if (params.xy_vel_i == 0.0) { + i_limit = params.tilt_max / params.xy_vel_i / 2.0; + } else { + i_limit = 1.0f; // not used really + } + pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max); } pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max); thrust_pid_set_parameters(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min); } - - paramcheck_counter = 0; } /* only check global position setpoint updates but not read */ From dd3033fa6fa8ea4b84582065ed9c92828d7c5f81 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 26 Jul 2013 15:16:48 +0200 Subject: [PATCH 305/486] Symbol cleanup for servo vs. battery voltage --- src/drivers/px4io/px4io.cpp | 6 +++--- src/modules/px4iofirmware/protocol.h | 12 ++++++------ src/modules/px4iofirmware/registers.c | 26 +++++++++----------------- 3 files changed, 18 insertions(+), 26 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index b019c4fc50..dd876f9037 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1315,7 +1315,7 @@ PX4IO::print_status() io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, 0xFFFF); if (_hardware == 1) { - printf("vbatt %u ibatt %u vbatt scale %u\n", + printf("vbatt mV %u ibatt mV %u vbatt scale %u\n", io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT), io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT), io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE)); @@ -1324,9 +1324,9 @@ PX4IO::print_status() (double)_battery_amp_bias, (double)_battery_mamphour_total); } else if (_hardware == 2) { - printf("vservo %u vservo scale %u\n", + printf("vservo %u mV vservo scale %u\n", io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VSERVO), - io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE)); + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VSERVO_SCALE)); printf("vrssi %u\n", io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VRSSI)); } printf("actuators"); diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index dc5c40638b..3c59a75a79 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -163,13 +163,13 @@ #define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */ #define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */ -#define PX4IO_P_SETUP_RELAYS_POWER1 (1<<0) /* [1] power relay 1 */ -#define PX4IO_P_SETUP_RELAYS_POWER2 (1<<1) /* [1] power relay 2 */ -#define PX4IO_P_SETUP_RELAYS_ACC1 (1<<2) /* [1] accessory power 1 */ -#define PX4IO_P_SETUP_RELAYS_ACC2 (1<<3) /* [1] accessory power 2 */ +#define PX4IO_P_SETUP_RELAYS_POWER1 (1<<0) /* hardware rev [1] power relay 1 */ +#define PX4IO_P_SETUP_RELAYS_POWER2 (1<<1) /* hardware rev [1] power relay 2 */ +#define PX4IO_P_SETUP_RELAYS_ACC1 (1<<2) /* hardware rev [1] accessory power 1 */ +#define PX4IO_P_SETUP_RELAYS_ACC2 (1<<3) /* hardware rev [1] accessory power 2 */ -#define PX4IO_P_SETUP_VBATT_SCALE 6 /* [1] battery voltage correction factor (float) */ -#define PX4IO_P_SETUP_VSERVO_SCALE 6 /* [2] servo voltage correction factor (float) */ +#define PX4IO_P_SETUP_VBATT_SCALE 6 /* hardware rev [1] battery voltage correction factor (float) */ +#define PX4IO_P_SETUP_VSERVO_SCALE 6 /* hardware rev [2] servo voltage correction factor (float) */ #define PX4IO_P_SETUP_DSM 7 /* DSM bind state */ enum { /* DSM bind states */ dsm_bind_power_down = 0, diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 9606faa867..3f241d29c9 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -146,7 +146,11 @@ volatile uint16_t r_page_setup[] = [PX4IO_P_SETUP_PWM_DEFAULTRATE] = 50, [PX4IO_P_SETUP_PWM_ALTRATE] = 200, [PX4IO_P_SETUP_RELAYS] = 0, +#ifdef ADC_VSERVO + [PX4IO_P_SETUP_VSERVO_SCALE] = 10000, +#else [PX4IO_P_SETUP_VBATT_SCALE] = 10000, +#endif [PX4IO_P_SETUP_SET_DEBUG] = 0, }; @@ -570,29 +574,17 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val * Coefficients here derived by measurement of the 5-16V * range on one unit: * - * V counts - * 5 1001 - * 6 1219 - * 7 1436 - * 8 1653 - * 9 1870 - * 10 2086 - * 11 2303 - * 12 2522 - * 13 2738 - * 14 2956 - * 15 3172 - * 16 3389 + * XXX pending measurements * - * slope = 0.0046067 - * intercept = 0.3863 + * slope = xxx + * intercept = xxx * - * Intercept corrected for best results @ 12V. + * Intercept corrected for best results @ 5.0V. */ unsigned counts = adc_measure(ADC_VSERVO); if (counts != 0xffff) { unsigned mV = (4150 + (counts * 46)) / 10 - 200; - unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000; + unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VSERVO_SCALE]) / 10000; r_page_status[PX4IO_P_STATUS_VSERVO] = corrected; } From 4e5eb9740b509e814e9c16aefe40a373d67bbc43 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 28 Jul 2013 14:50:27 +0200 Subject: [PATCH 306/486] Fixed led and reboot linker challenges in C++ environments --- makefiles/config_px4fmu-v2_test.mk | 1 + src/drivers/blinkm/blinkm.cpp | 14 +++++++++----- src/drivers/drv_led.h | 2 +- src/drivers/led/led.cpp | 2 +- src/modules/commander/state_machine_helper.c | 2 +- src/modules/systemlib/systemlib.c | 4 ++++ src/modules/systemlib/systemlib.h | 6 +++--- src/systemcmds/reboot/reboot.c | 2 +- 8 files changed, 21 insertions(+), 12 deletions(-) diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 9b3013a5bb..98fd6feda4 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -12,6 +12,7 @@ ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_test # MODULES += drivers/device MODULES += drivers/stm32 +MODULES += drivers/led MODULES += drivers/boards/px4fmuv2 MODULES += systemcmds/perf MODULES += systemcmds/reboot diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 3fabfd9a54..8d2eb056e9 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -92,7 +92,10 @@ #include +__BEGIN_DECLS #include +__END_DECLS +#include #include #include @@ -112,7 +115,6 @@ #include #include -#include #include #include #include @@ -486,15 +488,15 @@ BlinkM::led() /* get number of used satellites in navigation */ num_of_used_sats = 0; - //for(int satloop=0; satloop<20; satloop++) { - for(int satloop=0; satloop checking cells\n"); for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) { @@ -831,6 +833,8 @@ BlinkM::get_firmware_version(uint8_t version[2]) return transfer(&msg, sizeof(msg), version, 2); } +void blinkm_usage(); + void blinkm_usage() { fprintf(stderr, "missing command: try 'start', 'systemstate', 'ledoff', 'list' or a script name {options}\n"); fprintf(stderr, "options:\n"); diff --git a/src/drivers/drv_led.h b/src/drivers/drv_led.h index 21044f6200..97f2db107a 100644 --- a/src/drivers/drv_led.h +++ b/src/drivers/drv_led.h @@ -60,6 +60,6 @@ __BEGIN_DECLS /* * Initialise the LED driver. */ -__EXPORT extern void drv_led_start(void); +__EXPORT void drv_led_start(void); __END_DECLS diff --git a/src/drivers/led/led.cpp b/src/drivers/led/led.cpp index 27e43b2454..fe307223f2 100644 --- a/src/drivers/led/led.cpp +++ b/src/drivers/led/led.cpp @@ -117,4 +117,4 @@ drv_led_start(void) if (gLED != nullptr) gLED->init(); } -} \ No newline at end of file +} diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index ab728c7bbb..c264787852 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -141,7 +141,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con current_status->flag_system_armed = false; mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); usleep(500000); - up_systemreset(); + systemreset(); /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ } else { diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c index 3283aad8a5..a2b0d8cae8 100644 --- a/src/modules/systemlib/systemlib.c +++ b/src/modules/systemlib/systemlib.c @@ -50,6 +50,10 @@ #include "systemlib.h" +__EXPORT extern void systemreset(void) { + up_systemreset(); +} + static void kill_task(FAR struct tcb_s *tcb, FAR void *arg); void killall() diff --git a/src/modules/systemlib/systemlib.h b/src/modules/systemlib/systemlib.h index 0194b5e529..77fdfe08af 100644 --- a/src/modules/systemlib/systemlib.h +++ b/src/modules/systemlib/systemlib.h @@ -42,11 +42,11 @@ #include #include -/** Reboots the board */ -extern void up_systemreset(void) noreturn_function; - __BEGIN_DECLS +/** Reboots the board */ +__EXPORT void systemreset(void) noreturn_function; + /** Sends SIGUSR1 to all processes */ __EXPORT void killall(void); diff --git a/src/systemcmds/reboot/reboot.c b/src/systemcmds/reboot/reboot.c index 47d3cd9486..0fd1e27244 100644 --- a/src/systemcmds/reboot/reboot.c +++ b/src/systemcmds/reboot/reboot.c @@ -47,7 +47,7 @@ __EXPORT int reboot_main(int argc, char *argv[]); int reboot_main(int argc, char *argv[]) { - up_systemreset(); + systemreset(); } From 87cb066bed1eb9ddb36b233964a77403e0c2ba09 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 28 Jul 2013 14:50:45 +0200 Subject: [PATCH 307/486] Disabled serial port renumbering --- nuttx-configs/px4fmu-v2/nsh/defconfig | 1 + 1 file changed, 1 insertion(+) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 70500e4b0a..2e73a5a07f 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -94,6 +94,7 @@ CONFIG_ARCH_HAVE_MPU=y CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y CONFIG_ARMV7M_STACKCHECK=y CONFIG_SERIAL_TERMIOS=y +CONFIG_SERIAL_DISABLE_REORDERING=y CONFIG_SDIO_DMA=y # CONFIG_SDIO_DMAPRIO is not set # CONFIG_SDIO_WIDTH_D1_ONLY is not set From 1410625dea4204f758f42c1bdd06959f75a86ef9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 28 Jul 2013 16:12:40 +0200 Subject: [PATCH 308/486] Enabled additional drivers on v2, kill misplaced code in mkblctrl --- makefiles/config_px4fmu-v2_default.mk | 5 +++- src/drivers/mkblctrl/mkblctrl.cpp | 38 --------------------------- 2 files changed, 4 insertions(+), 39 deletions(-) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 73dc8bd9d4..4add744d0e 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -21,7 +21,6 @@ MODULES += drivers/boards/px4fmuv2 MODULES += drivers/rgbled MODULES += drivers/lsm303d MODULES += drivers/l3gd20 -#MODULES += drivers/mpu6000 MODULES += drivers/hmc5883 MODULES += drivers/ms5611 MODULES += drivers/mb12xx @@ -30,6 +29,10 @@ MODULES += drivers/hil MODULES += drivers/hott/hott_telemetry MODULES += drivers/hott/hott_sensors MODULES += drivers/blinkm +MODULES += drivers/mkblctrl +MODULES += drivers/airspeed +MODULES += drivers/ets_airspeed +MODULES += drivers/meas_airspeed MODULES += modules/sensors # diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index e78d965698..1f4a63cf35 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -1345,44 +1345,6 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, gpio_bits = 0; servo_mode = MK::MODE_NONE; - switch (new_mode) { - case PORT_FULL_GPIO: - case PORT_MODE_UNSET: - /* nothing more to do here */ - break; - - case PORT_FULL_SERIAL: - /* set all multi-GPIOs to serial mode */ - gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4; - break; - - case PORT_FULL_PWM: - /* select 4-pin PWM mode */ - servo_mode = MK::MODE_4PWM; - break; - - case PORT_GPIO_AND_SERIAL: - /* set RX/TX multi-GPIOs to serial mode */ - gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4; - break; - - case PORT_PWM_AND_SERIAL: - /* select 2-pin PWM mode */ - servo_mode = MK::MODE_2PWM; - /* set RX/TX multi-GPIOs to serial mode */ - gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4; - break; - - case PORT_PWM_AND_GPIO: - /* select 2-pin PWM mode */ - servo_mode = MK::MODE_2PWM; - break; - } - - /* adjust GPIO config for serial mode(s) */ - if (gpio_bits != 0) - g_mk->ioctl(0, GPIO_SET_ALT_1, gpio_bits); - /* native PX4 addressing) */ g_mk->set_px4mode(px4mode); From 8c1067a017714394955892e3159c8e0c61bd4ba1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 28 Jul 2013 23:12:16 +0400 Subject: [PATCH 309/486] State machine rewritten, compiles, WIP --- src/modules/commander/commander.cpp | 1226 ++++++++--------- .../commander/state_machine_helper.cpp | 779 +++++------ src/modules/commander/state_machine_helper.h | 12 +- src/modules/mavlink/mavlink.c | 55 +- src/modules/uORB/topics/vehicle_status.h | 55 +- 5 files changed, 944 insertions(+), 1183 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 1978d87824..b2336cbf6f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -177,6 +177,12 @@ void handle_command(int status_pub, struct vehicle_status_s *current_status, int */ int commander_thread_main(int argc, char *argv[]); +void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status); + +transition_result_t check_main_state_machine(struct vehicle_status_s *current_status); + +transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode); + /** * Loop that runs at a lower rate and priority for calibration and parameter tasks. */ @@ -198,11 +204,11 @@ int commander_main(int argc, char *argv[]) thread_should_exit = false; daemon_task = task_spawn_cmd("commander", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 40, - 3000, - commander_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 40, + 3000, + commander_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); } @@ -242,244 +248,171 @@ void handle_command(int status_pub, struct vehicle_status_s *current_status, int /* request to set different system mode */ switch (cmd->command) { - case VEHICLE_CMD_DO_SET_MODE: + case VEHICLE_CMD_DO_SET_MODE: + break; - /* request to activate HIL */ - if ((int)cmd->param1 & VEHICLE_MODE_FLAG_HIL_ENABLED) { + case VEHICLE_CMD_COMPONENT_ARM_DISARM: + break; - if (OK == hil_state_transition(HIL_STATE_ON, status_pub, current_status, control_mode_pub, current_control_mode, mavlink_fd)) { + case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: + break; + + case VEHICLE_CMD_PREFLIGHT_CALIBRATION: + + /* gyro calibration */ + if ((int)(cmd->param1) == 1) { + + /* check if no other task is scheduled */ + if (low_prio_task == LOW_PRIO_TASK_NONE) { + + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(current_status, ARMING_STATE_INIT, armed, mavlink_fd)) { + // TODO publish state result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION; + } else { result = VEHICLE_CMD_RESULT_DENIED; } - } - if ((int)cmd->param1 & VEHICLE_MODE_FLAG_SAFETY_ARMED) { - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } } else { - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } + + /* magnetometer calibration */ + if ((int)(cmd->param2) == 1) { + + /* check if no other task is scheduled */ + if (low_prio_task == LOW_PRIO_TASK_NONE) { + + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(current_status, ARMING_STATE_INIT, armed, mavlink_fd)) { + // TODO publish state result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION; + } else { result = VEHICLE_CMD_RESULT_DENIED; } + + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } + } - break; + // /* zero-altitude pressure calibration */ + // if ((int)(cmd->param3) == 1) { - case VEHICLE_CMD_COMPONENT_ARM_DISARM: + // /* check if no other task is scheduled */ + // if(low_prio_task == LOW_PRIO_TASK_NONE) { - /* request to arm */ - if ((int)cmd->param1 == 1) { - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) { + // /* try to go to INIT/PREFLIGHT arming state */ + // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { + // result = VEHICLE_CMD_RESULT_ACCEPTED; + // /* now set the task for the low prio thread */ + // low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION; + // } else { + // result = VEHICLE_CMD_RESULT_DENIED; + // } + // } else { + // result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + // } + // } + + // /* trim calibration */ + // if ((int)(cmd->param4) == 1) { + + // /* check if no other task is scheduled */ + // if(low_prio_task == LOW_PRIO_TASK_NONE) { + + // /* try to go to INIT/PREFLIGHT arming state */ + // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { + // result = VEHICLE_CMD_RESULT_ACCEPTED; + // /* now set the task for the low prio thread */ + // low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION; + // } else { + // result = VEHICLE_CMD_RESULT_DENIED; + // } + // } else { + // result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + // } + // } + + + /* accel calibration */ + if ((int)(cmd->param5) == 1) { + + /* check if no other task is scheduled */ + if (low_prio_task == LOW_PRIO_TASK_NONE) { + + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(current_status, ARMING_STATE_INIT, armed, mavlink_fd)) { + // TODO publish state result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION; + } else { result = VEHICLE_CMD_RESULT_DENIED; } - /* request to disarm */ - } else if ((int)cmd->param1 == 0) { - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) { + + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } + + /* airspeed calibration */ + if ((int)(cmd->param6) == 1) { + + /* check if no other task is scheduled */ + if (low_prio_task == LOW_PRIO_TASK_NONE) { + + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(current_status, ARMING_STATE_INIT, armed, mavlink_fd)) { + // TODO publish state result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION; + } else { result = VEHICLE_CMD_RESULT_DENIED; } + + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } + } - break; + break; - case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { + case VEHICLE_CMD_PREFLIGHT_STORAGE: - /* request for an autopilot reboot */ - if ((int)cmd->param1 == 1) { - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_REBOOT, armed_pub, armed, mavlink_fd)) { - /* reboot is executed later, after positive tune is sent */ - result = VEHICLE_CMD_RESULT_ACCEPTED; - tune_positive(); - mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); - usleep(500000); - up_systemreset(); - /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ + if (((int)(cmd->param1)) == 0) { + /* check if no other task is scheduled */ + if (low_prio_task == LOW_PRIO_TASK_NONE) { + low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; + result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - /* system may return here */ - result = VEHICLE_CMD_RESULT_DENIED; - tune_negative(); - } - } - } - break; - - // /* request to land */ - // case VEHICLE_CMD_NAV_LAND: - // { - // //TODO: add check if landing possible - // //TODO: add landing maneuver - // - // if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED)) { - // result = VEHICLE_CMD_RESULT_ACCEPTED; - // } } - // break; - // - // /* request to takeoff */ - // case VEHICLE_CMD_NAV_TAKEOFF: - // { - // //TODO: add check if takeoff possible - // //TODO: add takeoff maneuver - // - // if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_AUTO)) { - // result = VEHICLE_CMD_RESULT_ACCEPTED; - // } - // } - // break; - // - /* preflight calibration */ - case VEHICLE_CMD_PREFLIGHT_CALIBRATION: - - /* gyro calibration */ - if ((int)(cmd->param1) == 1) { - - /* check if no other task is scheduled */ - if(low_prio_task == LOW_PRIO_TASK_NONE) { - - /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - /* now set the task for the low prio thread */ - low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - } - - /* magnetometer calibration */ - if ((int)(cmd->param2) == 1) { - - /* check if no other task is scheduled */ - if(low_prio_task == LOW_PRIO_TASK_NONE) { - - /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - /* now set the task for the low prio thread */ - low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } - // /* zero-altitude pressure calibration */ - // if ((int)(cmd->param3) == 1) { + } else if (((int)(cmd->param1)) == 1) { - // /* check if no other task is scheduled */ - // if(low_prio_task == LOW_PRIO_TASK_NONE) { + /* check if no other task is scheduled */ + if (low_prio_task == LOW_PRIO_TASK_NONE) { + low_prio_task = LOW_PRIO_TASK_PARAM_SAVE; + result = VEHICLE_CMD_RESULT_ACCEPTED; - // /* try to go to INIT/PREFLIGHT arming state */ - // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { - // result = VEHICLE_CMD_RESULT_ACCEPTED; - // /* now set the task for the low prio thread */ - // low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION; - // } else { - // result = VEHICLE_CMD_RESULT_DENIED; - // } - // } else { - // result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - // } - // } - - - - // /* trim calibration */ - // if ((int)(cmd->param4) == 1) { - - // /* check if no other task is scheduled */ - // if(low_prio_task == LOW_PRIO_TASK_NONE) { - - // /* try to go to INIT/PREFLIGHT arming state */ - // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { - // result = VEHICLE_CMD_RESULT_ACCEPTED; - // /* now set the task for the low prio thread */ - // low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION; - // } else { - // result = VEHICLE_CMD_RESULT_DENIED; - // } - // } else { - // result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - // } - // } - - - /* accel calibration */ - if ((int)(cmd->param5) == 1) { - - /* check if no other task is scheduled */ - if(low_prio_task == LOW_PRIO_TASK_NONE) { - - /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - /* now set the task for the low prio thread */ - low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } + } - /* airspeed calibration */ - if ((int)(cmd->param6) == 1) { - - /* check if no other task is scheduled */ - if(low_prio_task == LOW_PRIO_TASK_NONE) { - - /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - /* now set the task for the low prio thread */ - low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - } - break; - - case VEHICLE_CMD_PREFLIGHT_STORAGE: - - if (((int)(cmd->param1)) == 0) { - - /* check if no other task is scheduled */ - if(low_prio_task == LOW_PRIO_TASK_NONE) { - low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; - result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - - - } else if (((int)(cmd->param1)) == 1) { - - /* check if no other task is scheduled */ - if(low_prio_task == LOW_PRIO_TASK_NONE) { - low_prio_task = LOW_PRIO_TASK_PARAM_SAVE; - result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - } break; default: @@ -546,12 +479,12 @@ int commander_thread_main(int argc, char *argv[]) if (mavlink_fd < 0) { warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first."); } - + /* Main state machine */ - struct vehicle_status_s current_status; + struct vehicle_status_s status; orb_advert_t status_pub; /* make sure we are in preflight state */ - memset(¤t_status, 0, sizeof(current_status)); + memset(&status, 0, sizeof(status)); /* armed topic */ struct actuator_armed_s armed; @@ -566,17 +499,18 @@ int commander_thread_main(int argc, char *argv[]) /* Initialize all flags to false */ memset(&control_mode, 0, sizeof(control_mode)); - current_status.navigation_state = NAVIGATION_STATE_INIT; - current_status.arming_state = ARMING_STATE_INIT; - current_status.hil_state = HIL_STATE_OFF; + status.main_state = MAIN_STATE_MANUAL; + status.navigation_state = NAVIGATION_STATE_STANDBY; + status.arming_state = ARMING_STATE_INIT; + status.hil_state = HIL_STATE_OFF; /* neither manual nor offboard control commands have been received */ - current_status.offboard_control_signal_found_once = false; - current_status.rc_signal_found_once = false; + status.offboard_control_signal_found_once = false; + status.rc_signal_found_once = false; /* mark all signals lost as long as they haven't been found */ - current_status.rc_signal_lost = true; - current_status.offboard_control_signal_lost = true; + status.rc_signal_lost = true; + status.offboard_control_signal_lost = true; /* allow manual override initially */ control_mode.flag_external_manual_override_ok = true; @@ -585,31 +519,31 @@ int commander_thread_main(int argc, char *argv[]) // current_status.flag_vector_flight_mode_ok = false; /* set battery warning flag */ - current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; - + status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; + /* set safety device detection flag */ /* XXX do we need this? */ //current_status.flag_safety_present = false; // XXX for now just set sensors as initialized - current_status.condition_system_sensors_initialized = true; + status.condition_system_sensors_initialized = true; // XXX just disable offboard control for now control_mode.flag_control_offboard_enabled = false; /* advertise to ORB */ - status_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); + status_pub = orb_advertise(ORB_ID(vehicle_status), &status); /* publish current state machine */ - + /* publish the new state */ - current_status.counter++; - current_status.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, ¤t_status); + status.counter++; + status.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_status), status_pub, &status); armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); - control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); + control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); /* home position */ orb_advert_t home_pub = -1; @@ -686,7 +620,7 @@ int commander_thread_main(int argc, char *argv[]) memset(&local_position, 0, sizeof(local_position)); uint64_t last_local_position_time = 0; - /* + /* * The home position is set based on GPS only, to prevent a dependency between * position estimator and commander. RAW GPS is more than good enough for a * non-flying vehicle. @@ -747,23 +681,28 @@ int commander_thread_main(int argc, char *argv[]) /* update parameters */ if (!armed.armed) { - if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { + if (param_get(_param_sys_type, &(status.system_type)) != OK) { warnx("failed getting new system type"); } /* disable manual override for all systems that rely on electronic stabilization */ - if (current_status.system_type == VEHICLE_TYPE_QUADROTOR || - current_status.system_type == VEHICLE_TYPE_HEXAROTOR || - current_status.system_type == VEHICLE_TYPE_OCTOROTOR) { + if (status.system_type == VEHICLE_TYPE_COAXIAL || + status.system_type == VEHICLE_TYPE_HELICOPTER || + status.system_type == VEHICLE_TYPE_TRICOPTER || + status.system_type == VEHICLE_TYPE_QUADROTOR || + status.system_type == VEHICLE_TYPE_HEXAROTOR || + status.system_type == VEHICLE_TYPE_OCTOROTOR) { control_mode.flag_external_manual_override_ok = false; + status.is_rotary_wing = true; } else { control_mode.flag_external_manual_override_ok = true; + status.is_rotary_wing = false; } /* check and update system / component ID */ - param_get(_param_system_id, &(current_status.system_id)); - param_get(_param_component_id, &(current_status.component_id)); + param_get(_param_system_id, &(status.system_id)); + param_get(_param_component_id, &(status.component_id)); } } @@ -800,7 +739,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - handle_command(status_pub, ¤t_status, control_mode_pub, &control_mode, &cmd, armed_pub, &armed); + handle_command(status_pub, &status, control_mode_pub, &control_mode, &cmd, armed_pub, &armed); } /* update safety topic */ @@ -830,34 +769,37 @@ int commander_thread_main(int argc, char *argv[]) /* set the condition to valid if there has recently been a local position estimate */ if (hrt_absolute_time() - last_local_position_time < LOCAL_POSITION_TIMEOUT) { - current_status.condition_local_position_valid = true; + status.condition_local_position_valid = true; + } else { - current_status.condition_local_position_valid = false; + status.condition_local_position_valid = false; } /* update battery status */ orb_check(battery_sub, &new_data); + if (new_data) { orb_copy(ORB_ID(battery_status), battery_sub, &battery); - current_status.battery_voltage = battery.voltage_v; - current_status.condition_battery_voltage_valid = true; + status.battery_voltage = battery.voltage_v; + status.condition_battery_voltage_valid = true; /* * Only update battery voltage estimate if system has * been running for two and a half seconds. */ - + } - if (hrt_absolute_time() - start_time > 2500000 && current_status.condition_battery_voltage_valid) { - current_status.battery_remaining = battery_remaining_estimate_voltage(current_status.battery_voltage); + if (hrt_absolute_time() - start_time > 2500000 && status.condition_battery_voltage_valid) { + status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage); + } else { - current_status.battery_voltage = 0.0f; + status.battery_voltage = 0.0f; } /* update subsystem */ orb_check(subsys_sub, &new_data); - + if (new_data) { orb_copy(ORB_ID(subsystem_info), subsys_sub, &info); @@ -865,26 +807,26 @@ int commander_thread_main(int argc, char *argv[]) /* mark / unmark as present */ if (info.present) { - current_status.onboard_control_sensors_present |= info.subsystem_type; + status.onboard_control_sensors_present |= info.subsystem_type; } else { - current_status.onboard_control_sensors_present &= ~info.subsystem_type; + status.onboard_control_sensors_present &= ~info.subsystem_type; } /* mark / unmark as enabled */ if (info.enabled) { - current_status.onboard_control_sensors_enabled |= info.subsystem_type; + status.onboard_control_sensors_enabled |= info.subsystem_type; } else { - current_status.onboard_control_sensors_enabled &= ~info.subsystem_type; + status.onboard_control_sensors_enabled &= ~info.subsystem_type; } /* mark / unmark as ok */ if (info.ok) { - current_status.onboard_control_sensors_health |= info.subsystem_type; + status.onboard_control_sensors_health |= info.subsystem_type; } else { - current_status.onboard_control_sensors_health &= ~info.subsystem_type; + status.onboard_control_sensors_health &= ~info.subsystem_type; } } @@ -899,6 +841,7 @@ int commander_thread_main(int argc, char *argv[]) } else if (armed.ready_to_arm && (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0)) { /* ready to arm */ led_toggle(LED_AMBER); + } else if (counter % (100000 / COMMANDER_MONITORING_INTERVAL) == 0) { /* not ready to arm, something is wrong */ led_toggle(LED_AMBER); @@ -908,7 +851,7 @@ int commander_thread_main(int argc, char *argv[]) /* toggle GPS (blue) led at 1 Hz if GPS present but no lock, make is solid once locked */ if ((hrt_absolute_time() - gps_position.timestamp_position < 2000000) - && (gps_position.fix_type == GPS_FIX_TYPE_3D)) { + && (gps_position.fix_type == GPS_FIX_TYPE_3D)) { /* GPS lock */ led_on(LED_BLUE); @@ -940,20 +883,20 @@ int commander_thread_main(int argc, char *argv[]) uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; if (last_idle_time > 0) - current_status.load = 1000 - (interval_runtime / 1000); //system load is time spent in non-idle + status.load = 1000 - (interval_runtime / 1000); //system load is time spent in non-idle last_idle_time = system_load.tasks[0].total_runtime; } - + /* if battery voltage is getting lower, warn using buzzer, etc. */ - if (current_status.condition_battery_voltage_valid && (current_status.battery_remaining < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS + if (status.condition_battery_voltage_valid && (status.battery_remaining < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { low_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] WARNING! LOW BATTERY!"); - current_status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING; + status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING; tune_low_bat(); } @@ -961,11 +904,11 @@ int commander_thread_main(int argc, char *argv[]) } /* Critical, this is rather an emergency, change state machine */ - else if (current_status.condition_battery_voltage_valid && (current_status.battery_remaining < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) { + else if (status.condition_battery_voltage_valid && (status.battery_remaining < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) { if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { critical_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!"); - current_status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; + status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; tune_critical_bat(); // XXX implement state change here } @@ -980,9 +923,11 @@ int commander_thread_main(int argc, char *argv[]) /* End battery voltage check */ /* If in INIT state, try to proceed to STANDBY state */ - if (current_status.arming_state == ARMING_STATE_INIT) { + if (status.arming_state == ARMING_STATE_INIT) { // XXX check for sensors - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); + arming_state_transition(&status, ARMING_STATE_STANDBY, &armed, mavlink_fd); + //TODO publish state + } else { // XXX: Add emergency stuff if sensors are lost } @@ -999,32 +944,32 @@ int commander_thread_main(int argc, char *argv[]) /* store current state to reason later about a state change */ // bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; - bool global_pos_valid = current_status.condition_global_position_valid; - bool local_pos_valid = current_status.condition_local_position_valid; - bool airspeed_valid = current_status.condition_airspeed_valid; + bool global_pos_valid = status.condition_global_position_valid; + bool local_pos_valid = status.condition_local_position_valid; + bool airspeed_valid = status.condition_airspeed_valid; /* check for global or local position updates, set a timeout of 2s */ if (hrt_absolute_time() - last_global_position_time < 2000000 && hrt_absolute_time() > 2000000 && global_position.valid) { - current_status.condition_global_position_valid = true; + status.condition_global_position_valid = true; } else { - current_status.condition_global_position_valid = false; + status.condition_global_position_valid = false; } if (hrt_absolute_time() - last_local_position_time < 2000000 && hrt_absolute_time() > 2000000 && local_position.valid) { - current_status.condition_local_position_valid = true; + status.condition_local_position_valid = true; } else { - current_status.condition_local_position_valid = false; + status.condition_local_position_valid = false; } /* Check for valid airspeed/differential pressure measurements */ if (hrt_absolute_time() - last_diff_pres_time < 2000000 && hrt_absolute_time() > 2000000) { - current_status.condition_airspeed_valid = true; + status.condition_airspeed_valid = true; } else { - current_status.condition_airspeed_valid = false; + status.condition_airspeed_valid = false; } /* @@ -1040,9 +985,9 @@ int commander_thread_main(int argc, char *argv[]) // } /* consolidate state change, flag as changed if required */ - if (global_pos_valid != current_status.condition_global_position_valid || - local_pos_valid != current_status.condition_local_position_valid || - airspeed_valid != current_status.condition_airspeed_valid) { + if (global_pos_valid != status.condition_global_position_valid || + local_pos_valid != status.condition_local_position_valid || + airspeed_valid != status.condition_airspeed_valid) { state_changed = true; } @@ -1059,7 +1004,7 @@ int commander_thread_main(int argc, char *argv[]) // if (!current_status.flag_valid_launch_position && // !vector_flight_mode_ok && current_status.flag_vector_flight_mode_ok && // !current_status.flag_system_armed) { - // first time a valid position, store it and emit it + // first time a valid position, store it and emit it // // XXX implement storage and publication of RTL position // current_status.flag_valid_launch_position = true; @@ -1068,6 +1013,7 @@ int commander_thread_main(int argc, char *argv[]) // } orb_check(gps_sub, &new_data); + if (new_data) { @@ -1093,11 +1039,11 @@ int commander_thread_main(int argc, char *argv[]) */ if (gps_position.fix_type == GPS_FIX_TYPE_3D - && (hdop_m < hdop_threshold_m) - && (vdop_m < vdop_threshold_m) // XXX note that vdop is 0 for mtk - && !home_position_set - && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) - && !armed.armed) { + && (hdop_m < hdop_threshold_m) + && (vdop_m < vdop_threshold_m) // XXX note that vdop is 0 for mtk + && !home_position_set + && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) + && !armed.armed) { warnx("setting home position"); /* copy position data to uORB home message, store it locally as well */ @@ -1114,6 +1060,7 @@ int commander_thread_main(int argc, char *argv[]) /* announce new home position */ if (home_pub > 0) { orb_publish(ORB_ID(home_position), home_pub, &home); + } else { home_pub = orb_advertise(ORB_ID(home_position), &home); } @@ -1125,326 +1072,136 @@ int commander_thread_main(int argc, char *argv[]) } /* ignore RC signals if in offboard control mode */ - if (!current_status.offboard_control_signal_found_once && sp_man.timestamp != 0) { - /* Start RC state check */ - + if (!status.offboard_control_signal_found_once && sp_man.timestamp != 0) { + /* start RC state check */ if ((hrt_absolute_time() - sp_man.timestamp) < 100000) { - - /* - * Check if manual control modes have to be switched - */ - if (!isfinite(sp_man.mode_switch)) { - - warnx("mode sw not finite"); - /* no valid stick position, go to default */ - current_status.mode_switch = MODE_SWITCH_MANUAL; - - } else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) { - - /* bottom stick position, go to manual mode */ - current_status.mode_switch = MODE_SWITCH_MANUAL; - - } else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) { - - /* top stick position, set auto/mission for all vehicle types */ - current_status.mode_switch = MODE_SWITCH_AUTO; - - } else { - - /* center stick position, set seatbelt/simple control */ - current_status.mode_switch = MODE_SWITCH_ASSISTED; - } - - // warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode); - - /* - * Check if land/RTL is requested - */ - if (!isfinite(sp_man.return_switch)) { - - /* this switch is not properly mapped, set default */ - current_status.return_switch = RETURN_SWITCH_NONE; - - } else if (sp_man.return_switch > STICK_ON_OFF_LIMIT) { - - /* top stick position */ - current_status.return_switch = RETURN_SWITCH_RETURN; - - } else { - /* center or bottom stick position, set default */ - current_status.return_switch = RETURN_SWITCH_NONE; - } - - /* check assisted switch */ - if (!isfinite(sp_man.assisted_switch)) { - - /* this switch is not properly mapped, set default */ - current_status.assisted_switch = ASSISTED_SWITCH_SEATBELT; - - } else if (sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { - - /* top stick position */ - current_status.assisted_switch = ASSISTED_SWITCH_SIMPLE; - - } else { - /* center or bottom stick position, set default */ - current_status.assisted_switch = ASSISTED_SWITCH_SEATBELT; - } - - /* check mission switch */ - if (!isfinite(sp_man.mission_switch)) { - - /* this switch is not properly mapped, set default */ - current_status.mission_switch = MISSION_SWITCH_NONE; - - } else if (sp_man.mission_switch > STICK_ON_OFF_LIMIT) { - - /* top switch position */ - current_status.mission_switch = MISSION_SWITCH_MISSION; - - } else if (sp_man.mission_switch < -STICK_ON_OFF_LIMIT) { - - /* bottom switch position */ - current_status.mission_switch = MISSION_SWITCH_NONE; - - } else { - - /* center switch position, set default */ - current_status.mission_switch = MISSION_SWITCH_NONE; // XXX default? - } - - /* Now it's time to handle the stick inputs */ - - switch (current_status.arming_state) { - - /* evaluate the navigation state when disarmed */ - case ARMING_STATE_STANDBY: - - /* just manual, XXX this might depend on the return switch */ - if (current_status.mode_switch == MODE_SWITCH_MANUAL) { - - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); - } - - /* Try assisted or fallback to manual */ - } else if (current_status.mode_switch == MODE_SWITCH_ASSISTED) { - - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); - } - } - - /* Try auto or fallback to seatbelt or even manual */ - } else if (current_status.mode_switch == MODE_SWITCH_AUTO) { - - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // first fallback to SEATBELT_STANDY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // or fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); - } - } - } - } - - break; - - /* evaluate the navigation state when armed */ - case ARMING_STATE_ARMED: - - if (current_status.mode_switch == MODE_SWITCH_MANUAL) { - /* MANUAL */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - - } else if (current_status.mode_switch == MODE_SWITCH_ASSISTED) { - /* ASSISTED */ - if (current_status.return_switch == RETURN_SWITCH_RETURN) { - /* ASSISTED_DESCENT */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - - } else { - if (current_status.assisted_switch == ASSISTED_SWITCH_SIMPLE) { - /* ASSISTED_SIMPLE */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SIMPLE, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to ASSISTED_SEATBELT - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - } - } else { - /* ASSISTED_SEATBELT */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - } - } - - } else if (current_status.mode_switch == MODE_SWITCH_AUTO) { - /* AUTO */ - if (current_status.return_switch == RETURN_SWITCH_RETURN) { - /* AUTO_RTL */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_RTL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to ASSISTED_DESCENT - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - } - } else { - if (current_status.mission_switch == MISSION_SWITCH_MISSION) { - /* AUTO_MISSION */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_MISSION, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to ASSISTED_SEATBELT - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - } - } else { - // TODO check this - if (current_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || current_status.navigation_state == NAVIGATION_STATE_AUTO_LAND) { - /* AUTO_READY */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_READY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - /* AUTO_READY */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - } - } else { - /* AUTO_LOITER */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_LOITER, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to ASSISTED_SEATBELT - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - } - } - } - } - } - break; - - // XXX we might be missing something that triggers a transition from RTL to LAND - - case ARMING_STATE_ARMED_ERROR: - - // XXX work out fail-safe scenarios - break; - - case ARMING_STATE_STANDBY_ERROR: - - // XXX work out fail-safe scenarios - break; - - case ARMING_STATE_REBOOT: - - // XXX I don't think we should end up here - break; - - case ARMING_STATE_IN_AIR_RESTORE: - - // XXX not sure what to do here - break; - default: - break; - } /* handle the case where RC signal was regained */ - if (!current_status.rc_signal_found_once) { - current_status.rc_signal_found_once = true; - mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME."); + if (!status.rc_signal_found_once) { + status.rc_signal_found_once = true; + mavlink_log_critical(mavlink_fd, "[cmd] detected RC signal first time"); } else { - if (current_status.rc_signal_lost) { + if (status.rc_signal_lost) { mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); } } - /* Check if left stick is in lower left position and we're in manual mode --> switch to standby state. - * Do this only for multirotors, not for fixed wing aircraft. - * TODO allow disarm when landed - */ - if (sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f && - control_mode.flag_control_manual_enabled && - ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) { - if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); - tune_positive(); - stick_off_counter = 0; + status.rc_signal_cutting_off = false; + status.rc_signal_lost = false; + status.rc_signal_lost_interval = 0; + + transition_result_t res; // store all transitions results here + + /* arm/disarm by RC */ + bool arming_state_changed; + + res = TRANSITION_NOT_CHANGED; + + /* check if left stick is in lower left position and we are in MANUAL or AUTO mode -> disarm + * do it only for rotary wings */ + if (status.is_rotary_wing && + (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && + (status.main_state == MAIN_STATE_MANUAL || status.navigation_state == NAVIGATION_STATE_AUTO_READY)) { + if (sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { + /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ + arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); + res = arming_state_transition(&status, new_arming_state, &armed, mavlink_fd); + + if (res == TRANSITION_CHANGED) + stick_off_counter = 0; + + } else { + stick_off_counter++; + stick_on_counter = 0; + } } else { - stick_off_counter++; - stick_on_counter = 0; - } - } - - /* check if left stick is in lower right position and we're in manual mode --> arm */ - if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { - if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, armed_pub, &armed, mavlink_fd); - stick_on_counter = 0; - tune_positive(); - - } else { - stick_on_counter++; stick_off_counter = 0; } } - current_status.rc_signal_cutting_off = false; - current_status.rc_signal_lost = false; - current_status.rc_signal_lost_interval = 0; + /* check if left stick is in lower right position and we're in manual mode -> arm */ + if (status.arming_state == ARMING_STATE_STANDBY && + status.main_state == MAIN_STATE_MANUAL) { + if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { + res = arming_state_transition(&status, ARMING_STATE_ARMED, &armed, mavlink_fd); + + if (res == TRANSITION_CHANGED) + stick_on_counter = 0; + + } else { + stick_on_counter++; + stick_off_counter = 0; + } + + } else { + stick_on_counter = 0; + } + } + + if (res == TRANSITION_CHANGED) { + if (status.arming_state == ARMING_STATE_ARMED) { + mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC"); + + } else { + mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); + } + + tune_positive(); + orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); + } + + /* fill current_status according to mode switches */ + check_mode_switches(&sp_man, &status); + + /* evaluate the main state machine */ + res = check_main_state_machine(&status); + + /* we should not get DENIED here */ + if (res == TRANSITION_DENIED) { + mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + } + + bool main_state_changed = (res == TRANSITION_CHANGED); + + /* evaluate the navigation state machine */ + res = check_navigation_state_machine(&status, &control_mode); + + /* we should not get DENIED here */ + if (res == TRANSITION_DENIED) { + mavlink_log_critical(mavlink_fd, "[cmd] ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state); + } + + bool navigation_state_changed = (res == TRANSITION_CHANGED); + + if (arming_state_changed || main_state_changed || navigation_state_changed) { + /* publish new vehicle status */ + status.counter++; + status.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_status), status_pub, &status); + mavlink_log_info(mavlink_fd, "[cmd] state: arm %s, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state); + } + + if (navigation_state_changed) { + /* publish new navigation state */ + control_mode.counter++; + control_mode.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); + } } else { /* print error message for first RC glitch and then every 5 s / 5000 ms) */ - if (!current_status.rc_signal_cutting_off || ((hrt_absolute_time() - last_print_time) > 5000000)) { + if (!status.rc_signal_cutting_off || ((hrt_absolute_time() - last_print_time) > 5000000)) { /* only complain if the offboard control is NOT active */ - current_status.rc_signal_cutting_off = true; + status.rc_signal_cutting_off = true; mavlink_log_critical(mavlink_fd, "CRITICAL - NO REMOTE SIGNAL!"); - if (!current_status.rc_signal_cutting_off) { + if (!status.rc_signal_cutting_off) { printf("Reason: not rc_signal_cutting_off\n"); + } else { printf("last print time: %llu\n", last_print_time); } @@ -1453,12 +1210,12 @@ int commander_thread_main(int argc, char *argv[]) } /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - current_status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp; + status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp; /* if the RC signal is gone for a full second, consider it lost */ - if (current_status.rc_signal_lost_interval > 1000000) { - current_status.rc_signal_lost = true; - current_status.failsave_lowlevel = true; + if (status.rc_signal_lost_interval > 1000000) { + status.rc_signal_lost = true; + status.failsave_lowlevel = true; state_changed = true; } @@ -1477,7 +1234,7 @@ int commander_thread_main(int argc, char *argv[]) /* State machine update for offboard control */ - if (!current_status.rc_signal_found_once && sp_offboard.timestamp != 0) { + if (!status.rc_signal_found_once && sp_offboard.timestamp != 0) { if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) { // /* decide about attitude control flag, enable in att/pos/vel */ @@ -1518,42 +1275,44 @@ int commander_thread_main(int argc, char *argv[]) // } // } - current_status.offboard_control_signal_weak = false; - current_status.offboard_control_signal_lost = false; - current_status.offboard_control_signal_lost_interval = 0; + status.offboard_control_signal_weak = false; + status.offboard_control_signal_lost = false; + status.offboard_control_signal_lost_interval = 0; // XXX check if this is correct /* arm / disarm on request */ if (sp_offboard.armed && !armed.armed) { - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, armed_pub, &armed, mavlink_fd); + arming_state_transition(&status, ARMING_STATE_ARMED, &armed, mavlink_fd); + // TODO publish state } else if (!sp_offboard.armed && armed.armed) { - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); + arming_state_transition(&status, ARMING_STATE_STANDBY, &armed, mavlink_fd); + // TODO publish state } } else { /* print error message for first RC glitch and then every 5 s / 5000 ms) */ - if (!current_status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) { - current_status.offboard_control_signal_weak = true; + if (!status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) { + status.offboard_control_signal_weak = true; mavlink_log_critical(mavlink_fd, "CRIT:NO OFFBOARD CONTROL!"); last_print_time = hrt_absolute_time(); } /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - current_status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp; + status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp; /* if the signal is gone for 0.1 seconds, consider it lost */ - if (current_status.offboard_control_signal_lost_interval > 100000) { - current_status.offboard_control_signal_lost = true; - current_status.failsave_lowlevel_start_time = hrt_absolute_time(); + if (status.offboard_control_signal_lost_interval > 100000) { + status.offboard_control_signal_lost = true; + status.failsave_lowlevel_start_time = hrt_absolute_time(); tune_positive(); /* kill motors after timeout */ - if (hrt_absolute_time() - current_status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) { - current_status.failsave_lowlevel = true; + if (hrt_absolute_time() - status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) { + status.failsave_lowlevel = true; state_changed = true; } } @@ -1563,8 +1322,8 @@ int commander_thread_main(int argc, char *argv[]) - current_status.counter++; - current_status.timestamp = hrt_absolute_time(); + status.counter++; + status.timestamp = hrt_absolute_time(); // XXX this is missing @@ -1580,36 +1339,39 @@ int commander_thread_main(int argc, char *argv[]) /* publish at least with 1 Hz */ if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) { - orb_publish(ORB_ID(vehicle_status), status_pub, ¤t_status); + orb_publish(ORB_ID(vehicle_status), status_pub, &status); state_changed = false; } /* Store old modes to detect and act on state transitions */ - voltage_previous = current_status.battery_voltage; + voltage_previous = status.battery_voltage; /* play tone according to evaluation result */ /* check if we recently armed */ - if (!arm_tune_played && armed.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available))) { + if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available))) { if (tune_arm() == OK) arm_tune_played = true; - /* Trigger audio event for low battery */ - } else if (current_status.battery_remaining < 0.1f && current_status.condition_battery_voltage_valid) { + /* Trigger audio event for low battery */ + + } else if (status.battery_remaining < 0.1f && status.condition_battery_voltage_valid) { if (tune_critical_bat() == OK) battery_tune_played = true; - } else if (current_status.battery_remaining < 0.2f && current_status.condition_battery_voltage_valid) { + + } else if (status.battery_remaining < 0.2f && status.condition_battery_voltage_valid) { if (tune_low_bat() == OK) battery_tune_played = true; - } else if(battery_tune_played) { + + } else if (battery_tune_played) { tune_stop(); battery_tune_played = false; } /* reset arm_tune_played when disarmed */ - if (!(armed.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available)))) { + if (!(armed.armed && (!safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available)))) { arm_tune_played = false; } @@ -1647,6 +1409,172 @@ int commander_thread_main(int argc, char *argv[]) return 0; } +void +check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status) +{ + /* main mode switch */ + if (!isfinite(sp_man->mode_switch)) { + warnx("mode sw not finite"); + current_status->mode_switch = MODE_SWITCH_MANUAL; + + } else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) { + current_status->mode_switch = MODE_SWITCH_AUTO; + + } else { + current_status->mode_switch = MODE_SWITCH_MANUAL; + } + + /* land switch */ + if (!isfinite(sp_man->return_switch)) { + current_status->return_switch = RETURN_SWITCH_NONE; + + } else if (sp_man->return_switch > STICK_ON_OFF_LIMIT) { + current_status->return_switch = RETURN_SWITCH_RETURN; + + } else { + current_status->return_switch = RETURN_SWITCH_NONE; + } + + /* assisted switch */ + if (!isfinite(sp_man->assisted_switch)) { + current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT; + + } else if (sp_man->assisted_switch > STICK_ON_OFF_LIMIT) { + current_status->assisted_switch = ASSISTED_SWITCH_EASY; + + } else { + current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT; + } + + /* mission switch */ + if (!isfinite(sp_man->mission_switch)) { + current_status->mission_switch = MISSION_SWITCH_MISSION; + + } else if (sp_man->mission_switch > STICK_ON_OFF_LIMIT) { + current_status->mission_switch = MISSION_SWITCH_NONE; + + } else { + current_status->mission_switch = MISSION_SWITCH_MISSION; + } +} + +transition_result_t +check_main_state_machine(struct vehicle_status_s *current_status) +{ + /* evaluate the main state machine */ + transition_result_t res = TRANSITION_DENIED; + + switch (current_status->mode_switch) { + case MODE_SWITCH_MANUAL: + res = main_state_transition(current_status, MAIN_STATE_MANUAL, mavlink_fd); + // TRANSITION_DENIED is not possible here + break; + + case MODE_SWITCH_ASSISTED: + if (current_status->assisted_switch == ASSISTED_SWITCH_EASY) { + res = main_state_transition(current_status, MAIN_STATE_EASY, mavlink_fd); + + if (res != TRANSITION_DENIED) + break; // changed successfully or already in this state + + // else fallback to SEATBELT + } + + res = main_state_transition(current_status, MAIN_STATE_SEATBELT, mavlink_fd); + + if (res != TRANSITION_DENIED) + break; // changed successfully or already in this mode + + // else fallback to MANUAL + res = main_state_transition(current_status, MAIN_STATE_MANUAL, mavlink_fd); + // TRANSITION_DENIED is not possible here + break; + + case MODE_SWITCH_AUTO: + res = main_state_transition(current_status, MAIN_STATE_AUTO, mavlink_fd); + + if (res != TRANSITION_DENIED) + break; // changed successfully or already in this state + + // else fallback to SEATBELT (EASY likely will not work too) + res = main_state_transition(current_status, MAIN_STATE_SEATBELT, mavlink_fd); + + if (res != TRANSITION_DENIED) + break; // changed successfully or already in this state + + // else fallback to MANUAL + res = main_state_transition(current_status, MAIN_STATE_MANUAL, mavlink_fd); + // TRANSITION_DENIED is not possible here + break; + + default: + break; + } + + return res; +} + +transition_result_t +check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode) +{ + transition_result_t res = TRANSITION_DENIED; + + if (current_status->arming_state == ARMING_STATE_ARMED || current_status->arming_state == ARMING_STATE_ARMED_ERROR) { + /* ARMED */ + switch (current_status->main_state) { + case MAIN_STATE_MANUAL: + res = navigation_state_transition(current_status, current_status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode, mavlink_fd); + break; + + case MAIN_STATE_SEATBELT: + res = navigation_state_transition(current_status, NAVIGATION_STATE_ALTHOLD, control_mode, mavlink_fd); + break; + + case MAIN_STATE_EASY: + res = navigation_state_transition(current_status, NAVIGATION_STATE_VECTOR, control_mode, mavlink_fd); + break; + + case MAIN_STATE_AUTO: + if (current_status->navigation_state != NAVIGATION_STATE_AUTO_TAKEOFF) { + /* don't act while taking off */ + if (current_status->condition_landed) { + /* if landed: transitions only to AUTO_READY are allowed */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode, mavlink_fd); + // TRANSITION_DENIED is not possible here + break; + + } else { + /* if not landed: act depending on switches */ + if (current_status->return_switch == RETURN_SWITCH_RETURN) { + /* RTL */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode, mavlink_fd); + + } else { + if (current_status->mission_switch == MISSION_SWITCH_MISSION) { + /* MISSION */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode, mavlink_fd); + + } else { + /* LOITER */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode, mavlink_fd); + } + } + } + } + + break; + + default: + break; + } + + } else { + /* DISARMED */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_STANDBY, control_mode, mavlink_fd); + } + + return res; +} void *commander_low_prio_loop(void *arg) { @@ -1657,72 +1585,76 @@ void *commander_low_prio_loop(void *arg) switch (low_prio_task) { - case LOW_PRIO_TASK_PARAM_LOAD: + case LOW_PRIO_TASK_PARAM_LOAD: - if (0 == param_load_default()) { - mavlink_log_info(mavlink_fd, "Param load success"); - } else { - mavlink_log_critical(mavlink_fd, "Param load ERROR"); - tune_error(); - } - low_prio_task = LOW_PRIO_TASK_NONE; - break; + if (0 == param_load_default()) { + mavlink_log_info(mavlink_fd, "Param load success"); - case LOW_PRIO_TASK_PARAM_SAVE: + } else { + mavlink_log_critical(mavlink_fd, "Param load ERROR"); + tune_error(); + } - if (0 == param_save_default()) { - mavlink_log_info(mavlink_fd, "Param save success"); - } else { - mavlink_log_critical(mavlink_fd, "Param save ERROR"); - tune_error(); - } - low_prio_task = LOW_PRIO_TASK_NONE; - break; + low_prio_task = LOW_PRIO_TASK_NONE; + break; - case LOW_PRIO_TASK_GYRO_CALIBRATION: + case LOW_PRIO_TASK_PARAM_SAVE: - do_gyro_calibration(mavlink_fd); + if (0 == param_save_default()) { + mavlink_log_info(mavlink_fd, "Param save success"); - low_prio_task = LOW_PRIO_TASK_NONE; - break; + } else { + mavlink_log_critical(mavlink_fd, "Param save ERROR"); + tune_error(); + } - case LOW_PRIO_TASK_MAG_CALIBRATION: + low_prio_task = LOW_PRIO_TASK_NONE; + break; - do_mag_calibration(mavlink_fd); + case LOW_PRIO_TASK_GYRO_CALIBRATION: - low_prio_task = LOW_PRIO_TASK_NONE; - break; + do_gyro_calibration(mavlink_fd); - case LOW_PRIO_TASK_ALTITUDE_CALIBRATION: + low_prio_task = LOW_PRIO_TASK_NONE; + break; - // do_baro_calibration(mavlink_fd); + case LOW_PRIO_TASK_MAG_CALIBRATION: - case LOW_PRIO_TASK_RC_CALIBRATION: + do_mag_calibration(mavlink_fd); - // do_rc_calibration(mavlink_fd); + low_prio_task = LOW_PRIO_TASK_NONE; + break; - low_prio_task = LOW_PRIO_TASK_NONE; - break; + case LOW_PRIO_TASK_ALTITUDE_CALIBRATION: - case LOW_PRIO_TASK_ACCEL_CALIBRATION: + // do_baro_calibration(mavlink_fd); - do_accel_calibration(mavlink_fd); + case LOW_PRIO_TASK_RC_CALIBRATION: - low_prio_task = LOW_PRIO_TASK_NONE; - break; + // do_rc_calibration(mavlink_fd); - case LOW_PRIO_TASK_AIRSPEED_CALIBRATION: + low_prio_task = LOW_PRIO_TASK_NONE; + break; - do_airspeed_calibration(mavlink_fd); + case LOW_PRIO_TASK_ACCEL_CALIBRATION: - low_prio_task = LOW_PRIO_TASK_NONE; - break; + do_accel_calibration(mavlink_fd); - case LOW_PRIO_TASK_NONE: - default: - /* slow down to 10Hz */ - usleep(100000); - break; + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_AIRSPEED_CALIBRATION: + + do_airspeed_calibration(mavlink_fd); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_NONE: + default: + /* slow down to 10Hz */ + usleep(100000); + break; } } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 4a7aa66b70..043ccfd0c9 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -62,102 +62,110 @@ #endif static const int ERROR = -1; -int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int armed_pub, struct actuator_armed_s *armed, const int mavlink_fd) { - - - int ret = ERROR; +transition_result_t +arming_state_transition(struct vehicle_status_s *status, arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd) +{ + transition_result_t ret = TRANSITION_DENIED; /* only check transition if the new state is actually different from the current one */ - if (new_arming_state == current_state->arming_state) { - ret = OK; + if (new_arming_state == status->arming_state) { + ret = TRANSITION_NOT_CHANGED; + } else { switch (new_arming_state) { - case ARMING_STATE_INIT: + case ARMING_STATE_INIT: - /* allow going back from INIT for calibration */ - if (current_state->arming_state == ARMING_STATE_STANDBY) { - ret = OK; + /* allow going back from INIT for calibration */ + if (status->arming_state == ARMING_STATE_STANDBY) { + ret = TRANSITION_CHANGED; + armed->armed = false; + armed->ready_to_arm = false; + } + + break; + + case ARMING_STATE_STANDBY: + + /* allow coming from INIT and disarming from ARMED */ + if (status->arming_state == ARMING_STATE_INIT + || status->arming_state == ARMING_STATE_ARMED) { + + /* sensors need to be initialized for STANDBY state */ + if (status->condition_system_sensors_initialized) { + ret = TRANSITION_CHANGED; armed->armed = false; - armed->ready_to_arm = false; + armed->ready_to_arm = true; + + } else { + mavlink_log_critical(mavlink_fd, "rej. STANDBY state, sensors not initialized"); } - break; - case ARMING_STATE_STANDBY: + } - /* allow coming from INIT and disarming from ARMED */ - if (current_state->arming_state == ARMING_STATE_INIT - || current_state->arming_state == ARMING_STATE_ARMED) { + break; - /* sensors need to be initialized for STANDBY state */ - if (current_state->condition_system_sensors_initialized) { - ret = OK; - armed->armed = false; - armed->ready_to_arm = true; - } else { - mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized"); - } - } - break; - case ARMING_STATE_ARMED: + case ARMING_STATE_ARMED: - /* allow arming from STANDBY and IN-AIR-RESTORE */ - if (current_state->arming_state == ARMING_STATE_STANDBY - || current_state->arming_state == ARMING_STATE_IN_AIR_RESTORE) { + /* allow arming from STANDBY and IN-AIR-RESTORE */ + if (status->arming_state == ARMING_STATE_STANDBY + || status->arming_state == ARMING_STATE_IN_AIR_RESTORE) { + ret = TRANSITION_CHANGED; + armed->armed = true; + armed->ready_to_arm = false; + } - /* XXX conditions for arming? */ - ret = OK; - armed->armed = true; - } - break; - case ARMING_STATE_ARMED_ERROR: + break; - /* an armed error happens when ARMED obviously */ - if (current_state->arming_state == ARMING_STATE_ARMED) { - - /* XXX conditions for an error state? */ - ret = OK; - armed->armed = true; - } - break; - case ARMING_STATE_STANDBY_ERROR: - /* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */ - if (current_state->arming_state == ARMING_STATE_STANDBY - || current_state->arming_state == ARMING_STATE_INIT - || current_state->arming_state == ARMING_STATE_ARMED_ERROR) { - ret = OK; - armed->armed = false; - armed->ready_to_arm = false; - } - break; - case ARMING_STATE_REBOOT: + case ARMING_STATE_ARMED_ERROR: - /* an armed error happens when ARMED obviously */ - if (current_state->arming_state == ARMING_STATE_INIT - || current_state->arming_state == ARMING_STATE_STANDBY - || current_state->arming_state == ARMING_STATE_STANDBY_ERROR) { + /* an armed error happens when ARMED obviously */ + if (status->arming_state == ARMING_STATE_ARMED) { + ret = TRANSITION_CHANGED; + armed->armed = true; + armed->ready_to_arm = false; + } - ret = OK; - armed->armed = false; - armed->ready_to_arm = false; + break; - } - break; - case ARMING_STATE_IN_AIR_RESTORE: + case ARMING_STATE_STANDBY_ERROR: - /* XXX implement */ - break; - default: - break; + /* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */ + if (status->arming_state == ARMING_STATE_STANDBY + || status->arming_state == ARMING_STATE_INIT + || status->arming_state == ARMING_STATE_ARMED_ERROR) { + ret = TRANSITION_CHANGED; + armed->armed = false; + armed->ready_to_arm = false; + } + + break; + + case ARMING_STATE_REBOOT: + + /* an armed error happens when ARMED obviously */ + if (status->arming_state == ARMING_STATE_INIT + || status->arming_state == ARMING_STATE_STANDBY + || status->arming_state == ARMING_STATE_STANDBY_ERROR) { + ret = TRANSITION_CHANGED; + armed->armed = false; + armed->ready_to_arm = false; + } + + break; + + case ARMING_STATE_IN_AIR_RESTORE: + + /* XXX implement */ + break; + + default: + break; } - if (ret == OK) { - current_state->arming_state = new_arming_state; - current_state->counter++; - current_state->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, current_state); - - armed->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(actuator_armed), armed_pub, armed); + if (ret == TRANSITION_CHANGED) { + hrt_abstime t = hrt_absolute_time(); + status->arming_state = new_arming_state; + armed->timestamp = t; } } @@ -165,400 +173,230 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta } - -/* - * This functions does not evaluate any input flags but only checks if the transitions - * are valid. - */ -int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, int control_mode_pub, struct vehicle_control_mode_s *control_mode, const int mavlink_fd) { - - int ret = ERROR; +transition_result_t +main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state, const int mavlink_fd) +{ + transition_result_t ret = TRANSITION_DENIED; /* only check transition if the new state is actually different from the current one */ - if (new_navigation_state == current_state->navigation_state) { - ret = OK; + if (new_main_state == current_state->main_state) { + ret = TRANSITION_NOT_CHANGED; + } else { - switch (new_navigation_state) { - case NAVIGATION_STATE_INIT: + switch (new_main_state) { + case MAIN_STATE_MANUAL: + ret = TRANSITION_CHANGED; + break; - /* transitions back to INIT are possible for calibration */ - if (current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY) { + case MAIN_STATE_SEATBELT: - ret = OK; - control_mode->flag_control_rates_enabled = false; - control_mode->flag_control_attitude_enabled = false; - control_mode->flag_control_velocity_enabled = false; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = false; - control_mode->flag_control_manual_enabled = false; - } - break; + /* need position estimate */ + // TODO only need altitude estimate really + if (!current_state->condition_local_position_valid) { + mavlink_log_critical(mavlink_fd, "rej. SEATBELT: no position estimate"); + tune_negative(); - case NAVIGATION_STATE_MANUAL_STANDBY: + } else { + ret = TRANSITION_CHANGED; + } - /* transitions from INIT and other STANDBY states as well as MANUAL are possible */ - if (current_state->navigation_state == NAVIGATION_STATE_INIT - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { + break; - /* need to be disarmed first */ - if (current_state->arming_state != ARMING_STATE_STANDBY) { - mavlink_log_critical(mavlink_fd, "Rej. MANUAL_STANDBY: not disarmed"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = false; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = false; - control_mode->flag_control_manual_enabled = true; - } - } - break; + case MAIN_STATE_EASY: - case NAVIGATION_STATE_MANUAL: + /* need position estimate */ + if (!current_state->condition_local_position_valid) { + mavlink_log_critical(mavlink_fd, "rej. EASY: no position estimate"); + tune_negative(); - /* need to be armed first */ - if (current_state->arming_state != ARMING_STATE_ARMED) { - mavlink_log_critical(mavlink_fd, "Rej. MANUAL: not armed"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = false; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = false; - control_mode->flag_control_manual_enabled = true; - } - break; + } else { + ret = TRANSITION_CHANGED; + } - case NAVIGATION_STATE_ASSISTED_STANDBY: + break; - /* transitions from INIT and other STANDBY states as well as SEATBELT and SEATBELT_DESCENT are possible */ - if (current_state->navigation_state == NAVIGATION_STATE_INIT - || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT) { + case MAIN_STATE_AUTO: - /* need to be disarmed and have a position estimate */ - if (current_state->arming_state != ARMING_STATE_STANDBY) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: not disarmed"); - tune_negative(); - } else if (!current_state->condition_local_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: no position estimate"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = true; - } - } - break; + /* need position estimate */ + if (!current_state->condition_local_position_valid) { + mavlink_log_critical(mavlink_fd, "rej. AUTO: no position estimate"); + tune_negative(); - case NAVIGATION_STATE_ASSISTED_SEATBELT: + } else { + ret = TRANSITION_CHANGED; + } - /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/ - if (current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT - || current_state->navigation_state == NAVIGATION_STATE_MANUAL - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - - /* need to be armed and have a position estimate */ - if (current_state->arming_state != ARMING_STATE_ARMED) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: not armed"); - tune_negative(); - } else if (!current_state->condition_local_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = true; - } - } - break; - - case NAVIGATION_STATE_ASSISTED_SIMPLE: - - /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/ - if (current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT - || current_state->navigation_state == NAVIGATION_STATE_MANUAL - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - - /* need to be armed and have a position estimate */ - if (current_state->arming_state != ARMING_STATE_ARMED) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: not armed"); - tune_negative(); - } else if (!current_state->condition_local_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = true; - } - } - break; - - case NAVIGATION_STATE_ASSISTED_DESCENT: - - /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT and SEATBELT_STANDBY */ - if (current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE - || current_state->navigation_state == NAVIGATION_STATE_MANUAL - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - - /* need to be armed and have a position estimate */ - if (current_state->arming_state != ARMING_STATE_ARMED) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: not armed"); - tune_negative(); - } else if (!current_state->condition_local_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no pos estimate"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = true; - } - } - break; - - case NAVIGATION_STATE_AUTO_STANDBY: - - /* transitions from INIT or from other STANDBY modes or from AUTO READY */ - if (current_state->navigation_state == NAVIGATION_STATE_INIT - || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) { - - /* need to be disarmed and have a position and home lock */ - if (current_state->arming_state != ARMING_STATE_STANDBY) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: not disarmed"); - tune_negative(); - } else if (!current_state->condition_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no pos lock"); - tune_negative(); - } else if (!current_state->condition_home_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no home pos"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - } - break; - - case NAVIGATION_STATE_AUTO_READY: - - /* transitions from AUTO_STANDBY or AUTO_LAND */ - if (current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND) { - - // XXX flag test needed? - - /* need to be armed and have a position and home lock */ - if (current_state->arming_state != ARMING_STATE_ARMED) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_READY: not armed"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - } - break; - - case NAVIGATION_STATE_AUTO_TAKEOFF: - - /* only transitions from AUTO_READY */ - if (current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) { - - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - break; - - case NAVIGATION_STATE_AUTO_LOITER: - - /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ - if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF - || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE - || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { - - /* need to have a position and home lock */ - if (!current_state->condition_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no pos lock"); - tune_negative(); - } else if (!current_state->condition_home_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no home pos"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - } - break; - - case NAVIGATION_STATE_AUTO_MISSION: - - /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ - if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE - || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { - - /* need to have a mission ready */ - if (!current_state-> condition_auto_mission_available) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_MISSION: no mission available"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - } - break; - - case NAVIGATION_STATE_AUTO_RTL: - - /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ - if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF - || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE - || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { - - /* need to have a position and home lock */ - if (!current_state->condition_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no pos lock"); - tune_negative(); - } else if (!current_state->condition_home_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no home pos"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - } - break; - - case NAVIGATION_STATE_AUTO_LAND: - /* after AUTO_RTL or when in AUTO_LOITER or AUTO_MISSION */ - if (current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER) { - - /* need to have a position and home lock */ - if (!current_state->condition_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no pos lock"); - tune_negative(); - } else if (!current_state->condition_home_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no home pos"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - } - break; - - default: - break; + break; } - if (ret == OK) { - current_state->navigation_state = new_navigation_state; - current_state->counter++; - current_state->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, current_state); - - control_mode->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, control_mode); + if (ret == TRANSITION_CHANGED) { + current_state->main_state = new_main_state; } } - + return ret; +} + +transition_result_t +navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode, const int mavlink_fd) +{ + transition_result_t ret = TRANSITION_DENIED; + + /* only check transition if the new state is actually different from the current one */ + if (new_navigation_state == current_status->navigation_state) { + ret = TRANSITION_NOT_CHANGED; + + } else { + + switch (new_navigation_state) { + case NAVIGATION_STATE_STANDBY: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = false; + control_mode->flag_control_attitude_enabled = false; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = false; + control_mode->flag_control_manual_enabled = false; + break; + + case NAVIGATION_STATE_DIRECT: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = false; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = false; + control_mode->flag_control_manual_enabled = true; + break; + + case NAVIGATION_STATE_STABILIZE: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = false; + control_mode->flag_control_manual_enabled = true; + break; + + case NAVIGATION_STATE_ALTHOLD: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = true; + break; + + case NAVIGATION_STATE_VECTOR: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = true; + break; + + case NAVIGATION_STATE_AUTO_READY: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = false; + break; + + case NAVIGATION_STATE_AUTO_TAKEOFF: + + /* only transitions from AUTO_READY */ + if (current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + + break; + + case NAVIGATION_STATE_AUTO_LOITER: + + /* deny transitions from landed states */ + if (current_status->navigation_state != NAVIGATION_STATE_STANDBY && + current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + + break; + + case NAVIGATION_STATE_AUTO_MISSION: + + /* deny transitions from landed states */ + if (current_status->navigation_state != NAVIGATION_STATE_STANDBY && + current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + + break; + + case NAVIGATION_STATE_AUTO_RTL: + + /* deny transitions from landed states */ + if (current_status->navigation_state != NAVIGATION_STATE_STANDBY && + current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + + break; + + case NAVIGATION_STATE_AUTO_LAND: + + /* deny transitions from landed states */ + if (current_status->navigation_state != NAVIGATION_STATE_STANDBY && + current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + + break; + + default: + break; + } + + if (ret == TRANSITION_CHANGED) { + current_status->navigation_state = new_navigation_state; + } + } return ret; } @@ -582,31 +420,33 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s switch (new_state) { - case HIL_STATE_OFF: + case HIL_STATE_OFF: - if (current_status->arming_state == ARMING_STATE_INIT - || current_status->arming_state == ARMING_STATE_STANDBY) { + if (current_status->arming_state == ARMING_STATE_INIT + || current_status->arming_state == ARMING_STATE_STANDBY) { - current_control_mode->flag_system_hil_enabled = false; - mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); - valid_transition = true; - } - break; + current_control_mode->flag_system_hil_enabled = false; + mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); + valid_transition = true; + } - case HIL_STATE_ON: + break; - if (current_status->arming_state == ARMING_STATE_INIT - || current_status->arming_state == ARMING_STATE_STANDBY) { + case HIL_STATE_ON: - current_control_mode->flag_system_hil_enabled = true; - mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); - valid_transition = true; - } - break; + if (current_status->arming_state == ARMING_STATE_INIT + || current_status->arming_state == ARMING_STATE_STANDBY) { - default: - warnx("Unknown hil state"); - break; + current_control_mode->flag_system_hil_enabled = true; + mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); + valid_transition = true; + } + + break; + + default: + warnx("Unknown hil state"); + break; } } @@ -621,6 +461,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, current_control_mode); ret = OK; + } else { mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 75fdd224c4..b59b66c8ba 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -49,10 +49,18 @@ #include #include +typedef enum { + TRANSITION_DENIED = -1, + TRANSITION_NOT_CHANGED = 0, + TRANSITION_CHANGED -int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int armed_pub, struct actuator_armed_s *armed, const int mavlink_fd); +} transition_result_t; -int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, int control_mode_pub, struct vehicle_control_mode_s *control_mode, const int mavlink_fd); +transition_result_t arming_state_transition(struct vehicle_status_s *current_state, arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd); + +transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state, const int mavlink_fd); + +transition_result_t navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode, const int mavlink_fd); int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd); diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index ae8e168e18..9132d1b491 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -196,13 +196,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) } /* autonomous mode */ - if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND - || v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER - || v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION - || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY - || v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL - || v_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + if (v_status.main_state == MAIN_STATE_AUTO) { *mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED; } @@ -215,15 +209,14 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; } - if (v_status.navigation_state == NAVIGATION_STATE_MANUAL - || v_status.navigation_state == NAVIGATION_STATE_MANUAL_STANDBY) { + if (v_status.main_state == MAIN_STATE_MANUAL + || v_status.main_state == MAIN_STATE_SEATBELT + || v_status.main_state == MAIN_STATE_EASY) { *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; } - if (v_status.navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT - || v_status.navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT - || v_status.navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY) { + if (v_status.navigation_state == MAIN_STATE_EASY) { *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; } @@ -235,41 +228,25 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) /* set calibration state */ if (v_status.preflight_calibration) { - *mavlink_state = MAV_STATE_CALIBRATING; - } else if (v_status.system_emergency) { - *mavlink_state = MAV_STATE_EMERGENCY; - - // XXX difference between active and armed? is AUTO_READY active? - } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND - || v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER - || v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION - || v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL - || v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF - || v_status.navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT - || v_status.navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT - || v_status.navigation_state == NAVIGATION_STATE_MANUAL) { - - *mavlink_state = MAV_STATE_ACTIVE; - - } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY // XXX correct? - || v_status.navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY - || v_status.navigation_state == NAVIGATION_STATE_MANUAL_STANDBY) { - - *mavlink_state = MAV_STATE_STANDBY; - - } else if (v_status.navigation_state == NAVIGATION_STATE_INIT) { - + } else if (v_status.arming_state == ARMING_STATE_INIT + || v_status.arming_state == ARMING_STATE_IN_AIR_RESTORE + || v_status.arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review *mavlink_state = MAV_STATE_UNINIT; + } else if (v_status.arming_state == ARMING_STATE_ARMED) { + *mavlink_state = MAV_STATE_ACTIVE; + } else if (v_status.arming_state == ARMING_STATE_ARMED_ERROR) { + *mavlink_state = MAV_STATE_CRITICAL; + } else if (v_status.arming_state == ARMING_STATE_STANDBY) { + *mavlink_state = MAV_STATE_STANDBY; + } else if (v_status.arming_state == ARMING_STATE_REBOOT) { + *mavlink_state = MAV_STATE_POWEROFF; } else { - warnx("Unknown mavlink state"); *mavlink_state = MAV_STATE_CRITICAL; } - } diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 9b91e834e0..e7feaa98eb 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -58,29 +58,28 @@ * @addtogroup topics @{ */ -/* State Machine */ +/* main state machine */ typedef enum { - NAVIGATION_STATE_INIT = 0, - NAVIGATION_STATE_MANUAL_STANDBY, - NAVIGATION_STATE_MANUAL, - NAVIGATION_STATE_ASSISTED_STANDBY, - NAVIGATION_STATE_ASSISTED_SEATBELT, - NAVIGATION_STATE_ASSISTED_SIMPLE, - NAVIGATION_STATE_ASSISTED_DESCENT, - NAVIGATION_STATE_AUTO_STANDBY, - NAVIGATION_STATE_AUTO_READY, - NAVIGATION_STATE_AUTO_TAKEOFF, - NAVIGATION_STATE_AUTO_LOITER, - NAVIGATION_STATE_AUTO_MISSION, - NAVIGATION_STATE_AUTO_RTL, - NAVIGATION_STATE_AUTO_LAND -} navigation_state_t; + MAIN_STATE_MANUAL = 0, + MAIN_STATE_SEATBELT, + MAIN_STATE_EASY, + MAIN_STATE_AUTO, +} main_state_t; +/* navigation state machine */ typedef enum { - MANUAL_STANDBY = 0, - MANUAL_READY, - MANUAL_IN_AIR -} manual_state_t; + NAVIGATION_STATE_STANDBY = 0, // standby state, disarmed + NAVIGATION_STATE_DIRECT, // true manual control, no any stabilization + NAVIGATION_STATE_STABILIZE, // attitude stabilization + NAVIGATION_STATE_ALTHOLD, // attitude + altitude stabilization + NAVIGATION_STATE_VECTOR, // attitude + altitude + position stabilization + NAVIGATION_STATE_AUTO_READY, // AUTO, landed, reeady for takeoff + NAVIGATION_STATE_AUTO_TAKEOFF, // detect takeoff using land detector and switch to desired AUTO mode + NAVIGATION_STATE_AUTO_LOITER, // pause mission + NAVIGATION_STATE_AUTO_MISSION, // fly mission + NAVIGATION_STATE_AUTO_RTL, // Return To Launch, when home position switch to LAND + NAVIGATION_STATE_AUTO_LAND // land and switch to AUTO_READY when landed (detect using land detector) +} navigation_state_t; typedef enum { ARMING_STATE_INIT = 0, @@ -103,16 +102,16 @@ typedef enum { MODE_SWITCH_AUTO } mode_switch_pos_t; +typedef enum { + ASSISTED_SWITCH_SEATBELT = 0, + ASSISTED_SWITCH_EASY +} assisted_switch_pos_t; + typedef enum { RETURN_SWITCH_NONE = 0, RETURN_SWITCH_RETURN } return_switch_pos_t; -typedef enum { - ASSISTED_SWITCH_SEATBELT = 0, - ASSISTED_SWITCH_SIMPLE -} assisted_switch_pos_t; - typedef enum { MISSION_SWITCH_NONE = 0, MISSION_SWITCH_MISSION @@ -175,7 +174,8 @@ struct vehicle_status_s uint64_t failsave_lowlevel_start_time; /**< time when the lowlevel failsafe flag was set */ // uint64_t failsave_highlevel_begin; TO BE COMPLETED - navigation_state_t navigation_state; /**< current system state */ + main_state_t main_state; /**< main state machine */ + navigation_state_t navigation_state; /**< navigation state machine */ arming_state_t arming_state; /**< current arming state */ hil_state_t hil_state; /**< current hil state */ @@ -183,6 +183,8 @@ struct vehicle_status_s int32_t system_id; /**< system id, inspired by MAVLink's system ID field */ int32_t component_id; /**< subsystem / component id, inspired by MAVLink's component ID field */ + bool is_rotary_wing; + mode_switch_pos_t mode_switch; return_switch_pos_t return_switch; assisted_switch_pos_t assisted_switch; @@ -198,6 +200,7 @@ struct vehicle_status_s bool condition_home_position_valid; /**< indicates a valid home position (a valid home position is not always a valid launch) */ bool condition_local_position_valid; bool condition_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */ + bool condition_landed; /**< true if vehicle is landed, always true if disarmed */ bool rc_signal_found_once; bool rc_signal_lost; /**< true if RC reception is terminally lost */ From 57cbf724f159cec88b21281a4ace415276df3a38 Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 29 Jul 2013 23:13:46 -0700 Subject: [PATCH 310/486] Fix the clock enable register for FMUv2 PWM outputs 1-4. Teach the stm32 pwm driver about the MOE bit on advanced timers. --- src/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c | 2 +- src/drivers/stm32/drv_pwm_servo.c | 12 ++++++++++++ 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/src/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c index d1656005b8..bcbc0010ca 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c +++ b/src/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c @@ -53,7 +53,7 @@ __EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = { { .base = STM32_TIM1_BASE, - .clock_register = STM32_RCC_APB1ENR, + .clock_register = STM32_RCC_APB2ENR, .clock_bit = RCC_APB2ENR_TIM1EN, .clock_freq = STM32_APB2_TIM1_CLKIN }, diff --git a/src/drivers/stm32/drv_pwm_servo.c b/src/drivers/stm32/drv_pwm_servo.c index 7b060412cd..c85e83ddca 100644 --- a/src/drivers/stm32/drv_pwm_servo.c +++ b/src/drivers/stm32/drv_pwm_servo.c @@ -88,6 +88,7 @@ #define rCCR4(_tmr) REG(_tmr, STM32_GTIM_CCR4_OFFSET) #define rDCR(_tmr) REG(_tmr, STM32_GTIM_DCR_OFFSET) #define rDMAR(_tmr) REG(_tmr, STM32_GTIM_DMAR_OFFSET) +#define rBDTR(_tmr) REG(_tmr, STM32_ATIM_BDTR_OFFSET) static void pwm_timer_init(unsigned timer); static void pwm_timer_set_rate(unsigned timer, unsigned rate); @@ -110,6 +111,11 @@ pwm_timer_init(unsigned timer) rCCER(timer) = 0; rDCR(timer) = 0; + if ((pwm_timers[timer].base == STM32_TIM1_BASE) || (pwm_timers[timer].base == STM32_TIM8_BASE)) { + /* master output enable = on */ + rBDTR(timer) = ATIM_BDTR_MOE; + } + /* configure the timer to free-run at 1MHz */ rPSC(timer) = (pwm_timers[timer].clock_freq / 1000000) - 1; @@ -163,6 +169,9 @@ pwm_channel_init(unsigned channel) rCCER(timer) |= GTIM_CCER_CC4E; break; } + + /* generate an update event; reloads the counter and all registers */ + rEGR(timer) = GTIM_EGR_UG; } int @@ -203,6 +212,9 @@ up_pwm_servo_set(unsigned channel, servo_position_t value) return -1; } + /* generate an update event; reloads the counter and all registers */ + rEGR(timer) = GTIM_EGR_UG; + return 0; } From 7e9a18da795e56e229957dba47ed7468eac10697 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 30 Jul 2013 15:10:22 +0200 Subject: [PATCH 311/486] Changed gyro scaling according to datasheet --- src/drivers/l3gd20/l3gd20.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index a9af6711b1..423adc76db 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -559,28 +559,32 @@ int L3GD20::set_range(unsigned max_dps) { uint8_t bits = REG4_BDU; + float new_range_scale_dps_digit; - if (max_dps == 0) + if (max_dps == 0) { max_dps = 2000; - + } if (max_dps <= 250) { _current_range = 250; bits |= RANGE_250DPS; + new_range_scale_dps_digit = 8.75e-3f; } else if (max_dps <= 500) { _current_range = 500; bits |= RANGE_500DPS; + new_range_scale_dps_digit = 17.5e-3f; } else if (max_dps <= 2000) { _current_range = 2000; bits |= RANGE_2000DPS; + new_range_scale_dps_digit = 70e-3f; } else { return -EINVAL; } _gyro_range_rad_s = _current_range / 180.0f * M_PI_F; - _gyro_range_scale = _gyro_range_rad_s / 32768.0f; + _gyro_range_scale = new_range_scale_dps_digit / 180.0f * M_PI_F; write_reg(ADDR_CTRL_REG4, bits); return OK; From b85d74336d62d467b21f98f69020c5db2f21efb0 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 30 Jul 2013 22:46:41 -0700 Subject: [PATCH 312/486] Add missing 'fi' at the end of rc script; if you had a microSD card that set MODE to something other than autostart the result was a prompt that ignored your commands. --- ROMFS/px4fmu_common/init.d/rcS | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 6d141f1b6e..ccbae2cbc0 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -119,3 +119,6 @@ if param compare SYS_AUTOSTART 31 then sh /etc/init.d/31_io_phantom fi + +# End of autostart +fi From feee1ccc65dff865270d22ff6796b6acedf67ea2 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 30 Jul 2013 22:47:17 -0700 Subject: [PATCH 313/486] Remove some timer reload events; these seem to break PWM output on IO. --- src/drivers/stm32/drv_pwm_servo.c | 6 ------ 1 file changed, 6 deletions(-) diff --git a/src/drivers/stm32/drv_pwm_servo.c b/src/drivers/stm32/drv_pwm_servo.c index c85e83ddca..dbb45a1380 100644 --- a/src/drivers/stm32/drv_pwm_servo.c +++ b/src/drivers/stm32/drv_pwm_servo.c @@ -169,9 +169,6 @@ pwm_channel_init(unsigned channel) rCCER(timer) |= GTIM_CCER_CC4E; break; } - - /* generate an update event; reloads the counter and all registers */ - rEGR(timer) = GTIM_EGR_UG; } int @@ -212,9 +209,6 @@ up_pwm_servo_set(unsigned channel, servo_position_t value) return -1; } - /* generate an update event; reloads the counter and all registers */ - rEGR(timer) = GTIM_EGR_UG; - return 0; } From 02d4480e8ed7c6c6460f95f531aeef2725951663 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 31 Jul 2013 20:58:27 +0400 Subject: [PATCH 314/486] commander rewrite almost completed, WIP --- src/modules/commander/commander.cpp | 873 ++++++++---------- src/modules/commander/commander_helper.cpp | 6 +- .../commander/state_machine_helper.cpp | 70 +- src/modules/commander/state_machine_helper.h | 12 +- 4 files changed, 460 insertions(+), 501 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b2336cbf6f..2012abcc09 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -73,7 +73,6 @@ #include #include #include -#include #include #include @@ -124,6 +123,26 @@ extern struct system_load_s system_load; #define LOCAL_POSITION_TIMEOUT 1000000 /**< consider the local position estimate invalid after 1s */ +#define PRINT_INTERVAL 5000000 +#define PRINT_MODE_REJECT_INTERVAL 2000000 + +// TODO include mavlink headers +/** @brief These flags encode the MAV mode. */ +#ifndef HAVE_ENUM_MAV_MODE_FLAG +#define HAVE_ENUM_MAV_MODE_FLAG +enum MAV_MODE_FLAG { + MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */ + MAV_MODE_FLAG_TEST_ENABLED = 2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */ + MAV_MODE_FLAG_AUTO_ENABLED = 4, /* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */ + MAV_MODE_FLAG_GUIDED_ENABLED = 8, /* 0b00001000 guided mode enabled, system flies MISSIONs / mission items. | */ + MAV_MODE_FLAG_STABILIZE_ENABLED = 16, /* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */ + MAV_MODE_FLAG_HIL_ENABLED = 32, /* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */ + MAV_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, /* 0b01000000 remote control input is enabled. | */ + MAV_MODE_FLAG_SAFETY_ARMED = 128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. | */ + MAV_MODE_FLAG_ENUM_END = 129, /* | */ +}; +#endif + /* Mavlink file descriptors */ static int mavlink_fd; @@ -135,9 +154,13 @@ static int daemon_task; /**< Handle of daemon task / thread */ /* timout until lowlevel failsafe */ static unsigned int failsafe_lowlevel_timeout_ms; +static unsigned int leds_counter; +/* To remember when last notification was sent */ +static uint64_t last_print_mode_reject_time = 0; + /* tasks waiting for low prio thread */ -enum { +typedef enum { LOW_PRIO_TASK_NONE = 0, LOW_PRIO_TASK_PARAM_SAVE, LOW_PRIO_TASK_PARAM_LOAD, @@ -147,8 +170,9 @@ enum { LOW_PRIO_TASK_RC_CALIBRATION, LOW_PRIO_TASK_ACCEL_CALIBRATION, LOW_PRIO_TASK_AIRSPEED_CALIBRATION -} low_prio_task; +} low_prio_task_t; +static low_prio_task_t low_prio_task = LOW_PRIO_TASK_NONE; /** * The daemon app only briefly exists to start @@ -170,17 +194,21 @@ void usage(const char *reason); /** * React to commands that are sent e.g. from the mavlink module. */ -void handle_command(int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed); +void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed); /** * Mainloop of commander. */ int commander_thread_main(int argc, char *argv[]); +void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gps_position_s *gps_position); + void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status); transition_result_t check_main_state_machine(struct vehicle_status_s *current_status); +void print_reject_mode(const char *msg); + transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode); /** @@ -241,15 +269,72 @@ void usage(const char *reason) exit(1); } -void handle_command(int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed) +void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) { /* result of the command */ uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; /* request to set different system mode */ switch (cmd->command) { - case VEHICLE_CMD_DO_SET_MODE: - break; + case VEHICLE_CMD_DO_SET_MODE: { + uint8_t base_mode = (int) cmd->param1; + uint8_t custom_mode = (int) cmd->param2; + // TODO remove debug code + mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_mode); + /* set arming state */ + transition_result_t arming_res = TRANSITION_NOT_CHANGED; + + if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { + arming_res = arming_state_transition(status, ARMING_STATE_ARMED, armed); + if (arming_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] ARMED by command"); + } + + } else { + if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) { + arming_state_t new_arming_state = (status->arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); + arming_res = arming_state_transition(status, new_arming_state, armed); + if (arming_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command"); + } + } else { + arming_res = TRANSITION_NOT_CHANGED; + } + } + + /* set main state */ + transition_result_t main_res = TRANSITION_DENIED; + + if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { + /* AUTO */ + main_res = main_state_transition(status, MAIN_STATE_AUTO); + + } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { + if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { + /* EASY */ + main_res = main_state_transition(status, MAIN_STATE_EASY); + + } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { + if (custom_mode & 1) { // TODO add custom mode flags definition + /* SEATBELT */ + main_res = main_state_transition(status, MAIN_STATE_SEATBELT); + + } else { + /* MANUAL */ + main_res = main_state_transition(status, MAIN_STATE_MANUAL); + } + } + } + + if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + + break; + } case VEHICLE_CMD_COMPONENT_ARM_DISARM: break; @@ -257,20 +342,39 @@ void handle_command(int status_pub, struct vehicle_status_s *current_status, int case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: break; - case VEHICLE_CMD_PREFLIGHT_CALIBRATION: + case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { + low_prio_task_t new_low_prio_task = LOW_PRIO_TASK_NONE; - /* gyro calibration */ - if ((int)(cmd->param1) == 1) { + if ((int)(cmd->param1) == 1) { + /* gyro calibration */ + new_low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION; - /* check if no other task is scheduled */ - if (low_prio_task == LOW_PRIO_TASK_NONE) { + } else if ((int)(cmd->param2) == 1) { + /* magnetometer calibration */ + new_low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION; + } else if ((int)(cmd->param3) == 1) { + /* zero-altitude pressure calibration */ + //new_low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION; + } else if ((int)(cmd->param4) == 1) { + /* RC calibration */ + new_low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION; + + } else if ((int)(cmd->param5) == 1) { + /* accelerometer calibration */ + new_low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION; + + } else if ((int)(cmd->param6) == 1) { + /* airspeed calibration */ + new_low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION; + } + + /* check if we have new task and no other task is scheduled */ + if (low_prio_task == LOW_PRIO_TASK_NONE && new_low_prio_task != LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(current_status, ARMING_STATE_INIT, armed, mavlink_fd)) { - // TODO publish state + if (TRANSITION_DENIED != arming_state_transition(status, ARMING_STATE_INIT, armed)) { result = VEHICLE_CMD_RESULT_ACCEPTED; - /* now set the task for the low prio thread */ - low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION; + low_prio_task = new_low_prio_task; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -279,159 +383,55 @@ void handle_command(int status_pub, struct vehicle_status_s *current_status, int } else { result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } + + break; } - /* magnetometer calibration */ - if ((int)(cmd->param2) == 1) { + case VEHICLE_CMD_PREFLIGHT_STORAGE: { + low_prio_task_t new_low_prio_task = LOW_PRIO_TASK_NONE; - /* check if no other task is scheduled */ - if (low_prio_task == LOW_PRIO_TASK_NONE) { - - /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(current_status, ARMING_STATE_INIT, armed, mavlink_fd)) { - // TODO publish state - result = VEHICLE_CMD_RESULT_ACCEPTED; - /* now set the task for the low prio thread */ - low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION; - - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - } - - // /* zero-altitude pressure calibration */ - // if ((int)(cmd->param3) == 1) { - - // /* check if no other task is scheduled */ - // if(low_prio_task == LOW_PRIO_TASK_NONE) { - - // /* try to go to INIT/PREFLIGHT arming state */ - // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { - // result = VEHICLE_CMD_RESULT_ACCEPTED; - // /* now set the task for the low prio thread */ - // low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION; - // } else { - // result = VEHICLE_CMD_RESULT_DENIED; - // } - // } else { - // result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - // } - // } - - // /* trim calibration */ - // if ((int)(cmd->param4) == 1) { - - // /* check if no other task is scheduled */ - // if(low_prio_task == LOW_PRIO_TASK_NONE) { - - // /* try to go to INIT/PREFLIGHT arming state */ - // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { - // result = VEHICLE_CMD_RESULT_ACCEPTED; - // /* now set the task for the low prio thread */ - // low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION; - // } else { - // result = VEHICLE_CMD_RESULT_DENIED; - // } - // } else { - // result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - // } - // } - - - /* accel calibration */ - if ((int)(cmd->param5) == 1) { - - /* check if no other task is scheduled */ - if (low_prio_task == LOW_PRIO_TASK_NONE) { - - /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(current_status, ARMING_STATE_INIT, armed, mavlink_fd)) { - // TODO publish state - result = VEHICLE_CMD_RESULT_ACCEPTED; - /* now set the task for the low prio thread */ - low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION; - - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - } - - /* airspeed calibration */ - if ((int)(cmd->param6) == 1) { - - /* check if no other task is scheduled */ - if (low_prio_task == LOW_PRIO_TASK_NONE) { - - /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(current_status, ARMING_STATE_INIT, armed, mavlink_fd)) { - // TODO publish state - result = VEHICLE_CMD_RESULT_ACCEPTED; - /* now set the task for the low prio thread */ - low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION; - - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - } - - break; - - case VEHICLE_CMD_PREFLIGHT_STORAGE: - - if (((int)(cmd->param1)) == 0) { - /* check if no other task is scheduled */ - if (low_prio_task == LOW_PRIO_TASK_NONE) { + if (((int)(cmd->param1)) == 0) { low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; - result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - - - } else if (((int)(cmd->param1)) == 1) { - - /* check if no other task is scheduled */ - if (low_prio_task == LOW_PRIO_TASK_NONE) { + } else if (((int)(cmd->param1)) == 1) { low_prio_task = LOW_PRIO_TASK_PARAM_SAVE; + } + + /* check if we have new task and no other task is scheduled */ + if (low_prio_task == LOW_PRIO_TASK_NONE && new_low_prio_task != LOW_PRIO_TASK_NONE) { result = VEHICLE_CMD_RESULT_ACCEPTED; + low_prio_task = new_low_prio_task; } else { result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } - } - break; + break; + } default: - mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command"); - result = VEHICLE_CMD_RESULT_UNSUPPORTED; break; } /* supported command handling stop */ - if (result == VEHICLE_CMD_RESULT_FAILED || - result == VEHICLE_CMD_RESULT_DENIED || - result == VEHICLE_CMD_RESULT_UNSUPPORTED || - result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) { + if (result == VEHICLE_CMD_RESULT_ACCEPTED) { + tune_positive(); + } else { tune_negative(); - } else if (result == VEHICLE_CMD_RESULT_ACCEPTED) { + if (result == VEHICLE_CMD_RESULT_DENIED) { + mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd->command); - tune_positive(); + } else if (result == VEHICLE_CMD_RESULT_FAILED) { + mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd->command); + + } else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) { + mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd->command); + + } else if (result == VEHICLE_CMD_RESULT_UNSUPPORTED) { + mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd->command); + } } /* send any requested ACKs */ @@ -515,16 +515,9 @@ int commander_thread_main(int argc, char *argv[]) /* allow manual override initially */ control_mode.flag_external_manual_override_ok = true; - /* flag position info as bad, do not allow auto mode */ - // current_status.flag_vector_flight_mode_ok = false; - /* set battery warning flag */ status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; - /* set safety device detection flag */ - /* XXX do we need this? */ - //current_status.flag_safety_present = false; - // XXX for now just set sensors as initialized status.condition_system_sensors_initialized = true; @@ -535,12 +528,11 @@ int commander_thread_main(int argc, char *argv[]) status_pub = orb_advertise(ORB_ID(vehicle_status), &status); /* publish current state machine */ - /* publish the new state */ + /* publish initial state */ status.counter++; status.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(vehicle_status), status_pub, &status); - armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); @@ -556,8 +548,7 @@ int commander_thread_main(int argc, char *argv[]) exit(ERROR); } - // XXX needed? - mavlink_log_info(mavlink_fd, "system is running"); + mavlink_log_info(mavlink_fd, "[cmd] started"); pthread_attr_t commander_low_prio_attr; pthread_attr_init(&commander_low_prio_attr); @@ -577,9 +568,10 @@ int commander_thread_main(int argc, char *argv[]) unsigned stick_on_counter = 0; /* To remember when last notification was sent */ - uint64_t last_print_time = 0; + uint64_t last_print_control_time = 0; - float voltage_previous = 0.0f; + enum VEHICLE_BATTERY_WARNING battery_warning_previous = VEHICLE_BATTERY_WARNING_NONE; + bool armed_previous = false; bool low_battery_voltage_actions_done; bool critical_battery_voltage_actions_done; @@ -588,10 +580,10 @@ int commander_thread_main(int argc, char *argv[]) uint64_t start_time = 0; - bool state_changed = true; + bool status_changed = true; bool param_init_forced = true; - bool new_data = false; + bool updated = false; /* Subscribe to safety topic */ int safety_sub = orb_subscribe(ORB_ID(safety)); @@ -612,13 +604,11 @@ int commander_thread_main(int argc, char *argv[]) int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); struct vehicle_global_position_s global_position; memset(&global_position, 0, sizeof(global_position)); - uint64_t last_global_position_time = 0; /* Subscribe to local position data */ int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); struct vehicle_local_position_s local_position; memset(&local_position, 0, sizeof(local_position)); - uint64_t last_local_position_time = 0; /* * The home position is set based on GPS only, to prevent a dependency between @@ -670,11 +660,12 @@ int commander_thread_main(int argc, char *argv[]) start_time = hrt_absolute_time(); while (!thread_should_exit) { + hrt_abstime t = hrt_absolute_time(); /* update parameters */ - orb_check(param_changed_sub, &new_data); + orb_check(param_changed_sub, &updated); - if (new_data || param_init_forced) { + if (updated || param_init_forced) { param_init_forced = false; /* parameters changed */ orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); @@ -703,94 +694,96 @@ int commander_thread_main(int argc, char *argv[]) /* check and update system / component ID */ param_get(_param_system_id, &(status.system_id)); param_get(_param_component_id, &(status.component_id)); - + status_changed = true; } } - orb_check(sp_man_sub, &new_data); + orb_check(sp_man_sub, &updated); - if (new_data) { + if (updated) { orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); } - orb_check(sp_offboard_sub, &new_data); + orb_check(sp_offboard_sub, &updated); - if (new_data) { + if (updated) { orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); } - orb_check(sensor_sub, &new_data); + orb_check(sensor_sub, &updated); - if (new_data) { + if (updated) { orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); } - orb_check(diff_pres_sub, &new_data); + orb_check(diff_pres_sub, &updated); - if (new_data) { + if (updated) { orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); last_diff_pres_time = diff_pres.timestamp; } - orb_check(cmd_sub, &new_data); + orb_check(cmd_sub, &updated); - if (new_data) { + if (updated) { /* got command */ orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - handle_command(status_pub, &status, control_mode_pub, &control_mode, &cmd, armed_pub, &armed); + handle_command(&status, &control_mode, &cmd, &armed); } /* update safety topic */ - orb_check(safety_sub, &new_data); + orb_check(safety_sub, &updated); - if (new_data) { + if (updated) { orb_copy(ORB_ID(safety), safety_sub, &safety); } /* update global position estimate */ - orb_check(global_position_sub, &new_data); + orb_check(global_position_sub, &updated); - if (new_data) { + if (updated) { /* position changed */ orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); - last_global_position_time = global_position.timestamp; } /* update local position estimate */ - orb_check(local_position_sub, &new_data); + orb_check(local_position_sub, &updated); - if (new_data) { + if (updated) { /* position changed */ orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); - last_local_position_time = local_position.timestamp; } /* set the condition to valid if there has recently been a local position estimate */ - if (hrt_absolute_time() - last_local_position_time < LOCAL_POSITION_TIMEOUT) { - status.condition_local_position_valid = true; + if (t - local_position.timestamp < LOCAL_POSITION_TIMEOUT) { + if (!status.condition_local_position_valid) { + status.condition_local_position_valid = true; + status_changed = true; + } } else { - status.condition_local_position_valid = false; + if (status.condition_local_position_valid) { + status.condition_local_position_valid = false; + status_changed = true; + } } /* update battery status */ - orb_check(battery_sub, &new_data); + orb_check(battery_sub, &updated); - if (new_data) { + if (updated) { orb_copy(ORB_ID(battery_status), battery_sub, &battery); status.battery_voltage = battery.voltage_v; status.condition_battery_voltage_valid = true; - - /* - * Only update battery voltage estimate if system has - * been running for two and a half seconds. - */ - } - if (hrt_absolute_time() - start_time > 2500000 && status.condition_battery_voltage_valid) { + /* + * Only update battery voltage estimate if system has + * been running for two and a half seconds. + */ + if (t - start_time > 2500000 && status.condition_battery_voltage_valid) { status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage); } else { @@ -798,12 +791,12 @@ int commander_thread_main(int argc, char *argv[]) } /* update subsystem */ - orb_check(subsys_sub, &new_data); + orb_check(subsys_sub, &updated); - if (new_data) { + if (updated) { orb_copy(ORB_ID(subsystem_info), subsys_sub, &info); - warnx("Subsys changed: %d\n", (int)info.subsystem_type); + warnx("subsystem changed: %d\n", (int)info.subsystem_type); /* mark / unmark as present */ if (info.present) { @@ -828,55 +821,11 @@ int commander_thread_main(int argc, char *argv[]) } else { status.onboard_control_sensors_health &= ~info.subsystem_type; } + + status_changed = true; } - /* Slow but important 8 Hz checks */ - if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) { - - /* XXX if armed */ - if (armed.armed) { - /* armed, solid */ - led_on(LED_AMBER); - - } else if (armed.ready_to_arm && (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0)) { - /* ready to arm */ - led_toggle(LED_AMBER); - - } else if (counter % (100000 / COMMANDER_MONITORING_INTERVAL) == 0) { - /* not ready to arm, something is wrong */ - led_toggle(LED_AMBER); - } - - if (hrt_absolute_time() - gps_position.timestamp_position < 2000000) { - - /* toggle GPS (blue) led at 1 Hz if GPS present but no lock, make is solid once locked */ - if ((hrt_absolute_time() - gps_position.timestamp_position < 2000000) - && (gps_position.fix_type == GPS_FIX_TYPE_3D)) { - /* GPS lock */ - led_on(LED_BLUE); - - } else if ((counter + 4) % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - /* no GPS lock, but GPS module is aquiring lock */ - led_toggle(LED_BLUE); - } - - } else { - /* no GPS info, don't light the blue led */ - led_off(LED_BLUE); - } - - - // /* toggle GPS led at 5 Hz in HIL mode */ - // if (current_status.flag_hil_enabled) { - // /* hil enabled */ - // led_toggle(LED_BLUE); - - // } else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) { - // /* toggle arming (red) at 5 Hz on low battery or error */ - // led_toggle(LED_AMBER); - // } - - } + toggle_status_leds(&status, &armed, &gps_position); if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { /* compute system load */ @@ -888,29 +837,26 @@ int commander_thread_main(int argc, char *argv[]) last_idle_time = system_load.tasks[0].total_runtime; } - - /* if battery voltage is getting lower, warn using buzzer, etc. */ - if (status.condition_battery_voltage_valid && (status.battery_remaining < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS - + if (status.condition_battery_voltage_valid && status.battery_remaining < 0.15f && !low_battery_voltage_actions_done) { + //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { low_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "[cmd] WARNING! LOW BATTERY!"); + mavlink_log_critical(mavlink_fd, "[cmd] WARNING: LOW BATTERY"); status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING; - tune_low_bat(); + status_changed = true; } low_voltage_counter++; - } - /* Critical, this is rather an emergency, change state machine */ - else if (status.condition_battery_voltage_valid && (status.battery_remaining < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) { + } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { + /* critical battery voltage, this is rather an emergency, change state machine */ if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { critical_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!"); + mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY: CRITICAL BATTERY"); status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; - tune_critical_bat(); - // XXX implement state change here + arming_state_transition(&status, ARMING_STATE_ARMED_ERROR, &armed); + status_changed = true; } critical_voltage_counter++; @@ -923,10 +869,9 @@ int commander_thread_main(int argc, char *argv[]) /* End battery voltage check */ /* If in INIT state, try to proceed to STANDBY state */ - if (status.arming_state == ARMING_STATE_INIT) { + if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { // XXX check for sensors - arming_state_transition(&status, ARMING_STATE_STANDBY, &armed, mavlink_fd); - //TODO publish state + arming_state_transition(&status, ARMING_STATE_STANDBY, &armed); } else { // XXX: Add emergency stuff if sensors are lost @@ -950,14 +895,14 @@ int commander_thread_main(int argc, char *argv[]) /* check for global or local position updates, set a timeout of 2s */ - if (hrt_absolute_time() - last_global_position_time < 2000000 && hrt_absolute_time() > 2000000 && global_position.valid) { + if (t - global_position.timestamp < 2000000 && t > 2000000 && global_position.valid) { status.condition_global_position_valid = true; } else { status.condition_global_position_valid = false; } - if (hrt_absolute_time() - last_local_position_time < 2000000 && hrt_absolute_time() > 2000000 && local_position.valid) { + if (t - local_position.timestamp < 2000000 && t > 2000000 && local_position.valid) { status.condition_local_position_valid = true; } else { @@ -965,66 +910,23 @@ int commander_thread_main(int argc, char *argv[]) } /* Check for valid airspeed/differential pressure measurements */ - if (hrt_absolute_time() - last_diff_pres_time < 2000000 && hrt_absolute_time() > 2000000) { + if (t - last_diff_pres_time < 2000000 && t > 2000000) { status.condition_airspeed_valid = true; } else { status.condition_airspeed_valid = false; } - /* - * Consolidate global position and local position valid flags - * for vector flight mode. - */ - // if (current_status.condition_local_position_valid || - // current_status.condition_global_position_valid) { - // current_status.flag_vector_flight_mode_ok = true; - - // } else { - // current_status.flag_vector_flight_mode_ok = false; - // } - - /* consolidate state change, flag as changed if required */ - if (global_pos_valid != status.condition_global_position_valid || - local_pos_valid != status.condition_local_position_valid || - airspeed_valid != status.condition_airspeed_valid) { - state_changed = true; - } - - /* - * Mark the position of the first position lock as return to launch (RTL) - * position. The MAV will return here on command or emergency. - * - * Conditions: - * - * 1) The system aquired position lock just now - * 2) The system has not aquired position lock before - * 3) The system is not armed (on the ground) - */ - // if (!current_status.flag_valid_launch_position && - // !vector_flight_mode_ok && current_status.flag_vector_flight_mode_ok && - // !current_status.flag_system_armed) { - // first time a valid position, store it and emit it - - // // XXX implement storage and publication of RTL position - // current_status.flag_valid_launch_position = true; - // current_status.flag_auto_flight_mode_ok = true; - // state_changed = true; - // } - - orb_check(gps_sub, &new_data); - - if (new_data) { - + orb_check(gps_sub, &updated); + if (updated) { orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); /* check for first, long-term and valid GPS lock -> set home position */ float hdop_m = gps_position.eph_m; float vdop_m = gps_position.epv_m; - /* check if gps fix is ok */ - // XXX magic number + /* check if GPS fix is ok */ float hdop_threshold_m = 4.0f; float vdop_threshold_m = 8.0f; @@ -1038,15 +940,12 @@ int commander_thread_main(int argc, char *argv[]) * position to the current position. */ - if (gps_position.fix_type == GPS_FIX_TYPE_3D - && (hdop_m < hdop_threshold_m) - && (vdop_m < vdop_threshold_m) // XXX note that vdop is 0 for mtk - && !home_position_set - && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) + if (!home_position_set && gps_position.fix_type == GPS_FIX_TYPE_3D && + (hdop_m < hdop_threshold_m) && (vdop_m < vdop_threshold_m) && // XXX note that vdop is 0 for mtk + (t - gps_position.timestamp_position < 2000000) && !armed.armed) { - warnx("setting home position"); - /* copy position data to uORB home message, store it locally as well */ + // TODO use global position estimate home.lat = gps_position.lat; home.lon = gps_position.lon; home.alt = gps_position.alt; @@ -1057,6 +956,11 @@ int commander_thread_main(int argc, char *argv[]) home.s_variance_m_s = gps_position.s_variance_m_s; home.p_variance_m = gps_position.p_variance_m; + double home_lat_d = home.lat * 1e-7; + double home_lon_d = home.lon * 1e-7; + warnx("home: lat = %.7f, lon = %.7f", home_lat_d, home_lon_d); + mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f", home_lat_d, home_lon_d); + /* announce new home position */ if (home_pub > 0) { orb_publish(ORB_ID(home_position), home_pub, &home); @@ -1073,16 +977,18 @@ int commander_thread_main(int argc, char *argv[]) /* ignore RC signals if in offboard control mode */ if (!status.offboard_control_signal_found_once && sp_man.timestamp != 0) { - /* start RC state check */ - if ((hrt_absolute_time() - sp_man.timestamp) < 100000) { + /* start RC input check */ + if ((t - sp_man.timestamp) < 100000) { /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; mavlink_log_critical(mavlink_fd, "[cmd] detected RC signal first time"); + status_changed = true; } else { if (status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); + mavlink_log_critical(mavlink_fd, "[cmd] RC signal regained"); + status_changed = true; } } @@ -1093,8 +999,6 @@ int commander_thread_main(int argc, char *argv[]) transition_result_t res; // store all transitions results here /* arm/disarm by RC */ - bool arming_state_changed; - res = TRANSITION_NOT_CHANGED; /* check if left stick is in lower left position and we are in MANUAL or AUTO mode -> disarm @@ -1106,16 +1010,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); - res = arming_state_transition(&status, new_arming_state, &armed, mavlink_fd); - - if (res == TRANSITION_CHANGED) - stick_off_counter = 0; + res = arming_state_transition(&status, new_arming_state, &armed); + stick_off_counter = 0; } else { stick_off_counter++; - stick_on_counter = 0; } + stick_on_counter = 0; + } else { stick_off_counter = 0; } @@ -1126,16 +1029,15 @@ int commander_thread_main(int argc, char *argv[]) status.main_state == MAIN_STATE_MANUAL) { if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - res = arming_state_transition(&status, ARMING_STATE_ARMED, &armed, mavlink_fd); - - if (res == TRANSITION_CHANGED) - stick_on_counter = 0; + res = arming_state_transition(&status, ARMING_STATE_ARMED, &armed); + stick_on_counter = 0; } else { stick_on_counter++; - stick_off_counter = 0; } + stick_off_counter = 0; + } else { stick_on_counter = 0; } @@ -1148,9 +1050,6 @@ int commander_thread_main(int argc, char *argv[]) } else { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); } - - tune_positive(); - orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); } /* fill current_status according to mode switches */ @@ -1159,83 +1058,54 @@ int commander_thread_main(int argc, char *argv[]) /* evaluate the main state machine */ res = check_main_state_machine(&status); - /* we should not get DENIED here */ - if (res == TRANSITION_DENIED) { + if (res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state); + tune_positive(); + + } else if (res == TRANSITION_DENIED) { + /* DENIED here indicates bug in the commander */ + warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); } - bool main_state_changed = (res == TRANSITION_CHANGED); - - /* evaluate the navigation state machine */ - res = check_navigation_state_machine(&status, &control_mode); - - /* we should not get DENIED here */ - if (res == TRANSITION_DENIED) { - mavlink_log_critical(mavlink_fd, "[cmd] ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state); - } - - bool navigation_state_changed = (res == TRANSITION_CHANGED); - - if (arming_state_changed || main_state_changed || navigation_state_changed) { - /* publish new vehicle status */ - status.counter++; - status.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, &status); - mavlink_log_info(mavlink_fd, "[cmd] state: arm %s, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state); - } - - if (navigation_state_changed) { - /* publish new navigation state */ - control_mode.counter++; - control_mode.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); - } - } else { - /* print error message for first RC glitch and then every 5 s / 5000 ms) */ - if (!status.rc_signal_cutting_off || ((hrt_absolute_time() - last_print_time) > 5000000)) { - /* only complain if the offboard control is NOT active */ - status.rc_signal_cutting_off = true; - mavlink_log_critical(mavlink_fd, "CRITICAL - NO REMOTE SIGNAL!"); - + /* print error message for first RC glitch and then every 5s */ + if (!status.rc_signal_cutting_off || (t - last_print_control_time) > PRINT_INTERVAL) { + // TODO remove debug code if (!status.rc_signal_cutting_off) { - printf("Reason: not rc_signal_cutting_off\n"); + warnx("Reason: not rc_signal_cutting_off\n"); } else { - printf("last print time: %llu\n", last_print_time); + warnx("last print time: %llu\n", last_print_control_time); } - last_print_time = hrt_absolute_time(); + /* only complain if the offboard control is NOT active */ + status.rc_signal_cutting_off = true; + mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: NO RC CONTROL"); + + last_print_control_time = t; } /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp; + status.rc_signal_lost_interval = t - sp_man.timestamp; /* if the RC signal is gone for a full second, consider it lost */ if (status.rc_signal_lost_interval > 1000000) { status.rc_signal_lost = true; status.failsave_lowlevel = true; - state_changed = true; + status_changed = true; } - - // if (hrt_absolute_time() - current_status.failsave_ll_start_time > failsafe_lowlevel_timeout_ms*1000) { - // publish_armed_status(¤t_status); - // } } } - - - - /* End mode switch */ - + /* END mode switch */ /* END RC state check */ - - /* State machine update for offboard control */ + // TODO check this + /* state machine update for offboard control */ if (!status.rc_signal_found_once && sp_offboard.timestamp != 0) { - if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) { + if ((t - sp_offboard.timestamp) < 5000000) { // TODO 5s is too long ? // /* decide about attitude control flag, enable in att/pos/vel */ // bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE || @@ -1282,87 +1152,94 @@ int commander_thread_main(int argc, char *argv[]) // XXX check if this is correct /* arm / disarm on request */ if (sp_offboard.armed && !armed.armed) { - - arming_state_transition(&status, ARMING_STATE_ARMED, &armed, mavlink_fd); - // TODO publish state + arming_state_transition(&status, ARMING_STATE_ARMED, &armed); } else if (!sp_offboard.armed && armed.armed) { - - arming_state_transition(&status, ARMING_STATE_STANDBY, &armed, mavlink_fd); - // TODO publish state + arming_state_transition(&status, ARMING_STATE_STANDBY, &armed); } } else { - /* print error message for first RC glitch and then every 5 s / 5000 ms) */ - if (!status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) { + /* print error message for first offboard signal glitch and then every 5s */ + if (!status.offboard_control_signal_weak || ((t - last_print_control_time) > PRINT_INTERVAL)) { status.offboard_control_signal_weak = true; - mavlink_log_critical(mavlink_fd, "CRIT:NO OFFBOARD CONTROL!"); - last_print_time = hrt_absolute_time(); + mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: NO OFFBOARD CONTROL"); + last_print_control_time = t; } /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp; + status.offboard_control_signal_lost_interval = t - sp_offboard.timestamp; /* if the signal is gone for 0.1 seconds, consider it lost */ if (status.offboard_control_signal_lost_interval > 100000) { status.offboard_control_signal_lost = true; - status.failsave_lowlevel_start_time = hrt_absolute_time(); + status.failsave_lowlevel_start_time = t; tune_positive(); /* kill motors after timeout */ - if (hrt_absolute_time() - status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) { + if (t - status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) { status.failsave_lowlevel = true; - state_changed = true; + status_changed = true; } } } } + /* evaluate the navigation state machine */ + transition_result_t res = check_navigation_state_machine(&status, &control_mode); - - - status.counter++; - status.timestamp = hrt_absolute_time(); - - - // XXX this is missing - /* If full run came back clean, transition to standby */ - // if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT && - // current_status.flag_preflight_gyro_calibration == false && - // current_status.flag_preflight_mag_calibration == false && - // current_status.flag_preflight_accel_calibration == false) { - // /* All ok, no calibration going on, go to standby */ - // do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - // } - - /* publish at least with 1 Hz */ - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) { - - orb_publish(ORB_ID(vehicle_status), status_pub, &status); - state_changed = false; + if (res == TRANSITION_DENIED) { + /* DENIED here indicates bug in the commander */ + warnx("ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state); + mavlink_log_critical(mavlink_fd, "[cmd] ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state); } + /* check which state machines for changes, clear "changed" flag */ + bool arming_state_changed = check_arming_state_changed(); + bool main_state_changed = check_main_state_changed(); + bool navigation_state_changed = check_navigation_state_changed(); + if (arming_state_changed || main_state_changed || navigation_state_changed) { + mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state); + status_changed = true; + } - /* Store old modes to detect and act on state transitions */ - voltage_previous = status.battery_voltage; + /* publish arming state */ + if (arming_state_changed) { + armed.timestamp = t; + orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); + } + /* publish control mode */ + if (navigation_state_changed) { + /* publish new navigation state */ + control_mode.counter++; + control_mode.timestamp = t; + orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); + } - /* play tone according to evaluation result */ - /* check if we recently armed */ - if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available))) { + /* publish vehicle status at least with 1 Hz */ + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) { + status.counter++; + status.timestamp = t; + orb_publish(ORB_ID(vehicle_status), status_pub, &status); + status_changed = false; + } + + /* play arming and battery warning tunes */ + if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || safety.safety_off)) { + /* play tune when armed */ if (tune_arm() == OK) arm_tune_played = true; - /* Trigger audio event for low battery */ - - } else if (status.battery_remaining < 0.1f && status.condition_battery_voltage_valid) { - if (tune_critical_bat() == OK) + } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { + /* play tune on battery warning */ + if (tune_low_bat() == OK) battery_tune_played = true; - } else if (status.battery_remaining < 0.2f && status.condition_battery_voltage_valid) { - if (tune_low_bat() == OK) + } else if (status.battery_remaining == VEHICLE_BATTERY_WARNING_ALERT) { + /* play tune on battery critical */ + if (tune_critical_bat() == OK) battery_tune_played = true; } else if (battery_tune_played) { @@ -1375,8 +1252,10 @@ int commander_thread_main(int argc, char *argv[]) arm_tune_played = false; } + /* store old modes to detect and act on state transitions */ + battery_warning_previous = status.battery_warning; + armed_previous = armed.armed; - /* XXX use this voltage_previous */ fflush(stdout); counter++; usleep(COMMANDER_MONITORING_INTERVAL); @@ -1409,6 +1288,46 @@ int commander_thread_main(int argc, char *argv[]) return 0; } +void +toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gps_position_s *gps_position) +{ + if (leds_counter % 2 == 0) { + /* run at 10Hz, full cycle is 16 ticks = 10/16Hz */ + if (armed->armed) { + /* armed, solid */ + led_on(LED_AMBER); + + } else if (armed->ready_to_arm) { + /* ready to arm, blink at 2.5Hz */ + if (leds_counter % 8 == 0) { + led_toggle(LED_AMBER); + } + + } else { + /* not ready to arm, blink at 10Hz */ + led_toggle(LED_AMBER); + } + + if (status->condition_global_position_valid) { + /* position estimated, solid */ + led_on(LED_BLUE); + + } else if (leds_counter == 0) { + /* waiting for position estimate, short blink at 1.25Hz */ + led_on(LED_BLUE); + + } else { + /* no position estimator available, off */ + led_off(LED_BLUE); + } + } + + leds_counter++; + + if (leds_counter >= 16) + leds_counter = 0; +} + void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status) { @@ -1420,8 +1339,11 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta } else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) { current_status->mode_switch = MODE_SWITCH_AUTO; - } else { + } else if (sp_man->mode_switch < -STICK_ON_OFF_LIMIT) { current_status->mode_switch = MODE_SWITCH_MANUAL; + + } else { + current_status->mode_switch = MODE_SWITCH_ASSISTED; } /* land switch */ @@ -1466,44 +1388,49 @@ check_main_state_machine(struct vehicle_status_s *current_status) switch (current_status->mode_switch) { case MODE_SWITCH_MANUAL: - res = main_state_transition(current_status, MAIN_STATE_MANUAL, mavlink_fd); + res = main_state_transition(current_status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; case MODE_SWITCH_ASSISTED: if (current_status->assisted_switch == ASSISTED_SWITCH_EASY) { - res = main_state_transition(current_status, MAIN_STATE_EASY, mavlink_fd); + res = main_state_transition(current_status, MAIN_STATE_EASY); if (res != TRANSITION_DENIED) break; // changed successfully or already in this state // else fallback to SEATBELT + print_reject_mode("EASY"); } - res = main_state_transition(current_status, MAIN_STATE_SEATBELT, mavlink_fd); + res = main_state_transition(current_status, MAIN_STATE_SEATBELT); if (res != TRANSITION_DENIED) break; // changed successfully or already in this mode + if (current_status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages + print_reject_mode("SEATBELT"); + // else fallback to MANUAL - res = main_state_transition(current_status, MAIN_STATE_MANUAL, mavlink_fd); + res = main_state_transition(current_status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; case MODE_SWITCH_AUTO: - res = main_state_transition(current_status, MAIN_STATE_AUTO, mavlink_fd); + res = main_state_transition(current_status, MAIN_STATE_AUTO); if (res != TRANSITION_DENIED) break; // changed successfully or already in this state // else fallback to SEATBELT (EASY likely will not work too) - res = main_state_transition(current_status, MAIN_STATE_SEATBELT, mavlink_fd); + print_reject_mode("AUTO"); + res = main_state_transition(current_status, MAIN_STATE_SEATBELT); if (res != TRANSITION_DENIED) break; // changed successfully or already in this state // else fallback to MANUAL - res = main_state_transition(current_status, MAIN_STATE_MANUAL, mavlink_fd); + res = main_state_transition(current_status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; @@ -1514,6 +1441,20 @@ check_main_state_machine(struct vehicle_status_s *current_status) return res; } +void +print_reject_mode(const char *msg) +{ + hrt_abstime t = hrt_absolute_time(); + + if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { + last_print_mode_reject_time = t; + char s[80]; + sprintf(s, "[cmd] WARNING: reject %s", msg); + mavlink_log_critical(mavlink_fd, s); + tune_negative(); + } +} + transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode) { @@ -1523,15 +1464,15 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v /* ARMED */ switch (current_status->main_state) { case MAIN_STATE_MANUAL: - res = navigation_state_transition(current_status, current_status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode, mavlink_fd); + res = navigation_state_transition(current_status, current_status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode); break; case MAIN_STATE_SEATBELT: - res = navigation_state_transition(current_status, NAVIGATION_STATE_ALTHOLD, control_mode, mavlink_fd); + res = navigation_state_transition(current_status, NAVIGATION_STATE_ALTHOLD, control_mode); break; case MAIN_STATE_EASY: - res = navigation_state_transition(current_status, NAVIGATION_STATE_VECTOR, control_mode, mavlink_fd); + res = navigation_state_transition(current_status, NAVIGATION_STATE_VECTOR, control_mode); break; case MAIN_STATE_AUTO: @@ -1539,7 +1480,7 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v /* don't act while taking off */ if (current_status->condition_landed) { /* if landed: transitions only to AUTO_READY are allowed */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode, mavlink_fd); + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); // TRANSITION_DENIED is not possible here break; @@ -1547,16 +1488,16 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v /* if not landed: act depending on switches */ if (current_status->return_switch == RETURN_SWITCH_RETURN) { /* RTL */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode, mavlink_fd); + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); } else { if (current_status->mission_switch == MISSION_SWITCH_MISSION) { /* MISSION */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode, mavlink_fd); + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); } else { /* LOITER */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode, mavlink_fd); + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); } } } @@ -1570,7 +1511,7 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v } else { /* DISARMED */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_STANDBY, control_mode, mavlink_fd); + res = navigation_state_transition(current_status, NAVIGATION_STATE_STANDBY, control_mode); } return res; @@ -1579,75 +1520,61 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v void *commander_low_prio_loop(void *arg) { /* Set thread name */ - prctl(PR_SET_NAME, "commander low prio", getpid()); + prctl(PR_SET_NAME, "commander_low_prio", getpid()); while (!thread_should_exit) { switch (low_prio_task) { - case LOW_PRIO_TASK_PARAM_LOAD: if (0 == param_load_default()) { - mavlink_log_info(mavlink_fd, "Param load success"); + mavlink_log_info(mavlink_fd, "[cmd] parameters loaded"); } else { - mavlink_log_critical(mavlink_fd, "Param load ERROR"); + mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); tune_error(); } - - low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_PARAM_SAVE: if (0 == param_save_default()) { - mavlink_log_info(mavlink_fd, "Param save success"); + mavlink_log_info(mavlink_fd, "[cmd] parameters saved"); } else { - mavlink_log_critical(mavlink_fd, "Param save ERROR"); + mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); tune_error(); } - - low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_GYRO_CALIBRATION: do_gyro_calibration(mavlink_fd); - - low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_MAG_CALIBRATION: do_mag_calibration(mavlink_fd); - - low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_ALTITUDE_CALIBRATION: // do_baro_calibration(mavlink_fd); + break; case LOW_PRIO_TASK_RC_CALIBRATION: // do_rc_calibration(mavlink_fd); - - low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_ACCEL_CALIBRATION: do_accel_calibration(mavlink_fd); - - low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_AIRSPEED_CALIBRATION: do_airspeed_calibration(mavlink_fd); - - low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_NONE: @@ -1657,6 +1584,8 @@ void *commander_low_prio_loop(void *arg) break; } + low_prio_task = LOW_PRIO_TASK_NONE; + } return 0; diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 9427bd8925..681a11568f 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -213,7 +213,7 @@ float battery_remaining_estimate_voltage(float voltage) ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty)); /* limit to sane values */ - ret = (ret < 0) ? 0 : ret; - ret = (ret > 1) ? 1 : ret; + ret = (ret < 0.0f) ? 0.0f : ret; + ret = (ret > 1.0f) ? 1.0f : ret; return ret; -} \ No newline at end of file +} diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 043ccfd0c9..06cd060c5d 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -62,8 +62,12 @@ #endif static const int ERROR = -1; +static bool arming_state_changed = true; +static bool main_state_changed = true; +static bool navigation_state_changed = true; + transition_result_t -arming_state_transition(struct vehicle_status_s *status, arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd) +arming_state_transition(struct vehicle_status_s *status, arming_state_t new_arming_state, struct actuator_armed_s *armed) { transition_result_t ret = TRANSITION_DENIED; @@ -96,9 +100,6 @@ arming_state_transition(struct vehicle_status_s *status, arming_state_t new_armi ret = TRANSITION_CHANGED; armed->armed = false; armed->ready_to_arm = true; - - } else { - mavlink_log_critical(mavlink_fd, "rej. STANDBY state, sensors not initialized"); } } @@ -163,18 +164,28 @@ arming_state_transition(struct vehicle_status_s *status, arming_state_t new_armi } if (ret == TRANSITION_CHANGED) { - hrt_abstime t = hrt_absolute_time(); status->arming_state = new_arming_state; - armed->timestamp = t; + arming_state_changed = true; } } return ret; } +bool +check_arming_state_changed() +{ + if (arming_state_changed) { + arming_state_changed = false; + return true; + + } else { + return false; + } +} transition_result_t -main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state, const int mavlink_fd) +main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state) { transition_result_t ret = TRANSITION_DENIED; @@ -193,11 +204,7 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m /* need position estimate */ // TODO only need altitude estimate really - if (!current_state->condition_local_position_valid) { - mavlink_log_critical(mavlink_fd, "rej. SEATBELT: no position estimate"); - tune_negative(); - - } else { + if (current_state->condition_local_position_valid) { ret = TRANSITION_CHANGED; } @@ -206,11 +213,7 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m case MAIN_STATE_EASY: /* need position estimate */ - if (!current_state->condition_local_position_valid) { - mavlink_log_critical(mavlink_fd, "rej. EASY: no position estimate"); - tune_negative(); - - } else { + if (current_state->condition_local_position_valid) { ret = TRANSITION_CHANGED; } @@ -219,11 +222,7 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m case MAIN_STATE_AUTO: /* need position estimate */ - if (!current_state->condition_local_position_valid) { - mavlink_log_critical(mavlink_fd, "rej. AUTO: no position estimate"); - tune_negative(); - - } else { + if (current_state->condition_local_position_valid) { ret = TRANSITION_CHANGED; } @@ -232,14 +231,27 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m if (ret == TRANSITION_CHANGED) { current_state->main_state = new_main_state; + main_state_changed = true; } } return ret; } +bool +check_main_state_changed() +{ + if (main_state_changed) { + main_state_changed = false; + return true; + + } else { + return false; + } +} + transition_result_t -navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode, const int mavlink_fd) +navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode) { transition_result_t ret = TRANSITION_DENIED; @@ -395,12 +407,24 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ if (ret == TRANSITION_CHANGED) { current_status->navigation_state = new_navigation_state; + navigation_state_changed = true; } } return ret; } +bool +check_navigation_state_changed() +{ + if (navigation_state_changed) { + navigation_state_changed = false; + return true; + + } else { + return false; + } +} /** * Transition from one hil state to another diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index b59b66c8ba..c8c77e5d8a 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -56,11 +56,17 @@ typedef enum { } transition_result_t; -transition_result_t arming_state_transition(struct vehicle_status_s *current_state, arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd); +transition_result_t arming_state_transition(struct vehicle_status_s *current_state, arming_state_t new_arming_state, struct actuator_armed_s *armed); -transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state, const int mavlink_fd); +bool check_arming_state_changed(); -transition_result_t navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode, const int mavlink_fd); +transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state); + +bool check_main_state_changed(); + +transition_result_t navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode); + +bool check_navigation_state_changed(); int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd); From f3764d0b5a219aea24d05274311d91ad08eb182d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 28 Jul 2013 17:20:12 +1000 Subject: [PATCH 315/486] px4fmu: expanded "fmu test" this now does testing in a similar manner as "px4io test", except that it tests both ioctl and write based setting of servos --- src/drivers/px4fmu/fmu.cpp | 80 ++++++++++++++++++++++++++++++++------ 1 file changed, 68 insertions(+), 12 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 70147d56af..b73f715724 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1104,28 +1104,84 @@ void test(void) { int fd; - - fd = open(PWM_OUTPUT_DEVICE_PATH, 0); + unsigned servo_count = 0; + unsigned pwm_value = 1000; + int direction = 1; + + fd = open(PX4FMU_DEVICE_PATH, O_RDWR); if (fd < 0) errx(1, "open fail"); if (ioctl(fd, PWM_SERVO_ARM, 0) < 0) err(1, "servo arm failed"); - if (ioctl(fd, PWM_SERVO_SET(0), 1000) < 0) err(1, "servo 1 set failed"); + if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count) != 0) { + err(1, "Unable to get servo count\n"); + } - if (ioctl(fd, PWM_SERVO_SET(1), 1200) < 0) err(1, "servo 2 set failed"); + warnx("Testing %u servos", (unsigned)servo_count); - if (ioctl(fd, PWM_SERVO_SET(2), 1400) < 0) err(1, "servo 3 set failed"); + int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY); + if (!console) + err(1, "failed opening console"); - if (ioctl(fd, PWM_SERVO_SET(3), 1600) < 0) err(1, "servo 4 set failed"); + warnx("Press CTRL-C or 'c' to abort."); -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) - if (ioctl(fd, PWM_SERVO_SET(4), 1800) < 0) err(1, "servo 5 set failed"); + for (;;) { + /* sweep all servos between 1000..2000 */ + servo_position_t servos[servo_count]; + for (unsigned i = 0; i < servo_count; i++) + servos[i] = pwm_value; - if (ioctl(fd, PWM_SERVO_SET(5), 2000) < 0) err(1, "servo 6 set failed"); -#endif + if (direction == 1) { + // use ioctl interface for one direction + for (unsigned i=0; i < servo_count; i++) { + if (ioctl(fd, PWM_SERVO_SET(i), servos[i]) < 0) { + err(1, "servo %u set failed", i); + } + } + } else { + // and use write interface for the other direction + int ret = write(fd, servos, sizeof(servos)); + if (ret != (int)sizeof(servos)) + err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret); + } + if (direction > 0) { + if (pwm_value < 2000) { + pwm_value++; + } else { + direction = -1; + } + } else { + if (pwm_value > 1000) { + pwm_value--; + } else { + direction = 1; + } + } + + /* readback servo values */ + for (unsigned i = 0; i < servo_count; i++) { + servo_position_t value; + + if (ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value)) + err(1, "error reading PWM servo %d", i); + if (value != servos[i]) + errx(1, "servo %d readback error, got %u expected %u", i, value, servos[i]); + } + + /* Check if user wants to quit */ + char c; + if (read(console, &c, 1) == 1) { + if (c == 0x03 || c == 0x63) { + warnx("User abort\n"); + break; + } + } + } + + close(console); close(fd); exit(0); @@ -1222,9 +1278,9 @@ fmu_main(int argc, char *argv[]) fprintf(stderr, "FMU: unrecognised command, try:\n"); #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) - fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n"); + fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test\n"); #elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) - fprintf(stderr, " mode_gpio, mode_pwm\n"); + fprintf(stderr, " mode_gpio, mode_pwm, test\n"); #endif exit(1); } From c7d8535151c336dfbc0bdf522efb358d0c250bd5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 31 Jul 2013 17:53:11 +1000 Subject: [PATCH 316/486] px4io: don't try the px4io serial interface on FMUv1 this caused px4io start to fail on FMUv1 --- src/drivers/px4io/px4io.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index dd876f9037..281debe5c0 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1659,11 +1659,13 @@ get_interface() { device::Device *interface = nullptr; +#ifndef CONFIG_ARCH_BOARD_PX4FMU_V1 /* try for a serial interface */ if (PX4IO_serial_interface != nullptr) interface = PX4IO_serial_interface(); if (interface != nullptr) goto got; +#endif /* try for an I2C interface if we haven't got a serial one */ if (PX4IO_i2c_interface != nullptr) From bee8e27f0479621f5b1ca5e43b4faf1a8f27bfcd Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 1 Aug 2013 22:15:16 +1000 Subject: [PATCH 317/486] adc: allow "adc test" to read 10 values --- src/drivers/stm32/adc/adc.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp index 48c95b3dd8..00e46d6b82 100644 --- a/src/drivers/stm32/adc/adc.cpp +++ b/src/drivers/stm32/adc/adc.cpp @@ -341,7 +341,7 @@ test(void) err(1, "can't open ADC device"); for (unsigned i = 0; i < 50; i++) { - adc_msg_s data[8]; + adc_msg_s data[10]; ssize_t count = read(fd, data, sizeof(data)); if (count < 0) From b9446af3f9758604211aefe64f91e27a7e71c631 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 2 Aug 2013 16:29:43 +0200 Subject: [PATCH 318/486] Show correct battery voltage for v2 boards --- src/modules/sensors/sensor_params.c | 6 +++++- src/modules/sensors/sensors.cpp | 31 +++++++++++++++++++++++++++-- 2 files changed, 34 insertions(+), 3 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 133cda8d65..ba4d2186c3 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -68,7 +68,7 @@ PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f); -PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 1667); +PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0); PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f); PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f); @@ -157,8 +157,12 @@ PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f); PARAM_DEFINE_INT32(RC_TYPE, 1); /** 1 = FUTABA, 2 = Spektrum, 3 = Graupner HoTT, 4 = Turnigy 9x */ PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */ +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); +#else /* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f)); +#endif PARAM_DEFINE_INT32(RC_MAP_ROLL, 1); PARAM_DEFINE_INT32(RC_MAP_PITCH, 2); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 7ff9e19b4c..d9185891b7 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -92,8 +92,35 @@ #define BARO_HEALTH_COUNTER_LIMIT_OK 5 #define ADC_HEALTH_COUNTER_LIMIT_OK 5 -#define ADC_BATTERY_VOLTAGE_CHANNEL 10 -#define ADC_AIRSPEED_VOLTAGE_CHANNEL 11 +/** + * Analog layout: + * FMU: + * IN2 - battery voltage + * IN3 - battery current + * IN4 - 5V sense + * IN10 - spare (we could actually trim these from the set) + * IN11 - spare (we could actually trim these from the set) + * IN12 - spare (we could actually trim these from the set) + * IN13 - aux1 + * IN14 - aux2 + * IN15 - pressure sensor + * + * IO: + * IN4 - servo supply rail + * IN5 - analog RSSI + */ + +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + #define ADC_BATTERY_VOLTAGE_CHANNEL 10 + #define ADC_AIRSPEED_VOLTAGE_CHANNEL 11 +#endif + +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + #define ADC_BATTERY_VOLTAGE_CHANNEL 2 + #define ADC_BATTERY_CURRENT_CHANNEL 3 + #define ADC_5V_RAIL_SENSE 4 + #define ADC_AIRSPEED_VOLTAGE_CHANNEL 15 +#endif #define BAT_VOL_INITIAL 0.f #define BAT_VOL_LOWPASS_1 0.99f From a9c1882ea01aa0cf00448bc874c97087853bb13c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 2 Aug 2013 23:09:40 +1000 Subject: [PATCH 319/486] l3gd20: fixed bit definitions for filter rates and allow requests for the rates in table 21 of the l3gd20H datasheet --- src/drivers/l3gd20/l3gd20.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 423adc76db..61a38b125c 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -86,8 +86,8 @@ static const int ERROR = -1; /* keep lowpass low to avoid noise issues */ #define RATE_95HZ_LP_25HZ ((0<<7) | (0<<6) | (0<<5) | (1<<4)) #define RATE_190HZ_LP_25HZ ((0<<7) | (1<<6) | (1<<5) | (1<<4)) -#define RATE_380HZ_LP_30HZ ((1<<7) | (0<<6) | (1<<5) | (1<<4)) -#define RATE_760HZ_LP_30HZ ((1<<7) | (1<<6) | (1<<5) | (1<<4)) +#define RATE_380HZ_LP_20HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4)) +#define RATE_760HZ_LP_30HZ ((1<<7) | (1<<6) | (0<<5) | (0<<4)) #define ADDR_CTRL_REG2 0x21 #define ADDR_CTRL_REG3 0x22 @@ -598,19 +598,20 @@ L3GD20::set_samplerate(unsigned frequency) if (frequency == 0) frequency = 760; - if (frequency <= 95) { + // use limits good for H or non-H models + if (frequency <= 100) { _current_rate = 95; bits |= RATE_95HZ_LP_25HZ; - } else if (frequency <= 190) { + } else if (frequency <= 200) { _current_rate = 190; bits |= RATE_190HZ_LP_25HZ; - } else if (frequency <= 380) { + } else if (frequency <= 400) { _current_rate = 380; - bits |= RATE_380HZ_LP_30HZ; + bits |= RATE_380HZ_LP_20HZ; - } else if (frequency <= 760) { + } else if (frequency <= 800) { _current_rate = 760; bits |= RATE_760HZ_LP_30HZ; From 9d6ec6b3655fcd902be7a7fe4f2a24033c714afb Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 2 Aug 2013 22:34:55 -0700 Subject: [PATCH 320/486] Restructure things so that the PX4 configs move out of the NuttX tree, and most of the PX4-specific board configuration data moves out of the config and into the board driver. Rename some directories that got left behind in the great board renaming. --- Makefile | 2 +- makefiles/config_px4fmu-v1_default.mk | 2 +- makefiles/config_px4fmu-v2_default.mk | 7 +- makefiles/config_px4fmu-v2_test.mk | 2 +- makefiles/config_px4io-v1_default.mk | 2 +- makefiles/config_px4io-v2_default.mk | 2 +- makefiles/firmware.mk | 5 + makefiles/setup.mk | 1 + nuttx-configs/px4fmu-v1/Kconfig | 21 + nuttx-configs/px4fmu-v1/common/Make.defs | 184 ++++ nuttx-configs/px4fmu-v1/common/ld.script | 149 +++ nuttx-configs/px4fmu-v1/include/board.h | 319 ++++++ .../px4fmu-v1/include/nsh_romfsimg.h | 42 + nuttx-configs/px4fmu-v1/nsh/Make.defs | 3 + nuttx-configs/px4fmu-v1/nsh/defconfig | 957 ++++++++++++++++++ nuttx-configs/px4fmu-v1/nsh/setenv.sh | 75 ++ nuttx-configs/px4fmu-v1/ostest/Make.defs | 122 +++ nuttx-configs/px4fmu-v1/ostest/defconfig | 583 +++++++++++ nuttx-configs/px4fmu-v1/ostest/setenv.sh | 75 ++ nuttx-configs/px4fmu-v1/scripts/gnu-elf.ld | 129 +++ nuttx-configs/px4fmu-v1/scripts/ld.script | 123 +++ nuttx-configs/px4fmu-v1/src/Makefile | 84 ++ nuttx-configs/px4fmu-v1/src/empty.c | 4 + nuttx-configs/px4fmu-v1/tools/px_mkfw.py | 110 ++ nuttx-configs/px4fmu-v1/tools/px_uploader.py | 416 ++++++++ nuttx-configs/px4fmu-v1/tools/upload.sh | 27 + nuttx-configs/px4fmu-v2/include/board.h | 58 -- nuttx-configs/px4io-v1/Kconfig | 21 + nuttx-configs/px4io-v1/README.txt | 806 +++++++++++++++ nuttx-configs/px4io-v1/common/Make.defs | 171 ++++ nuttx-configs/px4io-v1/common/ld.script | 129 +++ nuttx-configs/px4io-v1/common/setenv.sh | 47 + nuttx-configs/px4io-v1/include/README.txt | 1 + nuttx-configs/px4io-v1/include/board.h | 152 +++ .../px4io-v1/include/drv_i2c_device.h | 42 + nuttx-configs/px4io-v1/nsh/Make.defs | 3 + nuttx-configs/px4io-v1/nsh/appconfig | 32 + nuttx-configs/px4io-v1/nsh/defconfig | 559 ++++++++++ nuttx-configs/px4io-v1/nsh/setenv.sh | 47 + nuttx-configs/px4io-v1/src/Makefile | 84 ++ nuttx-configs/px4io-v1/src/README.txt | 1 + nuttx-configs/px4io-v1/src/drv_i2c_device.c | 618 +++++++++++ nuttx-configs/px4io-v1/src/empty.c | 4 + nuttx-configs/px4io-v2/include/board.h | 28 - src/drivers/blinkm/blinkm.cpp | 16 +- src/drivers/bma180/bma180.cpp | 2 +- .../board_config.h} | 88 +- .../boards/{px4fmu => px4fmu-v1}/module.mk | 0 .../boards/{px4fmu => px4fmu-v1}/px4fmu_can.c | 2 +- .../{px4fmu => px4fmu-v1}/px4fmu_init.c | 2 +- .../boards/{px4fmu => px4fmu-v1}/px4fmu_led.c | 2 +- .../{px4fmu => px4fmu-v1}/px4fmu_pwm_servo.c | 10 +- .../boards/{px4fmu => px4fmu-v1}/px4fmu_spi.c | 2 +- .../boards/{px4fmu => px4fmu-v1}/px4fmu_usb.c | 2 +- .../board_config.h} | 53 +- .../boards/{px4fmuv2 => px4fmu-v2}/module.mk | 0 .../{px4fmuv2 => px4fmu-v2}/px4fmu2_init.c | 2 +- .../{px4fmuv2 => px4fmu-v2}/px4fmu2_led.c | 2 +- .../{px4fmuv2 => px4fmu-v2}/px4fmu_can.c | 2 +- .../px4fmu_pwm_servo.c | 10 +- .../{px4fmuv2 => px4fmu-v2}/px4fmu_spi.c | 2 +- .../{px4fmuv2 => px4fmu-v2}/px4fmu_usb.c | 2 +- .../board_config.h} | 12 +- .../boards/{px4io => px4io-v1}/module.mk | 0 .../boards/{px4io => px4io-v1}/px4io_init.c | 2 +- .../{px4io => px4io-v1}/px4io_pwm_servo.c | 0 .../board_config.h} | 22 +- .../boards/{px4iov2 => px4io-v2}/module.mk | 0 .../{px4iov2 => px4io-v2}/px4iov2_init.c | 2 +- .../{px4iov2 => px4io-v2}/px4iov2_pwm_servo.c | 0 src/drivers/drv_gpio.h | 8 +- src/drivers/ets_airspeed/ets_airspeed.cpp | 2 +- src/drivers/hmc5883/hmc5883.cpp | 2 +- src/drivers/l3gd20/l3gd20.cpp | 3 +- src/drivers/lsm303d/lsm303d.cpp | 4 +- src/drivers/mb12xx/mb12xx.cpp | 4 +- src/drivers/meas_airspeed/meas_airspeed.cpp | 2 +- src/drivers/mkblctrl/mkblctrl.cpp | 3 +- src/drivers/mpu6000/mpu6000.cpp | 2 +- src/drivers/px4fmu/fmu.cpp | 16 +- src/drivers/px4io/px4io_serial.cpp | 2 +- src/drivers/px4io/px4io_uploader.cpp | 7 +- src/drivers/rgbled/rgbled.cpp | 3 +- src/drivers/stm32/drv_hrt.c | 7 +- src/drivers/stm32/tone_alarm/tone_alarm.cpp | 2 +- src/modules/px4iofirmware/px4io.h | 7 +- src/modules/systemlib/systemlib.c | 4 +- src/systemcmds/eeprom/eeprom.c | 2 +- src/systemcmds/i2c/i2c.c | 2 +- 89 files changed, 6364 insertions(+), 204 deletions(-) create mode 100644 nuttx-configs/px4fmu-v1/Kconfig create mode 100644 nuttx-configs/px4fmu-v1/common/Make.defs create mode 100644 nuttx-configs/px4fmu-v1/common/ld.script create mode 100644 nuttx-configs/px4fmu-v1/include/board.h create mode 100644 nuttx-configs/px4fmu-v1/include/nsh_romfsimg.h create mode 100644 nuttx-configs/px4fmu-v1/nsh/Make.defs create mode 100644 nuttx-configs/px4fmu-v1/nsh/defconfig create mode 100755 nuttx-configs/px4fmu-v1/nsh/setenv.sh create mode 100644 nuttx-configs/px4fmu-v1/ostest/Make.defs create mode 100644 nuttx-configs/px4fmu-v1/ostest/defconfig create mode 100755 nuttx-configs/px4fmu-v1/ostest/setenv.sh create mode 100644 nuttx-configs/px4fmu-v1/scripts/gnu-elf.ld create mode 100644 nuttx-configs/px4fmu-v1/scripts/ld.script create mode 100644 nuttx-configs/px4fmu-v1/src/Makefile create mode 100644 nuttx-configs/px4fmu-v1/src/empty.c create mode 100755 nuttx-configs/px4fmu-v1/tools/px_mkfw.py create mode 100755 nuttx-configs/px4fmu-v1/tools/px_uploader.py create mode 100755 nuttx-configs/px4fmu-v1/tools/upload.sh create mode 100644 nuttx-configs/px4io-v1/Kconfig create mode 100755 nuttx-configs/px4io-v1/README.txt create mode 100644 nuttx-configs/px4io-v1/common/Make.defs create mode 100755 nuttx-configs/px4io-v1/common/ld.script create mode 100755 nuttx-configs/px4io-v1/common/setenv.sh create mode 100755 nuttx-configs/px4io-v1/include/README.txt create mode 100755 nuttx-configs/px4io-v1/include/board.h create mode 100644 nuttx-configs/px4io-v1/include/drv_i2c_device.h create mode 100644 nuttx-configs/px4io-v1/nsh/Make.defs create mode 100644 nuttx-configs/px4io-v1/nsh/appconfig create mode 100755 nuttx-configs/px4io-v1/nsh/defconfig create mode 100755 nuttx-configs/px4io-v1/nsh/setenv.sh create mode 100644 nuttx-configs/px4io-v1/src/Makefile create mode 100644 nuttx-configs/px4io-v1/src/README.txt create mode 100644 nuttx-configs/px4io-v1/src/drv_i2c_device.c create mode 100644 nuttx-configs/px4io-v1/src/empty.c rename src/drivers/boards/{px4fmu/px4fmu_internal.h => px4fmu-v1/board_config.h} (79%) rename src/drivers/boards/{px4fmu => px4fmu-v1}/module.mk (100%) rename src/drivers/boards/{px4fmu => px4fmu-v1}/px4fmu_can.c (99%) rename src/drivers/boards/{px4fmu => px4fmu-v1}/px4fmu_init.c (99%) rename src/drivers/boards/{px4fmu => px4fmu-v1}/px4fmu_led.c (98%) rename src/drivers/boards/{px4fmu => px4fmu-v1}/px4fmu_pwm_servo.c (98%) rename src/drivers/boards/{px4fmu => px4fmu-v1}/px4fmu_spi.c (99%) rename src/drivers/boards/{px4fmu => px4fmu-v1}/px4fmu_usb.c (99%) rename src/drivers/boards/{px4fmuv2/px4fmu_internal.h => px4fmu-v2/board_config.h} (80%) rename src/drivers/boards/{px4fmuv2 => px4fmu-v2}/module.mk (100%) rename src/drivers/boards/{px4fmuv2 => px4fmu-v2}/px4fmu2_init.c (99%) rename src/drivers/boards/{px4fmuv2 => px4fmu-v2}/px4fmu2_led.c (98%) rename src/drivers/boards/{px4fmuv2 => px4fmu-v2}/px4fmu_can.c (99%) rename src/drivers/boards/{px4fmuv2 => px4fmu-v2}/px4fmu_pwm_servo.c (99%) rename src/drivers/boards/{px4fmuv2 => px4fmu-v2}/px4fmu_spi.c (99%) rename src/drivers/boards/{px4fmuv2 => px4fmu-v2}/px4fmu_usb.c (99%) rename src/drivers/boards/{px4io/px4io_internal.h => px4io-v1/board_config.h} (92%) rename src/drivers/boards/{px4io => px4io-v1}/module.mk (100%) rename src/drivers/boards/{px4io => px4io-v1}/px4io_init.c (99%) rename src/drivers/boards/{px4io => px4io-v1}/px4io_pwm_servo.c (100%) rename src/drivers/boards/{px4iov2/px4iov2_internal.h => px4io-v2/board_config.h} (88%) mode change 100755 => 100644 rename src/drivers/boards/{px4iov2 => px4io-v2}/module.mk (100%) rename src/drivers/boards/{px4iov2 => px4io-v2}/px4iov2_init.c (99%) rename src/drivers/boards/{px4iov2 => px4io-v2}/px4iov2_pwm_servo.c (100%) diff --git a/Makefile b/Makefile index a703bef4cf..6f58858f06 100644 --- a/Makefile +++ b/Makefile @@ -148,7 +148,7 @@ $(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) @$(ECHO) %% Configuring NuttX for $(board) $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) $(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean - $(Q) (cd $(NUTTX_SRC)/configs && ln -sf $(PX4_BASE)/nuttx-configs/* .) + $(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)/nuttx-configs/* .) $(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(board)/$(configuration)) @$(ECHO) %% Exporting NuttX for $(board) $(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) export diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index e6ec840f9a..255093e672 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -17,7 +17,7 @@ MODULES += drivers/stm32/tone_alarm MODULES += drivers/led MODULES += drivers/px4io MODULES += drivers/px4fmu -MODULES += drivers/boards/px4fmu +MODULES += drivers/boards/px4fmu-v1 MODULES += drivers/ardrone_interface MODULES += drivers/l3gd20 MODULES += drivers/bma180 diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 4add744d0e..75573e2c27 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -17,7 +17,7 @@ MODULES += drivers/stm32/tone_alarm MODULES += drivers/led MODULES += drivers/px4fmu MODULES += drivers/px4io -MODULES += drivers/boards/px4fmuv2 +MODULES += drivers/boards/px4fmu-v2 MODULES += drivers/rgbled MODULES += drivers/lsm303d MODULES += drivers/l3gd20 @@ -29,12 +29,15 @@ MODULES += drivers/hil MODULES += drivers/hott/hott_telemetry MODULES += drivers/hott/hott_sensors MODULES += drivers/blinkm -MODULES += drivers/mkblctrl MODULES += drivers/airspeed MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed MODULES += modules/sensors +# Needs to be burned to the ground and re-written; for now, +# just don't build it. +#MODULES += drivers/mkblctrl + # # System commands # diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 98fd6feda4..0f60e88b5e 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -13,7 +13,7 @@ ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_test MODULES += drivers/device MODULES += drivers/stm32 MODULES += drivers/led -MODULES += drivers/boards/px4fmuv2 +MODULES += drivers/boards/px4fmu-v2 MODULES += systemcmds/perf MODULES += systemcmds/reboot diff --git a/makefiles/config_px4io-v1_default.mk b/makefiles/config_px4io-v1_default.mk index cf70391bcf..73f8adf202 100644 --- a/makefiles/config_px4io-v1_default.mk +++ b/makefiles/config_px4io-v1_default.mk @@ -6,5 +6,5 @@ # Board support modules # MODULES += drivers/stm32 -MODULES += drivers/boards/px4io +MODULES += drivers/boards/px4io-v1 MODULES += modules/px4iofirmware \ No newline at end of file diff --git a/makefiles/config_px4io-v2_default.mk b/makefiles/config_px4io-v2_default.mk index f9f35d629d..dbeaba3d37 100644 --- a/makefiles/config_px4io-v2_default.mk +++ b/makefiles/config_px4io-v2_default.mk @@ -6,5 +6,5 @@ # Board support modules # MODULES += drivers/stm32 -MODULES += drivers/boards/px4iov2 +MODULES += drivers/boards/px4io-v2 MODULES += modules/px4iofirmware \ No newline at end of file diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index 3ad13088b0..2085d45dd2 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -176,6 +176,11 @@ GLOBAL_DEPS += $(MAKEFILE_LIST) # EXTRA_CLEANS = +# +# Append the per-board driver directory to the header search path. +# +INCLUDE_DIRS += $(PX4_MODULE_SRC)drivers/boards/$(BOARD) + ################################################################################ # NuttX libraries and paths ################################################################################ diff --git a/makefiles/setup.mk b/makefiles/setup.mk index a0e880a0d3..fdb1612014 100644 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -64,6 +64,7 @@ export INCLUDE_DIRS := $(PX4_MODULE_SRC) \ export MKFW = $(PX4_BASE)/Tools/px_mkfw.py export UPLOADER = $(PX4_BASE)/Tools/px_uploader.py export COPY = cp +export COPYDIR = cp -Rf export REMOVE = rm -f export RMDIR = rm -rf export GENROMFS = genromfs diff --git a/nuttx-configs/px4fmu-v1/Kconfig b/nuttx-configs/px4fmu-v1/Kconfig new file mode 100644 index 0000000000..edbafa06f8 --- /dev/null +++ b/nuttx-configs/px4fmu-v1/Kconfig @@ -0,0 +1,21 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# + +if ARCH_BOARD_PX4FMU_V1 + +config HRT_TIMER + bool "High resolution timer support" + default y + ---help--- + Enable high resolution timer for PPM capture and system clocks. + +config HRT_PPM + bool "PPM input capture" + default y + depends on HRT_TIMER + ---help--- + Enable PPM input capture via HRT (for CPPM / PPM sum RC inputs) + +endif diff --git a/nuttx-configs/px4fmu-v1/common/Make.defs b/nuttx-configs/px4fmu-v1/common/Make.defs new file mode 100644 index 0000000000..756286ccb5 --- /dev/null +++ b/nuttx-configs/px4fmu-v1/common/Make.defs @@ -0,0 +1,184 @@ +############################################################################ +# configs/px4fmu/common/Make.defs +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# +############################################################################ + +# +# Generic Make.defs for the PX4FMU +# Do not specify/use this file directly - it is included by config-specific +# Make.defs in the per-config directories. +# + +include ${TOPDIR}/tools/Config.mk + +# +# We only support building with the ARM bare-metal toolchain from +# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS. +# +CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI + +include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs + +CC = $(CROSSDEV)gcc +CXX = $(CROSSDEV)g++ +CPP = $(CROSSDEV)gcc -E +LD = $(CROSSDEV)ld +AR = $(CROSSDEV)ar rcs +NM = $(CROSSDEV)nm +OBJCOPY = $(CROSSDEV)objcopy +OBJDUMP = $(CROSSDEV)objdump + +MAXOPTIMIZATION = -O3 +ARCHCPUFLAGS = -mcpu=cortex-m4 \ + -mthumb \ + -march=armv7e-m \ + -mfpu=fpv4-sp-d16 \ + -mfloat-abi=hard + + +# enable precise stack overflow tracking +INSTRUMENTATIONDEFINES = -finstrument-functions \ + -ffixed-r10 + +# pull in *just* libm from the toolchain ... this is grody +LIBM = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}" +EXTRA_LIBS += $(LIBM) + +# use our linker script +LDSCRIPT = ld.script + +ifeq ($(WINTOOL),y) + # Windows-native toolchains + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" + ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" + ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT)}" +else + ifeq ($(PX4_WINTOOL),y) + # Windows-native toolchains (MSYS) + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT) + else + # Linux/Cygwin-native toolchain + MKDEP = $(TOPDIR)/tools/mkdeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT) + endif +endif + +# tool versions +ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} +ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} + +# optimisation flags +ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ + -fno-strict-aliasing \ + -fno-strength-reduce \ + -fomit-frame-pointer \ + -funsafe-math-optimizations \ + -fno-builtin-printf \ + -ffunction-sections \ + -fdata-sections + +ifeq ("${CONFIG_DEBUG_SYMBOLS}","y") +ARCHOPTIMIZATION += -g +endif + +ARCHCFLAGS = -std=gnu99 +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x +ARCHWARNINGS = -Wall \ + -Wextra \ + -Wdouble-promotion \ + -Wshadow \ + -Wfloat-equal \ + -Wframe-larger-than=1024 \ + -Wpointer-arith \ + -Wlogical-op \ + -Wmissing-declarations \ + -Wpacked \ + -Wno-unused-parameter +# -Wcast-qual - generates spurious noreturn attribute warnings, try again later +# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code +# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives + +ARCHCWARNINGS = $(ARCHWARNINGS) \ + -Wbad-function-cast \ + -Wstrict-prototypes \ + -Wold-style-declaration \ + -Wmissing-parameter-type \ + -Wmissing-prototypes \ + -Wnested-externs \ + -Wunsuffixed-float-constants +ARCHWARNINGSXX = $(ARCHWARNINGS) \ + -Wno-psabi +ARCHDEFINES = +ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 + +# this seems to be the only way to add linker flags +EXTRA_LIBS += --warn-common \ + --gc-sections + +CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common +CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) +CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe +CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) +CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) +AFLAGS = $(CFLAGS) -D__ASSEMBLY__ + +NXFLATLDFLAGS1 = -r -d -warn-common +NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections +LDNXFLATFLAGS = -e main -s 2048 + +OBJEXT = .o +LIBEXT = .a +EXEEXT = + + +# produce partially-linked $1 from files in $2 +define PRELINK + @echo "PRELINK: $1" + $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 +endef + +HOSTCC = gcc +HOSTINCLUDES = -I. +HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe +HOSTLDFLAGS = + diff --git a/nuttx-configs/px4fmu-v1/common/ld.script b/nuttx-configs/px4fmu-v1/common/ld.script new file mode 100644 index 0000000000..de8179e8db --- /dev/null +++ b/nuttx-configs/px4fmu-v1/common/ld.script @@ -0,0 +1,149 @@ +/**************************************************************************** + * configs/px4fmu/common/ld.script + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/* The STM32F405 has 1024Kb of FLASH beginning at address 0x0800:0000 and + * 192Kb of SRAM. SRAM is split up into three blocks: + * + * 1) 112Kb of SRAM beginning at address 0x2000:0000 + * 2) 16Kb of SRAM beginning at address 0x2001:c000 + * 3) 64Kb of TCM SRAM beginning at address 0x1000:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + * + * The first 0x4000 of flash is reserved for the bootloader. + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x08004000, LENGTH = 1008K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 128K + ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K +} + +OUTPUT_ARCH(arm) + +ENTRY(__start) /* treat __start as the anchor for dead code stripping */ +EXTERN(_vectors) /* force the vectors to be included in the output */ + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + /* + * This is a hack to make the newlib libm __errno() call + * use the NuttX get_errno_ptr() function. + */ + __errno = get_errno_ptr; + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + /* + * Construction data for parameters. + */ + __param ALIGN(4): { + __param_start = ABSOLUTE(.); + KEEP(*(__param*)) + __param_end = ABSOLUTE(.); + } > flash + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/nuttx-configs/px4fmu-v1/include/board.h b/nuttx-configs/px4fmu-v1/include/board.h new file mode 100644 index 0000000000..1921f7715b --- /dev/null +++ b/nuttx-configs/px4fmu-v1/include/board.h @@ -0,0 +1,319 @@ +/************************************************************************************ + * configs/stm32f4discovery/include/board.h + * include/arch/board/board.h + * + * Copyright (C) 2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ************************************************************************************/ + +#ifndef __CONFIG_PX4FMU_V1_INCLUDE_BOARD_H +#define __CONFIG_PX4FMU_V1_INCLUDE_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#ifndef __ASSEMBLY__ +# include +#endif + +#include "stm32_rcc.h" +#include "stm32_sdio.h" +#include "stm32.h" + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ +/* The PX4FMU uses a 24MHz crystal connected to the HSE. + * + * This is the canonical configuration: + * System Clock source : PLL (HSE) + * SYSCLK(Hz) : 168000000 Determined by PLL configuration + * HCLK(Hz) : 168000000 (STM32_RCC_CFGR_HPRE) + * AHB Prescaler : 1 (STM32_RCC_CFGR_HPRE) + * APB1 Prescaler : 4 (STM32_RCC_CFGR_PPRE1) + * APB2 Prescaler : 2 (STM32_RCC_CFGR_PPRE2) + * HSE Frequency(Hz) : 24000000 (STM32_BOARD_XTAL) + * PLLM : 24 (STM32_PLLCFG_PLLM) + * PLLN : 336 (STM32_PLLCFG_PLLN) + * PLLP : 2 (STM32_PLLCFG_PLLP) + * PLLQ : 7 (STM32_PLLCFG_PLLQ) + * Main regulator output voltage : Scale1 mode Needed for high speed SYSCLK + * Flash Latency(WS) : 5 + * Prefetch Buffer : OFF + * Instruction cache : ON + * Data cache : ON + * Require 48MHz for USB OTG FS, : Enabled + * SDIO and RNG clock + */ + +/* HSI - 16 MHz RC factory-trimmed + * LSI - 32 KHz RC + * HSE - On-board crystal frequency is 24MHz + * LSE - not installed + */ + +#define STM32_BOARD_XTAL 24000000ul + +#define STM32_HSI_FREQUENCY 16000000ul +#define STM32_LSI_FREQUENCY 32000 +#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL +//#define STM32_LSE_FREQUENCY 32768 + +/* Main PLL Configuration. + * + * PLL source is HSE + * PLL_VCO = (STM32_HSE_FREQUENCY / PLLM) * PLLN + * = (8,000,000 / 8) * 336 + * = 336,000,000 + * SYSCLK = PLL_VCO / PLLP + * = 336,000,000 / 2 = 168,000,000 + * USB OTG FS, SDIO and RNG Clock + * = PLL_VCO / PLLQ + * = 48,000,000 + */ + +#define STM32_PLLCFG_PLLM RCC_PLLCFG_PLLM(24) +#define STM32_PLLCFG_PLLN RCC_PLLCFG_PLLN(336) +#define STM32_PLLCFG_PLLP RCC_PLLCFG_PLLP_2 +#define STM32_PLLCFG_PLLQ RCC_PLLCFG_PLLQ(7) + +#define STM32_SYSCLK_FREQUENCY 168000000ul + +/* AHB clock (HCLK) is SYSCLK (168MHz) */ + +#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK /* HCLK = SYSCLK / 1 */ +#define STM32_HCLK_FREQUENCY STM32_SYSCLK_FREQUENCY +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ + +/* APB1 clock (PCLK1) is HCLK/4 (42MHz) */ + +#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLKd4 /* PCLK1 = HCLK / 4 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/4) + +/* Timers driven from APB1 will be twice PCLK1 */ + +#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM6_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM7_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* APB2 clock (PCLK2) is HCLK/2 (84MHz) */ + +#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* Timers driven from APB2 will be twice PCLK2 */ + +#define STM32_APB2_TIM1_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM9_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM10_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK2_FREQUENCY) + +/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx + * otherwise frequency is 2xAPBx. + * Note: TIM1,8 are on APB2, others on APB1 + */ + +#define STM32_TIM18_FREQUENCY (2*STM32_PCLK2_FREQUENCY) +#define STM32_TIM27_FREQUENCY (2*STM32_PCLK1_FREQUENCY) + +/* LED definitions ******************************************************************/ +/* If CONFIG_ARCH_LEDS is not defined, then the user can control the LEDs in any + * way. The following definitions are used to access individual LEDs. + */ + +/* LED index values for use with stm32_setled() */ + +#define BOARD_LED1 0 +#define BOARD_LED2 1 +#define BOARD_NLEDS 2 + +#define BOARD_LED_BLUE BOARD_LED1 +#define BOARD_LED_RED BOARD_LED2 + +/* LED bits for use with stm32_setleds() */ + +#define BOARD_LED1_BIT (1 << BOARD_LED1) +#define BOARD_LED2_BIT (1 << BOARD_LED2) + +/* If CONFIG_ARCH_LEDs is defined, then NuttX will control the 2 LEDs on board the + * px4fmu-v1. The following definitions describe how NuttX controls the LEDs: + */ + +#define LED_STARTED 0 /* LED1 */ +#define LED_HEAPALLOCATE 1 /* LED2 */ +#define LED_IRQSENABLED 2 /* LED1 */ +#define LED_STACKCREATED 3 /* LED1 + LED2 */ +#define LED_INIRQ 4 /* LED1 */ +#define LED_SIGNAL 5 /* LED2 */ +#define LED_ASSERTION 6 /* LED1 + LED2 */ +#define LED_PANIC 7 /* LED1 + LED2 */ + +/* Alternate function pin selections ************************************************/ + +/* + * UARTs. + * + * Note that UART5 has no optional pinout, so it is not listed here. + */ +#define GPIO_USART1_RX GPIO_USART1_RX_2 +#define GPIO_USART1_TX GPIO_USART1_TX_2 + +#define GPIO_USART2_RX GPIO_USART2_RX_1 +#define GPIO_USART2_TX GPIO_USART2_TX_1 +#define GPIO_USART2_RTS GPIO_USART2_RTS_1 +#define GPIO_USART2_CTS GPIO_USART2_CTS_1 + +#define GPIO_USART6_RX GPIO_USART6_RX_1 +#define GPIO_USART6_TX GPIO_USART6_TX_1 + +/* UART DMA configuration for USART1/6 */ +#define DMAMAP_USART1_RX DMAMAP_USART1_RX_2 +#define DMAMAP_USART6_RX DMAMAP_USART6_RX_2 + +/* + * CAN + * + * CAN2 is routed to the expansion connector. + */ + +#define GPIO_CAN2_RX GPIO_CAN2_RX_2 +#define GPIO_CAN2_TX GPIO_CAN2_TX_2 + +/* + * I2C + * + * The optional _GPIO configurations allow the I2C driver to manually + * reset the bus to clear stuck slaves. They match the pin configuration, + * but are normally-high GPIOs. + */ +#define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 +#define GPIO_I2C1_SDA GPIO_I2C1_SDA_2 +#define GPIO_I2C1_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN8) +#define GPIO_I2C1_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN9) + +#define GPIO_I2C2_SCL GPIO_I2C2_SCL_1 +#define GPIO_I2C2_SDA GPIO_I2C2_SDA_1 +#define GPIO_I2C2_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN10) +#define GPIO_I2C2_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11) + +#define GPIO_I2C3_SCL GPIO_I2C3_SCL_1 +#define GPIO_I2C3_SDA GPIO_I2C3_SDA_1 +#define GPIO_I2C3_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN8) +#define GPIO_I2C3_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN9) + +/* + * SPI + * + * There are sensors on SPI1, and SPI3 is connected to the microSD slot. + */ +#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 +#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 +#define GPIO_SPI1_SCK GPIO_SPI1_SCK_1 + +#define GPIO_SPI3_MISO GPIO_SPI3_MISO_2 +#define GPIO_SPI3_MOSI GPIO_SPI3_MOSI_1 +#define GPIO_SPI3_SCK GPIO_SPI3_SCK_2 +#define GPIO_SPI3_NSS GPIO_SPI3_NSS_2 + +/* SPI DMA configuration for SPI3 (microSD) */ +#define DMACHAN_SPI3_RX DMAMAP_SPI3_RX_1 +#define DMACHAN_SPI3_TX DMAMAP_SPI3_TX_2 +/* XXX since we allocate the HP work stack from CCM RAM on normal system startup, + SPI1 will never run in DMA mode - so we can just give it a random config here. + What we really need to do is to make DMA configurable per channel, and always + disable it for SPI1. */ +#define DMACHAN_SPI1_RX DMAMAP_SPI1_RX_1 +#define DMACHAN_SPI1_TX DMAMAP_SPI1_TX_2 + +/************************************************************************************ + * Public Data + ************************************************************************************/ + +#ifndef __ASSEMBLY__ + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +/************************************************************************************ + * Public Function Prototypes + ************************************************************************************/ +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +EXTERN void stm32_boardinitialize(void); + +/************************************************************************************ + * Name: stm32_ledinit, stm32_setled, and stm32_setleds + * + * Description: + * If CONFIG_ARCH_LEDS is defined, then NuttX will control the on-board LEDs. If + * CONFIG_ARCH_LEDS is not defined, then the following interfacesare available to + * control the LEDs from user applications. + * + ************************************************************************************/ + +#ifndef CONFIG_ARCH_LEDS +EXTERN void stm32_ledinit(void); +EXTERN void stm32_setled(int led, bool ledon); +EXTERN void stm32_setleds(uint8_t ledset); +#endif + +#undef EXTERN +#if defined(__cplusplus) +} +#endif + +#endif /* __ASSEMBLY__ */ +#endif /* __CONFIG_PX4FMU_V1_INCLUDE_BOARD_H */ diff --git a/nuttx-configs/px4fmu-v1/include/nsh_romfsimg.h b/nuttx-configs/px4fmu-v1/include/nsh_romfsimg.h new file mode 100644 index 0000000000..15e4e7a8d5 --- /dev/null +++ b/nuttx-configs/px4fmu-v1/include/nsh_romfsimg.h @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * nsh_romfsetc.h + * + * This file is a stub for 'make export' purposes; the actual ROMFS + * must be supplied by the library client. + */ + +extern unsigned char romfs_img[]; +extern unsigned int romfs_img_len; diff --git a/nuttx-configs/px4fmu-v1/nsh/Make.defs b/nuttx-configs/px4fmu-v1/nsh/Make.defs new file mode 100644 index 0000000000..81936334b3 --- /dev/null +++ b/nuttx-configs/px4fmu-v1/nsh/Make.defs @@ -0,0 +1,3 @@ +include ${TOPDIR}/.config + +include $(TOPDIR)/configs/px4fmu-v1/common/Make.defs diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig new file mode 100644 index 0000000000..eb225e22c5 --- /dev/null +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -0,0 +1,957 @@ +# +# Automatically generated file; DO NOT EDIT. +# Nuttx/ Configuration +# +CONFIG_NUTTX_NEWCONFIG=y + +# +# Build Setup +# +# CONFIG_EXPERIMENTAL is not set +# CONFIG_HOST_LINUX is not set +CONFIG_HOST_OSX=y +# CONFIG_HOST_WINDOWS is not set +# CONFIG_HOST_OTHER is not set + +# +# Build Configuration +# +CONFIG_APPS_DIR="../apps" +# CONFIG_BUILD_2PASS is not set + +# +# Binary Output Formats +# +# CONFIG_RRLOAD_BINARY is not set +# CONFIG_INTELHEX_BINARY is not set +# CONFIG_MOTOROLA_SREC is not set +CONFIG_RAW_BINARY=y + +# +# Customize Header Files +# +# CONFIG_ARCH_STDBOOL_H is not set +CONFIG_ARCH_MATH_H=y +# CONFIG_ARCH_FLOAT_H is not set +# CONFIG_ARCH_STDARG_H is not set + +# +# Debug Options +# +# CONFIG_DEBUG is not set +CONFIG_DEBUG_SYMBOLS=y + +# +# System Type +# +# CONFIG_ARCH_8051 is not set +CONFIG_ARCH_ARM=y +# CONFIG_ARCH_AVR is not set +# CONFIG_ARCH_HC is not set +# CONFIG_ARCH_MIPS is not set +# CONFIG_ARCH_RGMP is not set +# CONFIG_ARCH_SH is not set +# CONFIG_ARCH_SIM is not set +# CONFIG_ARCH_X86 is not set +# CONFIG_ARCH_Z16 is not set +# CONFIG_ARCH_Z80 is not set +CONFIG_ARCH="arm" + +# +# ARM Options +# +# CONFIG_ARCH_CHIP_C5471 is not set +# CONFIG_ARCH_CHIP_CALYPSO is not set +# CONFIG_ARCH_CHIP_DM320 is not set +# CONFIG_ARCH_CHIP_IMX is not set +# CONFIG_ARCH_CHIP_KINETIS is not set +# CONFIG_ARCH_CHIP_KL is not set +# CONFIG_ARCH_CHIP_LM is not set +# CONFIG_ARCH_CHIP_LPC17XX is not set +# CONFIG_ARCH_CHIP_LPC214X is not set +# CONFIG_ARCH_CHIP_LPC2378 is not set +# CONFIG_ARCH_CHIP_LPC31XX is not set +# CONFIG_ARCH_CHIP_LPC43XX is not set +# CONFIG_ARCH_CHIP_NUC1XX is not set +# CONFIG_ARCH_CHIP_SAM3U is not set +CONFIG_ARCH_CHIP_STM32=y +# CONFIG_ARCH_CHIP_STR71X is not set +CONFIG_ARCH_CORTEXM4=y +CONFIG_ARCH_HAVE_FPU=y +CONFIG_ARCH_FAMILY="armv7-m" +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARCH_HAVE_CMNVECTOR=y +CONFIG_ARMV7M_CMNVECTOR=y +CONFIG_ARMV7M_STACKCHECK=y +CONFIG_ARCH_FPU=y +CONFIG_ARCH_HAVE_MPU=y +# CONFIG_ARMV7M_MPU is not set + +# +# ARMV7M Configuration Options +# +# CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set +CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y +CONFIG_SERIAL_TERMIOS=y +CONFIG_SERIAL_DISABLE_REORDERING=y +CONFIG_SERIAL_IFLOWCONTROL=y +CONFIG_SERIAL_OFLOWCONTROL=y + +# +# STM32 Configuration Options +# +# CONFIG_ARCH_CHIP_STM32L151C6 is not set +# CONFIG_ARCH_CHIP_STM32L151C8 is not set +# CONFIG_ARCH_CHIP_STM32L151CB is not set +# CONFIG_ARCH_CHIP_STM32L151R6 is not set +# CONFIG_ARCH_CHIP_STM32L151R8 is not set +# CONFIG_ARCH_CHIP_STM32L151RB is not set +# CONFIG_ARCH_CHIP_STM32L151V6 is not set +# CONFIG_ARCH_CHIP_STM32L151V8 is not set +# CONFIG_ARCH_CHIP_STM32L151VB is not set +# CONFIG_ARCH_CHIP_STM32L152C6 is not set +# CONFIG_ARCH_CHIP_STM32L152C8 is not set +# CONFIG_ARCH_CHIP_STM32L152CB is not set +# CONFIG_ARCH_CHIP_STM32L152R6 is not set +# CONFIG_ARCH_CHIP_STM32L152R8 is not set +# CONFIG_ARCH_CHIP_STM32L152RB is not set +# CONFIG_ARCH_CHIP_STM32L152V6 is not set +# CONFIG_ARCH_CHIP_STM32L152V8 is not set +# CONFIG_ARCH_CHIP_STM32L152VB is not set +# CONFIG_ARCH_CHIP_STM32F100C8 is not set +# CONFIG_ARCH_CHIP_STM32F100CB is not set +# CONFIG_ARCH_CHIP_STM32F100R8 is not set +# CONFIG_ARCH_CHIP_STM32F100RB is not set +# CONFIG_ARCH_CHIP_STM32F100RC is not set +# CONFIG_ARCH_CHIP_STM32F100RD is not set +# CONFIG_ARCH_CHIP_STM32F100RE is not set +# CONFIG_ARCH_CHIP_STM32F100V8 is not set +# CONFIG_ARCH_CHIP_STM32F100VB is not set +# CONFIG_ARCH_CHIP_STM32F100VC is not set +# CONFIG_ARCH_CHIP_STM32F100VD is not set +# CONFIG_ARCH_CHIP_STM32F100VE is not set +# CONFIG_ARCH_CHIP_STM32F103C4 is not set +# CONFIG_ARCH_CHIP_STM32F103C8 is not set +# CONFIG_ARCH_CHIP_STM32F103RET6 is not set +# CONFIG_ARCH_CHIP_STM32F103VCT6 is not set +# CONFIG_ARCH_CHIP_STM32F103VET6 is not set +# CONFIG_ARCH_CHIP_STM32F103ZET6 is not set +# CONFIG_ARCH_CHIP_STM32F105VBT7 is not set +# CONFIG_ARCH_CHIP_STM32F107VC is not set +# CONFIG_ARCH_CHIP_STM32F207IG is not set +# CONFIG_ARCH_CHIP_STM32F302CB is not set +# CONFIG_ARCH_CHIP_STM32F302CC is not set +# CONFIG_ARCH_CHIP_STM32F302RB is not set +# CONFIG_ARCH_CHIP_STM32F302RC is not set +# CONFIG_ARCH_CHIP_STM32F302VB is not set +# CONFIG_ARCH_CHIP_STM32F302VC is not set +# CONFIG_ARCH_CHIP_STM32F303CB is not set +# CONFIG_ARCH_CHIP_STM32F303CC is not set +# CONFIG_ARCH_CHIP_STM32F303RB is not set +# CONFIG_ARCH_CHIP_STM32F303RC is not set +# CONFIG_ARCH_CHIP_STM32F303VB is not set +# CONFIG_ARCH_CHIP_STM32F303VC is not set +CONFIG_ARCH_CHIP_STM32F405RG=y +# CONFIG_ARCH_CHIP_STM32F405VG is not set +# CONFIG_ARCH_CHIP_STM32F405ZG is not set +# CONFIG_ARCH_CHIP_STM32F407VE is not set +# CONFIG_ARCH_CHIP_STM32F407VG is not set +# CONFIG_ARCH_CHIP_STM32F407ZE is not set +# CONFIG_ARCH_CHIP_STM32F407ZG is not set +# CONFIG_ARCH_CHIP_STM32F407IE is not set +# CONFIG_ARCH_CHIP_STM32F407IG is not set +# CONFIG_ARCH_CHIP_STM32F427V is not set +# CONFIG_ARCH_CHIP_STM32F427Z is not set +# CONFIG_ARCH_CHIP_STM32F427I is not set +# CONFIG_STM32_STM32L15XX is not set +# CONFIG_STM32_ENERGYLITE is not set +# CONFIG_STM32_STM32F10XX is not set +# CONFIG_STM32_VALUELINE is not set +# CONFIG_STM32_CONNECTIVITYLINE is not set +# CONFIG_STM32_PERFORMANCELINE is not set +# CONFIG_STM32_HIGHDENSITY is not set +# CONFIG_STM32_MEDIUMDENSITY is not set +# CONFIG_STM32_LOWDENSITY is not set +# CONFIG_STM32_STM32F20XX is not set +# CONFIG_STM32_STM32F30XX is not set +CONFIG_STM32_STM32F40XX=y +# CONFIG_STM32_DFU is not set + +# +# STM32 Peripheral Support +# +CONFIG_STM32_ADC1=y +# CONFIG_STM32_ADC2 is not set +# CONFIG_STM32_ADC3 is not set +CONFIG_STM32_BKPSRAM=y +# CONFIG_STM32_CAN1 is not set +# CONFIG_STM32_CAN2 is not set +CONFIG_STM32_CCMDATARAM=y +# CONFIG_STM32_CRC is not set +# CONFIG_STM32_CRYP is not set +CONFIG_STM32_DMA1=y +CONFIG_STM32_DMA2=y +# CONFIG_STM32_DAC1 is not set +# CONFIG_STM32_DAC2 is not set +# CONFIG_STM32_DCMI is not set +# CONFIG_STM32_ETHMAC is not set +# CONFIG_STM32_FSMC is not set +# CONFIG_STM32_HASH is not set +CONFIG_STM32_I2C1=y +CONFIG_STM32_I2C2=y +CONFIG_STM32_I2C3=y +CONFIG_STM32_OTGFS=y +# CONFIG_STM32_OTGHS is not set +CONFIG_STM32_PWR=y +# CONFIG_STM32_RNG is not set +# CONFIG_STM32_SDIO is not set +CONFIG_STM32_SPI1=y +# CONFIG_STM32_SPI2 is not set +CONFIG_STM32_SPI3=y +CONFIG_STM32_SYSCFG=y +# CONFIG_STM32_TIM1 is not set +# CONFIG_STM32_TIM2 is not set +# CONFIG_STM32_TIM3 is not set +CONFIG_STM32_TIM4=y +CONFIG_STM32_TIM5=y +CONFIG_STM32_TIM6=y +CONFIG_STM32_TIM7=y +# CONFIG_STM32_TIM8 is not set +CONFIG_STM32_TIM9=y +CONFIG_STM32_TIM10=y +CONFIG_STM32_TIM11=y +CONFIG_STM32_TIM12=y +CONFIG_STM32_TIM13=y +CONFIG_STM32_TIM14=y +CONFIG_STM32_USART1=y +CONFIG_STM32_USART2=y +# CONFIG_STM32_USART3 is not set +# CONFIG_STM32_UART4 is not set +CONFIG_STM32_UART5=y +CONFIG_STM32_USART6=y +# CONFIG_STM32_IWDG is not set +CONFIG_STM32_WWDG=y +CONFIG_STM32_ADC=y +CONFIG_STM32_SPI=y +CONFIG_STM32_I2C=y + +# +# Alternate Pin Mapping +# +CONFIG_STM32_FLASH_PREFETCH=y +# CONFIG_STM32_JTAG_DISABLE is not set +CONFIG_STM32_JTAG_FULL_ENABLE=y +# CONFIG_STM32_JTAG_NOJNTRST_ENABLE is not set +# CONFIG_STM32_JTAG_SW_ENABLE is not set +CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y +# CONFIG_STM32_FORCEPOWER is not set +# CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set +# CONFIG_STM32_CCMEXCLUDE is not set +# CONFIG_STM32_TIM4_PWM is not set +# CONFIG_STM32_TIM5_PWM is not set +# CONFIG_STM32_TIM9_PWM is not set +# CONFIG_STM32_TIM10_PWM is not set +# CONFIG_STM32_TIM11_PWM is not set +# CONFIG_STM32_TIM12_PWM is not set +# CONFIG_STM32_TIM13_PWM is not set +# CONFIG_STM32_TIM14_PWM is not set +# CONFIG_STM32_TIM4_ADC is not set +# CONFIG_STM32_TIM5_ADC is not set +CONFIG_STM32_USART=y + +# +# U[S]ART Configuration +# +# CONFIG_USART1_RS485 is not set +CONFIG_USART1_RXDMA=y +# CONFIG_USART2_RS485 is not set +CONFIG_USART2_RXDMA=y +# CONFIG_USART3_RXDMA is not set +# CONFIG_UART4_RXDMA is not set +# CONFIG_UART5_RS485 is not set +CONFIG_UART5_RXDMA=y +# CONFIG_USART6_RS485 is not set +CONFIG_USART6_RXDMA=y +# CONFIG_USART7_RXDMA is not set +# CONFIG_USART8_RXDMA is not set +CONFIG_STM32_USART_SINGLEWIRE=y + +# +# SPI Configuration +# +# CONFIG_STM32_SPI_INTERRUPTS is not set +# CONFIG_STM32_SPI_DMA is not set + +# +# I2C Configuration +# +# CONFIG_STM32_I2C_DYNTIMEO is not set +CONFIG_STM32_I2CTIMEOSEC=0 +CONFIG_STM32_I2CTIMEOMS=10 +CONFIG_STM32_I2CTIMEOTICKS=500 +# CONFIG_STM32_I2C_DUTY16_9 is not set + +# +# USB Host Configuration +# + +# +# USB Device Configuration +# + +# +# External Memory Configuration +# + +# +# Architecture Options +# +# CONFIG_ARCH_NOINTC is not set +# CONFIG_ARCH_VECNOTIRQ is not set +CONFIG_ARCH_DMA=y +CONFIG_ARCH_IRQPRIO=y +# CONFIG_CUSTOM_STACK is not set +# CONFIG_ADDRENV is not set +CONFIG_ARCH_HAVE_VFORK=y +CONFIG_ARCH_STACKDUMP=y +# CONFIG_ENDIAN_BIG is not set +# CONFIG_ARCH_HAVE_RAMFUNCS is not set +CONFIG_ARCH_HAVE_RAMVECTORS=y +# CONFIG_ARCH_RAMVECTORS is not set + +# +# Board Settings +# +CONFIG_BOARD_LOOPSPERMSEC=16717 +# CONFIG_ARCH_CALIBRATION is not set +CONFIG_DRAM_START=0x20000000 +CONFIG_DRAM_SIZE=196608 +CONFIG_ARCH_HAVE_INTERRUPTSTACK=y +CONFIG_ARCH_INTERRUPTSTACK=0 + +# +# Boot options +# +# CONFIG_BOOT_RUNFROMEXTSRAM is not set +CONFIG_BOOT_RUNFROMFLASH=y +# CONFIG_BOOT_RUNFROMISRAM is not set +# CONFIG_BOOT_RUNFROMSDRAM is not set +# CONFIG_BOOT_COPYTORAM is not set + +# +# Board Selection +# +CONFIG_ARCH_BOARD_PX4FMU_V1=y +# CONFIG_ARCH_BOARD_CUSTOM is not set +CONFIG_ARCH_BOARD="px4fmu-v1" + +# +# Common Board Options +# +CONFIG_ARCH_HAVE_LEDS=y +# CONFIG_ARCH_LEDS is not set +CONFIG_NSH_MMCSDMINOR=0 +CONFIG_NSH_MMCSDSLOTNO=0 +CONFIG_NSH_MMCSDSPIPORTNO=3 + +# +# Board-Specific Options +# +CONFIG_HRT_TIMER=y +CONFIG_HRT_PPM=y + +# +# RTOS Features +# +# CONFIG_BOARD_INITIALIZE is not set +CONFIG_MSEC_PER_TICK=1 +CONFIG_RR_INTERVAL=0 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_TASK_NAME_SIZE=24 +# CONFIG_SCHED_HAVE_PARENT is not set +# CONFIG_JULIAN_TIME is not set +CONFIG_START_YEAR=1970 +CONFIG_START_MONTH=1 +CONFIG_START_DAY=1 +# CONFIG_DEV_CONSOLE is not set +# CONFIG_MUTEX_TYPES is not set +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_NNESTPRIO=8 +# CONFIG_FDCLONE_DISABLE is not set +CONFIG_FDCLONE_STDIO=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SCHED_WAITPID=y +# CONFIG_SCHED_STARTHOOK is not set +CONFIG_SCHED_ATEXIT=y +CONFIG_SCHED_ATEXIT_MAX=1 +# CONFIG_SCHED_ONEXIT is not set +CONFIG_USER_ENTRYPOINT="nsh_main" +CONFIG_DISABLE_OS_API=y +# CONFIG_DISABLE_CLOCK is not set +# CONFIG_DISABLE_POSIX_TIMERS is not set +# CONFIG_DISABLE_PTHREAD is not set +# CONFIG_DISABLE_SIGNALS is not set +# CONFIG_DISABLE_MQUEUE is not set +# CONFIG_DISABLE_ENVIRON is not set + +# +# Signal Numbers +# +CONFIG_SIG_SIGUSR1=1 +CONFIG_SIG_SIGUSR2=2 +CONFIG_SIG_SIGALARM=3 +CONFIG_SIG_SIGCONDTIMEDOUT=16 +CONFIG_SIG_SIGWORK=4 + +# +# Sizes of configurable things (0 disables) +# +CONFIG_MAX_TASKS=32 +CONFIG_MAX_TASK_ARGS=10 +CONFIG_NPTHREAD_KEYS=4 +CONFIG_NFILE_DESCRIPTORS=32 +CONFIG_NFILE_STREAMS=25 +CONFIG_NAME_MAX=32 +CONFIG_PREALLOC_MQ_MSGS=4 +CONFIG_MQ_MAXMSGSIZE=32 +CONFIG_MAX_WDOGPARMS=2 +CONFIG_PREALLOC_WDOGS=50 +CONFIG_PREALLOC_TIMERS=50 + +# +# Stack and heap information +# +CONFIG_IDLETHREAD_STACKSIZE=6000 +CONFIG_USERMAIN_STACKSIZE=4096 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_PTHREAD_STACK_DEFAULT=2048 + +# +# Device Drivers +# +# CONFIG_DISABLE_POLL is not set +CONFIG_DEV_NULL=y +# CONFIG_DEV_ZERO is not set +# CONFIG_LOOP is not set +# CONFIG_RAMDISK is not set +# CONFIG_CAN is not set +# CONFIG_PWM is not set +CONFIG_I2C=y +# CONFIG_I2C_SLAVE is not set +CONFIG_I2C_TRANSFER=y +# CONFIG_I2C_WRITEREAD is not set +# CONFIG_I2C_POLLED is not set +# CONFIG_I2C_TRACE is not set +CONFIG_ARCH_HAVE_I2CRESET=y +CONFIG_I2C_RESET=y +CONFIG_SPI=y +# CONFIG_SPI_OWNBUS is not set +CONFIG_SPI_EXCHANGE=y +# CONFIG_SPI_CMDDATA is not set +# CONFIG_RTC is not set +CONFIG_WATCHDOG=y +# CONFIG_ANALOG is not set +# CONFIG_AUDIO_DEVICES is not set +# CONFIG_BCH is not set +# CONFIG_INPUT is not set +# CONFIG_LCD is not set +CONFIG_MMCSD=y +CONFIG_MMCSD_NSLOTS=1 +# CONFIG_MMCSD_READONLY is not set +# CONFIG_MMCSD_MULTIBLOCK_DISABLE is not set +# CONFIG_MMCSD_MMCSUPPORT is not set +# CONFIG_MMCSD_HAVECARDDETECT is not set +CONFIG_MMCSD_SPI=y +CONFIG_MMCSD_SPICLOCK=24000000 +# CONFIG_MMCSD_SDIO is not set +# CONFIG_MTD is not set +CONFIG_PIPES=y +# CONFIG_PM is not set +# CONFIG_POWER is not set +# CONFIG_SENSORS is not set +CONFIG_SERIAL=y +# CONFIG_DEV_LOWCONSOLE is not set +CONFIG_SERIAL_REMOVABLE=y +# CONFIG_16550_UART is not set +CONFIG_ARCH_HAVE_UART5=y +CONFIG_ARCH_HAVE_USART1=y +CONFIG_ARCH_HAVE_USART2=y +CONFIG_ARCH_HAVE_USART6=y +CONFIG_MCU_SERIAL=y +CONFIG_STANDARD_SERIAL=y +CONFIG_SERIAL_NPOLLWAITERS=2 +# CONFIG_USART1_SERIAL_CONSOLE is not set +# CONFIG_USART2_SERIAL_CONSOLE is not set +# CONFIG_UART5_SERIAL_CONSOLE is not set +# CONFIG_USART6_SERIAL_CONSOLE is not set +CONFIG_NO_SERIAL_CONSOLE=y + +# +# USART1 Configuration +# +CONFIG_USART1_RXBUFSIZE=512 +CONFIG_USART1_TXBUFSIZE=512 +CONFIG_USART1_BAUD=57600 +CONFIG_USART1_BITS=8 +CONFIG_USART1_PARITY=0 +CONFIG_USART1_2STOP=0 + +# +# USART2 Configuration +# +CONFIG_USART2_RXBUFSIZE=512 +CONFIG_USART2_TXBUFSIZE=512 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_BITS=8 +CONFIG_USART2_PARITY=0 +CONFIG_USART2_2STOP=0 + +# +# UART5 Configuration +# +CONFIG_UART5_RXBUFSIZE=32 +CONFIG_UART5_TXBUFSIZE=32 +CONFIG_UART5_BAUD=57600 +CONFIG_UART5_BITS=8 +CONFIG_UART5_PARITY=0 +CONFIG_UART5_2STOP=0 + +# +# USART6 Configuration +# +CONFIG_USART6_RXBUFSIZE=512 +CONFIG_USART6_TXBUFSIZE=512 +CONFIG_USART6_BAUD=57600 +CONFIG_USART6_BITS=8 +CONFIG_USART6_PARITY=0 +CONFIG_USART6_2STOP=0 +CONFIG_USBDEV=y + +# +# USB Device Controller Driver Options +# +# CONFIG_USBDEV_ISOCHRONOUS is not set +# CONFIG_USBDEV_DUALSPEED is not set +CONFIG_USBDEV_SELFPOWERED=y +# CONFIG_USBDEV_BUSPOWERED is not set +CONFIG_USBDEV_MAXPOWER=500 +# CONFIG_USBDEV_DMA is not set +# CONFIG_USBDEV_TRACE is not set + +# +# USB Device Class Driver Options +# +# CONFIG_USBDEV_COMPOSITE is not set +# CONFIG_PL2303 is not set +CONFIG_CDCACM=y +CONFIG_CDCACM_CONSOLE=y +CONFIG_CDCACM_EP0MAXPACKET=64 +CONFIG_CDCACM_EPINTIN=1 +CONFIG_CDCACM_EPINTIN_FSSIZE=64 +CONFIG_CDCACM_EPINTIN_HSSIZE=64 +CONFIG_CDCACM_EPBULKOUT=3 +CONFIG_CDCACM_EPBULKOUT_FSSIZE=64 +CONFIG_CDCACM_EPBULKOUT_HSSIZE=512 +CONFIG_CDCACM_EPBULKIN=2 +CONFIG_CDCACM_EPBULKIN_FSSIZE=64 +CONFIG_CDCACM_EPBULKIN_HSSIZE=512 +CONFIG_CDCACM_NWRREQS=4 +CONFIG_CDCACM_NRDREQS=4 +CONFIG_CDCACM_RXBUFSIZE=256 +CONFIG_CDCACM_TXBUFSIZE=256 +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_PRODUCTID=0x0010 +CONFIG_CDCACM_VENDORSTR="3D Robotics" +CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v1.x" +# CONFIG_USBMSC is not set +# CONFIG_USBHOST is not set +# CONFIG_WIRELESS is not set + +# +# System Logging Device Options +# + +# +# System Logging +# +# CONFIG_RAMLOG is not set + +# +# Networking Support +# +# CONFIG_NET is not set + +# +# File Systems +# + +# +# File system configuration +# +# CONFIG_DISABLE_MOUNTPOINT is not set +# CONFIG_FS_RAMMAP is not set +CONFIG_FS_FAT=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_MAXFNAME=32 +CONFIG_FS_FATTIME=y +# CONFIG_FAT_DMAMEMORY is not set +CONFIG_FS_NXFFS=y +CONFIG_NXFFS_PREALLOCATED=y +CONFIG_NXFFS_ERASEDSTATE=0xff +CONFIG_NXFFS_PACKTHRESHOLD=32 +CONFIG_NXFFS_MAXNAMLEN=32 +CONFIG_NXFFS_TAILTHRESHOLD=2048 +CONFIG_FS_ROMFS=y +# CONFIG_FS_SMARTFS is not set +CONFIG_FS_BINFS=y + +# +# System Logging +# +# CONFIG_SYSLOG_ENABLE is not set +CONFIG_SYSLOG=y +CONFIG_SYSLOG_CHAR=y +CONFIG_SYSLOG_DEVPATH="/dev/ttyS0" + +# +# Graphics Support +# +# CONFIG_NX is not set + +# +# Memory Management +# +# CONFIG_MM_MULTIHEAP is not set +# CONFIG_MM_SMALL is not set +CONFIG_MM_REGIONS=2 +CONFIG_GRAN=y +CONFIG_GRAN_SINGLE=y +CONFIG_GRAN_INTR=y + +# +# Audio Support +# +# CONFIG_AUDIO is not set + +# +# Binary Formats +# +# CONFIG_BINFMT_DISABLE is not set +# CONFIG_BINFMT_EXEPATH is not set +# CONFIG_NXFLAT is not set +# CONFIG_ELF is not set +CONFIG_BUILTIN=y +# CONFIG_PIC is not set +# CONFIG_SYMTAB_ORDEREDBYNAME is not set + +# +# Library Routines +# + +# +# Standard C Library Options +# +CONFIG_STDIO_BUFFER_SIZE=256 +CONFIG_STDIO_LINEBUFFER=y +CONFIG_NUNGET_CHARS=2 +CONFIG_LIB_HOMEDIR="/" +# CONFIG_NOPRINTF_FIELDWIDTH is not set +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIB_RAND_ORDER=1 +# CONFIG_EOL_IS_CR is not set +# CONFIG_EOL_IS_LF is not set +# CONFIG_EOL_IS_BOTH_CRLF is not set +CONFIG_EOL_IS_EITHER_CRLF=y +# CONFIG_LIBC_EXECFUNCS is not set +CONFIG_POSIX_SPAWN_PROXY_STACKSIZE=1024 +CONFIG_TASK_SPAWN_DEFAULT_STACKSIZE=2048 +CONFIG_LIBC_STRERROR=y +# CONFIG_LIBC_STRERROR_SHORT is not set +# CONFIG_LIBC_PERROR_STDOUT is not set +CONFIG_ARCH_LOWPUTC=y +CONFIG_LIB_SENDFILE_BUFSIZE=512 +# CONFIG_ARCH_ROMGETC is not set +CONFIG_ARCH_OPTIMIZED_FUNCTIONS=y +CONFIG_ARCH_MEMCPY=y +# CONFIG_ARCH_MEMCMP is not set +# CONFIG_ARCH_MEMMOVE is not set +# CONFIG_ARCH_MEMSET is not set +# CONFIG_MEMSET_OPTSPEED is not set +# CONFIG_ARCH_STRCHR is not set +# CONFIG_ARCH_STRCMP is not set +# CONFIG_ARCH_STRCPY is not set +# CONFIG_ARCH_STRNCPY is not set +# CONFIG_ARCH_STRLEN is not set +# CONFIG_ARCH_STRNLEN is not set +# CONFIG_ARCH_BZERO is not set + +# +# Non-standard Library Support +# +CONFIG_SCHED_WORKQUEUE=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_WORKPRIORITY=192 +CONFIG_SCHED_WORKPERIOD=5000 +CONFIG_SCHED_WORKSTACKSIZE=2048 +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKPERIOD=50000 +CONFIG_SCHED_LPWORKSTACKSIZE=2048 +# CONFIG_LIB_KBDCODEC is not set +# CONFIG_LIB_SLCDCODEC is not set + +# +# Basic CXX Support +# +# CONFIG_C99_BOOL8 is not set +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +# CONFIG_CXX_NEWLONG is not set + +# +# uClibc++ Standard C++ Library +# +# CONFIG_UCLIBCXX is not set + +# +# Application Configuration +# + +# +# Built-In Applications +# +CONFIG_BUILTIN_PROXY_STACKSIZE=1024 + +# +# Examples +# +# CONFIG_EXAMPLES_BUTTONS is not set +# CONFIG_EXAMPLES_CAN is not set +CONFIG_EXAMPLES_CDCACM=y +# CONFIG_EXAMPLES_COMPOSITE is not set +# CONFIG_EXAMPLES_CXXTEST is not set +# CONFIG_EXAMPLES_DHCPD is not set +# CONFIG_EXAMPLES_ELF is not set +# CONFIG_EXAMPLES_FTPC is not set +# CONFIG_EXAMPLES_FTPD is not set +# CONFIG_EXAMPLES_HELLO is not set +# CONFIG_EXAMPLES_HELLOXX is not set +# CONFIG_EXAMPLES_JSON is not set +# CONFIG_EXAMPLES_HIDKBD is not set +# CONFIG_EXAMPLES_KEYPADTEST is not set +# CONFIG_EXAMPLES_IGMP is not set +# CONFIG_EXAMPLES_LCDRW is not set +# CONFIG_EXAMPLES_MM is not set +# CONFIG_EXAMPLES_MODBUS is not set +CONFIG_EXAMPLES_MOUNT=y +CONFIG_EXAMPLES_NSH=y +# CONFIG_EXAMPLES_NULL is not set +# CONFIG_EXAMPLES_NX is not set +# CONFIG_EXAMPLES_NXCONSOLE is not set +# CONFIG_EXAMPLES_NXFFS is not set +# CONFIG_EXAMPLES_NXFLAT is not set +# CONFIG_EXAMPLES_NXHELLO is not set +# CONFIG_EXAMPLES_NXIMAGE is not set +# CONFIG_EXAMPLES_NXLINES is not set +# CONFIG_EXAMPLES_NXTEXT is not set +# CONFIG_EXAMPLES_OSTEST is not set +# CONFIG_EXAMPLES_PASHELLO is not set +# CONFIG_EXAMPLES_PIPE is not set +# CONFIG_EXAMPLES_POSIXSPAWN is not set +# CONFIG_EXAMPLES_QENCODER is not set +# CONFIG_EXAMPLES_RGMP is not set +# CONFIG_EXAMPLES_ROMFS is not set +# CONFIG_EXAMPLES_SENDMAIL is not set +# CONFIG_EXAMPLES_SERLOOP is not set +# CONFIG_EXAMPLES_SLCD is not set +# CONFIG_EXAMPLES_SMART_TEST is not set +# CONFIG_EXAMPLES_SMART is not set +# CONFIG_EXAMPLES_TCPECHO is not set +# CONFIG_EXAMPLES_TELNETD is not set +# CONFIG_EXAMPLES_THTTPD is not set +# CONFIG_EXAMPLES_TIFF is not set +# CONFIG_EXAMPLES_TOUCHSCREEN is not set +# CONFIG_EXAMPLES_UDP is not set +# CONFIG_EXAMPLES_UIP is not set +# CONFIG_EXAMPLES_USBSERIAL is not set +# CONFIG_EXAMPLES_USBMSC is not set +# CONFIG_EXAMPLES_USBTERM is not set +# CONFIG_EXAMPLES_WATCHDOG is not set + +# +# Graphics Support +# +# CONFIG_TIFF is not set + +# +# Interpreters +# +# CONFIG_INTERPRETERS_FICL is not set +# CONFIG_INTERPRETERS_PCODE is not set + +# +# Network Utilities +# + +# +# Networking Utilities +# +# CONFIG_NETUTILS_CODECS is not set +# CONFIG_NETUTILS_DHCPC is not set +# CONFIG_NETUTILS_DHCPD is not set +# CONFIG_NETUTILS_FTPC is not set +# CONFIG_NETUTILS_FTPD is not set +# CONFIG_NETUTILS_JSON is not set +# CONFIG_NETUTILS_RESOLV is not set +# CONFIG_NETUTILS_SMTP is not set +# CONFIG_NETUTILS_TELNETD is not set +# CONFIG_NETUTILS_TFTPC is not set +# CONFIG_NETUTILS_THTTPD is not set +# CONFIG_NETUTILS_UIPLIB is not set +# CONFIG_NETUTILS_WEBCLIENT is not set + +# +# FreeModBus +# +# CONFIG_MODBUS is not set + +# +# NSH Library +# +CONFIG_NSH_LIBRARY=y +CONFIG_NSH_BUILTIN_APPS=y + +# +# Disable Individual commands +# +# CONFIG_NSH_DISABLE_CAT is not set +# CONFIG_NSH_DISABLE_CD is not set +# CONFIG_NSH_DISABLE_CP is not set +# CONFIG_NSH_DISABLE_DD is not set +# CONFIG_NSH_DISABLE_ECHO is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_FREE is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_HELP is not set +# CONFIG_NSH_DISABLE_HEXDUMP is not set +# CONFIG_NSH_DISABLE_IFCONFIG is not set +# CONFIG_NSH_DISABLE_KILL is not set +# CONFIG_NSH_DISABLE_LOSETUP is not set +# CONFIG_NSH_DISABLE_LS is not set +# CONFIG_NSH_DISABLE_MB is not set +# CONFIG_NSH_DISABLE_MKDIR is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_MKFIFO is not set +# CONFIG_NSH_DISABLE_MKRD is not set +# CONFIG_NSH_DISABLE_MH is not set +# CONFIG_NSH_DISABLE_MOUNT is not set +# CONFIG_NSH_DISABLE_MW is not set +# CONFIG_NSH_DISABLE_NSFMOUNT is not set +# CONFIG_NSH_DISABLE_PS is not set +# CONFIG_NSH_DISABLE_PING is not set +# CONFIG_NSH_DISABLE_PUT is not set +# CONFIG_NSH_DISABLE_PWD is not set +# CONFIG_NSH_DISABLE_RM is not set +# CONFIG_NSH_DISABLE_RMDIR is not set +# CONFIG_NSH_DISABLE_SET is not set +# CONFIG_NSH_DISABLE_SH is not set +# CONFIG_NSH_DISABLE_SLEEP is not set +# CONFIG_NSH_DISABLE_TEST is not set +# CONFIG_NSH_DISABLE_UMOUNT is not set +# CONFIG_NSH_DISABLE_UNSET is not set +# CONFIG_NSH_DISABLE_USLEEP is not set +# CONFIG_NSH_DISABLE_WGET is not set +# CONFIG_NSH_DISABLE_XD is not set + +# +# Configure Command Options +# +# CONFIG_NSH_CMDOPT_DF_H is not set +CONFIG_NSH_CODECS_BUFSIZE=128 +CONFIG_NSH_FILEIOSIZE=512 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=12 +CONFIG_NSH_NESTDEPTH=8 +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLEBG is not set +CONFIG_NSH_ROMFSETC=y +# CONFIG_NSH_ROMFSRC is not set +CONFIG_NSH_ROMFSMOUNTPT="/etc" +CONFIG_NSH_INITSCRIPT="init.d/rcS" +CONFIG_NSH_ROMFSDEVNO=0 +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_ARCHROMFS=y +CONFIG_NSH_FATDEVNO=1 +CONFIG_NSH_FATSECTSIZE=512 +CONFIG_NSH_FATNSECTORS=1024 +CONFIG_NSH_FATMOUNTPT="/tmp" +CONFIG_NSH_CONSOLE=y +# CONFIG_NSH_USBCONSOLE is not set + +# +# USB Trace Support +# +# CONFIG_NSH_CONDEV is not set +CONFIG_NSH_ARCHINIT=y + +# +# NxWidgets/NxWM +# + +# +# System NSH Add-Ons +# + +# +# Custom Free Memory Command +# +# CONFIG_SYSTEM_FREE is not set + +# +# I2C tool +# +# CONFIG_SYSTEM_I2CTOOL is not set + +# +# FLASH Program Installation +# +# CONFIG_SYSTEM_INSTALL is not set + +# +# FLASH Erase-all Command +# + +# +# readline() +# +CONFIG_SYSTEM_READLINE=y +CONFIG_READLINE_ECHO=y + +# +# Power Off +# +# CONFIG_SYSTEM_POWEROFF is not set + +# +# RAMTRON +# +# CONFIG_SYSTEM_RAMTRON is not set + +# +# SD Card +# +# CONFIG_SYSTEM_SDCARD is not set + +# +# Sysinfo +# +CONFIG_SYSTEM_SYSINFO=y + +# +# USB Monitor +# diff --git a/nuttx-configs/px4fmu-v1/nsh/setenv.sh b/nuttx-configs/px4fmu-v1/nsh/setenv.sh new file mode 100755 index 0000000000..db372217cd --- /dev/null +++ b/nuttx-configs/px4fmu-v1/nsh/setenv.sh @@ -0,0 +1,75 @@ +#!/bin/bash +# configs/px4fmu-v1/usbnsh/setenv.sh +# +# Copyright (C) 2013 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# + +if [ "$_" = "$0" ] ; then + echo "You must source this script, not run it!" 1>&2 + exit 1 +fi + +WD=`pwd` +if [ ! -x "setenv.sh" ]; then + echo "This script must be executed from the top-level NuttX build directory" + exit 1 +fi + +if [ -z "${PATH_ORIG}" ]; then + export PATH_ORIG="${PATH}" +fi + +# This is the Cygwin path to the location where I installed the RIDE +# toolchain under windows. You will also have to edit this if you install +# the RIDE toolchain in any other location +#export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/Raisonance/Ride/arm-gcc/bin" + +# This is the Cygwin path to the location where I installed the CodeSourcery +# toolchain under windows. You will also have to edit this if you install +# the CodeSourcery toolchain in any other location +export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/CodeSourcery/Sourcery G++ Lite/bin" + +# These are the Cygwin paths to the locations where I installed the Atollic +# toolchain under windows. You will also have to edit this if you install +# the Atollic toolchain in any other location. /usr/bin is added before +# the Atollic bin path because there is are binaries named gcc.exe and g++.exe +# at those locations as well. +#export TOOLCHAIN_BIN="/usr/bin:/cygdrive/c/Program Files (x86)/Atollic/TrueSTUDIO for ARM Pro 2.3.0/ARMTools/bin" +#export TOOLCHAIN_BIN="/usr/bin:/cygdrive/c/Program Files (x86)/Atollic/TrueSTUDIO for STMicroelectronics STM32 Lite 2.3.0/ARMTools/bin" + +# This is the Cygwin path to the location where I build the buildroot +# toolchain. +#export TOOLCHAIN_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin" + +# Add the path to the toolchain to the PATH varialble +export PATH="${TOOLCHAIN_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" + +echo "PATH : ${PATH}" diff --git a/nuttx-configs/px4fmu-v1/ostest/Make.defs b/nuttx-configs/px4fmu-v1/ostest/Make.defs new file mode 100644 index 0000000000..7b807abdbf --- /dev/null +++ b/nuttx-configs/px4fmu-v1/ostest/Make.defs @@ -0,0 +1,122 @@ +############################################################################ +# configs/stm32f4discovery/ostest/Make.defs +# +# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# +############################################################################ + +include ${TOPDIR}/.config +include ${TOPDIR}/tools/Config.mk +include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs + +LDSCRIPT = ld.script + +ifeq ($(WINTOOL),y) + # Windows-native toolchains + ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" + ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" + ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)}" + MAXOPTIMIZATION = -O2 +else + # Linux/Cygwin-native toolchain + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) +endif + +CC = $(CROSSDEV)gcc +CXX = $(CROSSDEV)g++ +CPP = $(CROSSDEV)gcc -E +LD = $(CROSSDEV)ld +AR = $(ARCROSSDEV)ar rcs +NM = $(ARCROSSDEV)nm +OBJCOPY = $(CROSSDEV)objcopy +OBJDUMP = $(CROSSDEV)objdump + +ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} +ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} + +ifeq ($(CONFIG_DEBUG_SYMBOLS),y) + ARCHOPTIMIZATION = -g +else + ARCHOPTIMIZATION = $(MAXOPTIMIZATION) -fno-strict-aliasing -fno-strength-reduce -fomit-frame-pointer +endif + +ARCHCFLAGS = -fno-builtin +ARCHCXXFLAGS = -fno-builtin -fno-exceptions -fno-rtti +ARCHWARNINGS = -Wall -Wstrict-prototypes -Wshadow +ARCHWARNINGSXX = -Wall -Wshadow +ARCHDEFINES = +ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 + +CFLAGS = $(ARCHCFLAGS) $(ARCHWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe +CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) +CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe +CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) +CPPFLAGS = $(ARCHINCLUDES) $(ARCHDEFINES) $(EXTRADEFINES) +AFLAGS = $(CFLAGS) -D__ASSEMBLY__ + +NXFLATLDFLAGS1 = -r -d -warn-common +NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat-pcrel.ld -no-check-sections +LDNXFLATFLAGS = -e main -s 2048 + +OBJEXT = .o +LIBEXT = .a +EXEEXT = + +ifneq ($(CROSSDEV),arm-nuttx-elf-) + LDFLAGS += -nostartfiles -nodefaultlibs +endif +ifeq ($(CONFIG_DEBUG_SYMBOLS),y) + LDFLAGS += -g +endif + + +HOSTCC = gcc +HOSTINCLUDES = -I. +HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe +HOSTLDFLAGS = +ifeq ($(CONFIG_HOST_WINDOWS),y) + HOSTEXEEXT = .exe +else + HOSTEXEEXT = +endif + +ifeq ($(WINTOOL),y) + # Windows-native host tools + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh +else + # Linux/Cygwin-native host tools + MKDEP = $(TOPDIR)/tools/mkdeps$(HOSTEXEEXT) +endif + diff --git a/nuttx-configs/px4fmu-v1/ostest/defconfig b/nuttx-configs/px4fmu-v1/ostest/defconfig new file mode 100644 index 0000000000..c7fb6b2a5a --- /dev/null +++ b/nuttx-configs/px4fmu-v1/ostest/defconfig @@ -0,0 +1,583 @@ +# +# Automatically generated file; DO NOT EDIT. +# Nuttx/ Configuration +# +CONFIG_NUTTX_NEWCONFIG=y + +# +# Build Setup +# +# CONFIG_EXPERIMENTAL is not set +# CONFIG_HOST_LINUX is not set +# CONFIG_HOST_OSX is not set +CONFIG_HOST_WINDOWS=y +# CONFIG_HOST_OTHER is not set +# CONFIG_WINDOWS_NATIVE is not set +CONFIG_WINDOWS_CYGWIN=y +# CONFIG_WINDOWS_MSYS is not set +# CONFIG_WINDOWS_OTHER is not set +# CONFIG_WINDOWS_MKLINK is not set + +# +# Build Configuration +# +# CONFIG_APPS_DIR="../apps" +# CONFIG_BUILD_2PASS is not set + +# +# Binary Output Formats +# +# CONFIG_RRLOAD_BINARY is not set +CONFIG_INTELHEX_BINARY=y +# CONFIG_MOTOROLA_SREC is not set +CONFIG_RAW_BINARY=y + +# +# Customize Header Files +# +# CONFIG_ARCH_STDBOOL_H is not set +# CONFIG_ARCH_MATH_H is not set +# CONFIG_ARCH_FLOAT_H is not set +# CONFIG_ARCH_STDARG_H is not set + +# +# Debug Options +# +# CONFIG_DEBUG is not set +# CONFIG_DEBUG_SYMBOLS is not set + +# +# System Type +# +# CONFIG_ARCH_8051 is not set +CONFIG_ARCH_ARM=y +# CONFIG_ARCH_AVR is not set +# CONFIG_ARCH_HC is not set +# CONFIG_ARCH_MIPS is not set +# CONFIG_ARCH_RGMP is not set +# CONFIG_ARCH_SH is not set +# CONFIG_ARCH_SIM is not set +# CONFIG_ARCH_X86 is not set +# CONFIG_ARCH_Z16 is not set +# CONFIG_ARCH_Z80 is not set +CONFIG_ARCH="arm" + +# +# ARM Options +# +# CONFIG_ARCH_CHIP_C5471 is not set +# CONFIG_ARCH_CHIP_CALYPSO is not set +# CONFIG_ARCH_CHIP_DM320 is not set +# CONFIG_ARCH_CHIP_IMX is not set +# CONFIG_ARCH_CHIP_KINETIS is not set +# CONFIG_ARCH_CHIP_LM3S is not set +# CONFIG_ARCH_CHIP_LPC17XX is not set +# CONFIG_ARCH_CHIP_LPC214X is not set +# CONFIG_ARCH_CHIP_LPC2378 is not set +# CONFIG_ARCH_CHIP_LPC31XX is not set +# CONFIG_ARCH_CHIP_LPC43XX is not set +# CONFIG_ARCH_CHIP_SAM3U is not set +CONFIG_ARCH_CHIP_STM32=y +# CONFIG_ARCH_CHIP_STR71X is not set +CONFIG_ARCH_CORTEXM4=y +CONFIG_ARCH_FAMILY="armv7-m" +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARCH_HAVE_CMNVECTOR=y +# CONFIG_ARMV7M_CMNVECTOR is not set +# CONFIG_ARCH_FPU is not set +CONFIG_ARCH_HAVE_MPU=y +# CONFIG_ARMV7M_MPU is not set +CONFIG_ARCH_IRQPRIO=y +CONFIG_BOARD_LOOPSPERMSEC=16717 +# CONFIG_ARCH_CALIBRATION is not set +# CONFIG_SERIAL_TERMIOS is not set + +# +# STM32 Configuration Options +# +# CONFIG_ARCH_CHIP_STM32F100C8 is not set +# CONFIG_ARCH_CHIP_STM32F100CB is not set +# CONFIG_ARCH_CHIP_STM32F100R8 is not set +# CONFIG_ARCH_CHIP_STM32F100RB is not set +# CONFIG_ARCH_CHIP_STM32F100RC is not set +# CONFIG_ARCH_CHIP_STM32F100RD is not set +# CONFIG_ARCH_CHIP_STM32F100RE is not set +# CONFIG_ARCH_CHIP_STM32F100V8 is not set +# CONFIG_ARCH_CHIP_STM32F100VB is not set +# CONFIG_ARCH_CHIP_STM32F100VC is not set +# CONFIG_ARCH_CHIP_STM32F100VD is not set +# CONFIG_ARCH_CHIP_STM32F100VE is not set +# CONFIG_ARCH_CHIP_STM32F103RET6 is not set +# CONFIG_ARCH_CHIP_STM32F103VCT6 is not set +# CONFIG_ARCH_CHIP_STM32F103VET6 is not set +# CONFIG_ARCH_CHIP_STM32F103ZET6 is not set +# CONFIG_ARCH_CHIP_STM32F105VBT7 is not set +# CONFIG_ARCH_CHIP_STM32F107VC is not set +# CONFIG_ARCH_CHIP_STM32F207IG is not set +# CONFIG_ARCH_CHIP_STM32F405RG is not set +# CONFIG_ARCH_CHIP_STM32F405VG is not set +# CONFIG_ARCH_CHIP_STM32F405ZG is not set +# CONFIG_ARCH_CHIP_STM32F407VE is not set +CONFIG_ARCH_CHIP_STM32F407VG=y +# CONFIG_ARCH_CHIP_STM32F407ZE is not set +# CONFIG_ARCH_CHIP_STM32F407ZG is not set +# CONFIG_ARCH_CHIP_STM32F407IE is not set +# CONFIG_ARCH_CHIP_STM32F407IG is not set +CONFIG_STM32_STM32F40XX=y +# CONFIG_STM32_CODESOURCERYW is not set +CONFIG_STM32_CODESOURCERYL=y +# CONFIG_STM32_ATOLLIC_LITE is not set +# CONFIG_STM32_ATOLLIC_PRO is not set +# CONFIG_STM32_DEVKITARM is not set +# CONFIG_STM32_RAISONANCE is not set +# CONFIG_STM32_BUILDROOT is not set +# CONFIG_STM32_DFU is not set + +# +# STM32 Peripheral Support +# +# CONFIG_STM32_ADC1 is not set +# CONFIG_STM32_ADC2 is not set +# CONFIG_STM32_ADC3 is not set +# CONFIG_STM32_BKPSRAM is not set +# CONFIG_STM32_CAN1 is not set +# CONFIG_STM32_CAN2 is not set +# CONFIG_STM32_CCMDATARAM is not set +# CONFIG_STM32_CRC is not set +# CONFIG_STM32_CRYP is not set +# CONFIG_STM32_DMA1 is not set +# CONFIG_STM32_DMA2 is not set +# CONFIG_STM32_DAC1 is not set +# CONFIG_STM32_DAC2 is not set +# CONFIG_STM32_DCMI is not set +# CONFIG_STM32_ETHMAC is not set +# CONFIG_STM32_FSMC is not set +# CONFIG_STM32_HASH is not set +# CONFIG_STM32_I2C1 is not set +# CONFIG_STM32_I2C2 is not set +# CONFIG_STM32_I2C3 is not set +# CONFIG_STM32_IWDG is not set +# CONFIG_STM32_OTGFS is not set +# CONFIG_STM32_OTGHS is not set +# CONFIG_STM32_PWR is not set +# CONFIG_STM32_RNG is not set +# CONFIG_STM32_SDIO is not set +# CONFIG_STM32_SPI1 is not set +# CONFIG_STM32_SPI2 is not set +# CONFIG_STM32_SPI3 is not set +CONFIG_STM32_SYSCFG=y +# CONFIG_STM32_TIM1 is not set +# CONFIG_STM32_TIM2 is not set +# CONFIG_STM32_TIM3 is not set +# CONFIG_STM32_TIM4 is not set +# CONFIG_STM32_TIM5 is not set +# CONFIG_STM32_TIM6 is not set +# CONFIG_STM32_TIM7 is not set +# CONFIG_STM32_TIM8 is not set +# CONFIG_STM32_TIM9 is not set +# CONFIG_STM32_TIM10 is not set +# CONFIG_STM32_TIM11 is not set +# CONFIG_STM32_TIM12 is not set +# CONFIG_STM32_TIM13 is not set +# CONFIG_STM32_TIM14 is not set +# CONFIG_STM32_USART1 is not set +CONFIG_STM32_USART2=y +# CONFIG_STM32_USART3 is not set +# CONFIG_STM32_UART4 is not set +# CONFIG_STM32_UART5 is not set +# CONFIG_STM32_USART6 is not set +# CONFIG_STM32_WWDG is not set + +# +# Alternate Pin Mapping +# +# CONFIG_STM32_JTAG_DISABLE is not set +# CONFIG_STM32_JTAG_FULL_ENABLE is not set +# CONFIG_STM32_JTAG_NOJNTRST_ENABLE is not set +CONFIG_STM32_JTAG_SW_ENABLE=y +# CONFIG_STM32_FORCEPOWER is not set +# CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set +# CONFIG_STM32_CCMEXCLUDE is not set + +# +# USB Host Configuration +# + +# +# Architecture Options +# +# CONFIG_ARCH_NOINTC is not set +# CONFIG_ARCH_DMA is not set +CONFIG_ARCH_STACKDUMP=y + +# +# Board Settings +# +CONFIG_DRAM_START=0x20000000 +CONFIG_DRAM_SIZE=114688 +CONFIG_ARCH_HAVE_INTERRUPTSTACK=y +CONFIG_ARCH_INTERRUPTSTACK=0 + +# +# Boot options +# +# CONFIG_BOOT_RUNFROMEXTSRAM is not set +CONFIG_BOOT_RUNFROMFLASH=y +# CONFIG_BOOT_RUNFROMISRAM is not set +# CONFIG_BOOT_RUNFROMSDRAM is not set +# CONFIG_BOOT_COPYTORAM is not set + +# +# Board Selection +# +CONFIG_ARCH_BOARD_STM32F4_DISCOVERY=y +# CONFIG_ARCH_BOARD_CUSTOM is not set +CONFIG_ARCH_BOARD="stm32f4discovery" + +# +# Common Board Options +# +CONFIG_ARCH_HAVE_LEDS=y +CONFIG_ARCH_LEDS=y +CONFIG_ARCH_HAVE_BUTTONS=y +# CONFIG_ARCH_BUTTONS is not set +CONFIG_ARCH_HAVE_IRQBUTTONS=y + +# +# Board-Specific Options +# + +# +# RTOS Features +# +CONFIG_MSEC_PER_TICK=10 +CONFIG_RR_INTERVAL=200 +# CONFIG_SCHED_INSTRUMENTATION is not set +CONFIG_TASK_NAME_SIZE=0 +# CONFIG_JULIAN_TIME is not set +CONFIG_START_YEAR=2009 +CONFIG_START_MONTH=9 +CONFIG_START_DAY=21 +CONFIG_DEV_CONSOLE=y +# CONFIG_MUTEX_TYPES is not set +# CONFIG_PRIORITY_INHERITANCE is not set +# CONFIG_FDCLONE_DISABLE is not set +# CONFIG_FDCLONE_STDIO is not set +CONFIG_SDCLONE_DISABLE=y +# CONFIG_SCHED_WORKQUEUE is not set +# CONFIG_SCHED_WAITPID is not set +# CONFIG_SCHED_ATEXIT is not set +# CONFIG_SCHED_ONEXIT is not set +CONFIG_USER_ENTRYPOINT="ostest_main" +CONFIG_DISABLE_OS_API=y +# CONFIG_DISABLE_CLOCK is not set +# CONFIG_DISABLE_POSIX_TIMERS is not set +# CONFIG_DISABLE_PTHREAD is not set +# CONFIG_DISABLE_SIGNALS is not set +# CONFIG_DISABLE_MQUEUE is not set +CONFIG_DISABLE_MOUNTPOINT=y +CONFIG_DISABLE_ENVIRON=y +CONFIG_DISABLE_POLL=y + +# +# Sizes of configurable things (0 disables) +# +CONFIG_MAX_TASKS=16 +CONFIG_MAX_TASK_ARGS=4 +CONFIG_NPTHREAD_KEYS=4 +CONFIG_NFILE_DESCRIPTORS=8 +CONFIG_NFILE_STREAMS=8 +CONFIG_NAME_MAX=32 +CONFIG_PREALLOC_MQ_MSGS=4 +CONFIG_MQ_MAXMSGSIZE=32 +CONFIG_MAX_WDOGPARMS=2 +CONFIG_PREALLOC_WDOGS=4 +CONFIG_PREALLOC_TIMERS=4 + +# +# Stack and heap information +# +# CONFIG_CUSTOM_STACK is not set +CONFIG_IDLETHREAD_STACKSIZE=1024 +CONFIG_USERMAIN_STACKSIZE=2048 +CONFIG_PTHREAD_STACK_MIN=256 +CONFIG_PTHREAD_STACK_DEFAULT=2048 + +# +# Device Drivers +# +CONFIG_DEV_NULL=y +# CONFIG_DEV_ZERO is not set +# CONFIG_LOOP is not set +# CONFIG_RAMDISK is not set +# CONFIG_CAN is not set +# CONFIG_PWM is not set +# CONFIG_I2C is not set +CONFIG_ARCH_HAVE_I2CRESET=y +# CONFIG_SPI is not set +# CONFIG_RTC is not set +# CONFIG_WATCHDOG is not set +# CONFIG_ANALOG is not set +# CONFIG_BCH is not set +# CONFIG_INPUT is not set +# CONFIG_LCD is not set +# CONFIG_MMCSD is not set +# CONFIG_MTD is not set +# CONFIG_PIPES is not set +# CONFIG_PM is not set +# CONFIG_POWER is not set +# CONFIG_SENSORS is not set +# CONFIG_SERCOMM_CONSOLE is not set +CONFIG_SERIAL=y +CONFIG_DEV_LOWCONSOLE=y +# CONFIG_16550_UART is not set +CONFIG_ARCH_HAVE_USART2=y +CONFIG_MCU_SERIAL=y +CONFIG_STANDARD_SERIAL=y +CONFIG_USART2_SERIAL_CONSOLE=y +# CONFIG_NO_SERIAL_CONSOLE is not set + +# +# USART2 Configuration +# +CONFIG_USART2_RXBUFSIZE=128 +CONFIG_USART2_TXBUFSIZE=128 +CONFIG_USART2_BAUD=115200 +CONFIG_USART2_BITS=8 +CONFIG_USART2_PARITY=0 +CONFIG_USART2_2STOP=0 +# CONFIG_USBDEV is not set +# CONFIG_USBHOST is not set +# CONFIG_WIRELESS is not set + +# +# System Logging Device Options +# + +# +# System Logging +# +# CONFIG_RAMLOG is not set + +# +# Networking Support +# +# CONFIG_NET is not set + +# +# File Systems +# + +# +# File system configuration +# +# CONFIG_FS_RAMMAP is not set + +# +# System Logging +# +# CONFIG_SYSLOG is not set + +# +# Graphics Support +# +# CONFIG_NX is not set + +# +# Memory Management +# +# CONFIG_MM_SMALL is not set +CONFIG_MM_REGIONS=2 +# CONFIG_GRAN is not set + +# +# Binary Formats +# +# CONFIG_BINFMT_DISABLE is not set +# CONFIG_NXFLAT is not set +# CONFIG_ELF is not set +CONFIG_SYMTAB_ORDEREDBYNAME=y + +# +# Library Routines +# +CONFIG_STDIO_BUFFER_SIZE=64 +CONFIG_STDIO_LINEBUFFER=y +CONFIG_NUNGET_CHARS=2 +# CONFIG_LIBM is not set +# CONFIG_NOPRINTF_FIELDWIDTH is not set +# CONFIG_LIBC_FLOATINGPOINT is not set +# CONFIG_EOL_IS_CR is not set +# CONFIG_EOL_IS_LF is not set +# CONFIG_EOL_IS_BOTH_CRLF is not set +CONFIG_EOL_IS_EITHER_CRLF=y +# CONFIG_LIBC_STRERROR is not set +# CONFIG_LIBC_PERROR_STDOUT is not set +CONFIG_ARCH_LOWPUTC=y +CONFIG_LIB_SENDFILE_BUFSIZE=512 +# CONFIG_ARCH_ROMGETC is not set +# CONFIG_ARCH_OPTIMIZED_FUNCTIONS is not set + +# +# Basic CXX Support +# +# CONFIG_HAVE_CXX is not set + +# +# Application Configuration +# + +# +# Named Applications +# +# CONFIG_BUILTIN is not set + +# +# Examples +# +# CONFIG_EXAMPLES_BUTTONS is not set +# CONFIG_EXAMPLES_CAN is not set +# CONFIG_EXAMPLES_CDCACM is not set +# CONFIG_EXAMPLES_COMPOSITE is not set +# CONFIG_EXAMPLES_DHCPD is not set +# CONFIG_EXAMPLES_ELF is not set +# CONFIG_EXAMPLES_FTPC is not set +# CONFIG_EXAMPLES_FTPD is not set +# CONFIG_EXAMPLES_HELLO is not set +# CONFIG_EXAMPLES_HELLOXX is not set +# CONFIG_EXAMPLES_JSON is not set +# CONFIG_EXAMPLES_HIDKBD is not set +# CONFIG_EXAMPLES_IGMP is not set +# CONFIG_EXAMPLES_LCDRW is not set +# CONFIG_EXAMPLES_MM is not set +# CONFIG_EXAMPLES_MOUNT is not set +# CONFIG_EXAMPLES_MODBUS is not set +# CONFIG_EXAMPLES_NETTEST is not set +# CONFIG_EXAMPLES_NSH is not set +# CONFIG_EXAMPLES_NULL is not set +# CONFIG_EXAMPLES_NX is not set +# CONFIG_EXAMPLES_NXCONSOLE is not set +# CONFIG_EXAMPLES_NXFFS is not set +# CONFIG_EXAMPLES_NXFLAT is not set +# CONFIG_EXAMPLES_NXHELLO is not set +# CONFIG_EXAMPLES_NXIMAGE is not set +# CONFIG_EXAMPLES_NXLINES is not set +# CONFIG_EXAMPLES_NXTEXT is not set +CONFIG_EXAMPLES_OSTEST=y +# CONFIG_EXAMPLES_OSTEST_BUILTIN is not set +CONFIG_EXAMPLES_OSTEST_LOOPS=1 +CONFIG_EXAMPLES_OSTEST_STACKSIZE=2048 +CONFIG_EXAMPLES_OSTEST_NBARRIER_THREADS=3 +CONFIG_EXAMPLES_OSTEST_RR_RANGE=10000 +CONFIG_EXAMPLES_OSTEST_RR_RUNS=10 +# CONFIG_EXAMPLES_PASHELLO is not set +# CONFIG_EXAMPLES_PIPE is not set +# CONFIG_EXAMPLES_POLL is not set +# CONFIG_EXAMPLES_QENCODER is not set +# CONFIG_EXAMPLES_RGMP is not set +# CONFIG_EXAMPLES_ROMFS is not set +# CONFIG_EXAMPLES_SENDMAIL is not set +# CONFIG_EXAMPLES_SERLOOP is not set +# CONFIG_EXAMPLES_TELNETD is not set +# CONFIG_EXAMPLES_THTTPD is not set +# CONFIG_EXAMPLES_TIFF is not set +# CONFIG_EXAMPLES_TOUCHSCREEN is not set +# CONFIG_EXAMPLES_UDP is not set +# CONFIG_EXAMPLES_UIP is not set +# CONFIG_EXAMPLES_USBSERIAL is not set +# CONFIG_EXAMPLES_USBMSC is not set +# CONFIG_EXAMPLES_USBTERM is not set +# CONFIG_EXAMPLES_WATCHDOG is not set +# CONFIG_EXAMPLES_WLAN is not set + +# +# Interpreters +# + +# +# Interpreters +# +# CONFIG_INTERPRETERS_FICL is not set +# CONFIG_INTERPRETERS_PCODE is not set + +# +# Network Utilities +# + +# +# Networking Utilities +# +# CONFIG_NETUTILS_CODECS is not set +# CONFIG_NETUTILS_DHCPC is not set +# CONFIG_NETUTILS_DHCPD is not set +# CONFIG_NETUTILS_FTPC is not set +# CONFIG_NETUTILS_FTPD is not set +# CONFIG_NETUTILS_JSON is not set +# CONFIG_NETUTILS_RESOLV is not set +# CONFIG_NETUTILS_SMTP is not set +# CONFIG_NETUTILS_TELNETD is not set +# CONFIG_NETUTILS_TFTPC is not set +# CONFIG_NETUTILS_THTTPD is not set +# CONFIG_NETUTILS_UIPLIB is not set +# CONFIG_NETUTILS_WEBCLIENT is not set + +# +# ModBus +# + +# +# FreeModbus +# +# CONFIG_MODBUS is not set + +# +# NSH Library +# +# CONFIG_NSH_LIBRARY is not set + +# +# NxWidgets/NxWM +# + +# +# System NSH Add-Ons +# + +# +# Custom Free Memory Command +# +# CONFIG_SYSTEM_FREE is not set + +# +# I2C tool +# + +# +# FLASH Program Installation +# +# CONFIG_SYSTEM_INSTALL is not set + +# +# readline() +# +# CONFIG_SYSTEM_READLINE is not set + +# +# Power Off +# +# CONFIG_SYSTEM_POWEROFF is not set + +# +# RAMTRON +# +# CONFIG_SYSTEM_RAMTRON is not set + +# +# SD Card +# +# CONFIG_SYSTEM_SDCARD is not set + +# +# Sysinfo +# +# CONFIG_SYSTEM_SYSINFO is not set diff --git a/nuttx-configs/px4fmu-v1/ostest/setenv.sh b/nuttx-configs/px4fmu-v1/ostest/setenv.sh new file mode 100755 index 0000000000..a67fdc5a8a --- /dev/null +++ b/nuttx-configs/px4fmu-v1/ostest/setenv.sh @@ -0,0 +1,75 @@ +#!/bin/bash +# configs/stm32f4discovery/ostest/setenv.sh +# +# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# + +if [ "$_" = "$0" ] ; then + echo "You must source this script, not run it!" 1>&2 + exit 1 +fi + +WD=`pwd` +if [ ! -x "setenv.sh" ]; then + echo "This script must be executed from the top-level NuttX build directory" + exit 1 +fi + +if [ -z "${PATH_ORIG}" ]; then + export PATH_ORIG="${PATH}" +fi + +# This is the Cygwin path to the location where I installed the RIDE +# toolchain under windows. You will also have to edit this if you install +# the RIDE toolchain in any other location +#export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/Raisonance/Ride/arm-gcc/bin" + +# This is the Cygwin path to the location where I installed the CodeSourcery +# toolchain under windows. You will also have to edit this if you install +# the CodeSourcery toolchain in any other location +export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/CodeSourcery/Sourcery G++ Lite/bin" + +# These are the Cygwin paths to the locations where I installed the Atollic +# toolchain under windows. You will also have to edit this if you install +# the Atollic toolchain in any other location. /usr/bin is added before +# the Atollic bin path because there is are binaries named gcc.exe and g++.exe +# at those locations as well. +#export TOOLCHAIN_BIN="/usr/bin:/cygdrive/c/Program Files (x86)/Atollic/TrueSTUDIO for ARM Pro 2.3.0/ARMTools/bin" +#export TOOLCHAIN_BIN="/usr/bin:/cygdrive/c/Program Files (x86)/Atollic/TrueSTUDIO for STMicroelectronics STM32 Lite 2.3.0/ARMTools/bin" + +# This is the Cygwin path to the location where I build the buildroot +# toolchain. +#export TOOLCHAIN_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin" + +# Add the path to the toolchain to the PATH varialble +export PATH="${TOOLCHAIN_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" + +echo "PATH : ${PATH}" diff --git a/nuttx-configs/px4fmu-v1/scripts/gnu-elf.ld b/nuttx-configs/px4fmu-v1/scripts/gnu-elf.ld new file mode 100644 index 0000000000..1f29f02f5b --- /dev/null +++ b/nuttx-configs/px4fmu-v1/scripts/gnu-elf.ld @@ -0,0 +1,129 @@ +/**************************************************************************** + * configs/stm32f4discovery/scripts/gnu-elf.ld + * + * Copyright (C) 2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ****************************************************************************/ + +SECTIONS +{ + .text 0x00000000 : + { + _stext = . ; + *(.text) + *(.text.*) + *(.gnu.warning) + *(.stub) + *(.glue_7) + *(.glue_7t) + *(.jcr) + + /* C++ support: The .init and .fini sections contain specific logic + * to manage static constructors and destructors. + */ + + *(.gnu.linkonce.t.*) + *(.init) /* Old ABI */ + *(.fini) /* Old ABI */ + _etext = . ; + } + + .rodata : + { + _srodata = . ; + *(.rodata) + *(.rodata1) + *(.rodata.*) + *(.gnu.linkonce.r*) + _erodata = . ; + } + + .data : + { + _sdata = . ; + *(.data) + *(.data1) + *(.data.*) + *(.gnu.linkonce.d*) + _edata = . ; + } + + /* C++ support. For each global and static local C++ object, + * GCC creates a small subroutine to construct the object. Pointers + * to these routines (not the routines themselves) are stored as + * simple, linear arrays in the .ctors section of the object file. + * Similarly, pointers to global/static destructor routines are + * stored in .dtors. + */ + + .ctors : + { + _sctors = . ; + *(.ctors) /* Old ABI: Unallocated */ + *(.init_array) /* New ABI: Allocated */ + _edtors = . ; + } + + .dtors : + { + _sdtors = . ; + *(.dtors) /* Old ABI: Unallocated */ + *(.fini_array) /* New ABI: Allocated */ + _edtors = . ; + } + + .bss : + { + _sbss = . ; + *(.bss) + *(.bss.*) + *(.sbss) + *(.sbss.*) + *(.gnu.linkonce.b*) + *(COMMON) + _ebss = . ; + } + + /* Stabs debugging sections. */ + + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/nuttx-configs/px4fmu-v1/scripts/ld.script b/nuttx-configs/px4fmu-v1/scripts/ld.script new file mode 100644 index 0000000000..f6560743bc --- /dev/null +++ b/nuttx-configs/px4fmu-v1/scripts/ld.script @@ -0,0 +1,123 @@ +/**************************************************************************** + * configs/stm32f4discovery/scripts/ld.script + * + * Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/* The STM32F405RG has 1024Kb of FLASH beginning at address 0x0800:0000 and + * 192Kb of SRAM. SRAM is split up into three blocks: + * + * 1) 112Kb of SRAM beginning at address 0x2000:0000 + * 2) 16Kb of SRAM beginning at address 0x2001:c000 + * 3) 64Kb of CCM SRAM beginning at address 0x1000:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address + * range. + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x08004000, LENGTH = 1008K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 112K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +ENTRY(_stext) +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + } > flash + + .init_section : { + _sinit = ABSOLUTE(.); + *(.init_array .init_array.*) + _einit = ABSOLUTE(.); + } > flash + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/nuttx-configs/px4fmu-v1/src/Makefile b/nuttx-configs/px4fmu-v1/src/Makefile new file mode 100644 index 0000000000..6ef8b7d6af --- /dev/null +++ b/nuttx-configs/px4fmu-v1/src/Makefile @@ -0,0 +1,84 @@ +############################################################################ +# configs/px4fmu-v1/src/Makefile +# +# Copyright (C) 2013 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# +############################################################################ + +-include $(TOPDIR)/Make.defs + +CFLAGS += -I$(TOPDIR)/sched + +ASRCS = +AOBJS = $(ASRCS:.S=$(OBJEXT)) + +CSRCS = empty.c +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ARCH_SRCDIR = $(TOPDIR)/arch/$(CONFIG_ARCH)/src +ifeq ($(WINTOOL),y) + CFLAGS += -I "${shell cygpath -w $(ARCH_SRCDIR)/chip}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/common}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/armv7-m}" +else + CFLAGS += -I$(ARCH_SRCDIR)/chip -I$(ARCH_SRCDIR)/common -I$(ARCH_SRCDIR)/armv7-m +endif + +all: libboard$(LIBEXT) + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS) $(LINKOBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +libboard$(LIBEXT): $(OBJS) + $(call ARCHIVE, $@, $(OBJS)) + +.depend: Makefile $(SRCS) + $(Q) $(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + $(Q) touch $@ + +depend: .depend + +clean: + $(call DELFILE, libboard$(LIBEXT)) + $(call CLEAN) + +distclean: clean + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) + +-include Make.dep + diff --git a/nuttx-configs/px4fmu-v1/src/empty.c b/nuttx-configs/px4fmu-v1/src/empty.c new file mode 100644 index 0000000000..ace900866c --- /dev/null +++ b/nuttx-configs/px4fmu-v1/src/empty.c @@ -0,0 +1,4 @@ +/* + * There are no source files here, but libboard.a can't be empty, so + * we have this empty source file to keep it company. + */ diff --git a/nuttx-configs/px4fmu-v1/tools/px_mkfw.py b/nuttx-configs/px4fmu-v1/tools/px_mkfw.py new file mode 100755 index 0000000000..9f4ddad62e --- /dev/null +++ b/nuttx-configs/px4fmu-v1/tools/px_mkfw.py @@ -0,0 +1,110 @@ +#!/usr/bin/env python +############################################################################ +# +# Copyright (C) 2012 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. +# +############################################################################ + +# +# PX4 firmware image generator +# +# The PX4 firmware file is a JSON-encoded Python object, containing +# metadata fields and a zlib-compressed base64-encoded firmware image. +# + +import sys +import argparse +import json +import base64 +import zlib +import time +import subprocess + +# +# Construct a basic firmware description +# +def mkdesc(): + proto = {} + proto['magic'] = "PX4FWv1" + proto['board_id'] = 0 + proto['board_revision'] = 0 + proto['version'] = "" + proto['summary'] = "" + proto['description'] = "" + proto['git_identity'] = "" + proto['build_time'] = 0 + proto['image'] = base64.b64encode(bytearray()) + proto['image_size'] = 0 + return proto + +# Parse commandline +parser = argparse.ArgumentParser(description="Firmware generator for the PX autopilot system.") +parser.add_argument("--prototype", action="store", help="read a prototype description from a file") +parser.add_argument("--board_id", action="store", help="set the board ID required") +parser.add_argument("--board_revision", action="store", help="set the board revision required") +parser.add_argument("--version", action="store", help="set a version string") +parser.add_argument("--summary", action="store", help="set a brief description") +parser.add_argument("--description", action="store", help="set a longer description") +parser.add_argument("--git_identity", action="store", help="the working directory to check for git identity") +parser.add_argument("--image", action="store", help="the firmware image") +args = parser.parse_args() + +# Fetch the firmware descriptor prototype if specified +if args.prototype != None: + f = open(args.prototype,"r") + desc = json.load(f) + f.close() +else: + desc = mkdesc() + +desc['build_time'] = int(time.time()) + +if args.board_id != None: + desc['board_id'] = int(args.board_id) +if args.board_revision != None: + desc['board_revision'] = int(args.board_revision) +if args.version != None: + desc['version'] = str(args.version) +if args.summary != None: + desc['summary'] = str(args.summary) +if args.description != None: + desc['description'] = str(args.description) +if args.git_identity != None: + cmd = " ".join(["git", "--git-dir", args.git_identity + "/.git", "describe", "--always", "--dirty"]) + p = subprocess.Popen(cmd, shell=True, stdout=subprocess.PIPE).stdout + desc['git_identity'] = p.read().strip() + p.close() +if args.image != None: + f = open(args.image, "rb") + bytes = f.read() + desc['image_size'] = len(bytes) + desc['image'] = base64.b64encode(zlib.compress(bytes,9)) + +print json.dumps(desc, indent=4) diff --git a/nuttx-configs/px4fmu-v1/tools/px_uploader.py b/nuttx-configs/px4fmu-v1/tools/px_uploader.py new file mode 100755 index 0000000000..3b23f4f83f --- /dev/null +++ b/nuttx-configs/px4fmu-v1/tools/px_uploader.py @@ -0,0 +1,416 @@ +#!/usr/bin/env python +############################################################################ +# +# Copyright (C) 2012 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. +# +############################################################################ + +# +# Serial firmware uploader for the PX4FMU bootloader +# +# The PX4 firmware file is a JSON-encoded Python object, containing +# metadata fields and a zlib-compressed base64-encoded firmware image. +# +# The uploader uses the following fields from the firmware file: +# +# image +# The firmware that will be uploaded. +# image_size +# The size of the firmware in bytes. +# board_id +# The board for which the firmware is intended. +# board_revision +# Currently only used for informational purposes. +# + +import sys +import argparse +import binascii +import serial +import os +import struct +import json +import zlib +import base64 +import time +import array + +from sys import platform as _platform + +class firmware(object): + '''Loads a firmware file''' + + desc = {} + image = bytes() + crctab = array.array('I', [ + 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, + 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, + 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, + 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, + 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, + 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, + 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, + 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, + 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, + 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, + 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, + 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, + 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, + 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, + 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, + 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, + 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, + 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, + 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, + 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, + 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, + 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, + 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, + 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, + 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, + 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, + 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, + 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, + 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, + 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, + 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, + 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d ]) + crcpad = bytearray('\xff\xff\xff\xff') + + def __init__(self, path): + + # read the file + f = open(path, "r") + self.desc = json.load(f) + f.close() + + self.image = bytearray(zlib.decompress(base64.b64decode(self.desc['image']))) + + # pad image to 4-byte length + while ((len(self.image) % 4) != 0): + self.image.append('\xff') + + def property(self, propname): + return self.desc[propname] + + def __crc32(self, bytes, state): + for byte in bytes: + index = (state ^ byte) & 0xff + state = self.crctab[index] ^ (state >> 8) + return state + + def crc(self, padlen): + state = self.__crc32(self.image, int(0)) + for i in range(len(self.image), (padlen - 1), 4): + state = self.__crc32(self.crcpad, state) + return state + +class uploader(object): + '''Uploads a firmware file to the PX FMU bootloader''' + + # protocol bytes + INSYNC = chr(0x12) + EOC = chr(0x20) + + # reply bytes + OK = chr(0x10) + FAILED = chr(0x11) + INVALID = chr(0x13) # rev3+ + + # command bytes + NOP = chr(0x00) # guaranteed to be discarded by the bootloader + GET_SYNC = chr(0x21) + GET_DEVICE = chr(0x22) + CHIP_ERASE = chr(0x23) + CHIP_VERIFY = chr(0x24) # rev2 only + PROG_MULTI = chr(0x27) + READ_MULTI = chr(0x28) # rev2 only + GET_CRC = chr(0x29) # rev3+ + REBOOT = chr(0x30) + + INFO_BL_REV = chr(1) # bootloader protocol revision + BL_REV_MIN = 2 # minimum supported bootloader protocol + BL_REV_MAX = 3 # maximum supported bootloader protocol + INFO_BOARD_ID = chr(2) # board type + INFO_BOARD_REV = chr(3) # board revision + INFO_FLASH_SIZE = chr(4) # max firmware size in bytes + + PROG_MULTI_MAX = 60 # protocol max is 255, must be multiple of 4 + READ_MULTI_MAX = 60 # protocol max is 255, something overflows with >= 64 + + def __init__(self, portname, baudrate): + # open the port, keep the default timeout short so we can poll quickly + self.port = serial.Serial(portname, baudrate, timeout=0.25) + + def close(self): + if self.port is not None: + self.port.close() + + def __send(self, c): +# print("send " + binascii.hexlify(c)) + self.port.write(str(c)) + + def __recv(self, count = 1): + c = self.port.read(count) + if len(c) < 1: + raise RuntimeError("timeout waiting for data") +# print("recv " + binascii.hexlify(c)) + return c + + def __recv_int(self): + raw = self.__recv(4) + val = struct.unpack("= 3: + self.__getSync() + + # split a sequence into a list of size-constrained pieces + def __split_len(self, seq, length): + return [seq[i:i+length] for i in range(0, len(seq), length)] + + # upload code + def __program(self, fw): + code = fw.image + groups = self.__split_len(code, uploader.PROG_MULTI_MAX) + for bytes in groups: + self.__program_multi(bytes) + + # verify code + def __verify_v2(self, fw): + self.__send(uploader.CHIP_VERIFY + + uploader.EOC) + self.__getSync() + code = fw.image + groups = self.__split_len(code, uploader.READ_MULTI_MAX) + for bytes in groups: + if (not self.__verify_multi(bytes)): + raise RuntimeError("Verification failed") + + def __verify_v3(self, fw): + expect_crc = fw.crc(self.fw_maxsize) + self.__send(uploader.GET_CRC + + uploader.EOC) + report_crc = self.__recv_int() + self.__getSync() + if report_crc != expect_crc: + print("Expected 0x%x" % expect_crc) + print("Got 0x%x" % report_crc) + raise RuntimeError("Program CRC failed") + + # get basic data about the board + def identify(self): + # make sure we are in sync before starting + self.__sync() + + # get the bootloader protocol ID first + self.bl_rev = self.__getInfo(uploader.INFO_BL_REV) + if (self.bl_rev < uploader.BL_REV_MIN) or (self.bl_rev > uploader.BL_REV_MAX): + print("Unsupported bootloader protocol %d" % uploader.INFO_BL_REV) + raise RuntimeError("Bootloader protocol mismatch") + + self.board_type = self.__getInfo(uploader.INFO_BOARD_ID) + self.board_rev = self.__getInfo(uploader.INFO_BOARD_REV) + self.fw_maxsize = self.__getInfo(uploader.INFO_FLASH_SIZE) + + # upload the firmware + def upload(self, fw): + # Make sure we are doing the right thing + if self.board_type != fw.property('board_id'): + raise RuntimeError("Firmware not suitable for this board (run 'make configure_px4fmu && make clean' or 'make configure_px4io && make clean' to reconfigure).") + if self.fw_maxsize < fw.property('image_size'): + raise RuntimeError("Firmware image is too large for this board") + + print("erase...") + self.__erase() + + print("program...") + self.__program(fw) + + print("verify...") + if self.bl_rev == 2: + self.__verify_v2(fw) + else: + self.__verify_v3(fw) + + print("done, rebooting.") + self.__reboot() + self.port.close() + + +# Parse commandline arguments +parser = argparse.ArgumentParser(description="Firmware uploader for the PX autopilot system.") +parser.add_argument('--port', action="store", required=True, help="Serial port(s) to which the FMU may be attached") +parser.add_argument('--baud', action="store", type=int, default=115200, help="Baud rate of the serial port (default is 115200), only required for true serial ports.") +parser.add_argument('firmware', action="store", help="Firmware file to be uploaded") +args = parser.parse_args() + +# Load the firmware file +fw = firmware(args.firmware) +print("Loaded firmware for %x,%x, waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision'))) + +# Spin waiting for a device to show up +while True: + for port in args.port.split(","): + + #print("Trying %s" % port) + + # create an uploader attached to the port + try: + if "linux" in _platform: + # Linux, don't open Mac OS and Win ports + if not "COM" in port and not "tty.usb" in port: + up = uploader(port, args.baud) + elif "darwin" in _platform: + # OS X, don't open Windows and Linux ports + if not "COM" in port and not "ACM" in port: + up = uploader(port, args.baud) + elif "win" in _platform: + # Windows, don't open POSIX ports + if not "/" in port: + up = uploader(port, args.baud) + except: + # open failed, rate-limit our attempts + time.sleep(0.05) + + # and loop to the next port + continue + + # port is open, try talking to it + try: + # identify the bootloader + up.identify() + print("Found board %x,%x bootloader rev %x on %s" % (up.board_type, up.board_rev, up.bl_rev, port)) + + except: + # most probably a timeout talking to the port, no bootloader + continue + + try: + # ok, we have a bootloader, try flashing it + up.upload(fw) + + except RuntimeError as ex: + + # print the error + print("ERROR: %s" % ex.args) + + finally: + # always close the port + up.close() + + # we could loop here if we wanted to wait for more boards... + sys.exit(0) diff --git a/nuttx-configs/px4fmu-v1/tools/upload.sh b/nuttx-configs/px4fmu-v1/tools/upload.sh new file mode 100755 index 0000000000..4e6597b3a6 --- /dev/null +++ b/nuttx-configs/px4fmu-v1/tools/upload.sh @@ -0,0 +1,27 @@ +#!/bin/sh +# +# Wrapper to upload a PX4 firmware binary +# +TOOLS=`dirname $0` +MKFW=${TOOLS}/px_mkfw.py +UPLOAD=${TOOLS}/px_uploader.py + +BINARY=nuttx.bin +PAYLOAD=nuttx.px4 +PORTS="/dev/tty.usbmodemPX1,/dev/tty.usbmodemPX2,/dev/tty.usbmodemPX3,/dev/tty.usbmodemPX4,/dev/tty.usbmodem1,/dev/tty.usbmodem2,/dev/tty.usbmodem3,/dev/tty.usbmodem4" + +function abort() { + echo "ABORT: $*" + exit 1 +} + +if [ ! -f ${MKFW} -o ! -f ${UPLOAD} ]; then + abort "Missing tools ${MKFW} and/or ${UPLOAD}" +fi +if [ ! -f ${BINARY} ]; then + abort "Missing nuttx binary in current directory." +fi + +rm -f ${PAYLOAD} +${MKFW} --board_id 5 --image ${BINARY} > ${PAYLOAD} +${UPLOAD} --port ${PORTS} ${PAYLOAD} diff --git a/nuttx-configs/px4fmu-v2/include/board.h b/nuttx-configs/px4fmu-v2/include/board.h index a6cdfb4d28..507df70a23 100755 --- a/nuttx-configs/px4fmu-v2/include/board.h +++ b/nuttx-configs/px4fmu-v2/include/board.h @@ -194,11 +194,6 @@ #define DMAMAP_SDIO DMAMAP_SDIO_1 -/* High-resolution timer - */ -#define HRT_TIMER 8 /* use timer8 for the HRT */ -#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */ - /* Alternate function pin selections ************************************************/ /* @@ -232,35 +227,12 @@ #define DMAMAP_USART1_RX DMAMAP_USART1_RX_2 #define DMAMAP_USART6_RX DMAMAP_USART6_RX_2 -/* - * PWM - * - * Six PWM outputs are configured. - * - * Pins: - * - * CH1 : PE14 : TIM1_CH4 - * CH2 : PE13 : TIM1_CH3 - * CH3 : PE11 : TIM1_CH2 - * CH4 : PE9 : TIM1_CH1 - * CH5 : PD13 : TIM4_CH2 - * CH6 : PD14 : TIM4_CH3 - * - */ -#define GPIO_TIM1_CH1OUT GPIO_TIM1_CH1OUT_2 -#define GPIO_TIM1_CH2OUT GPIO_TIM1_CH2OUT_2 -#define GPIO_TIM1_CH3OUT GPIO_TIM1_CH3OUT_2 -#define GPIO_TIM1_CH4OUT GPIO_TIM1_CH4OUT_2 -#define GPIO_TIM4_CH2OUT GPIO_TIM4_CH2OUT_2 -#define GPIO_TIM4_CH3OUT GPIO_TIM4_CH3OUT_2 - /* * CAN * * CAN1 is routed to the onboard transceiver. * CAN2 is routed to the expansion connector. */ - #define GPIO_CAN1_RX GPIO_CAN1_RX_3 #define GPIO_CAN1_TX GPIO_CAN1_TX_3 #define GPIO_CAN2_RX GPIO_CAN2_RX_1 @@ -283,20 +255,6 @@ #define GPIO_I2C2_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN10) #define GPIO_I2C2_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11) -/* - * I2C busses - */ -#define PX4_I2C_BUS_EXPANSION 1 -#define PX4_I2C_BUS_LED 2 - -/* - * Devices on the onboard bus. - * - * Note that these are unshifted addresses. - */ -#define PX4_I2C_OBDEV_LED 0x55 -#define PX4_I2C_OBDEV_HMC5883 0x1e - /* * SPI * @@ -310,22 +268,6 @@ #define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_1 #define GPIO_SPI2_SCK GPIO_SPI2_SCK_2 -/* - * Use these in place of the spi_dev_e enumeration to - * select a specific SPI device on SPI1 - */ -#define PX4_SPIDEV_GYRO 1 -#define PX4_SPIDEV_ACCEL_MAG 2 -#define PX4_SPIDEV_BARO 3 - -/* - * Tone alarm output - */ -#define TONE_ALARM_TIMER 2 /* timer 2 */ -#define TONE_ALARM_CHANNEL 1 /* channel 1 */ -#define GPIO_TONE_ALARM_IDLE (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15) -#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN15) - /************************************************************************************ * Public Data ************************************************************************************/ diff --git a/nuttx-configs/px4io-v1/Kconfig b/nuttx-configs/px4io-v1/Kconfig new file mode 100644 index 0000000000..fbf74d7f0d --- /dev/null +++ b/nuttx-configs/px4io-v1/Kconfig @@ -0,0 +1,21 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# + +if ARCH_BOARD_PX4IO_V1 + +config HRT_TIMER + bool "High resolution timer support" + default y + ---help--- + Enable high resolution timer for PPM capture and system clocks. + +config HRT_PPM + bool "PPM input capture" + default y + depends on HRT_TIMER + ---help--- + Enable PPM input capture via HRT (for CPPM / PPM sum RC inputs) + +endif diff --git a/nuttx-configs/px4io-v1/README.txt b/nuttx-configs/px4io-v1/README.txt new file mode 100755 index 0000000000..9b1615f42d --- /dev/null +++ b/nuttx-configs/px4io-v1/README.txt @@ -0,0 +1,806 @@ +README +====== + +This README discusses issues unique to NuttX configurations for the +STMicro STM3210E-EVAL development board. + +Contents +======== + + - Development Environment + - GNU Toolchain Options + - IDEs + - NuttX buildroot Toolchain + - DFU and JTAG + - OpenOCD + - LEDs + - Temperature Sensor + - RTC + - STM3210E-EVAL-specific Configuration Options + - Configurations + +Development Environment +======================= + + Either Linux or Cygwin on Windows can be used for the development environment. + The source has been built only using the GNU toolchain (see below). Other + toolchains will likely cause problems. Testing was performed using the Cygwin + environment because the Raisonance R-Link emulatator and some RIDE7 development tools + were used and those tools works only under Windows. + +GNU Toolchain Options +===================== + + The NuttX make system has been modified to support the following different + toolchain options. + + 1. The CodeSourcery GNU toolchain, + 2. The devkitARM GNU toolchain, + 3. Raisonance GNU toolchain, or + 4. The NuttX buildroot Toolchain (see below). + + All testing has been conducted using the NuttX buildroot toolchain. However, + the make system is setup to default to use the devkitARM toolchain. To use + the CodeSourcery, devkitARM or Raisonance GNU toolchain, you simply need to + add one of the following configuration options to your .config (or defconfig) + file: + + CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows + CONFIG_STM32_CODESOURCERYL=y : CodeSourcery under Linux + CONFIG_STM32_DEVKITARM=y : devkitARM under Windows + CONFIG_STM32_RAISONANCE=y : Raisonance RIDE7 under Windows + CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin (default) + + If you are not using CONFIG_STM32_BUILDROOT, then you may also have to modify + the PATH in the setenv.h file if your make cannot find the tools. + + NOTE: the CodeSourcery (for Windows), devkitARM, and Raisonance toolchains are + Windows native toolchains. The CodeSourcey (for Linux) and NuttX buildroot + toolchains are Cygwin and/or Linux native toolchains. There are several limitations + to using a Windows based toolchain in a Cygwin environment. The three biggest are: + + 1. The Windows toolchain cannot follow Cygwin paths. Path conversions are + performed automatically in the Cygwin makefiles using the 'cygpath' utility + but you might easily find some new path problems. If so, check out 'cygpath -w' + + 2. Windows toolchains cannot follow Cygwin symbolic links. Many symbolic links + are used in Nuttx (e.g., include/arch). The make system works around these + problems for the Windows tools by copying directories instead of linking them. + But this can also cause some confusion for you: For example, you may edit + a file in a "linked" directory and find that your changes had no effect. + That is because you are building the copy of the file in the "fake" symbolic + directory. If you use a Windows toolchain, you should get in the habit of + making like this: + + make clean_context all + + An alias in your .bashrc file might make that less painful. + + 3. Dependencies are not made when using Windows versions of the GCC. This is + because the dependencies are generated using Windows pathes which do not + work with the Cygwin make. + + Support has been added for making dependencies with the windows-native toolchains. + That support can be enabled by modifying your Make.defs file as follows: + + - MKDEP = $(TOPDIR)/tools/mknulldeps.sh + + MKDEP = $(TOPDIR)/tools/mkdeps.sh --winpaths "$(TOPDIR)" + + If you have problems with the dependency build (for example, if you are not + building on C:), then you may need to modify tools/mkdeps.sh + + NOTE 1: The CodeSourcery toolchain (2009q1) does not work with default optimization + level of -Os (See Make.defs). It will work with -O0, -O1, or -O2, but not with + -Os. + + NOTE 2: The devkitARM toolchain includes a version of MSYS make. Make sure that + the paths to Cygwin's /bin and /usr/bin directories appear BEFORE the devkitARM + path or will get the wrong version of make. + +IDEs +==== + + NuttX is built using command-line make. It can be used with an IDE, but some + effort will be required to create the project (There is a simple RIDE project + in the RIDE subdirectory). + + Makefile Build + -------------- + Under Eclipse, it is pretty easy to set up an "empty makefile project" and + simply use the NuttX makefile to build the system. That is almost for free + under Linux. Under Windows, you will need to set up the "Cygwin GCC" empty + makefile project in order to work with Windows (Google for "Eclipse Cygwin" - + there is a lot of help on the internet). + + Native Build + ------------ + Here are a few tips before you start that effort: + + 1) Select the toolchain that you will be using in your .config file + 2) Start the NuttX build at least one time from the Cygwin command line + before trying to create your project. This is necessary to create + certain auto-generated files and directories that will be needed. + 3) Set up include pathes: You will need include/, arch/arm/src/stm32, + arch/arm/src/common, arch/arm/src/armv7-m, and sched/. + 4) All assembly files need to have the definition option -D __ASSEMBLY__ + on the command line. + + Startup files will probably cause you some headaches. The NuttX startup file + is arch/arm/src/stm32/stm32_vectors.S. With RIDE, I have to build NuttX + one time from the Cygwin command line in order to obtain the pre-built + startup object needed by RIDE. + +NuttX buildroot Toolchain +========================= + + A GNU GCC-based toolchain is assumed. The files */setenv.sh should + be modified to point to the correct path to the Cortex-M3 GCC toolchain (if + different from the default in your PATH variable). + + If you have no Cortex-M3 toolchain, one can be downloaded from the NuttX + SourceForge download site (https://sourceforge.net/project/showfiles.php?group_id=189573). + This GNU toolchain builds and executes in the Linux or Cygwin environment. + + 1. You must have already configured Nuttx in /nuttx. + + cd tools + ./configure.sh stm3210e-eval/ + + 2. Download the latest buildroot package into + + 3. unpack the buildroot tarball. The resulting directory may + have versioning information on it like buildroot-x.y.z. If so, + rename /buildroot-x.y.z to /buildroot. + + 4. cd /buildroot + + 5. cp configs/cortexm3-defconfig-4.3.3 .config + + 6. make oldconfig + + 7. make + + 8. Edit setenv.h, if necessary, so that the PATH variable includes + the path to the newly built binaries. + + See the file configs/README.txt in the buildroot source tree. That has more + detailed PLUS some special instructions that you will need to follow if you are + building a Cortex-M3 toolchain for Cygwin under Windows. + +DFU and JTAG +============ + + Enbling Support for the DFU Bootloader + -------------------------------------- + The linker files in these projects can be configured to indicate that you + will be loading code using STMicro built-in USB Device Firmware Upgrade (DFU) + loader or via some JTAG emulator. You can specify the DFU bootloader by + adding the following line: + + CONFIG_STM32_DFU=y + + to your .config file. Most of the configurations in this directory are set + up to use the DFU loader. + + If CONFIG_STM32_DFU is defined, the code will not be positioned at the beginning + of FLASH (0x08000000) but will be offset to 0x08003000. This offset is needed + to make space for the DFU loader and 0x08003000 is where the DFU loader expects + to find new applications at boot time. If you need to change that origin for some + other bootloader, you will need to edit the file(s) ld.script.dfu for each + configuration. + + The DFU SE PC-based software is available from the STMicro website, + http://www.st.com. General usage instructions: + + 1. Convert the NuttX Intel Hex file (nuttx.ihx) into a special DFU + file (nuttx.dfu)... see below for details. + 2. Connect the STM3210E-EVAL board to your computer using a USB + cable. + 3. Start the DFU loader on the STM3210E-EVAL board. You do this by + resetting the board while holding the "Key" button. Windows should + recognize that the DFU loader has been installed. + 3. Run the DFU SE program to load nuttx.dfu into FLASH. + + What if the DFU loader is not in FLASH? The loader code is available + inside of the Demo dirctory of the USBLib ZIP file that can be downloaded + from the STMicro Website. You can build it using RIDE (or other toolchains); + you will need a JTAG emulator to burn it into FLASH the first time. + + In order to use STMicro's built-in DFU loader, you will have to get + the NuttX binary into a special format with a .dfu extension. The + DFU SE PC_based software installation includes a file "DFU File Manager" + conversion program that a file in Intel Hex format to the special DFU + format. When you successfully build NuttX, you will find a file called + nutt.ihx in the top-level directory. That is the file that you should + provide to the DFU File Manager. You will need to rename it to nuttx.hex + in order to find it with the DFU File Manager. You will end up with + a file called nuttx.dfu that you can use with the STMicro DFU SE program. + + Enabling JTAG + ------------- + If you are not using the DFU, then you will probably also need to enable + JTAG support. By default, all JTAG support is disabled but there NuttX + configuration options to enable JTAG in various different ways. + + These configurations effect the setting of the SWJ_CFG[2:0] bits in the AFIO + MAPR register. These bits are used to configure the SWJ and trace alternate function I/Os. The SWJ (SerialWire JTAG) supports JTAG or SWD access to the + Cortex debug port. The default state in this port is for all JTAG support + to be disable. + + CONFIG_STM32_JTAG_FULL_ENABLE - sets SWJ_CFG[2:0] to 000 which enables full + SWJ (JTAG-DP + SW-DP) + + CONFIG_STM32_JTAG_NOJNTRST_ENABLE - sets SWJ_CFG[2:0] to 001 which enable + full SWJ (JTAG-DP + SW-DP) but without JNTRST. + + CONFIG_STM32_JTAG_SW_ENABLE - sets SWJ_CFG[2:0] to 010 which would set JTAG-DP + disabled and SW-DP enabled + + The default setting (none of the above defined) is SWJ_CFG[2:0] set to 100 + which disable JTAG-DP and SW-DP. + +OpenOCD +======= + +I have also used OpenOCD with the STM3210E-EVAL. In this case, I used +the Olimex USB ARM OCD. See the script in configs/stm3210e-eval/tools/oocd.sh +for more information. Using the script: + +1) Start the OpenOCD GDB server + + cd + configs/stm3210e-eval/tools/oocd.sh $PWD + +2) Load Nuttx + + cd + arm-none-eabi-gdb nuttx + gdb> target remote localhost:3333 + gdb> mon reset + gdb> mon halt + gdb> load nuttx + +3) Running NuttX + + gdb> mon reset + gdb> c + +LEDs +==== + +The STM3210E-EVAL board has four LEDs labeled LD1, LD2, LD3 and LD4 on the +board.. These LEDs are not used by the board port unless CONFIG_ARCH_LEDS is +defined. In that case, the usage by the board port is defined in +include/board.h and src/up_leds.c. The LEDs are used to encode OS-related +events as follows: + + SYMBOL Meaning LED1* LED2 LED3 LED4 + ---------------- ----------------------- ----- ----- ----- ----- + LED_STARTED NuttX has been started ON OFF OFF OFF + LED_HEAPALLOCATE Heap has been allocated OFF ON OFF OFF + LED_IRQSENABLED Interrupts enabled ON ON OFF OFF + LED_STACKCREATED Idle stack created OFF OFF ON OFF + LED_INIRQ In an interrupt** ON N/C N/C OFF + LED_SIGNAL In a signal handler*** N/C ON N/C OFF + LED_ASSERTION An assertion failed ON ON N/C OFF + LED_PANIC The system has crashed N/C N/C N/C ON + LED_IDLE STM32 is is sleep mode (Optional, not used) + + * If LED1, LED2, LED3 are statically on, then NuttX probably failed to boot + and these LEDs will give you some indication of where the failure was + ** The normal state is LED3 ON and LED1 faintly glowing. This faint glow + is because of timer interupts that result in the LED being illuminated + on a small proportion of the time. +*** LED2 may also flicker normally if signals are processed. + +Temperature Sensor +================== + +Support for the on-board LM-75 temperature sensor is available. This supported +has been verified, but has not been included in any of the available the +configurations. To set up the temperature sensor, add the following to the +NuttX configuration file + + CONFIG_I2C=y + CONFIG_I2C_LM75=y + +Then you can implement logic like the following to use the temperature sensor: + + #include + #include + + ret = stm32_lm75initialize("/dev/temp"); /* Register the temperature sensor */ + fd = open("/dev/temp", O_RDONLY); /* Open the temperature sensor device */ + ret = ioctl(fd, SNIOC_FAHRENHEIT, 0); /* Select Fahrenheit */ + bytesread = read(fd, buffer, 8*sizeof(b16_t)); /* Read temperature samples */ + +More complex temperature sensor operations are also available. See the IOCTAL +commands enumerated in include/nuttx/sensors/lm75.h. Also read the descriptions +of the stm32_lm75initialize() and stm32_lm75attach() interfaces in the +arch/board/board.h file (sames as configs/stm3210e-eval/include/board.h). + +RTC +=== + + The STM32 RTC may configured using the following settings. + + CONFIG_RTC - Enables general support for a hardware RTC. Specific + architectures may require other specific settings. + CONFIG_RTC_HIRES - The typical RTC keeps time to resolution of 1 + second, usually supporting a 32-bit time_t value. In this case, + the RTC is used to "seed" the normal NuttX timer and the + NuttX timer provides for higher resoution time. If CONFIG_RTC_HIRES + is enabled in the NuttX configuration, then the RTC provides higher + resolution time and completely replaces the system timer for purpose of + date and time. + CONFIG_RTC_FREQUENCY - If CONFIG_RTC_HIRES is defined, then the + frequency of the high resolution RTC must be provided. If CONFIG_RTC_HIRES + is not defined, CONFIG_RTC_FREQUENCY is assumed to be one. + CONFIG_RTC_ALARM - Enable if the RTC hardware supports setting of an alarm. + A callback function will be executed when the alarm goes off + + In hi-res mode, the STM32 RTC operates only at 16384Hz. Overflow interrupts + are handled when the 32-bit RTC counter overflows every 3 days and 43 minutes. + A BKP register is incremented on each overflow interrupt creating, effectively, + a 48-bit RTC counter. + + In the lo-res mode, the RTC operates at 1Hz. Overflow interrupts are not handled + (because the next overflow is not expected until the year 2106. + + WARNING: Overflow interrupts are lost whenever the STM32 is powered down. The + overflow interrupt may be lost even if the STM32 is powered down only momentarily. + Therefore hi-res solution is only useful in systems where the power is always on. + +STM3210E-EVAL-specific Configuration Options +============================================ + + CONFIG_ARCH - Identifies the arch/ subdirectory. This should + be set to: + + CONFIG_ARCH=arm + + CONFIG_ARCH_family - For use in C code: + + CONFIG_ARCH_ARM=y + + CONFIG_ARCH_architecture - For use in C code: + + CONFIG_ARCH_CORTEXM3=y + + CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory + + CONFIG_ARCH_CHIP=stm32 + + CONFIG_ARCH_CHIP_name - For use in C code to identify the exact + chip: + + CONFIG_ARCH_CHIP_STM32F103ZET6 + + CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG - Enables special STM32 clock + configuration features. + + CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG=n + + CONFIG_ARCH_BOARD - Identifies the configs subdirectory and + hence, the board that supports the particular chip or SoC. + + CONFIG_ARCH_BOARD=stm3210e_eval (for the STM3210E-EVAL development board) + + CONFIG_ARCH_BOARD_name - For use in C code + + CONFIG_ARCH_BOARD_STM3210E_EVAL=y + + CONFIG_ARCH_LOOPSPERMSEC - Must be calibrated for correct operation + of delay loops + + CONFIG_ENDIAN_BIG - define if big endian (default is little + endian) + + CONFIG_DRAM_SIZE - Describes the installed DRAM (SRAM in this case): + + CONFIG_DRAM_SIZE=0x00010000 (64Kb) + + CONFIG_DRAM_START - The start address of installed DRAM + + CONFIG_DRAM_START=0x20000000 + + CONFIG_DRAM_END - Last address+1 of installed RAM + + CONFIG_DRAM_END=(CONFIG_DRAM_START+CONFIG_DRAM_SIZE) + + CONFIG_ARCH_IRQPRIO - The STM32F103Z supports interrupt prioritization + + CONFIG_ARCH_IRQPRIO=y + + CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to boards that + have LEDs + + CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt + stack. If defined, this symbol is the size of the interrupt + stack in bytes. If not defined, the user task stacks will be + used during interrupt handling. + + CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions + + CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to board architecture. + + CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that + cause a 100 second delay during boot-up. This 100 second delay + serves no purpose other than it allows you to calibratre + CONFIG_ARCH_LOOPSPERMSEC. You simply use a stop watch to measure + the 100 second delay then adjust CONFIG_ARCH_LOOPSPERMSEC until + the delay actually is 100 seconds. + + Individual subsystems can be enabled: + AHB + --- + CONFIG_STM32_DMA1 + CONFIG_STM32_DMA2 + CONFIG_STM32_CRC + CONFIG_STM32_FSMC + CONFIG_STM32_SDIO + + APB1 + ---- + CONFIG_STM32_TIM2 + CONFIG_STM32_TIM3 + CONFIG_STM32_TIM4 + CONFIG_STM32_TIM5 + CONFIG_STM32_TIM6 + CONFIG_STM32_TIM7 + CONFIG_STM32_WWDG + CONFIG_STM32_SPI2 + CONFIG_STM32_SPI4 + CONFIG_STM32_USART2 + CONFIG_STM32_USART3 + CONFIG_STM32_UART4 + CONFIG_STM32_UART5 + CONFIG_STM32_I2C1 + CONFIG_STM32_I2C2 + CONFIG_STM32_USB + CONFIG_STM32_CAN + CONFIG_STM32_BKP + CONFIG_STM32_PWR + CONFIG_STM32_DAC1 + CONFIG_STM32_DAC2 + CONFIG_STM32_USB + + APB2 + ---- + CONFIG_STM32_ADC1 + CONFIG_STM32_ADC2 + CONFIG_STM32_TIM1 + CONFIG_STM32_SPI1 + CONFIG_STM32_TIM8 + CONFIG_STM32_USART1 + CONFIG_STM32_ADC3 + + Timer and I2C devices may need to the following to force power to be applied + unconditionally at power up. (Otherwise, the device is powered when it is + initialized). + + CONFIG_STM32_FORCEPOWER + + Timer devices may be used for different purposes. One special purpose is + to generate modulated outputs for such things as motor control. If CONFIG_STM32_TIMn + is defined (as above) then the following may also be defined to indicate that + the timer is intended to be used for pulsed output modulation, ADC conversion, + or DAC conversion. Note that ADC/DAC require two definition: Not only do you have + to assign the timer (n) for used by the ADC or DAC, but then you also have to + configure which ADC or DAC (m) it is assigned to. + + CONFIG_STM32_TIMn_PWM Reserve timer n for use by PWM, n=1,..,8 + CONFIG_STM32_TIMn_ADC Reserve timer n for use by ADC, n=1,..,8 + CONFIG_STM32_TIMn_ADCm Reserve timer n to trigger ADCm, n=1,..,8, m=1,..,3 + CONFIG_STM32_TIMn_DAC Reserve timer n for use by DAC, n=1,..,8 + CONFIG_STM32_TIMn_DACm Reserve timer n to trigger DACm, n=1,..,8, m=1,..,2 + + For each timer that is enabled for PWM usage, we need the following additional + configuration settings: + + CONFIG_STM32_TIMx_CHANNEL - Specifies the timer output channel {1,..,4} + + NOTE: The STM32 timers are each capable of generating different signals on + each of the four channels with different duty cycles. That capability is + not supported by this driver: Only one output channel per timer. + + Alternate pin mappings (should not be used with the STM3210E-EVAL board): + + CONFIG_STM32_TIM1_FULL_REMAP + CONFIG_STM32_TIM1_PARTIAL_REMAP + CONFIG_STM32_TIM2_FULL_REMAP + CONFIG_STM32_TIM2_PARTIAL_REMAP_1 + CONFIG_STM32_TIM2_PARTIAL_REMAP_2 + CONFIG_STM32_TIM3_FULL_REMAP + CONFIG_STM32_TIM3_PARTIAL_REMAP + CONFIG_STM32_TIM4_REMAP + CONFIG_STM32_USART1_REMAP + CONFIG_STM32_USART2_REMAP + CONFIG_STM32_USART3_FULL_REMAP + CONFIG_STM32_USART3_PARTIAL_REMAP + CONFIG_STM32_SPI1_REMAP + CONFIG_STM32_SPI3_REMAP + CONFIG_STM32_I2C1_REMAP + CONFIG_STM32_CAN1_FULL_REMAP + CONFIG_STM32_CAN1_PARTIAL_REMAP + CONFIG_STM32_CAN2_REMAP + + JTAG Enable settings (by default JTAG-DP and SW-DP are disabled): + CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) + CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) + but without JNTRST. + CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled + + STM32F103Z specific device driver settings + + CONFIG_U[S]ARTn_SERIAL_CONSOLE - selects the USARTn (n=1,2,3) or UART + m (m=4,5) for the console and ttys0 (default is the USART1). + CONFIG_U[S]ARTn_RXBUFSIZE - Characters are buffered as received. + This specific the size of the receive buffer + CONFIG_U[S]ARTn_TXBUFSIZE - Characters are buffered before + being sent. This specific the size of the transmit buffer + CONFIG_U[S]ARTn_BAUD - The configure BAUD of the UART. Must be + CONFIG_U[S]ARTn_BITS - The number of bits. Must be either 7 or 8. + CONFIG_U[S]ARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity + CONFIG_U[S]ARTn_2STOP - Two stop bits + + CONFIG_STM32_SPI_INTERRUPTS - Select to enable interrupt driven SPI + support. Non-interrupt-driven, poll-waiting is recommended if the + interrupt rate would be to high in the interrupt driven case. + CONFIG_STM32_SPI_DMA - Use DMA to improve SPI transfer performance. + Cannot be used with CONFIG_STM32_SPI_INTERRUPT. + + CONFIG_SDIO_DMA - Support DMA data transfers. Requires CONFIG_STM32_SDIO + and CONFIG_STM32_DMA2. + CONFIG_SDIO_PRI - Select SDIO interrupt prority. Default: 128 + CONFIG_SDIO_DMAPRIO - Select SDIO DMA interrupt priority. + Default: Medium + CONFIG_SDIO_WIDTH_D1_ONLY - Select 1-bit transfer mode. Default: + 4-bit transfer mode. + + STM3210E-EVAL CAN Configuration + + CONFIG_CAN - Enables CAN support (one or both of CONFIG_STM32_CAN1 or + CONFIG_STM32_CAN2 must also be defined) + CONFIG_CAN_EXTID - Enables support for the 29-bit extended ID. Default + Standard 11-bit IDs. + CONFIG_CAN_FIFOSIZE - The size of the circular buffer of CAN messages. + Default: 8 + CONFIG_CAN_NPENDINGRTR - The size of the list of pending RTR requests. + Default: 4 + CONFIG_CAN_LOOPBACK - A CAN driver may or may not support a loopback + mode for testing. The STM32 CAN driver does support loopback mode. + CONFIG_CAN1_BAUD - CAN1 BAUD rate. Required if CONFIG_STM32_CAN1 is defined. + CONFIG_CAN2_BAUD - CAN1 BAUD rate. Required if CONFIG_STM32_CAN2 is defined. + CONFIG_CAN_TSEG1 - The number of CAN time quanta in segment 1. Default: 6 + CONFIG_CAN_TSEG2 - the number of CAN time quanta in segment 2. Default: 7 + CONFIG_CAN_REGDEBUG - If CONFIG_DEBUG is set, this will generate an + dump of all CAN registers. + + STM3210E-EVAL LCD Hardware Configuration + + CONFIG_LCD_LANDSCAPE - Define for 320x240 display "landscape" + support. Default is this 320x240 "landscape" orientation + (this setting is informative only... not used). + CONFIG_LCD_PORTRAIT - Define for 240x320 display "portrait" + orientation support. In this orientation, the STM3210E-EVAL's + LCD ribbon cable is at the bottom of the display. Default is + 320x240 "landscape" orientation. + CONFIG_LCD_RPORTRAIT - Define for 240x320 display "reverse + portrait" orientation support. In this orientation, the + STM3210E-EVAL's LCD ribbon cable is at the top of the display. + Default is 320x240 "landscape" orientation. + CONFIG_LCD_BACKLIGHT - Define to support a backlight. + CONFIG_LCD_PWM - If CONFIG_STM32_TIM1 is also defined, then an + adjustable backlight will be provided using timer 1 to generate + various pulse widthes. The granularity of the settings is + determined by CONFIG_LCD_MAXPOWER. If CONFIG_LCD_PWM (or + CONFIG_STM32_TIM1) is not defined, then a simple on/off backlight + is provided. + CONFIG_LCD_RDSHIFT - When reading 16-bit gram data, there appears + to be a shift in the returned data. This value fixes the offset. + Default 5. + + The LCD driver dynamically selects the LCD based on the reported LCD + ID value. However, code size can be reduced by suppressing support for + individual LCDs using: + + CONFIG_STM32_AM240320_DISABLE + CONFIG_STM32_SPFD5408B_DISABLE + CONFIG_STM32_R61580_DISABLE + +Configurations +============== + +Each STM3210E-EVAL configuration is maintained in a sudirectory and +can be selected as follow: + + cd tools + ./configure.sh stm3210e-eval/ + cd - + . ./setenv.sh + +Where is one of the following: + + buttons: + -------- + + Uses apps/examples/buttons to exercise STM3210E-EVAL buttons and + button interrupts. + + CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows + + composite + --------- + + This configuration exercises a composite USB interface consisting + of a CDC/ACM device and a USB mass storage device. This configuration + uses apps/examples/composite. + + nsh and nsh2: + ------------ + Configure the NuttShell (nsh) located at examples/nsh. + + Differences between the two NSH configurations: + + =========== ======================= ================================ + nsh nsh2 + =========== ======================= ================================ + Toolchain: NuttX buildroot for Codesourcery for Windows (1) + Linux or Cygwin (1,2) + ----------- ----------------------- -------------------------------- + Loader: DfuSe DfuSe + ----------- ----------------------- -------------------------------- + Serial Debug output: USART1 Debug output: USART1 + Console: NSH output: USART1 NSH output: USART1 (3) + ----------- ----------------------- -------------------------------- + microSD Yes Yes + Support + ----------- ----------------------- -------------------------------- + FAT FS CONFIG_FAT_LCNAME=y CONFIG_FAT_LCNAME=y + Config CONFIG_FAT_LFN=n CONFIG_FAT_LFN=y (4) + ----------- ----------------------- -------------------------------- + Support for No Yes + Built-in + Apps + ----------- ----------------------- -------------------------------- + Built-in None apps/examples/nx + Apps apps/examples/nxhello + apps/examples/usbstorage (5) + =========== ======================= ================================ + + (1) You will probably need to modify nsh/setenv.sh or nsh2/setenv.sh + to set up the correct PATH variable for whichever toolchain you + may use. + (2) Since DfuSe is assumed, this configuration may only work under + Cygwin without modification. + (3) When any other device other than /dev/console is used for a user + interface, (1) linefeeds (\n) will not be expanded to carriage return + / linefeeds \r\n). You will need to configure your terminal program + to account for this. And (2) input is not automatically echoed so + you will have to turn local echo on. + (4) Microsoft holds several patents related to the design of + long file names in the FAT file system. Please refer to the + details in the top-level COPYING file. Please do not use FAT + long file name unless you are familiar with these patent issues. + (5) When built as an NSH add-on command (CONFIG_EXAMPLES_USBMSC_BUILTIN=y), + Caution should be used to assure that the SD drive is not in use when + the USB storage device is configured. Specifically, the SD driver + should be unmounted like: + + nsh> mount -t vfat /dev/mmcsd0 /mnt/sdcard # Card is mounted in NSH + ... + nsh> umount /mnd/sdcard # Unmount before connecting USB!!! + nsh> msconn # Connect the USB storage device + ... + nsh> msdis # Disconnect USB storate device + nsh> mount -t vfat /dev/mmcsd0 /mnt/sdcard # Restore the mount + + Failure to do this could result in corruption of the SD card format. + + nx: + --- + An example using the NuttX graphics system (NX). This example + focuses on general window controls, movement, mouse and keyboard + input. + + CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows + CONFIG_LCD_RPORTRAIT=y : 240x320 reverse portrait + + nxlines: + ------ + Another example using the NuttX graphics system (NX). This + example focuses on placing lines on the background in various + orientations. + + CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows + CONFIG_LCD_RPORTRAIT=y : 240x320 reverse portrait + + nxtext: + ------ + Another example using the NuttX graphics system (NX). This + example focuses on placing text on the background while pop-up + windows occur. Text should continue to update normally with + or without the popup windows present. + + CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin + CONFIG_LCD_RPORTRAIT=y : 240x320 reverse portrait + + NOTE: When I tried building this example with the CodeSourcery + tools, I got a hardfault inside of its libgcc. I haven't + retested since then, but beware if you choose to change the + toolchain. + + ostest: + ------ + This configuration directory, performs a simple OS test using + examples/ostest. By default, this project assumes that you are + using the DFU bootloader. + + CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin + + RIDE + ---- + This configuration builds a trivial bring-up binary. It is + useful only because it words with the RIDE7 IDE and R-Link debugger. + + CONFIG_STM32_RAISONANCE=y : Raisonance RIDE7 under Windows + + usbserial: + --------- + This configuration directory exercises the USB serial class + driver at examples/usbserial. See examples/README.txt for + more information. + + CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin + + USB debug output can be enabled as by changing the following + settings in the configuration file: + + -CONFIG_DEBUG=n + -CONFIG_DEBUG_VERBOSE=n + -CONFIG_DEBUG_USB=n + +CONFIG_DEBUG=y + +CONFIG_DEBUG_VERBOSE=y + +CONFIG_DEBUG_USB=y + + -CONFIG_EXAMPLES_USBSERIAL_TRACEINIT=n + -CONFIG_EXAMPLES_USBSERIAL_TRACECLASS=n + -CONFIG_EXAMPLES_USBSERIAL_TRACETRANSFERS=n + -CONFIG_EXAMPLES_USBSERIAL_TRACECONTROLLER=n + -CONFIG_EXAMPLES_USBSERIAL_TRACEINTERRUPTS=n + +CONFIG_EXAMPLES_USBSERIAL_TRACEINIT=y + +CONFIG_EXAMPLES_USBSERIAL_TRACECLASS=y + +CONFIG_EXAMPLES_USBSERIAL_TRACETRANSFERS=y + +CONFIG_EXAMPLES_USBSERIAL_TRACECONTROLLER=y + +CONFIG_EXAMPLES_USBSERIAL_TRACEINTERRUPTS=y + + By default, the usbserial example uses the Prolific PL2303 + serial/USB converter emulation. The example can be modified + to use the CDC/ACM serial class by making the following changes + to the configuration file: + + -CONFIG_PL2303=y + +CONFIG_PL2303=n + + -CONFIG_CDCACM=n + +CONFIG_CDCACM=y + + The example can also be converted to use the alternative + USB serial example at apps/examples/usbterm by changing the + following: + + -CONFIGURED_APPS += examples/usbserial + +CONFIGURED_APPS += examples/usbterm + + In either the original appconfig file (before configuring) + or in the final apps/.config file (after configuring). + + usbstorage: + ---------- + This configuration directory exercises the USB mass storage + class driver at examples/usbstorage. See examples/README.txt for + more information. + + CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin + diff --git a/nuttx-configs/px4io-v1/common/Make.defs b/nuttx-configs/px4io-v1/common/Make.defs new file mode 100644 index 0000000000..74b183067c --- /dev/null +++ b/nuttx-configs/px4io-v1/common/Make.defs @@ -0,0 +1,171 @@ +############################################################################ +# configs/px4fmu/common/Make.defs +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# +############################################################################ + +# +# Generic Make.defs for the PX4FMU +# Do not specify/use this file directly - it is included by config-specific +# Make.defs in the per-config directories. +# + +include ${TOPDIR}/tools/Config.mk + +# +# We only support building with the ARM bare-metal toolchain from +# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS. +# +CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI + +include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs + +CC = $(CROSSDEV)gcc +CXX = $(CROSSDEV)g++ +CPP = $(CROSSDEV)gcc -E +LD = $(CROSSDEV)ld +AR = $(CROSSDEV)ar rcs +NM = $(CROSSDEV)nm +OBJCOPY = $(CROSSDEV)objcopy +OBJDUMP = $(CROSSDEV)objdump + +MAXOPTIMIZATION = -O3 +ARCHCPUFLAGS = -mcpu=cortex-m3 \ + -mthumb \ + -march=armv7-m + +# use our linker script +LDSCRIPT = ld.script + +ifeq ($(WINTOOL),y) + # Windows-native toolchains + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" + ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" + ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT)}" +else + ifeq ($(PX4_WINTOOL),y) + # Windows-native toolchains (MSYS) + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT) + else + # Linux/Cygwin-native toolchain + MKDEP = $(TOPDIR)/tools/mkdeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT) + endif +endif + +# tool versions +ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} +ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} + +# optimisation flags +ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ + -fno-strict-aliasing \ + -fno-strength-reduce \ + -fomit-frame-pointer \ + -funsafe-math-optimizations \ + -fno-builtin-printf \ + -ffunction-sections \ + -fdata-sections + +ifeq ("${CONFIG_DEBUG_SYMBOLS}","y") +ARCHOPTIMIZATION += -g +endif + +ARCHCFLAGS = -std=gnu99 +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x +ARCHWARNINGS = -Wall \ + -Wextra \ + -Wdouble-promotion \ + -Wshadow \ + -Wfloat-equal \ + -Wframe-larger-than=1024 \ + -Wpointer-arith \ + -Wlogical-op \ + -Wmissing-declarations \ + -Wpacked \ + -Wno-unused-parameter +# -Wcast-qual - generates spurious noreturn attribute warnings, try again later +# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code +# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives + +ARCHCWARNINGS = $(ARCHWARNINGS) \ + -Wbad-function-cast \ + -Wstrict-prototypes \ + -Wold-style-declaration \ + -Wmissing-parameter-type \ + -Wmissing-prototypes \ + -Wnested-externs \ + -Wunsuffixed-float-constants +ARCHWARNINGSXX = $(ARCHWARNINGS) +ARCHDEFINES = +ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 + +# this seems to be the only way to add linker flags +EXTRA_LIBS += --warn-common \ + --gc-sections + +CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common +CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) +CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe +CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) +CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) +AFLAGS = $(CFLAGS) -D__ASSEMBLY__ + +NXFLATLDFLAGS1 = -r -d -warn-common +NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections +LDNXFLATFLAGS = -e main -s 2048 + +OBJEXT = .o +LIBEXT = .a +EXEEXT = + +# produce partially-linked $1 from files in $2 +define PRELINK + @echo "PRELINK: $1" + $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 +endef + +HOSTCC = gcc +HOSTINCLUDES = -I. +HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe +HOSTLDFLAGS = + diff --git a/nuttx-configs/px4io-v1/common/ld.script b/nuttx-configs/px4io-v1/common/ld.script new file mode 100755 index 0000000000..69c2f9cb2e --- /dev/null +++ b/nuttx-configs/px4io-v1/common/ld.script @@ -0,0 +1,129 @@ +/**************************************************************************** + * configs/stm3210e-eval/nsh/ld.script + * + * Copyright (C) 2009, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/* The STM32F100C8 has 64Kb of FLASH beginning at address 0x0800:0000 and + * 8Kb of SRAM beginning at address 0x2000:0000. When booting from FLASH, + * FLASH memory is aliased to address 0x0000:0000 where the code expects to + * begin execution by jumping to the entry point in the 0x0800:0000 address + * range. + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x08001000, LENGTH = 60K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 8K +} + +OUTPUT_ARCH(arm) +ENTRY(__start) /* treat __start as the anchor for dead code stripping */ +EXTERN(_vectors) /* force the vectors to be included in the output */ + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + /* The STM32F100CB has 8Kb of SRAM beginning at the following address */ + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/nuttx-configs/px4io-v1/common/setenv.sh b/nuttx-configs/px4io-v1/common/setenv.sh new file mode 100755 index 0000000000..ff9a4bf8ae --- /dev/null +++ b/nuttx-configs/px4io-v1/common/setenv.sh @@ -0,0 +1,47 @@ +#!/bin/bash +# configs/stm3210e-eval/dfu/setenv.sh +# +# Copyright (C) 2009 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# + +if [ "$(basename $0)" = "setenv.sh" ] ; then + echo "You must source this script, not run it!" 1>&2 + exit 1 +fi + +if [ -z "${PATH_ORIG}" ]; then export PATH_ORIG="${PATH}"; fi + +WD=`pwd` +export RIDE_BIN="/cygdrive/c/Program Files/Raisonance/Ride/arm-gcc/bin" +export BUILDROOT_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin" +export PATH="${BUILDROOT_BIN}:${RIDE_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" + +echo "PATH : ${PATH}" diff --git a/nuttx-configs/px4io-v1/include/README.txt b/nuttx-configs/px4io-v1/include/README.txt new file mode 100755 index 0000000000..2264a80aa8 --- /dev/null +++ b/nuttx-configs/px4io-v1/include/README.txt @@ -0,0 +1 @@ +This directory contains header files unique to the PX4IO board. diff --git a/nuttx-configs/px4io-v1/include/board.h b/nuttx-configs/px4io-v1/include/board.h new file mode 100755 index 0000000000..8150792665 --- /dev/null +++ b/nuttx-configs/px4io-v1/include/board.h @@ -0,0 +1,152 @@ +/************************************************************************************ + * configs/px4io/include/board.h + * include/arch/board/board.h + * + * Copyright (C) 2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * Copyright (C) 2012 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 NuttX 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. + * + ************************************************************************************/ + +#ifndef __ARCH_BOARD_BOARD_H +#define __ARCH_BOARD_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include +#ifndef __ASSEMBLY__ +# include +# include +#endif +#include +#include +#include + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ + +/* On-board crystal frequency is 24MHz (HSE) */ + +#define STM32_BOARD_XTAL 24000000ul + +/* Use the HSE output as the system clock */ + +#define STM32_SYSCLK_SW RCC_CFGR_SW_HSE +#define STM32_SYSCLK_SWS RCC_CFGR_SWS_HSE +#define STM32_SYSCLK_FREQUENCY STM32_BOARD_XTAL + +/* AHB clock (HCLK) is SYSCLK (24MHz) */ + +#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK +#define STM32_HCLK_FREQUENCY STM32_SYSCLK_FREQUENCY +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ + +/* APB2 clock (PCLK2) is HCLK (24MHz) */ + +#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLK +#define STM32_PCLK2_FREQUENCY STM32_HCLK_FREQUENCY +#define STM32_APB2_CLKIN (STM32_PCLK2_FREQUENCY) /* Timers 2-4 */ + +/* APB2 timer 1 will receive PCLK2. */ + +#define STM32_APB2_TIM1_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (STM32_PCLK2_FREQUENCY) + +/* APB1 clock (PCLK1) is HCLK (24MHz) */ + +#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLK +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY) + +/* All timers run off PCLK */ + +#define STM32_APB1_TIM1_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB1_TIM2_CLKIN (STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (STM32_PCLK1_FREQUENCY) + +/* + * Some of the USART pins are not available; override the GPIO + * definitions with an invalid pin configuration. + */ +#undef GPIO_USART2_CTS +#define GPIO_USART2_CTS 0xffffffff +#undef GPIO_USART2_RTS +#define GPIO_USART2_RTS 0xffffffff +#undef GPIO_USART2_CK +#define GPIO_USART2_CK 0xffffffff +#undef GPIO_USART3_TX +#define GPIO_USART3_TX 0xffffffff +#undef GPIO_USART3_CK +#define GPIO_USART3_CK 0xffffffff +#undef GPIO_USART3_CTS +#define GPIO_USART3_CTS 0xffffffff +#undef GPIO_USART3_RTS +#define GPIO_USART3_RTS 0xffffffff + +/************************************************************************************ + * Public Data + ************************************************************************************/ + +#ifndef __ASSEMBLY__ + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +/************************************************************************************ + * Public Function Prototypes + ************************************************************************************/ +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +EXTERN void stm32_boardinitialize(void); + +#if defined(__cplusplus) +} +#endif + +#endif /* __ASSEMBLY__ */ +#endif /* __ARCH_BOARD_BOARD_H */ diff --git a/nuttx-configs/px4io-v1/include/drv_i2c_device.h b/nuttx-configs/px4io-v1/include/drv_i2c_device.h new file mode 100644 index 0000000000..02582bc092 --- /dev/null +++ b/nuttx-configs/px4io-v1/include/drv_i2c_device.h @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 A simple, polled I2C slave-mode driver. + * + * The master writes to and reads from a byte buffer, which the caller + * can update inbetween calls to the FSM. + */ + +extern void i2c_fsm_init(uint8_t *buffer, size_t buffer_size); +extern bool i2c_fsm(void); diff --git a/nuttx-configs/px4io-v1/nsh/Make.defs b/nuttx-configs/px4io-v1/nsh/Make.defs new file mode 100644 index 0000000000..c7f6effd9a --- /dev/null +++ b/nuttx-configs/px4io-v1/nsh/Make.defs @@ -0,0 +1,3 @@ +include ${TOPDIR}/.config + +include $(TOPDIR)/configs/px4io-v1/common/Make.defs diff --git a/nuttx-configs/px4io-v1/nsh/appconfig b/nuttx-configs/px4io-v1/nsh/appconfig new file mode 100644 index 0000000000..48a41bcdb8 --- /dev/null +++ b/nuttx-configs/px4io-v1/nsh/appconfig @@ -0,0 +1,32 @@ +############################################################################ +# +# Copyright (C) 2012 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. +# +############################################################################ diff --git a/nuttx-configs/px4io-v1/nsh/defconfig b/nuttx-configs/px4io-v1/nsh/defconfig new file mode 100755 index 0000000000..5256722333 --- /dev/null +++ b/nuttx-configs/px4io-v1/nsh/defconfig @@ -0,0 +1,559 @@ +############################################################################ +# configs/px4io-v1/nsh/defconfig +# +# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# +############################################################################ +# +# architecture selection +# +# CONFIG_ARCH - identifies the arch subdirectory and, hence, the +# processor architecture. +# CONFIG_ARCH_family - for use in C code. This identifies the +# particular chip family that the architecture is implemented +# in. +# CONFIG_ARCH_architecture - for use in C code. This identifies the +# specific architecture within the chip family. +# CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory +# CONFIG_ARCH_CHIP_name - For use in C code +# CONFIG_ARCH_BOARD - identifies the configs subdirectory and, hence, +# the board that supports the particular chip or SoC. +# CONFIG_ARCH_BOARD_name - for use in C code +# CONFIG_ENDIAN_BIG - define if big endian (default is little endian) +# CONFIG_BOARD_LOOPSPERMSEC - for delay loops +# CONFIG_DRAM_SIZE - Describes the installed DRAM. +# CONFIG_DRAM_START - The start address of DRAM (physical) +# CONFIG_ARCH_IRQPRIO - The ST32F100CB supports interrupt prioritization +# CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt +# stack. If defined, this symbol is the size of the interrupt +# stack in bytes. If not defined, the user task stacks will be +# used during interrupt handling. +# CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions +# CONFIG_ARCH_BOOTLOADER - Set if you are using a bootloader. +# CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to board architecture. +# CONFIG_ARCH_BUTTONS - Enable support for buttons. Unique to board architecture. +# CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that +# cause a 100 second delay during boot-up. This 100 second delay +# serves no purpose other than it allows you to calibrate +# CONFIG_BOARD_LOOPSPERMSEC. You simply use a stop watch to measure +# the 100 second delay then adjust CONFIG_BOARD_LOOPSPERMSEC until +# the delay actually is 100 seconds. +# CONFIG_ARCH_DMA - Support DMA initialization +# +CONFIG_ARCH="arm" +CONFIG_ARCH_ARM=y +CONFIG_ARCH_CORTEXM3=y +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARCH_CHIP_STM32F100C8=y +# +# Board Selection +# +CONFIG_ARCH_BOARD_PX4IO_V1=y +# CONFIG_ARCH_BOARD_CUSTOM is not set +CONFIG_ARCH_BOARD="px4io-v1" +CONFIG_BOARD_LOOPSPERMSEC=2000 +CONFIG_DRAM_SIZE=0x00002000 +CONFIG_DRAM_START=0x20000000 +CONFIG_ARCH_IRQPRIO=y +CONFIG_ARCH_INTERRUPTSTACK=n +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARCH_BOOTLOADER=n +CONFIG_ARCH_LEDS=n +CONFIG_ARCH_BUTTONS=n +CONFIG_ARCH_CALIBRATION=n +CONFIG_ARCH_DMA=y +CONFIG_ARCH_MATH_H=y + +CONFIG_ARMV7M_CMNVECTOR=y +# CONFIG_ARMV7M_STACKCHECK is not set + +# +# JTAG Enable settings (by default JTAG-DP and SW-DP are disabled): +# +# CONFIG_STM32_DFU - Use the DFU bootloader, not JTAG +# +# JTAG Enable options: +# +# CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) +# CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) +# but without JNTRST. +# CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled +# +CONFIG_STM32_DFU=n +CONFIG_STM32_JTAG_FULL_ENABLE=y +CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n +CONFIG_STM32_JTAG_SW_ENABLE=n + +# +# Individual subsystems can be enabled: +# +# AHB: +CONFIG_STM32_DMA1=y +CONFIG_STM32_DMA2=n +CONFIG_STM32_CRC=n +# APB1: +# Timers 2,3 and 4 are owned by the PWM driver +CONFIG_STM32_TIM2=n +CONFIG_STM32_TIM3=n +CONFIG_STM32_TIM4=n +CONFIG_STM32_TIM5=n +CONFIG_STM32_TIM6=n +CONFIG_STM32_TIM7=n +CONFIG_STM32_WWDG=n +CONFIG_STM32_SPI2=n +CONFIG_STM32_USART2=y +CONFIG_STM32_USART3=y +CONFIG_STM32_I2C1=y +CONFIG_STM32_I2C2=n +CONFIG_STM32_BKP=n +CONFIG_STM32_PWR=n +CONFIG_STM32_DAC=n +# APB2: +# We use our own ADC driver, but leave this on for clocking purposes. +CONFIG_STM32_ADC1=y +CONFIG_STM32_ADC2=n +# TIM1 is owned by the HRT +CONFIG_STM32_TIM1=n +CONFIG_STM32_SPI1=n +CONFIG_STM32_TIM8=n +CONFIG_STM32_USART1=y +CONFIG_STM32_ADC3=n + + +# +# STM32F100 specific serial device driver settings +# +# CONFIG_USARTn_SERIAL_CONSOLE - selects the USARTn for the +# console and ttys0 (default is the USART1). +# CONFIG_USARTn_RXBUFSIZE - Characters are buffered as received. +# This specific the size of the receive buffer +# CONFIG_USARTn_TXBUFSIZE - Characters are buffered before +# being sent. This specific the size of the transmit buffer +# CONFIG_USARTn_BAUD - The configure BAUD of the UART. Must be +# CONFIG_USARTn_BITS - The number of bits. Must be either 7 or 8. +# CONFIG_USARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity +# CONFIG_USARTn_2STOP - Two stop bits +# +CONFIG_SERIAL_TERMIOS=y +CONFIG_STANDARD_SERIAL=y + +CONFIG_USART1_SERIAL_CONSOLE=y +CONFIG_USART2_SERIAL_CONSOLE=n +CONFIG_USART3_SERIAL_CONSOLE=n + +CONFIG_USART1_TXBUFSIZE=64 +CONFIG_USART2_TXBUFSIZE=64 +CONFIG_USART3_TXBUFSIZE=64 + +CONFIG_USART1_RXBUFSIZE=64 +CONFIG_USART2_RXBUFSIZE=64 +CONFIG_USART3_RXBUFSIZE=64 + +CONFIG_USART1_BAUD=115200 +CONFIG_USART2_BAUD=115200 +CONFIG_USART3_BAUD=115200 + +CONFIG_USART1_BITS=8 +CONFIG_USART2_BITS=8 +CONFIG_USART3_BITS=8 + +CONFIG_USART1_PARITY=0 +CONFIG_USART2_PARITY=0 +CONFIG_USART3_PARITY=0 + +CONFIG_USART1_2STOP=0 +CONFIG_USART2_2STOP=0 +CONFIG_USART3_2STOP=0 + +CONFIG_USART1_RXDMA=y +SERIAL_HAVE_CONSOLE_DMA=y +# Conflicts with I2C1 DMA +CONFIG_USART2_RXDMA=n +CONFIG_USART3_RXDMA=y + +# +# PX4IO specific driver settings +# +# CONFIG_HRT_TIMER +# Enables the high-resolution timer. The board definition must +# set HRT_TIMER and HRT_TIMER_CHANNEL to the timer and capture/ +# compare channels to be used. +# CONFIG_HRT_PPM +# Enables R/C PPM input using the HRT. The board definition must +# set HRT_PPM_CHANNEL to the timer capture/compare channel to be +# used, and define GPIO_PPM_IN to configure the appropriate timer +# GPIO. +# CONFIG_PWM_SERVO +# Enables the PWM servo driver. The driver configuration must be +# supplied by the board support at initialisation time. +# Note that USART2 must be disabled on the PX4 board for this to +# be available. +# +# +CONFIG_HRT_TIMER=y +CONFIG_HRT_PPM=y + +# +# General build options +# +# CONFIG_RRLOAD_BINARY - make the rrload binary format used with +# BSPs from www.ridgerun.com using the tools/mkimage.sh script +# CONFIG_INTELHEX_BINARY - make the Intel HEX binary format +# used with many different loaders using the GNU objcopy program +# Should not be selected if you are not using the GNU toolchain. +# CONFIG_MOTOROLA_SREC - make the Motorola S-Record binary format +# used with many different loaders using the GNU objcopy program +# Should not be selected if you are not using the GNU toolchain. +# CONFIG_RAW_BINARY - make a raw binary format file used with many +# different loaders using the GNU objcopy program. This option +# should not be selected if you are not using the GNU toolchain. +# CONFIG_HAVE_LIBM - toolchain supports libm.a +# +CONFIG_RRLOAD_BINARY=n +CONFIG_INTELHEX_BINARY=n +CONFIG_MOTOROLA_SREC=n +CONFIG_RAW_BINARY=y +CONFIG_HAVE_LIBM=n + +# +# General OS setup +# +# CONFIG_APPS_DIR - Identifies the relative path to the directory +# that builds the application to link with NuttX. Default: ../apps +# CONFIG_DEBUG - enables built-in debug options +# CONFIG_DEBUG_VERBOSE - enables verbose debug output +# CONFIG_DEBUG_SYMBOLS - build without optimization and with +# debug symbols (needed for use with a debugger). +# CONFIG_HAVE_CXX - Enable support for C++ +# CONFIG_HAVE_CXXINITIALIZE - The platform-specific logic includes support +# for initialization of static C++ instances for this architecture +# and for the selected toolchain (via up_cxxinitialize()). +# CONFIG_MM_REGIONS - If the architecture includes multiple +# regions of memory to allocate from, this specifies the +# number of memory regions that the memory manager must +# handle and enables the API mm_addregion(start, end); +# CONFIG_ARCH_LOWPUTC - architecture supports low-level, boot +# time console output +# CONFIG_MSEC_PER_TICK - The default system timer is 100Hz +# or MSEC_PER_TICK=10. This setting may be defined to +# inform NuttX that the processor hardware is providing +# system timer interrupts at some interrupt interval other +# than 10 msec. +# CONFIG_RR_INTERVAL - The round robin timeslice will be set +# this number of milliseconds; Round robin scheduling can +# be disabled by setting this value to zero. +# CONFIG_SCHED_INSTRUMENTATION - enables instrumentation in +# scheduler to monitor system performance +# CONFIG_TASK_NAME_SIZE - Spcifies that maximum size of a +# task name to save in the TCB. Useful if scheduler +# instrumentation is selected. Set to zero to disable. +# CONFIG_START_YEAR, CONFIG_START_MONTH, CONFIG_START_DAY - +# Used to initialize the internal time logic. +# CONFIG_GREGORIAN_TIME - Enables Gregorian time conversions. +# You would only need this if you are concerned about accurate +# time conversions in the past or in the distant future. +# CONFIG_JULIAN_TIME - Enables Julian time conversions. You +# would only need this if you are concerned about accurate +# time conversion in the distand past. You must also define +# CONFIG_GREGORIAN_TIME in order to use Julian time. +# CONFIG_DEV_CONSOLE - Set if architecture-specific logic +# provides /dev/console. Enables stdout, stderr, stdin. +# CONFIG_DEV_LOWCONSOLE - Use the simple, low-level serial console +# driver (minimul support) +# CONFIG_MUTEX_TYPES: Set to enable support for recursive and +# errorcheck mutexes. Enables pthread_mutexattr_settype(). +# CONFIG_PRIORITY_INHERITANCE : Set to enable support for priority +# inheritance on mutexes and semaphores. +# CONFIG_SEM_PREALLOCHOLDERS: This setting is only used if priority +# inheritance is enabled. It defines the maximum number of +# different threads (minus one) that can take counts on a +# semaphore with priority inheritance support. This may be +# set to zero if priority inheritance is disabled OR if you +# are only using semaphores as mutexes (only one holder) OR +# if no more than two threads participate using a counting +# semaphore. +# CONFIG_SEM_NNESTPRIO. If priority inheritance is enabled, +# then this setting is the maximum number of higher priority +# threads (minus 1) than can be waiting for another thread +# to release a count on a semaphore. This value may be set +# to zero if no more than one thread is expected to wait for +# a semaphore. +# CONFIG_FDCLONE_DISABLE. Disable cloning of all file descriptors +# by task_create() when a new task is started. If set, all +# files/drivers will appear to be closed in the new task. +# CONFIG_FDCLONE_STDIO. Disable cloning of all but the first +# three file descriptors (stdin, stdout, stderr) by task_create() +# when a new task is started. If set, all files/drivers will +# appear to be closed in the new task except for stdin, stdout, +# and stderr. +# CONFIG_SDCLONE_DISABLE. Disable cloning of all socket +# desciptors by task_create() when a new task is started. If +# set, all sockets will appear to be closed in the new task. +# CONFIG_SCHED_WORKQUEUE. Create a dedicated "worker" thread to +# handle delayed processing from interrupt handlers. This feature +# is required for some drivers but, if there are not complaints, +# can be safely disabled. The worker thread also performs +# garbage collection -- completing any delayed memory deallocations +# from interrupt handlers. If the worker thread is disabled, +# then that clean will be performed by the IDLE thread instead +# (which runs at the lowest of priority and may not be appropriate +# if memory reclamation is of high priority). If CONFIG_SCHED_WORKQUEUE +# is enabled, then the following options can also be used: +# CONFIG_SCHED_WORKPRIORITY - The execution priority of the worker +# thread. Default: 50 +# CONFIG_SCHED_WORKPERIOD - How often the worker thread checks for +# work in units of microseconds. Default: 50*1000 (50 MS). +# CONFIG_SCHED_WORKSTACKSIZE - The stack size allocated for the worker +# thread. Default: CONFIG_IDLETHREAD_STACKSIZE. +# CONFIG_SIG_SIGWORK - The signal number that will be used to wake-up +# the worker thread. Default: 4 +# CONFIG_SCHED_WAITPID - Enable the waitpid() API +# CONFIG_SCHED_ATEXIT - Enabled the atexit() API +# +CONFIG_USER_ENTRYPOINT="user_start" +#CONFIG_APPS_DIR= +CONFIG_DEBUG=n +CONFIG_DEBUG_VERBOSE=n +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_FS=n +CONFIG_DEBUG_GRAPHICS=n +CONFIG_DEBUG_LCD=n +CONFIG_DEBUG_USB=n +CONFIG_DEBUG_NET=n +CONFIG_DEBUG_RTC=n +CONFIG_DEBUG_ANALOG=n +CONFIG_DEBUG_PWM=n +CONFIG_DEBUG_CAN=n +CONFIG_DEBUG_I2C=n +CONFIG_DEBUG_INPUT=n + +CONFIG_MSEC_PER_TICK=1 +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_MM_REGIONS=1 +CONFIG_MM_SMALL=y +CONFIG_ARCH_LOWPUTC=y +CONFIG_RR_INTERVAL=0 +CONFIG_SCHED_INSTRUMENTATION=n +CONFIG_TASK_NAME_SIZE=8 +CONFIG_START_YEAR=1970 +CONFIG_START_MONTH=1 +CONFIG_START_DAY=1 +CONFIG_GREGORIAN_TIME=n +CONFIG_JULIAN_TIME=n +# this eats ~1KiB of RAM ... work out why +CONFIG_DEV_CONSOLE=y +CONFIG_DEV_LOWCONSOLE=n +CONFIG_MUTEX_TYPES=n +CONFIG_PRIORITY_INHERITANCE=n +CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_NNESTPRIO=0 +CONFIG_FDCLONE_DISABLE=y +CONFIG_FDCLONE_STDIO=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SCHED_WORKQUEUE=n +CONFIG_SCHED_WORKPRIORITY=50 +CONFIG_SCHED_WORKPERIOD=50000 +CONFIG_SCHED_WORKSTACKSIZE=1024 +CONFIG_SIG_SIGWORK=4 +CONFIG_SCHED_WAITPID=n +CONFIG_SCHED_ATEXIT=n + +# +# The following can be used to disable categories of +# APIs supported by the OS. If the compiler supports +# weak functions, then it should not be necessary to +# disable functions unless you want to restrict usage +# of those APIs. +# +# There are certain dependency relationships in these +# features. +# +# o mq_notify logic depends on signals to awaken tasks +# waiting for queues to become full or empty. +# o pthread_condtimedwait() depends on signals to wake +# up waiting tasks. +# +CONFIG_DISABLE_CLOCK=n +CONFIG_DISABLE_POSIX_TIMERS=y +CONFIG_DISABLE_PTHREAD=y +CONFIG_DISABLE_SIGNALS=y +CONFIG_DISABLE_MQUEUE=y +CONFIG_DISABLE_MOUNTPOINT=y +CONFIG_DISABLE_ENVIRON=y +CONFIG_DISABLE_POLL=y + +# +# Misc libc settings +# +# CONFIG_NOPRINTF_FIELDWIDTH - sprintf-related logic is a +# little smaller if we do not support fieldwidthes +# +CONFIG_NOPRINTF_FIELDWIDTH=n + +# +# Allow for architecture optimized implementations +# +# The architecture can provide optimized versions of the +# following to improve system performance +# +CONFIG_ARCH_MEMCPY=n +CONFIG_ARCH_MEMCMP=n +CONFIG_ARCH_MEMMOVE=n +CONFIG_ARCH_MEMSET=n +CONFIG_ARCH_STRCMP=n +CONFIG_ARCH_STRCPY=n +CONFIG_ARCH_STRNCPY=n +CONFIG_ARCH_STRLEN=n +CONFIG_ARCH_STRNLEN=n +CONFIG_ARCH_BZERO=n + +# +# Sizes of configurable things (0 disables) +# +# CONFIG_MAX_TASKS - The maximum number of simultaneously +# active tasks. This value must be a power of two. +# CONFIG_MAX_TASK_ARGS - This controls the maximum number of +# of parameters that a task may receive (i.e., maxmum value +# of 'argc') +# CONFIG_NPTHREAD_KEYS - The number of items of thread- +# specific data that can be retained +# CONFIG_NFILE_DESCRIPTORS - The maximum number of file +# descriptors (one for each open) +# CONFIG_NFILE_STREAMS - The maximum number of streams that +# can be fopen'ed +# CONFIG_NAME_MAX - The maximum size of a file name. +# CONFIG_STDIO_BUFFER_SIZE - Size of the buffer to allocate +# on fopen. (Only if CONFIG_NFILE_STREAMS > 0) +# CONFIG_STDIO_LINEBUFFER - If standard C buffered I/O is enabled +# (CONFIG_STDIO_BUFFER_SIZE > 0), then this option may be added +# to force automatic, line-oriented flushing the output buffer +# for putc(), fputc(), putchar(), puts(), fputs(), printf(), +# fprintf(), and vfprintf(). When a newline is encountered in +# the output string, the output buffer will be flushed. This +# (slightly) increases the NuttX footprint but supports the kind +# of behavior that people expect for printf(). +# CONFIG_NUNGET_CHARS - Number of characters that can be +# buffered by ungetc() (Only if CONFIG_NFILE_STREAMS > 0) +# CONFIG_PREALLOC_MQ_MSGS - The number of pre-allocated message +# structures. The system manages a pool of preallocated +# message structures to minimize dynamic allocations +# CONFIG_MQ_MAXMSGSIZE - Message structures are allocated with +# a fixed payload size given by this settin (does not include +# other message structure overhead. +# CONFIG_MAX_WDOGPARMS - Maximum number of parameters that +# can be passed to a watchdog handler +# CONFIG_PREALLOC_WDOGS - The number of pre-allocated watchdog +# structures. The system manages a pool of preallocated +# watchdog structures to minimize dynamic allocations +# CONFIG_PREALLOC_TIMERS - The number of pre-allocated POSIX +# timer structures. The system manages a pool of preallocated +# timer structures to minimize dynamic allocations. Set to +# zero for all dynamic allocations. +# +CONFIG_MAX_TASKS=4 +CONFIG_MAX_TASK_ARGS=4 +CONFIG_NPTHREAD_KEYS=2 +CONFIG_NFILE_DESCRIPTORS=8 +CONFIG_NFILE_STREAMS=0 +CONFIG_NAME_MAX=12 +CONFIG_STDIO_BUFFER_SIZE=32 +CONFIG_STDIO_LINEBUFFER=n +CONFIG_NUNGET_CHARS=2 +CONFIG_PREALLOC_MQ_MSGS=4 +CONFIG_MQ_MAXMSGSIZE=32 +CONFIG_MAX_WDOGPARMS=2 +CONFIG_PREALLOC_WDOGS=4 +CONFIG_PREALLOC_TIMERS=0 + + +# +# Settings for apps/nshlib +# +# CONFIG_NSH_BUILTIN_APPS - Support external registered, +# "named" applications that can be executed from the NSH +# command line (see apps/README.txt for more information). +# CONFIG_NSH_FILEIOSIZE - Size of a static I/O buffer +# CONFIG_NSH_STRERROR - Use strerror(errno) +# CONFIG_NSH_LINELEN - Maximum length of one command line +# CONFIG_NSH_NESTDEPTH - Max number of nested if-then[-else]-fi +# CONFIG_NSH_DISABLESCRIPT - Disable scripting support +# CONFIG_NSH_DISABLEBG - Disable background commands +# CONFIG_NSH_ROMFSETC - Use startup script in /etc +# CONFIG_NSH_CONSOLE - Use serial console front end +# CONFIG_NSH_TELNET - Use telnetd console front end +# CONFIG_NSH_ARCHINIT - Platform provides architecture +# specific initialization (nsh_archinitialize()). +# + +# Disable NSH completely +CONFIG_NSH_CONSOLE=n + +# +# Stack and heap information +# +# CONFIG_BOOT_RUNFROMFLASH - Some configurations support XIP +# operation from FLASH but must copy initialized .data sections to RAM. +# (should also be =n for the STM3210E-EVAL which always runs from flash) +# CONFIG_BOOT_COPYTORAM - Some configurations boot in FLASH +# but copy themselves entirely into RAM for better performance. +# CONFIG_CUSTOM_STACK - The up_ implementation will handle +# all stack operations outside of the nuttx model. +# CONFIG_STACK_POINTER - The initial stack pointer (arm7tdmi only) +# CONFIG_IDLETHREAD_STACKSIZE - The size of the initial stack. +# This is the thread that (1) performs the inital boot of the system up +# to the point where user_start() is spawned, and (2) there after is the +# IDLE thread that executes only when there is no other thread ready to +# run. +# CONFIG_USERMAIN_STACKSIZE - The size of the stack to allocate +# for the main user thread that begins at the user_start() entry point. +# CONFIG_PTHREAD_STACK_MIN - Minimum pthread stack size +# CONFIG_PTHREAD_STACK_DEFAULT - Default pthread stack size +# CONFIG_HEAP_BASE - The beginning of the heap +# CONFIG_HEAP_SIZE - The size of the heap +# +CONFIG_BOOT_RUNFROMFLASH=n +CONFIG_BOOT_COPYTORAM=n +CONFIG_CUSTOM_STACK=n +CONFIG_STACK_POINTER= +CONFIG_IDLETHREAD_STACKSIZE=1024 +CONFIG_USERMAIN_STACKSIZE=1200 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_PTHREAD_STACK_DEFAULT=1024 +CONFIG_HEAP_BASE= +CONFIG_HEAP_SIZE= + +# +# NSH Library +# +# CONFIG_NSH_LIBRARY is not set +# CONFIG_NSH_BUILTIN_APPS is not set diff --git a/nuttx-configs/px4io-v1/nsh/setenv.sh b/nuttx-configs/px4io-v1/nsh/setenv.sh new file mode 100755 index 0000000000..ff9a4bf8ae --- /dev/null +++ b/nuttx-configs/px4io-v1/nsh/setenv.sh @@ -0,0 +1,47 @@ +#!/bin/bash +# configs/stm3210e-eval/dfu/setenv.sh +# +# Copyright (C) 2009 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# + +if [ "$(basename $0)" = "setenv.sh" ] ; then + echo "You must source this script, not run it!" 1>&2 + exit 1 +fi + +if [ -z "${PATH_ORIG}" ]; then export PATH_ORIG="${PATH}"; fi + +WD=`pwd` +export RIDE_BIN="/cygdrive/c/Program Files/Raisonance/Ride/arm-gcc/bin" +export BUILDROOT_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin" +export PATH="${BUILDROOT_BIN}:${RIDE_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" + +echo "PATH : ${PATH}" diff --git a/nuttx-configs/px4io-v1/src/Makefile b/nuttx-configs/px4io-v1/src/Makefile new file mode 100644 index 0000000000..bb9539d16a --- /dev/null +++ b/nuttx-configs/px4io-v1/src/Makefile @@ -0,0 +1,84 @@ +############################################################################ +# configs/stm3210e-eval/src/Makefile +# +# Copyright (C) 2009-2010 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# +############################################################################ + +-include $(TOPDIR)/Make.defs + +CFLAGS += -I$(TOPDIR)/sched + +ASRCS = +AOBJS = $(ASRCS:.S=$(OBJEXT)) + +CSRCS = empty.c + +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ARCH_SRCDIR = $(TOPDIR)/arch/$(CONFIG_ARCH)/src +ifeq ($(WINTOOL),y) + CFLAGS += -I "${shell cygpath -w $(ARCH_SRCDIR)/chip}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/common}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/armv7-m}" +else + CFLAGS += -I$(ARCH_SRCDIR)/chip -I$(ARCH_SRCDIR)/common -I$(ARCH_SRCDIR)/armv7-m +endif + +all: libboard$(LIBEXT) + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS) $(LINKOBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +libboard$(LIBEXT): $(OBJS) + $(call ARCHIVE, $@, $(OBJS)) + +.depend: Makefile $(SRCS) + @$(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @touch $@ + +depend: .depend + +clean: + $(call DELFILE, libboard$(LIBEXT)) + $(call CLEAN) + +distclean: clean + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) + +-include Make.dep diff --git a/nuttx-configs/px4io-v1/src/README.txt b/nuttx-configs/px4io-v1/src/README.txt new file mode 100644 index 0000000000..d4eda82fd7 --- /dev/null +++ b/nuttx-configs/px4io-v1/src/README.txt @@ -0,0 +1 @@ +This directory contains drivers unique to the STMicro STM3210E-EVAL development board. diff --git a/nuttx-configs/px4io-v1/src/drv_i2c_device.c b/nuttx-configs/px4io-v1/src/drv_i2c_device.c new file mode 100644 index 0000000000..1f5931ae5e --- /dev/null +++ b/nuttx-configs/px4io-v1/src/drv_i2c_device.c @@ -0,0 +1,618 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 A simple, polled I2C slave-mode driver. + * + * The master writes to and reads from a byte buffer, which the caller + * can update inbetween calls to the FSM. + */ + +#include + +#include "stm32_i2c.h" + +#include + +/* + * I2C register definitions. + */ +#define I2C_BASE STM32_I2C1_BASE + +#define REG(_reg) (*(volatile uint32_t *)(I2C_BASE + _reg)) + +#define rCR1 REG(STM32_I2C_CR1_OFFSET) +#define rCR2 REG(STM32_I2C_CR2_OFFSET) +#define rOAR1 REG(STM32_I2C_OAR1_OFFSET) +#define rOAR2 REG(STM32_I2C_OAR2_OFFSET) +#define rDR REG(STM32_I2C_DR_OFFSET) +#define rSR1 REG(STM32_I2C_SR1_OFFSET) +#define rSR2 REG(STM32_I2C_SR2_OFFSET) +#define rCCR REG(STM32_I2C_CCR_OFFSET) +#define rTRISE REG(STM32_I2C_TRISE_OFFSET) + +/* + * "event" values (cr2 << 16 | cr1) as described in the ST DriverLib + */ +#define I2C_EVENT_SLAVE_RECEIVER_ADDRESS_MATCHED ((uint32_t)0x00020002) /* BUSY and ADDR flags */ +#define I2C_EVENT_SLAVE_TRANSMITTER_ADDRESS_MATCHED ((uint32_t)0x00060082) /* TRA, BUSY, TXE and ADDR flags */ +#define I2C_EVENT_SLAVE_BYTE_RECEIVED ((uint32_t)0x00020040) /* BUSY and RXNE flags */ +#define I2C_EVENT_SLAVE_STOP_DETECTED ((uint32_t)0x00000010) /* STOPF flag */ +#define I2C_EVENT_SLAVE_BYTE_TRANSMITTED ((uint32_t)0x00060084) /* TRA, BUSY, TXE and BTF flags */ +#define I2C_EVENT_SLAVE_BYTE_TRANSMITTING ((uint32_t)0x00060080) /* TRA, BUSY and TXE flags */ +#define I2C_EVENT_SLAVE_ACK_FAILURE ((uint32_t)0x00000400) /* AF flag */ + +/** + * States implemented by the I2C FSM. + */ +enum fsm_state { + BAD_PHASE, // must be zero, default exit on a bad state transition + + WAIT_FOR_MASTER, + + /* write from master */ + WAIT_FOR_COMMAND, + RECEIVE_COMMAND, + RECEIVE_DATA, + HANDLE_COMMAND, + + /* read from master */ + WAIT_TO_SEND, + SEND_STATUS, + SEND_DATA, + + NUM_STATES +}; + +/** + * Events recognised by the I2C FSM. + */ +enum fsm_event { + /* automatic transition */ + AUTO, + + /* write from master */ + ADDRESSED_WRITE, + BYTE_RECEIVED, + STOP_RECEIVED, + + /* read from master */ + ADDRESSED_READ, + BYTE_SENDABLE, + ACK_FAILED, + + NUM_EVENTS +}; + +/** + * Context for the I2C FSM + */ +static struct fsm_context { + enum fsm_state state; + + /* XXX want to eliminate these */ + uint8_t command; + uint8_t status; + + uint8_t *data_ptr; + uint32_t data_count; + + size_t buffer_size; + uint8_t *buffer; +} context; + +/** + * Structure defining one FSM state and its outgoing transitions. + */ +struct fsm_transition { + void (*handler)(void); + enum fsm_state next_state[NUM_EVENTS]; +}; + +static bool i2c_command_received; + +static void fsm_event(enum fsm_event event); + +static void go_bad(void); +static void go_wait_master(void); + +static void go_wait_command(void); +static void go_receive_command(void); +static void go_receive_data(void); +static void go_handle_command(void); + +static void go_wait_send(void); +static void go_send_status(void); +static void go_send_buffer(void); + +/** + * The FSM state graph. + */ +static const struct fsm_transition fsm[NUM_STATES] = { + [BAD_PHASE] = { + .handler = go_bad, + .next_state = { + [AUTO] = WAIT_FOR_MASTER, + }, + }, + + [WAIT_FOR_MASTER] = { + .handler = go_wait_master, + .next_state = { + [ADDRESSED_WRITE] = WAIT_FOR_COMMAND, + [ADDRESSED_READ] = WAIT_TO_SEND, + }, + }, + + /* write from master*/ + [WAIT_FOR_COMMAND] = { + .handler = go_wait_command, + .next_state = { + [BYTE_RECEIVED] = RECEIVE_COMMAND, + [STOP_RECEIVED] = WAIT_FOR_MASTER, + }, + }, + [RECEIVE_COMMAND] = { + .handler = go_receive_command, + .next_state = { + [BYTE_RECEIVED] = RECEIVE_DATA, + [STOP_RECEIVED] = HANDLE_COMMAND, + }, + }, + [RECEIVE_DATA] = { + .handler = go_receive_data, + .next_state = { + [BYTE_RECEIVED] = RECEIVE_DATA, + [STOP_RECEIVED] = HANDLE_COMMAND, + }, + }, + [HANDLE_COMMAND] = { + .handler = go_handle_command, + .next_state = { + [AUTO] = WAIT_FOR_MASTER, + }, + }, + + /* buffer send */ + [WAIT_TO_SEND] = { + .handler = go_wait_send, + .next_state = { + [BYTE_SENDABLE] = SEND_STATUS, + }, + }, + [SEND_STATUS] = { + .handler = go_send_status, + .next_state = { + [BYTE_SENDABLE] = SEND_DATA, + [ACK_FAILED] = WAIT_FOR_MASTER, + }, + }, + [SEND_DATA] = { + .handler = go_send_buffer, + .next_state = { + [BYTE_SENDABLE] = SEND_DATA, + [ACK_FAILED] = WAIT_FOR_MASTER, + }, + }, +}; + + +/* debug support */ +#if 1 +struct fsm_logentry { + char kind; + uint32_t code; +}; + +#define LOG_ENTRIES 32 +static struct fsm_logentry fsm_log[LOG_ENTRIES]; +int fsm_logptr; +#define LOG_NEXT(_x) (((_x) + 1) % LOG_ENTRIES) +#define LOGx(_kind, _code) \ + do { \ + fsm_log[fsm_logptr].kind = _kind; \ + fsm_log[fsm_logptr].code = _code; \ + fsm_logptr = LOG_NEXT(fsm_logptr); \ + fsm_log[fsm_logptr].kind = 0; \ + } while(0) + +#define LOG(_kind, _code) \ + do {\ + if (fsm_logptr < LOG_ENTRIES) { \ + fsm_log[fsm_logptr].kind = _kind; \ + fsm_log[fsm_logptr].code = _code; \ + fsm_logptr++;\ + }\ + }while(0) + +#else +#define LOG(_kind, _code) +#endif + + +static void i2c_setclock(uint32_t frequency); + +/** + * Initialise I2C + * + */ +void +i2c_fsm_init(uint8_t *buffer, size_t buffer_size) +{ + /* save the buffer */ + context.buffer = buffer; + context.buffer_size = buffer_size; + + // initialise the FSM + context.status = 0; + context.command = 0; + context.state = BAD_PHASE; + fsm_event(AUTO); + +#if 0 + // enable the i2c block clock and reset it + modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_I2C1EN); + modifyreg32(STM32_RCC_APB1RSTR, 0, RCC_APB1RSTR_I2C1RST); + modifyreg32(STM32_RCC_APB1RSTR, RCC_APB1RSTR_I2C1RST, 0); + + // configure the i2c GPIOs + stm32_configgpio(GPIO_I2C1_SCL); + stm32_configgpio(GPIO_I2C1_SDA); + + // set the peripheral clock to match the APB clock + rCR2 = STM32_PCLK1_FREQUENCY / 1000000; + + // configure for 100kHz operation + i2c_setclock(100000); + + // enable i2c + rCR1 = I2C_CR1_PE; +#endif +} + +/** + * Run the I2C FSM for some period. + * + * @return True if the buffer has been updated by a command. + */ +bool +i2c_fsm(void) +{ + uint32_t event; + int idle_iterations = 0; + + for (;;) { + // handle bus error states by discarding the current operation + if (rSR1 & I2C_SR1_BERR) { + context.state = WAIT_FOR_MASTER; + rSR1 = ~I2C_SR1_BERR; + } + + // we do not anticipate over/underrun errors as clock-stretching is enabled + + // fetch the most recent event + event = ((rSR2 << 16) | rSR1) & 0x00ffffff; + + // generate FSM events based on I2C events + switch (event) { + case I2C_EVENT_SLAVE_RECEIVER_ADDRESS_MATCHED: + LOG('w', 0); + fsm_event(ADDRESSED_WRITE); + break; + + case I2C_EVENT_SLAVE_TRANSMITTER_ADDRESS_MATCHED: + LOG('r', 0); + fsm_event(ADDRESSED_READ); + break; + + case I2C_EVENT_SLAVE_BYTE_RECEIVED: + LOG('R', 0); + fsm_event(BYTE_RECEIVED); + break; + + case I2C_EVENT_SLAVE_STOP_DETECTED: + LOG('s', 0); + fsm_event(STOP_RECEIVED); + break; + + case I2C_EVENT_SLAVE_BYTE_TRANSMITTING: + //case I2C_EVENT_SLAVE_BYTE_TRANSMITTED: + LOG('T', 0); + fsm_event(BYTE_SENDABLE); + break; + + case I2C_EVENT_SLAVE_ACK_FAILURE: + LOG('a', 0); + fsm_event(ACK_FAILED); + break; + + default: + idle_iterations++; +// if ((event) && (event != 0x00020000)) +// LOG('e', event); + break; + } + + /* if we have just received something, drop out and let the caller handle it */ + if (i2c_command_received) { + i2c_command_received = false; + return true; + } + + /* if we have done nothing recently, drop out and let the caller have a slice */ + if (idle_iterations > 1000) + return false; + } +} + +/** + * Update the FSM with an event + * + * @param event New event. + */ +static void +fsm_event(enum fsm_event event) +{ + // move to the next state + context.state = fsm[context.state].next_state[event]; + + LOG('f', context.state); + + // call the state entry handler + if (fsm[context.state].handler) { + fsm[context.state].handler(); + } +} + +static void +go_bad() +{ + LOG('B', 0); + fsm_event(AUTO); +} + +/** + * Wait for the master to address us. + * + */ +static void +go_wait_master() +{ + // We currently don't have a command byte. + // + context.command = '\0'; + + // The data pointer starts pointing to the start of the data buffer. + // + context.data_ptr = context.buffer; + + // The data count is either: + // - the size of the data buffer + // - some value less than or equal the size of the data buffer during a write or a read + // + context.data_count = context.buffer_size; + + // (re)enable the peripheral, clear the stop event flag in + // case we just finished receiving data + rCR1 |= I2C_CR1_PE; + + // clear the ACK failed flag in case we just finished sending data + rSR1 = ~I2C_SR1_AF; +} + +/** + * Prepare to receive a command byte. + * + */ +static void +go_wait_command() +{ + // NOP +} + +/** + * Command byte has been received, save it and prepare to handle the data. + * + */ +static void +go_receive_command() +{ + + // fetch the command byte + context.command = (uint8_t)rDR; + LOG('c', context.command); + +} + +/** + * Receive a data byte. + * + */ +static void +go_receive_data() +{ + uint8_t d; + + // fetch the byte + d = (uint8_t)rDR; + LOG('d', d); + + // if we have somewhere to put it, do so + if (context.data_count) { + *context.data_ptr++ = d; + context.data_count--; + } +} + +/** + * Handle a command once the host is done sending it to us. + * + */ +static void +go_handle_command() +{ + // presume we are happy with the command + context.status = 0; + + // make a note that the buffer contains a fresh command + i2c_command_received = true; + + // kick along to the next state + fsm_event(AUTO); +} + +/** + * Wait to be able to send the status byte. + * + */ +static void +go_wait_send() +{ + // NOP +} + +/** + * Send the status byte. + * + */ +static void +go_send_status() +{ + rDR = context.status; + LOG('?', context.status); +} + +/** + * Send a data or pad byte. + * + */ +static void +go_send_buffer() +{ + if (context.data_count) { + LOG('D', *context.data_ptr); + rDR = *(context.data_ptr++); + context.data_count--; + } else { + LOG('-', 0); + rDR = 0xff; + } +} + +/* cribbed directly from the NuttX master driver */ +static void +i2c_setclock(uint32_t frequency) +{ + uint16_t cr1; + uint16_t ccr; + uint16_t trise; + uint16_t freqmhz; + uint16_t speed; + + /* Disable the selected I2C peripheral to configure TRISE */ + + cr1 = rCR1; + rCR1 &= ~I2C_CR1_PE; + + /* Update timing and control registers */ + + freqmhz = (uint16_t)(STM32_PCLK1_FREQUENCY / 1000000); + ccr = 0; + + /* Configure speed in standard mode */ + + if (frequency <= 100000) { + /* Standard mode speed calculation */ + + speed = (uint16_t)(STM32_PCLK1_FREQUENCY / (frequency << 1)); + + /* The CCR fault must be >= 4 */ + + if (speed < 4) { + /* Set the minimum allowed value */ + + speed = 4; + } + ccr |= speed; + + /* Set Maximum Rise Time for standard mode */ + + trise = freqmhz + 1; + + /* Configure speed in fast mode */ + } else { /* (frequency <= 400000) */ + /* Fast mode speed calculation with Tlow/Thigh = 16/9 */ + +#ifdef CONFIG_I2C_DUTY16_9 + speed = (uint16_t)(STM32_PCLK1_FREQUENCY / (frequency * 25)); + + /* Set DUTY and fast speed bits */ + + ccr |= (I2C_CCR_DUTY|I2C_CCR_FS); +#else + /* Fast mode speed calculation with Tlow/Thigh = 2 */ + + speed = (uint16_t)(STM32_PCLK1_FREQUENCY / (frequency * 3)); + + /* Set fast speed bit */ + + ccr |= I2C_CCR_FS; +#endif + + /* Verify that the CCR speed value is nonzero */ + + if (speed < 1) { + /* Set the minimum allowed value */ + + speed = 1; + } + ccr |= speed; + + /* Set Maximum Rise Time for fast mode */ + + trise = (uint16_t)(((freqmhz * 300) / 1000) + 1); + } + + /* Write the new values of the CCR and TRISE registers */ + + rCCR = ccr; + rTRISE = trise; + + /* Bit 14 of OAR1 must be configured and kept at 1 */ + + rOAR1 = I2C_OAR1_ONE); + + /* Re-enable the peripheral (or not) */ + + rCR1 = cr1; +} diff --git a/nuttx-configs/px4io-v1/src/empty.c b/nuttx-configs/px4io-v1/src/empty.c new file mode 100644 index 0000000000..ace900866c --- /dev/null +++ b/nuttx-configs/px4io-v1/src/empty.c @@ -0,0 +1,4 @@ +/* + * There are no source files here, but libboard.a can't be empty, so + * we have this empty source file to keep it company. + */ diff --git a/nuttx-configs/px4io-v2/include/board.h b/nuttx-configs/px4io-v2/include/board.h index b93ad42203..4b9c906385 100755 --- a/nuttx-configs/px4io-v2/include/board.h +++ b/nuttx-configs/px4io-v2/include/board.h @@ -112,34 +112,6 @@ #undef GPIO_USART3_RTS #define GPIO_USART3_RTS 0xffffffff -/* - * High-resolution timer - */ -#define HRT_TIMER 1 /* use timer1 for the HRT */ -#define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 2 */ - -/* - * PPM - * - * PPM input is handled by the HRT timer. - * - * Pin is PA8, timer 1, channel 1 - */ -#define HRT_PPM_CHANNEL 1 /* use capture/compare channel 1 */ -#define GPIO_PPM_IN GPIO_TIM1_CH1IN - -/* LED definitions ******************************************************************/ -/* PX4 has two LEDs that we will encode as: */ - -#define LED_STARTED 0 /* LED? */ -#define LED_HEAPALLOCATE 1 /* LED? */ -#define LED_IRQSENABLED 2 /* LED? + LED? */ -#define LED_STACKCREATED 3 /* LED? */ -#define LED_INIRQ 4 /* LED? + LED? */ -#define LED_SIGNAL 5 /* LED? + LED? */ -#define LED_ASSERTION 6 /* LED? + LED? + LED? */ -#define LED_PANIC 7 /* N/C + N/C + N/C + LED? */ - /************************************************************************************ * Public Data ************************************************************************************/ diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 8d2eb056e9..b4c5fa06e8 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -92,12 +92,6 @@ #include -__BEGIN_DECLS -#include -__END_DECLS -#include -#include - #include #include #include @@ -107,15 +101,19 @@ __END_DECLS #include #include #include - -#include +#include #include #include #include +#include + +#include + +#include +#include -#include #include #include #include diff --git a/src/drivers/bma180/bma180.cpp b/src/drivers/bma180/bma180.cpp index 4409a8a9cb..cfb6256704 100644 --- a/src/drivers/bma180/bma180.cpp +++ b/src/drivers/bma180/bma180.cpp @@ -59,7 +59,7 @@ #include #include -#include +#include #include #include diff --git a/src/drivers/boards/px4fmu/px4fmu_internal.h b/src/drivers/boards/px4fmu-v1/board_config.h similarity index 79% rename from src/drivers/boards/px4fmu/px4fmu_internal.h rename to src/drivers/boards/px4fmu-v1/board_config.h index 56173abf62..9d7c81f859 100644 --- a/src/drivers/boards/px4fmu/px4fmu_internal.h +++ b/src/drivers/boards/px4fmu-v1/board_config.h @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file px4fmu_internal.h + * @file board_config.h * * PX4FMU internal definitions */ @@ -51,7 +51,7 @@ __BEGIN_DECLS /* these headers are not C++ safe */ #include - +#include /**************************************************************************************************** * Definitions @@ -77,15 +77,45 @@ __BEGIN_DECLS /* External interrupts */ #define GPIO_EXTI_COMPASS (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN1) -// XXX MPU6000 DRDY? /* SPI chip selects */ - #define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN14) #define GPIO_SPI_CS_ACCEL (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) #define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0) #define GPIO_SPI_CS_SDCARD (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4) +/* + * Use these in place of the spi_dev_e enumeration to + * select a specific SPI device on SPI1 + */ +#define PX4_SPIDEV_GYRO 1 +#define PX4_SPIDEV_ACCEL 2 +#define PX4_SPIDEV_MPU 3 + +/* + * Optional devices on IO's external port + */ +#define PX4_SPIDEV_ACCEL_MAG 2 + +/* + * I2C busses + */ +#define PX4_I2C_BUS_ESC 1 +#define PX4_I2C_BUS_ONBOARD 2 +#define PX4_I2C_BUS_EXPANSION 3 + +/* + * Devices on the onboard bus. + * + * Note that these are unshifted addresses. + */ +#define PX4_I2C_OBDEV_HMC5883 0x1e +#define PX4_I2C_OBDEV_MS5611 0x76 +#define PX4_I2C_OBDEV_EEPROM NOTDEFINED + +#define PX4_I2C_OBDEV_PX4IO_BL 0x18 +#define PX4_I2C_OBDEV_PX4IO 0x1a + /* User GPIOs * * GPIO0-1 are the buffered high-power GPIOs. @@ -110,31 +140,45 @@ __BEGIN_DECLS #define GPIO_GPIO7_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN12) #define GPIO_GPIO_DIR (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) +/* + * Tone alarm output + */ +#define TONE_ALARM_TIMER 3 /* timer 3 */ +#define TONE_ALARM_CHANNEL 3 /* channel 3 */ +#define GPIO_TONE_ALARM_IDLE (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN8) +#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF2|GPIO_SPEED_2MHz|GPIO_FLOAT|GPIO_PUSHPULL|GPIO_PORTC|GPIO_PIN8) + +/* + * PWM + * + * Four PWM outputs can be configured on pins otherwise shared with + * USART2; two can take the flow control pins if they are not being used. + * + * Pins: + * + * CTS - PA0 - TIM2CH1 + * RTS - PA1 - TIM2CH2 + * TX - PA2 - TIM2CH3 + * RX - PA3 - TIM2CH4 + * + */ +#define GPIO_TIM2_CH1OUT GPIO_TIM2_CH1OUT_1 +#define GPIO_TIM2_CH2OUT GPIO_TIM2_CH2OUT_1 +#define GPIO_TIM2_CH3OUT GPIO_TIM2_CH3OUT_1 +#define GPIO_TIM2_CH4OUT GPIO_TIM2_CH4OUT_1 + /* USB OTG FS * * PA9 OTG_FS_VBUS VBUS sensing (also connected to the green LED) */ #define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9) -/* PWM - * - * The PX4FMU has five PWM outputs, of which only the output on - * pin PC8 is fixed assigned to this function. The other four possible - * pwm sources are the TX, RX, RTS and CTS pins of USART2 - * - * Alternate function mapping: - * PC8 - BUZZER - TIM8_CH3/SDIO_D0 /TIM3_CH3/ USART6_CK / DCMI_D2 +/* High-resolution timer */ - -#ifdef CONFIG_PWM -# if defined(CONFIG_STM32_TIM3_PWM) -# define BUZZER_PWMCHANNEL 3 -# define BUZZER_PWMTIMER 3 -# elif defined(CONFIG_STM32_TIM8_PWM) -# define BUZZER_PWMCHANNEL 3 -# define BUZZER_PWMTIMER 8 -# endif -#endif +#define HRT_TIMER 1 /* use timer1 for the HRT */ +#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */ +#define HRT_PPM_CHANNEL 3 /* use capture/compare channel 3 */ +#define GPIO_PPM_IN (GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN10) /**************************************************************************************************** * Public Types diff --git a/src/drivers/boards/px4fmu/module.mk b/src/drivers/boards/px4fmu-v1/module.mk similarity index 100% rename from src/drivers/boards/px4fmu/module.mk rename to src/drivers/boards/px4fmu-v1/module.mk diff --git a/src/drivers/boards/px4fmu/px4fmu_can.c b/src/drivers/boards/px4fmu-v1/px4fmu_can.c similarity index 99% rename from src/drivers/boards/px4fmu/px4fmu_can.c rename to src/drivers/boards/px4fmu-v1/px4fmu_can.c index 187689ff9a..1e1f100406 100644 --- a/src/drivers/boards/px4fmu/px4fmu_can.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_can.c @@ -54,7 +54,7 @@ #include "stm32.h" #include "stm32_can.h" -#include "px4fmu_internal.h" +#include "board_config.h" #ifdef CONFIG_CAN diff --git a/src/drivers/boards/px4fmu/px4fmu_init.c b/src/drivers/boards/px4fmu-v1/px4fmu_init.c similarity index 99% rename from src/drivers/boards/px4fmu/px4fmu_init.c rename to src/drivers/boards/px4fmu-v1/px4fmu_init.c index 36af2298c2..964f5069c6 100644 --- a/src/drivers/boards/px4fmu/px4fmu_init.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_init.c @@ -59,7 +59,7 @@ #include #include "stm32.h" -#include "px4fmu_internal.h" +#include "board_config.h" #include "stm32_uart.h" #include diff --git a/src/drivers/boards/px4fmu/px4fmu_led.c b/src/drivers/boards/px4fmu-v1/px4fmu_led.c similarity index 98% rename from src/drivers/boards/px4fmu/px4fmu_led.c rename to src/drivers/boards/px4fmu-v1/px4fmu_led.c index 31b25984e1..aa83ec130d 100644 --- a/src/drivers/boards/px4fmu/px4fmu_led.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_led.c @@ -42,7 +42,7 @@ #include #include "stm32.h" -#include "px4fmu_internal.h" +#include "board_config.h" #include diff --git a/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmu-v1/px4fmu_pwm_servo.c similarity index 98% rename from src/drivers/boards/px4fmu/px4fmu_pwm_servo.c rename to src/drivers/boards/px4fmu-v1/px4fmu_pwm_servo.c index d85131dd88..848e21d798 100644 --- a/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_pwm_servo.c @@ -41,15 +41,15 @@ #include -#include - -#include -#include - #include #include #include +#include +#include + +#include "board_config.h" + __EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = { { .base = STM32_TIM2_BASE, diff --git a/src/drivers/boards/px4fmu/px4fmu_spi.c b/src/drivers/boards/px4fmu-v1/px4fmu_spi.c similarity index 99% rename from src/drivers/boards/px4fmu/px4fmu_spi.c rename to src/drivers/boards/px4fmu-v1/px4fmu_spi.c index e05ddecf3e..17e6862f7c 100644 --- a/src/drivers/boards/px4fmu/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_spi.c @@ -53,7 +53,7 @@ #include "up_arch.h" #include "chip.h" #include "stm32.h" -#include "px4fmu_internal.h" +#include "board_config.h" /************************************************************************************ * Public Functions diff --git a/src/drivers/boards/px4fmu/px4fmu_usb.c b/src/drivers/boards/px4fmu-v1/px4fmu_usb.c similarity index 99% rename from src/drivers/boards/px4fmu/px4fmu_usb.c rename to src/drivers/boards/px4fmu-v1/px4fmu_usb.c index 0be981c1e1..0fc8569aa4 100644 --- a/src/drivers/boards/px4fmu/px4fmu_usb.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_usb.c @@ -53,7 +53,7 @@ #include "up_arch.h" #include "stm32.h" -#include "px4fmu_internal.h" +#include "board_config.h" /************************************************************************************ * Definitions diff --git a/src/drivers/boards/px4fmuv2/px4fmu_internal.h b/src/drivers/boards/px4fmu-v2/board_config.h similarity index 80% rename from src/drivers/boards/px4fmuv2/px4fmu_internal.h rename to src/drivers/boards/px4fmu-v2/board_config.h index ad66ce563c..ec8dde4993 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu_internal.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file px4fmu_internal.h + * @file board_config.h * * PX4FMU internal definitions */ @@ -51,7 +51,7 @@ __BEGIN_DECLS /* these headers are not C++ safe */ #include - +#include /**************************************************************************************************** * Definitions @@ -76,6 +76,9 @@ __BEGIN_DECLS #define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN12) /* External interrupts */ +#define GPIO_EXTI_GYRO_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN0) +#define GPIO_EXTI_MAG_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN1) +#define GPIO_EXTI_ACCEL_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN4) /* SPI chip selects */ #define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) @@ -83,6 +86,22 @@ __BEGIN_DECLS #define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7) #define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) +/* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */ +#define PX4_SPIDEV_GYRO 1 +#define PX4_SPIDEV_ACCEL_MAG 2 +#define PX4_SPIDEV_BARO 3 + +/* I2C busses */ +#define PX4_I2C_BUS_EXPANSION 1 +#define PX4_I2C_BUS_LED 2 + +/* Devices on the onboard bus. + * + * Note that these are unshifted addresses. + */ +#define PX4_I2C_OBDEV_LED 0x55 +#define PX4_I2C_OBDEV_HMC5883 0x1e + /* User GPIOs * * GPIO0-5 are the PWM servo outputs. @@ -108,12 +127,42 @@ __BEGIN_DECLS #define GPIO_VDD_5V_HIPOWER_OC (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN10) #define GPIO_VDD_5V_PERIPH_OC (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN15) +/* Tone alarm output */ +#define TONE_ALARM_TIMER 2 /* timer 2 */ +#define TONE_ALARM_CHANNEL 1 /* channel 1 */ +#define GPIO_TONE_ALARM_IDLE (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15) +#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN15) + +/* PWM + * + * Six PWM outputs are configured. + * + * Pins: + * + * CH1 : PE14 : TIM1_CH4 + * CH2 : PE13 : TIM1_CH3 + * CH3 : PE11 : TIM1_CH2 + * CH4 : PE9 : TIM1_CH1 + * CH5 : PD13 : TIM4_CH2 + * CH6 : PD14 : TIM4_CH3 + */ +#define GPIO_TIM1_CH1OUT GPIO_TIM1_CH1OUT_2 +#define GPIO_TIM1_CH2OUT GPIO_TIM1_CH2OUT_2 +#define GPIO_TIM1_CH3OUT GPIO_TIM1_CH3OUT_2 +#define GPIO_TIM1_CH4OUT GPIO_TIM1_CH4OUT_2 +#define GPIO_TIM4_CH2OUT GPIO_TIM4_CH2OUT_2 +#define GPIO_TIM4_CH3OUT GPIO_TIM4_CH3OUT_2 + /* USB OTG FS * * PA9 OTG_FS_VBUS VBUS sensing (also connected to the green LED) */ #define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9) +/* High-resolution timer */ +#define HRT_TIMER 8 /* use timer8 for the HRT */ +#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */ + /**************************************************************************************************** * Public Types ****************************************************************************************************/ diff --git a/src/drivers/boards/px4fmuv2/module.mk b/src/drivers/boards/px4fmu-v2/module.mk similarity index 100% rename from src/drivers/boards/px4fmuv2/module.mk rename to src/drivers/boards/px4fmu-v2/module.mk diff --git a/src/drivers/boards/px4fmuv2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c similarity index 99% rename from src/drivers/boards/px4fmuv2/px4fmu2_init.c rename to src/drivers/boards/px4fmu-v2/px4fmu2_init.c index 13829d5c41..135767b26a 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu2_init.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c @@ -60,7 +60,7 @@ #include #include -#include "px4fmu_internal.h" +#include "board_config.h" #include #include diff --git a/src/drivers/boards/px4fmuv2/px4fmu2_led.c b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c similarity index 98% rename from src/drivers/boards/px4fmuv2/px4fmu2_led.c rename to src/drivers/boards/px4fmu-v2/px4fmu2_led.c index 11a5c7211b..5ded4294e6 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu2_led.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c @@ -42,7 +42,7 @@ #include #include "stm32.h" -#include "px4fmu_internal.h" +#include "board_config.h" #include diff --git a/src/drivers/boards/px4fmuv2/px4fmu_can.c b/src/drivers/boards/px4fmu-v2/px4fmu_can.c similarity index 99% rename from src/drivers/boards/px4fmuv2/px4fmu_can.c rename to src/drivers/boards/px4fmu-v2/px4fmu_can.c index 86ba183b83..f66c7cd79f 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu_can.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu_can.c @@ -54,7 +54,7 @@ #include "stm32.h" #include "stm32_can.h" -#include "px4fmu_internal.h" +#include "board_config.h" #ifdef CONFIG_CAN diff --git a/src/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmu-v2/px4fmu_pwm_servo.c similarity index 99% rename from src/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c rename to src/drivers/boards/px4fmu-v2/px4fmu_pwm_servo.c index bcbc0010ca..600dfef412 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu_pwm_servo.c @@ -41,15 +41,15 @@ #include -#include - -#include -#include - #include #include #include +#include +#include + +#include "board_config.h" + __EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = { { .base = STM32_TIM1_BASE, diff --git a/src/drivers/boards/px4fmuv2/px4fmu_spi.c b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c similarity index 99% rename from src/drivers/boards/px4fmuv2/px4fmu_spi.c rename to src/drivers/boards/px4fmu-v2/px4fmu_spi.c index 5e90c227d7..09838d02fe 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c @@ -53,7 +53,7 @@ #include #include #include -#include "px4fmu_internal.h" +#include "board_config.h" /************************************************************************************ * Public Functions diff --git a/src/drivers/boards/px4fmuv2/px4fmu_usb.c b/src/drivers/boards/px4fmu-v2/px4fmu_usb.c similarity index 99% rename from src/drivers/boards/px4fmuv2/px4fmu_usb.c rename to src/drivers/boards/px4fmu-v2/px4fmu_usb.c index 3492e2c684..f329e06ff9 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu_usb.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu_usb.c @@ -53,7 +53,7 @@ #include #include -#include "px4fmu_internal.h" +#include "board_config.h" /************************************************************************************ * Definitions diff --git a/src/drivers/boards/px4io/px4io_internal.h b/src/drivers/boards/px4io-v1/board_config.h similarity index 92% rename from src/drivers/boards/px4io/px4io_internal.h rename to src/drivers/boards/px4io-v1/board_config.h index 6638e715ef..48aadbd768 100644 --- a/src/drivers/boards/px4io/px4io_internal.h +++ b/src/drivers/boards/px4io-v1/board_config.h @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file px4io_internal.h + * @file board_config.h * * PX4IO hardware definitions. */ @@ -47,7 +47,9 @@ #include #include +/* these headers are not C++ safe */ #include +#include /************************************************************************************ * Definitions @@ -83,3 +85,11 @@ #define GPIO_ADC_VBATT (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN4) #define GPIO_ADC_IN5 (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN5) + +/* + * High-resolution timer + */ +#define HRT_TIMER 1 /* use timer1 for the HRT */ +#define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 2 */ +#define HRT_PPM_CHANNEL 1 /* use capture/compare channel 1 */ +#define GPIO_PPM_IN GPIO_TIM1_CH1IN diff --git a/src/drivers/boards/px4io/module.mk b/src/drivers/boards/px4io-v1/module.mk similarity index 100% rename from src/drivers/boards/px4io/module.mk rename to src/drivers/boards/px4io-v1/module.mk diff --git a/src/drivers/boards/px4io/px4io_init.c b/src/drivers/boards/px4io-v1/px4io_init.c similarity index 99% rename from src/drivers/boards/px4io/px4io_init.c rename to src/drivers/boards/px4io-v1/px4io_init.c index 15c59e4236..8292da9e1c 100644 --- a/src/drivers/boards/px4io/px4io_init.c +++ b/src/drivers/boards/px4io-v1/px4io_init.c @@ -55,7 +55,7 @@ #include #include "stm32.h" -#include "px4io_internal.h" +#include "board_config.h" #include "stm32_uart.h" #include diff --git a/src/drivers/boards/px4io/px4io_pwm_servo.c b/src/drivers/boards/px4io-v1/px4io_pwm_servo.c similarity index 100% rename from src/drivers/boards/px4io/px4io_pwm_servo.c rename to src/drivers/boards/px4io-v1/px4io_pwm_servo.c diff --git a/src/drivers/boards/px4iov2/px4iov2_internal.h b/src/drivers/boards/px4io-v2/board_config.h old mode 100755 new mode 100644 similarity index 88% rename from src/drivers/boards/px4iov2/px4iov2_internal.h rename to src/drivers/boards/px4io-v2/board_config.h index 2bf65e0470..818b644362 --- a/src/drivers/boards/px4iov2/px4iov2_internal.h +++ b/src/drivers/boards/px4io-v2/board_config.h @@ -49,7 +49,7 @@ /* these headers are not C++ safe */ #include - +#include /****************************************************************************** * Definitions @@ -116,3 +116,23 @@ #define GPIO_SBUS_OUTPUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) #define GPIO_SBUS_OENABLE (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN4) +/* + * High-resolution timer + */ +#define HRT_TIMER 1 /* use timer1 for the HRT */ +#define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 2 */ +#define HRT_PPM_CHANNEL 1 /* use capture/compare channel 1 */ +#define GPIO_PPM_IN GPIO_TIM1_CH1IN + +/* LED definitions ******************************************************************/ +/* PX4 has two LEDs that we will encode as: */ + +#define LED_STARTED 0 /* LED? */ +#define LED_HEAPALLOCATE 1 /* LED? */ +#define LED_IRQSENABLED 2 /* LED? + LED? */ +#define LED_STACKCREATED 3 /* LED? */ +#define LED_INIRQ 4 /* LED? + LED? */ +#define LED_SIGNAL 5 /* LED? + LED? */ +#define LED_ASSERTION 6 /* LED? + LED? + LED? */ +#define LED_PANIC 7 /* N/C + N/C + N/C + LED? */ + diff --git a/src/drivers/boards/px4iov2/module.mk b/src/drivers/boards/px4io-v2/module.mk similarity index 100% rename from src/drivers/boards/px4iov2/module.mk rename to src/drivers/boards/px4io-v2/module.mk diff --git a/src/drivers/boards/px4iov2/px4iov2_init.c b/src/drivers/boards/px4io-v2/px4iov2_init.c similarity index 99% rename from src/drivers/boards/px4iov2/px4iov2_init.c rename to src/drivers/boards/px4io-v2/px4iov2_init.c index e95298bf5d..0ea95bded9 100644 --- a/src/drivers/boards/px4iov2/px4iov2_init.c +++ b/src/drivers/boards/px4io-v2/px4iov2_init.c @@ -55,7 +55,7 @@ #include #include -#include "px4iov2_internal.h" +#include "board_config.h" #include diff --git a/src/drivers/boards/px4iov2/px4iov2_pwm_servo.c b/src/drivers/boards/px4io-v2/px4iov2_pwm_servo.c similarity index 100% rename from src/drivers/boards/px4iov2/px4iov2_pwm_servo.c rename to src/drivers/boards/px4io-v2/px4iov2_pwm_servo.c diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h index c7c25048a3..37af26d522 100644 --- a/src/drivers/drv_gpio.h +++ b/src/drivers/drv_gpio.h @@ -59,9 +59,7 @@ # define GPIO_CAN_RX (1<<7) /**< CAN2 RX */ /** - * Default GPIO device - other devices may also support this protocol if - * they also export GPIO-like things. This is always the GPIOs on the - * main board. + * Device paths for things that support the GPIO ioctl protocol. */ # define PX4FMU_DEVICE_PATH "/dev/px4fmu" # define PX4IO_DEVICE_PATH "/dev/px4io" @@ -89,9 +87,7 @@ # define GPIO_5V_PERIPH_OC (1<<11) /**< PE10 - !VDD_5V_PERIPH_OC */ /** - * Default GPIO device - other devices may also support this protocol if - * they also export GPIO-like things. This is always the GPIOs on the - * main board. + * Device paths for things that support the GPIO ioctl protocol. */ # define PX4FMU_DEVICE_PATH "/dev/px4fmu" # define PX4IO_DEVICE_PATH "/dev/px4io" diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index de8028b0f5..f79b0b1a3c 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -59,7 +59,7 @@ #include #include -#include +#include #include #include diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index ac3bdc1325..039b496f4b 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -58,7 +58,7 @@ #include #include -#include +#include #include #include diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 61a38b125c..3bb9a77648 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -59,11 +59,10 @@ #include #include -#include - #include #include +#include /* oddly, ERROR is not defined for c++ */ #ifdef ERROR diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 844cc3884f..d9801f88ff 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -59,12 +59,12 @@ #include #include -#include - #include #include #include +#include + #include "iirFilter.h" /* oddly, ERROR is not defined for c++ */ diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index 397686e8bf..c5f49fb36e 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -59,8 +59,6 @@ #include #include -#include - #include #include @@ -70,6 +68,8 @@ #include #include +#include + /* Configuration Constants */ #define MB12XX_BUS PX4_I2C_BUS_EXPANSION #define MB12XX_BASEADDR 0x70 /* 7-bit address. 8-bit address is 0xE0 */ diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 7a2e22c018..b19a1a0e6a 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -69,7 +69,7 @@ #include #include -#include +#include #include #include diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 1f4a63cf35..4ad13fc045 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -59,10 +59,11 @@ #include #include +#include + #include #include #include -#include #include #include diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 1fd6cb17e7..0e27a382a2 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -63,7 +63,7 @@ #include #include -#include +#include #include #include diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index b73f715724..a6f3374866 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -59,15 +59,7 @@ #include #include -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) -# include -# define FMU_HAVE_PPM -#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) -# include -# undef FMU_HAVE_PPM -#else -# error Unrecognised FMU board. -#endif +# include #include #include @@ -80,7 +72,7 @@ #include #include -#ifdef FMU_HAVE_PPM +#ifdef HRT_PPM_CHANNEL # include #endif @@ -455,7 +447,7 @@ PX4FMU::task_main() fds[1].fd = _t_armed; fds[1].events = POLLIN; -#ifdef FMU_HAVE_PPM +#ifdef HRT_PPM_CHANNEL // rc input, published to ORB struct rc_input_values rc_in; orb_advert_t to_input_rc = 0; @@ -585,7 +577,7 @@ PX4FMU::task_main() } } -#ifdef FMU_HAVE_PPM +#ifdef HRT_PPM_CHANNEL // see if we have new PPM input data if (ppm_last_valid_decode != rc_in.timestamp) { // we have a new PPM frame. Publish it. diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp index 840b96f5be..236cb918dc 100644 --- a/src/drivers/px4io/px4io_serial.cpp +++ b/src/drivers/px4io/px4io_serial.cpp @@ -61,7 +61,7 @@ #include #include -#include /* XXX should really not be hardcoding v2 here */ +#include #include diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index ec22a5e174..c7ce60b5ee 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -56,12 +56,7 @@ #include "uploader.h" -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 -#include -#endif -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 -#include -#endif +#include // define for comms logging //#define UDEBUG diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index dae41d934f..5c4fa4bb6f 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -41,7 +41,6 @@ #include -#include #include #include @@ -60,6 +59,8 @@ #include #include +#include + #include "device/rgbled.h" #define LED_ONTIME 120 diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index 1cc18afda7..58529fb039 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -59,7 +59,7 @@ #include #include -#include +#include #include #include "chip.h" @@ -70,8 +70,6 @@ #include "stm32_gpio.h" #include "stm32_tim.h" -#ifdef HRT_TIMER - /* HRT configuration */ #if HRT_TIMER == 1 # define HRT_TIMER_BASE STM32_TIM1_BASE @@ -905,6 +903,3 @@ hrt_latency_update(void) /* catch-all at the end */ latency_counters[index]++; } - - -#endif /* HRT_TIMER */ diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index 2284be84de..24eec52af1 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -104,7 +104,7 @@ #include #include -#include +#include #include #include diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index bd78f2638f..9eb092a639 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -42,12 +42,7 @@ #include #include -#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 -# include -#endif -#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 -# include -#endif +#include #include "protocol.h" diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c index a2b0d8cae8..96276b56a3 100644 --- a/src/modules/systemlib/systemlib.c +++ b/src/modules/systemlib/systemlib.c @@ -43,11 +43,13 @@ #include #include #include -#include #include #include #include +#include +#include + #include "systemlib.h" __EXPORT extern void systemreset(void) { diff --git a/src/systemcmds/eeprom/eeprom.c b/src/systemcmds/eeprom/eeprom.c index 49da513580..2aed80e011 100644 --- a/src/systemcmds/eeprom/eeprom.c +++ b/src/systemcmds/eeprom/eeprom.c @@ -55,7 +55,7 @@ #include #include -#include +#include #include "systemlib/systemlib.h" #include "systemlib/param/param.h" diff --git a/src/systemcmds/i2c/i2c.c b/src/systemcmds/i2c/i2c.c index 4da238039c..34405c4969 100644 --- a/src/systemcmds/i2c/i2c.c +++ b/src/systemcmds/i2c/i2c.c @@ -52,7 +52,7 @@ #include -#include +#include #include "systemlib/systemlib.h" #include "systemlib/err.h" From ecc7bc5bca40e9d5e0a7271105dea9b3441af0a8 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 2 Aug 2013 23:11:04 -0700 Subject: [PATCH 321/486] Clean out unused trash from the NuttX configs. --- Makefile | 3 +- nuttx-configs/px4fmu-v1/Kconfig | 21 - nuttx-configs/px4fmu-v1/common/Make.defs | 184 --- nuttx-configs/px4fmu-v1/common/ld.script | 149 --- nuttx-configs/px4fmu-v1/nsh/Make.defs | 180 ++- nuttx-configs/px4fmu-v1/ostest/Make.defs | 122 -- nuttx-configs/px4fmu-v1/ostest/defconfig | 583 --------- nuttx-configs/px4fmu-v1/ostest/setenv.sh | 75 -- nuttx-configs/px4fmu-v1/scripts/gnu-elf.ld | 129 -- nuttx-configs/px4fmu-v1/scripts/ld.script | 58 +- nuttx-configs/px4fmu-v1/tools/px_mkfw.py | 110 -- nuttx-configs/px4fmu-v1/tools/px_uploader.py | 416 ------- nuttx-configs/px4fmu-v1/tools/upload.sh | 27 - nuttx-configs/px4fmu-v2/Kconfig | 7 - nuttx-configs/px4fmu-v2/common/Make.defs | 184 --- nuttx-configs/px4fmu-v2/nsh/Make.defs | 180 ++- nuttx-configs/px4fmu-v2/nsh/defconfig.prev | 1067 ----------------- .../px4fmu-v2/{common => scripts}/ld.script | 0 nuttx-configs/px4io-v1/Kconfig | 21 - nuttx-configs/px4io-v1/README.txt | 806 ------------- nuttx-configs/px4io-v1/common/Make.defs | 171 --- nuttx-configs/px4io-v1/common/setenv.sh | 47 - .../px4io-v1/include/drv_i2c_device.h | 42 - nuttx-configs/px4io-v1/nsh/Make.defs | 167 ++- .../px4io-v1/{common => scripts}/ld.script | 0 nuttx-configs/px4io-v1/src/README.txt | 1 - nuttx-configs/px4io-v1/src/drv_i2c_device.c | 618 ---------- nuttx-configs/px4io-v2/README.txt | 806 ------------- nuttx-configs/px4io-v2/common/Make.defs | 175 --- nuttx-configs/px4io-v2/common/setenv.sh | 47 - nuttx-configs/px4io-v2/include/README.txt | 1 - nuttx-configs/px4io-v2/nsh/Make.defs | 171 ++- .../px4io-v2/{common => scripts}/ld.script | 0 nuttx-configs/px4io-v2/src/README.txt | 1 - nuttx-configs/px4io-v2/test/Make.defs | 3 - nuttx-configs/px4io-v2/test/appconfig | 44 - nuttx-configs/px4io-v2/test/defconfig | 566 --------- nuttx-configs/px4io-v2/test/setenv.sh | 47 - 38 files changed, 734 insertions(+), 6495 deletions(-) delete mode 100644 nuttx-configs/px4fmu-v1/Kconfig delete mode 100644 nuttx-configs/px4fmu-v1/common/Make.defs delete mode 100644 nuttx-configs/px4fmu-v1/common/ld.script delete mode 100644 nuttx-configs/px4fmu-v1/ostest/Make.defs delete mode 100644 nuttx-configs/px4fmu-v1/ostest/defconfig delete mode 100755 nuttx-configs/px4fmu-v1/ostest/setenv.sh delete mode 100644 nuttx-configs/px4fmu-v1/scripts/gnu-elf.ld delete mode 100755 nuttx-configs/px4fmu-v1/tools/px_mkfw.py delete mode 100755 nuttx-configs/px4fmu-v1/tools/px_uploader.py delete mode 100755 nuttx-configs/px4fmu-v1/tools/upload.sh delete mode 100644 nuttx-configs/px4fmu-v2/Kconfig delete mode 100644 nuttx-configs/px4fmu-v2/common/Make.defs delete mode 100755 nuttx-configs/px4fmu-v2/nsh/defconfig.prev rename nuttx-configs/px4fmu-v2/{common => scripts}/ld.script (100%) delete mode 100644 nuttx-configs/px4io-v1/Kconfig delete mode 100755 nuttx-configs/px4io-v1/README.txt delete mode 100644 nuttx-configs/px4io-v1/common/Make.defs delete mode 100755 nuttx-configs/px4io-v1/common/setenv.sh delete mode 100644 nuttx-configs/px4io-v1/include/drv_i2c_device.h rename nuttx-configs/px4io-v1/{common => scripts}/ld.script (100%) delete mode 100644 nuttx-configs/px4io-v1/src/README.txt delete mode 100644 nuttx-configs/px4io-v1/src/drv_i2c_device.c delete mode 100755 nuttx-configs/px4io-v2/README.txt delete mode 100644 nuttx-configs/px4io-v2/common/Make.defs delete mode 100755 nuttx-configs/px4io-v2/common/setenv.sh delete mode 100755 nuttx-configs/px4io-v2/include/README.txt rename nuttx-configs/px4io-v2/{common => scripts}/ld.script (100%) delete mode 100644 nuttx-configs/px4io-v2/src/README.txt delete mode 100644 nuttx-configs/px4io-v2/test/Make.defs delete mode 100644 nuttx-configs/px4io-v2/test/appconfig delete mode 100755 nuttx-configs/px4io-v2/test/defconfig delete mode 100755 nuttx-configs/px4io-v2/test/setenv.sh diff --git a/Makefile b/Makefile index 6f58858f06..d1a192fa2d 100644 --- a/Makefile +++ b/Makefile @@ -148,12 +148,13 @@ $(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) @$(ECHO) %% Configuring NuttX for $(board) $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) $(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean - $(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)/nuttx-configs/* .) + $(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)/nuttx-configs/$(board) .) $(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(board)/$(configuration)) @$(ECHO) %% Exporting NuttX for $(board) $(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) export $(Q) mkdir -p $(dir $@) $(Q) $(COPY) $(NUTTX_SRC)nuttx-export.zip $@ + $(Q) (cd $(NUTTX_SRC)/configs && $(RMDIR) $(board)) $(NUTTX_SRC): @$(ECHO) "" diff --git a/nuttx-configs/px4fmu-v1/Kconfig b/nuttx-configs/px4fmu-v1/Kconfig deleted file mode 100644 index edbafa06f8..0000000000 --- a/nuttx-configs/px4fmu-v1/Kconfig +++ /dev/null @@ -1,21 +0,0 @@ -# -# For a description of the syntax of this configuration file, -# see misc/tools/kconfig-language.txt. -# - -if ARCH_BOARD_PX4FMU_V1 - -config HRT_TIMER - bool "High resolution timer support" - default y - ---help--- - Enable high resolution timer for PPM capture and system clocks. - -config HRT_PPM - bool "PPM input capture" - default y - depends on HRT_TIMER - ---help--- - Enable PPM input capture via HRT (for CPPM / PPM sum RC inputs) - -endif diff --git a/nuttx-configs/px4fmu-v1/common/Make.defs b/nuttx-configs/px4fmu-v1/common/Make.defs deleted file mode 100644 index 756286ccb5..0000000000 --- a/nuttx-configs/px4fmu-v1/common/Make.defs +++ /dev/null @@ -1,184 +0,0 @@ -############################################################################ -# configs/px4fmu/common/Make.defs -# -# Copyright (C) 2011 Gregory Nutt. All rights reserved. -# Author: Gregory Nutt -# -# 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 NuttX 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. -# -############################################################################ - -# -# Generic Make.defs for the PX4FMU -# Do not specify/use this file directly - it is included by config-specific -# Make.defs in the per-config directories. -# - -include ${TOPDIR}/tools/Config.mk - -# -# We only support building with the ARM bare-metal toolchain from -# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS. -# -CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI - -include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs - -CC = $(CROSSDEV)gcc -CXX = $(CROSSDEV)g++ -CPP = $(CROSSDEV)gcc -E -LD = $(CROSSDEV)ld -AR = $(CROSSDEV)ar rcs -NM = $(CROSSDEV)nm -OBJCOPY = $(CROSSDEV)objcopy -OBJDUMP = $(CROSSDEV)objdump - -MAXOPTIMIZATION = -O3 -ARCHCPUFLAGS = -mcpu=cortex-m4 \ - -mthumb \ - -march=armv7e-m \ - -mfpu=fpv4-sp-d16 \ - -mfloat-abi=hard - - -# enable precise stack overflow tracking -INSTRUMENTATIONDEFINES = -finstrument-functions \ - -ffixed-r10 - -# pull in *just* libm from the toolchain ... this is grody -LIBM = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}" -EXTRA_LIBS += $(LIBM) - -# use our linker script -LDSCRIPT = ld.script - -ifeq ($(WINTOOL),y) - # Windows-native toolchains - DIRLINK = $(TOPDIR)/tools/copydir.sh - DIRUNLINK = $(TOPDIR)/tools/unlink.sh - MKDEP = $(TOPDIR)/tools/mknulldeps.sh - ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" - ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" - ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT)}" -else - ifeq ($(PX4_WINTOOL),y) - # Windows-native toolchains (MSYS) - DIRLINK = $(TOPDIR)/tools/copydir.sh - DIRUNLINK = $(TOPDIR)/tools/unlink.sh - MKDEP = $(TOPDIR)/tools/mknulldeps.sh - ARCHINCLUDES = -I. -isystem $(TOPDIR)/include - ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx - ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT) - else - # Linux/Cygwin-native toolchain - MKDEP = $(TOPDIR)/tools/mkdeps.sh - ARCHINCLUDES = -I. -isystem $(TOPDIR)/include - ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx - ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT) - endif -endif - -# tool versions -ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} -ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} - -# optimisation flags -ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ - -fno-strict-aliasing \ - -fno-strength-reduce \ - -fomit-frame-pointer \ - -funsafe-math-optimizations \ - -fno-builtin-printf \ - -ffunction-sections \ - -fdata-sections - -ifeq ("${CONFIG_DEBUG_SYMBOLS}","y") -ARCHOPTIMIZATION += -g -endif - -ARCHCFLAGS = -std=gnu99 -ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x -ARCHWARNINGS = -Wall \ - -Wextra \ - -Wdouble-promotion \ - -Wshadow \ - -Wfloat-equal \ - -Wframe-larger-than=1024 \ - -Wpointer-arith \ - -Wlogical-op \ - -Wmissing-declarations \ - -Wpacked \ - -Wno-unused-parameter -# -Wcast-qual - generates spurious noreturn attribute warnings, try again later -# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code -# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives - -ARCHCWARNINGS = $(ARCHWARNINGS) \ - -Wbad-function-cast \ - -Wstrict-prototypes \ - -Wold-style-declaration \ - -Wmissing-parameter-type \ - -Wmissing-prototypes \ - -Wnested-externs \ - -Wunsuffixed-float-constants -ARCHWARNINGSXX = $(ARCHWARNINGS) \ - -Wno-psabi -ARCHDEFINES = -ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 - -# this seems to be the only way to add linker flags -EXTRA_LIBS += --warn-common \ - --gc-sections - -CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common -CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) -CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) -CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -AFLAGS = $(CFLAGS) -D__ASSEMBLY__ - -NXFLATLDFLAGS1 = -r -d -warn-common -NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections -LDNXFLATFLAGS = -e main -s 2048 - -OBJEXT = .o -LIBEXT = .a -EXEEXT = - - -# produce partially-linked $1 from files in $2 -define PRELINK - @echo "PRELINK: $1" - $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 -endef - -HOSTCC = gcc -HOSTINCLUDES = -I. -HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe -HOSTLDFLAGS = - diff --git a/nuttx-configs/px4fmu-v1/common/ld.script b/nuttx-configs/px4fmu-v1/common/ld.script deleted file mode 100644 index de8179e8db..0000000000 --- a/nuttx-configs/px4fmu-v1/common/ld.script +++ /dev/null @@ -1,149 +0,0 @@ -/**************************************************************************** - * configs/px4fmu/common/ld.script - * - * Copyright (C) 2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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 NuttX 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. - * - ****************************************************************************/ - -/* The STM32F405 has 1024Kb of FLASH beginning at address 0x0800:0000 and - * 192Kb of SRAM. SRAM is split up into three blocks: - * - * 1) 112Kb of SRAM beginning at address 0x2000:0000 - * 2) 16Kb of SRAM beginning at address 0x2001:c000 - * 3) 64Kb of TCM SRAM beginning at address 0x1000:0000 - * - * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 - * where the code expects to begin execution by jumping to the entry point in - * the 0x0800:0000 address range. - * - * The first 0x4000 of flash is reserved for the bootloader. - */ - -MEMORY -{ - flash (rx) : ORIGIN = 0x08004000, LENGTH = 1008K - sram (rwx) : ORIGIN = 0x20000000, LENGTH = 128K - ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K -} - -OUTPUT_ARCH(arm) - -ENTRY(__start) /* treat __start as the anchor for dead code stripping */ -EXTERN(_vectors) /* force the vectors to be included in the output */ - -/* - * Ensure that abort() is present in the final object. The exception handling - * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). - */ -EXTERN(abort) - -SECTIONS -{ - .text : { - _stext = ABSOLUTE(.); - *(.vectors) - *(.text .text.*) - *(.fixup) - *(.gnu.warning) - *(.rodata .rodata.*) - *(.gnu.linkonce.t.*) - *(.got) - *(.gcc_except_table) - *(.gnu.linkonce.r.*) - _etext = ABSOLUTE(.); - - /* - * This is a hack to make the newlib libm __errno() call - * use the NuttX get_errno_ptr() function. - */ - __errno = get_errno_ptr; - } > flash - - /* - * Init functions (static constructors and the like) - */ - .init_section : { - _sinit = ABSOLUTE(.); - KEEP(*(.init_array .init_array.*)) - _einit = ABSOLUTE(.); - } > flash - - /* - * Construction data for parameters. - */ - __param ALIGN(4): { - __param_start = ABSOLUTE(.); - KEEP(*(__param*)) - __param_end = ABSOLUTE(.); - } > flash - - .ARM.extab : { - *(.ARM.extab*) - } > flash - - __exidx_start = ABSOLUTE(.); - .ARM.exidx : { - *(.ARM.exidx*) - } > flash - __exidx_end = ABSOLUTE(.); - - _eronly = ABSOLUTE(.); - - .data : { - _sdata = ABSOLUTE(.); - *(.data .data.*) - *(.gnu.linkonce.d.*) - CONSTRUCTORS - _edata = ABSOLUTE(.); - } > sram AT > flash - - .bss : { - _sbss = ABSOLUTE(.); - *(.bss .bss.*) - *(.gnu.linkonce.b.*) - *(COMMON) - _ebss = ABSOLUTE(.); - } > sram - - /* Stabs debugging sections. */ - .stab 0 : { *(.stab) } - .stabstr 0 : { *(.stabstr) } - .stab.excl 0 : { *(.stab.excl) } - .stab.exclstr 0 : { *(.stab.exclstr) } - .stab.index 0 : { *(.stab.index) } - .stab.indexstr 0 : { *(.stab.indexstr) } - .comment 0 : { *(.comment) } - .debug_abbrev 0 : { *(.debug_abbrev) } - .debug_info 0 : { *(.debug_info) } - .debug_line 0 : { *(.debug_line) } - .debug_pubnames 0 : { *(.debug_pubnames) } - .debug_aranges 0 : { *(.debug_aranges) } -} diff --git a/nuttx-configs/px4fmu-v1/nsh/Make.defs b/nuttx-configs/px4fmu-v1/nsh/Make.defs index 81936334b3..7b2ea703a2 100644 --- a/nuttx-configs/px4fmu-v1/nsh/Make.defs +++ b/nuttx-configs/px4fmu-v1/nsh/Make.defs @@ -1,3 +1,179 @@ -include ${TOPDIR}/.config +############################################################################ +# configs/px4fmu-v1/nsh/Make.defs +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# +############################################################################ + +include ${TOPDIR}/.config +include ${TOPDIR}/tools/Config.mk + +# +# We only support building with the ARM bare-metal toolchain from +# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS. +# +CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI + +include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs + +CC = $(CROSSDEV)gcc +CXX = $(CROSSDEV)g++ +CPP = $(CROSSDEV)gcc -E +LD = $(CROSSDEV)ld +AR = $(CROSSDEV)ar rcs +NM = $(CROSSDEV)nm +OBJCOPY = $(CROSSDEV)objcopy +OBJDUMP = $(CROSSDEV)objdump + +MAXOPTIMIZATION = -O3 +ARCHCPUFLAGS = -mcpu=cortex-m4 \ + -mthumb \ + -march=armv7e-m \ + -mfpu=fpv4-sp-d16 \ + -mfloat-abi=hard + + +# enable precise stack overflow tracking +INSTRUMENTATIONDEFINES = -finstrument-functions \ + -ffixed-r10 + +# pull in *just* libm from the toolchain ... this is grody +LIBM = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}" +EXTRA_LIBS += $(LIBM) + +# use our linker script +LDSCRIPT = ld.script + +ifeq ($(WINTOOL),y) + # Windows-native toolchains + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" + ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" + ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)}" +else + ifeq ($(PX4_WINTOOL),y) + # Windows-native toolchains (MSYS) + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) + else + # Linux/Cygwin-native toolchain + MKDEP = $(TOPDIR)/tools/mkdeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) + endif +endif + +# tool versions +ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} +ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} + +# optimisation flags +ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ + -fno-strict-aliasing \ + -fno-strength-reduce \ + -fomit-frame-pointer \ + -funsafe-math-optimizations \ + -fno-builtin-printf \ + -ffunction-sections \ + -fdata-sections + +ifeq ("${CONFIG_DEBUG_SYMBOLS}","y") +ARCHOPTIMIZATION += -g +endif + +ARCHCFLAGS = -std=gnu99 +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x +ARCHWARNINGS = -Wall \ + -Wextra \ + -Wdouble-promotion \ + -Wshadow \ + -Wfloat-equal \ + -Wframe-larger-than=1024 \ + -Wpointer-arith \ + -Wlogical-op \ + -Wmissing-declarations \ + -Wpacked \ + -Wno-unused-parameter +# -Wcast-qual - generates spurious noreturn attribute warnings, try again later +# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code +# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives + +ARCHCWARNINGS = $(ARCHWARNINGS) \ + -Wbad-function-cast \ + -Wstrict-prototypes \ + -Wold-style-declaration \ + -Wmissing-parameter-type \ + -Wmissing-prototypes \ + -Wnested-externs \ + -Wunsuffixed-float-constants +ARCHWARNINGSXX = $(ARCHWARNINGS) \ + -Wno-psabi +ARCHDEFINES = +ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 + +# this seems to be the only way to add linker flags +EXTRA_LIBS += --warn-common \ + --gc-sections + +CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common +CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) +CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe +CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) +CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) +AFLAGS = $(CFLAGS) -D__ASSEMBLY__ + +NXFLATLDFLAGS1 = -r -d -warn-common +NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections +LDNXFLATFLAGS = -e main -s 2048 + +OBJEXT = .o +LIBEXT = .a +EXEEXT = + + +# produce partially-linked $1 from files in $2 +define PRELINK + @echo "PRELINK: $1" + $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 +endef + +HOSTCC = gcc +HOSTINCLUDES = -I. +HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe +HOSTLDFLAGS = -include $(TOPDIR)/configs/px4fmu-v1/common/Make.defs diff --git a/nuttx-configs/px4fmu-v1/ostest/Make.defs b/nuttx-configs/px4fmu-v1/ostest/Make.defs deleted file mode 100644 index 7b807abdbf..0000000000 --- a/nuttx-configs/px4fmu-v1/ostest/Make.defs +++ /dev/null @@ -1,122 +0,0 @@ -############################################################################ -# configs/stm32f4discovery/ostest/Make.defs -# -# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. -# Author: Gregory Nutt -# -# 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 NuttX 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. -# -############################################################################ - -include ${TOPDIR}/.config -include ${TOPDIR}/tools/Config.mk -include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs - -LDSCRIPT = ld.script - -ifeq ($(WINTOOL),y) - # Windows-native toolchains - ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" - ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" - ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)}" - MAXOPTIMIZATION = -O2 -else - # Linux/Cygwin-native toolchain - ARCHINCLUDES = -I. -isystem $(TOPDIR)/include - ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx - ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) -endif - -CC = $(CROSSDEV)gcc -CXX = $(CROSSDEV)g++ -CPP = $(CROSSDEV)gcc -E -LD = $(CROSSDEV)ld -AR = $(ARCROSSDEV)ar rcs -NM = $(ARCROSSDEV)nm -OBJCOPY = $(CROSSDEV)objcopy -OBJDUMP = $(CROSSDEV)objdump - -ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} -ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} - -ifeq ($(CONFIG_DEBUG_SYMBOLS),y) - ARCHOPTIMIZATION = -g -else - ARCHOPTIMIZATION = $(MAXOPTIMIZATION) -fno-strict-aliasing -fno-strength-reduce -fomit-frame-pointer -endif - -ARCHCFLAGS = -fno-builtin -ARCHCXXFLAGS = -fno-builtin -fno-exceptions -fno-rtti -ARCHWARNINGS = -Wall -Wstrict-prototypes -Wshadow -ARCHWARNINGSXX = -Wall -Wshadow -ARCHDEFINES = -ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 - -CFLAGS = $(ARCHCFLAGS) $(ARCHWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) -CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) -CPPFLAGS = $(ARCHINCLUDES) $(ARCHDEFINES) $(EXTRADEFINES) -AFLAGS = $(CFLAGS) -D__ASSEMBLY__ - -NXFLATLDFLAGS1 = -r -d -warn-common -NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat-pcrel.ld -no-check-sections -LDNXFLATFLAGS = -e main -s 2048 - -OBJEXT = .o -LIBEXT = .a -EXEEXT = - -ifneq ($(CROSSDEV),arm-nuttx-elf-) - LDFLAGS += -nostartfiles -nodefaultlibs -endif -ifeq ($(CONFIG_DEBUG_SYMBOLS),y) - LDFLAGS += -g -endif - - -HOSTCC = gcc -HOSTINCLUDES = -I. -HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe -HOSTLDFLAGS = -ifeq ($(CONFIG_HOST_WINDOWS),y) - HOSTEXEEXT = .exe -else - HOSTEXEEXT = -endif - -ifeq ($(WINTOOL),y) - # Windows-native host tools - DIRLINK = $(TOPDIR)/tools/copydir.sh - DIRUNLINK = $(TOPDIR)/tools/unlink.sh - MKDEP = $(TOPDIR)/tools/mknulldeps.sh -else - # Linux/Cygwin-native host tools - MKDEP = $(TOPDIR)/tools/mkdeps$(HOSTEXEEXT) -endif - diff --git a/nuttx-configs/px4fmu-v1/ostest/defconfig b/nuttx-configs/px4fmu-v1/ostest/defconfig deleted file mode 100644 index c7fb6b2a5a..0000000000 --- a/nuttx-configs/px4fmu-v1/ostest/defconfig +++ /dev/null @@ -1,583 +0,0 @@ -# -# Automatically generated file; DO NOT EDIT. -# Nuttx/ Configuration -# -CONFIG_NUTTX_NEWCONFIG=y - -# -# Build Setup -# -# CONFIG_EXPERIMENTAL is not set -# CONFIG_HOST_LINUX is not set -# CONFIG_HOST_OSX is not set -CONFIG_HOST_WINDOWS=y -# CONFIG_HOST_OTHER is not set -# CONFIG_WINDOWS_NATIVE is not set -CONFIG_WINDOWS_CYGWIN=y -# CONFIG_WINDOWS_MSYS is not set -# CONFIG_WINDOWS_OTHER is not set -# CONFIG_WINDOWS_MKLINK is not set - -# -# Build Configuration -# -# CONFIG_APPS_DIR="../apps" -# CONFIG_BUILD_2PASS is not set - -# -# Binary Output Formats -# -# CONFIG_RRLOAD_BINARY is not set -CONFIG_INTELHEX_BINARY=y -# CONFIG_MOTOROLA_SREC is not set -CONFIG_RAW_BINARY=y - -# -# Customize Header Files -# -# CONFIG_ARCH_STDBOOL_H is not set -# CONFIG_ARCH_MATH_H is not set -# CONFIG_ARCH_FLOAT_H is not set -# CONFIG_ARCH_STDARG_H is not set - -# -# Debug Options -# -# CONFIG_DEBUG is not set -# CONFIG_DEBUG_SYMBOLS is not set - -# -# System Type -# -# CONFIG_ARCH_8051 is not set -CONFIG_ARCH_ARM=y -# CONFIG_ARCH_AVR is not set -# CONFIG_ARCH_HC is not set -# CONFIG_ARCH_MIPS is not set -# CONFIG_ARCH_RGMP is not set -# CONFIG_ARCH_SH is not set -# CONFIG_ARCH_SIM is not set -# CONFIG_ARCH_X86 is not set -# CONFIG_ARCH_Z16 is not set -# CONFIG_ARCH_Z80 is not set -CONFIG_ARCH="arm" - -# -# ARM Options -# -# CONFIG_ARCH_CHIP_C5471 is not set -# CONFIG_ARCH_CHIP_CALYPSO is not set -# CONFIG_ARCH_CHIP_DM320 is not set -# CONFIG_ARCH_CHIP_IMX is not set -# CONFIG_ARCH_CHIP_KINETIS is not set -# CONFIG_ARCH_CHIP_LM3S is not set -# CONFIG_ARCH_CHIP_LPC17XX is not set -# CONFIG_ARCH_CHIP_LPC214X is not set -# CONFIG_ARCH_CHIP_LPC2378 is not set -# CONFIG_ARCH_CHIP_LPC31XX is not set -# CONFIG_ARCH_CHIP_LPC43XX is not set -# CONFIG_ARCH_CHIP_SAM3U is not set -CONFIG_ARCH_CHIP_STM32=y -# CONFIG_ARCH_CHIP_STR71X is not set -CONFIG_ARCH_CORTEXM4=y -CONFIG_ARCH_FAMILY="armv7-m" -CONFIG_ARCH_CHIP="stm32" -CONFIG_ARCH_HAVE_CMNVECTOR=y -# CONFIG_ARMV7M_CMNVECTOR is not set -# CONFIG_ARCH_FPU is not set -CONFIG_ARCH_HAVE_MPU=y -# CONFIG_ARMV7M_MPU is not set -CONFIG_ARCH_IRQPRIO=y -CONFIG_BOARD_LOOPSPERMSEC=16717 -# CONFIG_ARCH_CALIBRATION is not set -# CONFIG_SERIAL_TERMIOS is not set - -# -# STM32 Configuration Options -# -# CONFIG_ARCH_CHIP_STM32F100C8 is not set -# CONFIG_ARCH_CHIP_STM32F100CB is not set -# CONFIG_ARCH_CHIP_STM32F100R8 is not set -# CONFIG_ARCH_CHIP_STM32F100RB is not set -# CONFIG_ARCH_CHIP_STM32F100RC is not set -# CONFIG_ARCH_CHIP_STM32F100RD is not set -# CONFIG_ARCH_CHIP_STM32F100RE is not set -# CONFIG_ARCH_CHIP_STM32F100V8 is not set -# CONFIG_ARCH_CHIP_STM32F100VB is not set -# CONFIG_ARCH_CHIP_STM32F100VC is not set -# CONFIG_ARCH_CHIP_STM32F100VD is not set -# CONFIG_ARCH_CHIP_STM32F100VE is not set -# CONFIG_ARCH_CHIP_STM32F103RET6 is not set -# CONFIG_ARCH_CHIP_STM32F103VCT6 is not set -# CONFIG_ARCH_CHIP_STM32F103VET6 is not set -# CONFIG_ARCH_CHIP_STM32F103ZET6 is not set -# CONFIG_ARCH_CHIP_STM32F105VBT7 is not set -# CONFIG_ARCH_CHIP_STM32F107VC is not set -# CONFIG_ARCH_CHIP_STM32F207IG is not set -# CONFIG_ARCH_CHIP_STM32F405RG is not set -# CONFIG_ARCH_CHIP_STM32F405VG is not set -# CONFIG_ARCH_CHIP_STM32F405ZG is not set -# CONFIG_ARCH_CHIP_STM32F407VE is not set -CONFIG_ARCH_CHIP_STM32F407VG=y -# CONFIG_ARCH_CHIP_STM32F407ZE is not set -# CONFIG_ARCH_CHIP_STM32F407ZG is not set -# CONFIG_ARCH_CHIP_STM32F407IE is not set -# CONFIG_ARCH_CHIP_STM32F407IG is not set -CONFIG_STM32_STM32F40XX=y -# CONFIG_STM32_CODESOURCERYW is not set -CONFIG_STM32_CODESOURCERYL=y -# CONFIG_STM32_ATOLLIC_LITE is not set -# CONFIG_STM32_ATOLLIC_PRO is not set -# CONFIG_STM32_DEVKITARM is not set -# CONFIG_STM32_RAISONANCE is not set -# CONFIG_STM32_BUILDROOT is not set -# CONFIG_STM32_DFU is not set - -# -# STM32 Peripheral Support -# -# CONFIG_STM32_ADC1 is not set -# CONFIG_STM32_ADC2 is not set -# CONFIG_STM32_ADC3 is not set -# CONFIG_STM32_BKPSRAM is not set -# CONFIG_STM32_CAN1 is not set -# CONFIG_STM32_CAN2 is not set -# CONFIG_STM32_CCMDATARAM is not set -# CONFIG_STM32_CRC is not set -# CONFIG_STM32_CRYP is not set -# CONFIG_STM32_DMA1 is not set -# CONFIG_STM32_DMA2 is not set -# CONFIG_STM32_DAC1 is not set -# CONFIG_STM32_DAC2 is not set -# CONFIG_STM32_DCMI is not set -# CONFIG_STM32_ETHMAC is not set -# CONFIG_STM32_FSMC is not set -# CONFIG_STM32_HASH is not set -# CONFIG_STM32_I2C1 is not set -# CONFIG_STM32_I2C2 is not set -# CONFIG_STM32_I2C3 is not set -# CONFIG_STM32_IWDG is not set -# CONFIG_STM32_OTGFS is not set -# CONFIG_STM32_OTGHS is not set -# CONFIG_STM32_PWR is not set -# CONFIG_STM32_RNG is not set -# CONFIG_STM32_SDIO is not set -# CONFIG_STM32_SPI1 is not set -# CONFIG_STM32_SPI2 is not set -# CONFIG_STM32_SPI3 is not set -CONFIG_STM32_SYSCFG=y -# CONFIG_STM32_TIM1 is not set -# CONFIG_STM32_TIM2 is not set -# CONFIG_STM32_TIM3 is not set -# CONFIG_STM32_TIM4 is not set -# CONFIG_STM32_TIM5 is not set -# CONFIG_STM32_TIM6 is not set -# CONFIG_STM32_TIM7 is not set -# CONFIG_STM32_TIM8 is not set -# CONFIG_STM32_TIM9 is not set -# CONFIG_STM32_TIM10 is not set -# CONFIG_STM32_TIM11 is not set -# CONFIG_STM32_TIM12 is not set -# CONFIG_STM32_TIM13 is not set -# CONFIG_STM32_TIM14 is not set -# CONFIG_STM32_USART1 is not set -CONFIG_STM32_USART2=y -# CONFIG_STM32_USART3 is not set -# CONFIG_STM32_UART4 is not set -# CONFIG_STM32_UART5 is not set -# CONFIG_STM32_USART6 is not set -# CONFIG_STM32_WWDG is not set - -# -# Alternate Pin Mapping -# -# CONFIG_STM32_JTAG_DISABLE is not set -# CONFIG_STM32_JTAG_FULL_ENABLE is not set -# CONFIG_STM32_JTAG_NOJNTRST_ENABLE is not set -CONFIG_STM32_JTAG_SW_ENABLE=y -# CONFIG_STM32_FORCEPOWER is not set -# CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set -# CONFIG_STM32_CCMEXCLUDE is not set - -# -# USB Host Configuration -# - -# -# Architecture Options -# -# CONFIG_ARCH_NOINTC is not set -# CONFIG_ARCH_DMA is not set -CONFIG_ARCH_STACKDUMP=y - -# -# Board Settings -# -CONFIG_DRAM_START=0x20000000 -CONFIG_DRAM_SIZE=114688 -CONFIG_ARCH_HAVE_INTERRUPTSTACK=y -CONFIG_ARCH_INTERRUPTSTACK=0 - -# -# Boot options -# -# CONFIG_BOOT_RUNFROMEXTSRAM is not set -CONFIG_BOOT_RUNFROMFLASH=y -# CONFIG_BOOT_RUNFROMISRAM is not set -# CONFIG_BOOT_RUNFROMSDRAM is not set -# CONFIG_BOOT_COPYTORAM is not set - -# -# Board Selection -# -CONFIG_ARCH_BOARD_STM32F4_DISCOVERY=y -# CONFIG_ARCH_BOARD_CUSTOM is not set -CONFIG_ARCH_BOARD="stm32f4discovery" - -# -# Common Board Options -# -CONFIG_ARCH_HAVE_LEDS=y -CONFIG_ARCH_LEDS=y -CONFIG_ARCH_HAVE_BUTTONS=y -# CONFIG_ARCH_BUTTONS is not set -CONFIG_ARCH_HAVE_IRQBUTTONS=y - -# -# Board-Specific Options -# - -# -# RTOS Features -# -CONFIG_MSEC_PER_TICK=10 -CONFIG_RR_INTERVAL=200 -# CONFIG_SCHED_INSTRUMENTATION is not set -CONFIG_TASK_NAME_SIZE=0 -# CONFIG_JULIAN_TIME is not set -CONFIG_START_YEAR=2009 -CONFIG_START_MONTH=9 -CONFIG_START_DAY=21 -CONFIG_DEV_CONSOLE=y -# CONFIG_MUTEX_TYPES is not set -# CONFIG_PRIORITY_INHERITANCE is not set -# CONFIG_FDCLONE_DISABLE is not set -# CONFIG_FDCLONE_STDIO is not set -CONFIG_SDCLONE_DISABLE=y -# CONFIG_SCHED_WORKQUEUE is not set -# CONFIG_SCHED_WAITPID is not set -# CONFIG_SCHED_ATEXIT is not set -# CONFIG_SCHED_ONEXIT is not set -CONFIG_USER_ENTRYPOINT="ostest_main" -CONFIG_DISABLE_OS_API=y -# CONFIG_DISABLE_CLOCK is not set -# CONFIG_DISABLE_POSIX_TIMERS is not set -# CONFIG_DISABLE_PTHREAD is not set -# CONFIG_DISABLE_SIGNALS is not set -# CONFIG_DISABLE_MQUEUE is not set -CONFIG_DISABLE_MOUNTPOINT=y -CONFIG_DISABLE_ENVIRON=y -CONFIG_DISABLE_POLL=y - -# -# Sizes of configurable things (0 disables) -# -CONFIG_MAX_TASKS=16 -CONFIG_MAX_TASK_ARGS=4 -CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=8 -CONFIG_NFILE_STREAMS=8 -CONFIG_NAME_MAX=32 -CONFIG_PREALLOC_MQ_MSGS=4 -CONFIG_MQ_MAXMSGSIZE=32 -CONFIG_MAX_WDOGPARMS=2 -CONFIG_PREALLOC_WDOGS=4 -CONFIG_PREALLOC_TIMERS=4 - -# -# Stack and heap information -# -# CONFIG_CUSTOM_STACK is not set -CONFIG_IDLETHREAD_STACKSIZE=1024 -CONFIG_USERMAIN_STACKSIZE=2048 -CONFIG_PTHREAD_STACK_MIN=256 -CONFIG_PTHREAD_STACK_DEFAULT=2048 - -# -# Device Drivers -# -CONFIG_DEV_NULL=y -# CONFIG_DEV_ZERO is not set -# CONFIG_LOOP is not set -# CONFIG_RAMDISK is not set -# CONFIG_CAN is not set -# CONFIG_PWM is not set -# CONFIG_I2C is not set -CONFIG_ARCH_HAVE_I2CRESET=y -# CONFIG_SPI is not set -# CONFIG_RTC is not set -# CONFIG_WATCHDOG is not set -# CONFIG_ANALOG is not set -# CONFIG_BCH is not set -# CONFIG_INPUT is not set -# CONFIG_LCD is not set -# CONFIG_MMCSD is not set -# CONFIG_MTD is not set -# CONFIG_PIPES is not set -# CONFIG_PM is not set -# CONFIG_POWER is not set -# CONFIG_SENSORS is not set -# CONFIG_SERCOMM_CONSOLE is not set -CONFIG_SERIAL=y -CONFIG_DEV_LOWCONSOLE=y -# CONFIG_16550_UART is not set -CONFIG_ARCH_HAVE_USART2=y -CONFIG_MCU_SERIAL=y -CONFIG_STANDARD_SERIAL=y -CONFIG_USART2_SERIAL_CONSOLE=y -# CONFIG_NO_SERIAL_CONSOLE is not set - -# -# USART2 Configuration -# -CONFIG_USART2_RXBUFSIZE=128 -CONFIG_USART2_TXBUFSIZE=128 -CONFIG_USART2_BAUD=115200 -CONFIG_USART2_BITS=8 -CONFIG_USART2_PARITY=0 -CONFIG_USART2_2STOP=0 -# CONFIG_USBDEV is not set -# CONFIG_USBHOST is not set -# CONFIG_WIRELESS is not set - -# -# System Logging Device Options -# - -# -# System Logging -# -# CONFIG_RAMLOG is not set - -# -# Networking Support -# -# CONFIG_NET is not set - -# -# File Systems -# - -# -# File system configuration -# -# CONFIG_FS_RAMMAP is not set - -# -# System Logging -# -# CONFIG_SYSLOG is not set - -# -# Graphics Support -# -# CONFIG_NX is not set - -# -# Memory Management -# -# CONFIG_MM_SMALL is not set -CONFIG_MM_REGIONS=2 -# CONFIG_GRAN is not set - -# -# Binary Formats -# -# CONFIG_BINFMT_DISABLE is not set -# CONFIG_NXFLAT is not set -# CONFIG_ELF is not set -CONFIG_SYMTAB_ORDEREDBYNAME=y - -# -# Library Routines -# -CONFIG_STDIO_BUFFER_SIZE=64 -CONFIG_STDIO_LINEBUFFER=y -CONFIG_NUNGET_CHARS=2 -# CONFIG_LIBM is not set -# CONFIG_NOPRINTF_FIELDWIDTH is not set -# CONFIG_LIBC_FLOATINGPOINT is not set -# CONFIG_EOL_IS_CR is not set -# CONFIG_EOL_IS_LF is not set -# CONFIG_EOL_IS_BOTH_CRLF is not set -CONFIG_EOL_IS_EITHER_CRLF=y -# CONFIG_LIBC_STRERROR is not set -# CONFIG_LIBC_PERROR_STDOUT is not set -CONFIG_ARCH_LOWPUTC=y -CONFIG_LIB_SENDFILE_BUFSIZE=512 -# CONFIG_ARCH_ROMGETC is not set -# CONFIG_ARCH_OPTIMIZED_FUNCTIONS is not set - -# -# Basic CXX Support -# -# CONFIG_HAVE_CXX is not set - -# -# Application Configuration -# - -# -# Named Applications -# -# CONFIG_BUILTIN is not set - -# -# Examples -# -# CONFIG_EXAMPLES_BUTTONS is not set -# CONFIG_EXAMPLES_CAN is not set -# CONFIG_EXAMPLES_CDCACM is not set -# CONFIG_EXAMPLES_COMPOSITE is not set -# CONFIG_EXAMPLES_DHCPD is not set -# CONFIG_EXAMPLES_ELF is not set -# CONFIG_EXAMPLES_FTPC is not set -# CONFIG_EXAMPLES_FTPD is not set -# CONFIG_EXAMPLES_HELLO is not set -# CONFIG_EXAMPLES_HELLOXX is not set -# CONFIG_EXAMPLES_JSON is not set -# CONFIG_EXAMPLES_HIDKBD is not set -# CONFIG_EXAMPLES_IGMP is not set -# CONFIG_EXAMPLES_LCDRW is not set -# CONFIG_EXAMPLES_MM is not set -# CONFIG_EXAMPLES_MOUNT is not set -# CONFIG_EXAMPLES_MODBUS is not set -# CONFIG_EXAMPLES_NETTEST is not set -# CONFIG_EXAMPLES_NSH is not set -# CONFIG_EXAMPLES_NULL is not set -# CONFIG_EXAMPLES_NX is not set -# CONFIG_EXAMPLES_NXCONSOLE is not set -# CONFIG_EXAMPLES_NXFFS is not set -# CONFIG_EXAMPLES_NXFLAT is not set -# CONFIG_EXAMPLES_NXHELLO is not set -# CONFIG_EXAMPLES_NXIMAGE is not set -# CONFIG_EXAMPLES_NXLINES is not set -# CONFIG_EXAMPLES_NXTEXT is not set -CONFIG_EXAMPLES_OSTEST=y -# CONFIG_EXAMPLES_OSTEST_BUILTIN is not set -CONFIG_EXAMPLES_OSTEST_LOOPS=1 -CONFIG_EXAMPLES_OSTEST_STACKSIZE=2048 -CONFIG_EXAMPLES_OSTEST_NBARRIER_THREADS=3 -CONFIG_EXAMPLES_OSTEST_RR_RANGE=10000 -CONFIG_EXAMPLES_OSTEST_RR_RUNS=10 -# CONFIG_EXAMPLES_PASHELLO is not set -# CONFIG_EXAMPLES_PIPE is not set -# CONFIG_EXAMPLES_POLL is not set -# CONFIG_EXAMPLES_QENCODER is not set -# CONFIG_EXAMPLES_RGMP is not set -# CONFIG_EXAMPLES_ROMFS is not set -# CONFIG_EXAMPLES_SENDMAIL is not set -# CONFIG_EXAMPLES_SERLOOP is not set -# CONFIG_EXAMPLES_TELNETD is not set -# CONFIG_EXAMPLES_THTTPD is not set -# CONFIG_EXAMPLES_TIFF is not set -# CONFIG_EXAMPLES_TOUCHSCREEN is not set -# CONFIG_EXAMPLES_UDP is not set -# CONFIG_EXAMPLES_UIP is not set -# CONFIG_EXAMPLES_USBSERIAL is not set -# CONFIG_EXAMPLES_USBMSC is not set -# CONFIG_EXAMPLES_USBTERM is not set -# CONFIG_EXAMPLES_WATCHDOG is not set -# CONFIG_EXAMPLES_WLAN is not set - -# -# Interpreters -# - -# -# Interpreters -# -# CONFIG_INTERPRETERS_FICL is not set -# CONFIG_INTERPRETERS_PCODE is not set - -# -# Network Utilities -# - -# -# Networking Utilities -# -# CONFIG_NETUTILS_CODECS is not set -# CONFIG_NETUTILS_DHCPC is not set -# CONFIG_NETUTILS_DHCPD is not set -# CONFIG_NETUTILS_FTPC is not set -# CONFIG_NETUTILS_FTPD is not set -# CONFIG_NETUTILS_JSON is not set -# CONFIG_NETUTILS_RESOLV is not set -# CONFIG_NETUTILS_SMTP is not set -# CONFIG_NETUTILS_TELNETD is not set -# CONFIG_NETUTILS_TFTPC is not set -# CONFIG_NETUTILS_THTTPD is not set -# CONFIG_NETUTILS_UIPLIB is not set -# CONFIG_NETUTILS_WEBCLIENT is not set - -# -# ModBus -# - -# -# FreeModbus -# -# CONFIG_MODBUS is not set - -# -# NSH Library -# -# CONFIG_NSH_LIBRARY is not set - -# -# NxWidgets/NxWM -# - -# -# System NSH Add-Ons -# - -# -# Custom Free Memory Command -# -# CONFIG_SYSTEM_FREE is not set - -# -# I2C tool -# - -# -# FLASH Program Installation -# -# CONFIG_SYSTEM_INSTALL is not set - -# -# readline() -# -# CONFIG_SYSTEM_READLINE is not set - -# -# Power Off -# -# CONFIG_SYSTEM_POWEROFF is not set - -# -# RAMTRON -# -# CONFIG_SYSTEM_RAMTRON is not set - -# -# SD Card -# -# CONFIG_SYSTEM_SDCARD is not set - -# -# Sysinfo -# -# CONFIG_SYSTEM_SYSINFO is not set diff --git a/nuttx-configs/px4fmu-v1/ostest/setenv.sh b/nuttx-configs/px4fmu-v1/ostest/setenv.sh deleted file mode 100755 index a67fdc5a8a..0000000000 --- a/nuttx-configs/px4fmu-v1/ostest/setenv.sh +++ /dev/null @@ -1,75 +0,0 @@ -#!/bin/bash -# configs/stm32f4discovery/ostest/setenv.sh -# -# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. -# Author: Gregory Nutt -# -# 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 NuttX 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. -# - -if [ "$_" = "$0" ] ; then - echo "You must source this script, not run it!" 1>&2 - exit 1 -fi - -WD=`pwd` -if [ ! -x "setenv.sh" ]; then - echo "This script must be executed from the top-level NuttX build directory" - exit 1 -fi - -if [ -z "${PATH_ORIG}" ]; then - export PATH_ORIG="${PATH}" -fi - -# This is the Cygwin path to the location where I installed the RIDE -# toolchain under windows. You will also have to edit this if you install -# the RIDE toolchain in any other location -#export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/Raisonance/Ride/arm-gcc/bin" - -# This is the Cygwin path to the location where I installed the CodeSourcery -# toolchain under windows. You will also have to edit this if you install -# the CodeSourcery toolchain in any other location -export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/CodeSourcery/Sourcery G++ Lite/bin" - -# These are the Cygwin paths to the locations where I installed the Atollic -# toolchain under windows. You will also have to edit this if you install -# the Atollic toolchain in any other location. /usr/bin is added before -# the Atollic bin path because there is are binaries named gcc.exe and g++.exe -# at those locations as well. -#export TOOLCHAIN_BIN="/usr/bin:/cygdrive/c/Program Files (x86)/Atollic/TrueSTUDIO for ARM Pro 2.3.0/ARMTools/bin" -#export TOOLCHAIN_BIN="/usr/bin:/cygdrive/c/Program Files (x86)/Atollic/TrueSTUDIO for STMicroelectronics STM32 Lite 2.3.0/ARMTools/bin" - -# This is the Cygwin path to the location where I build the buildroot -# toolchain. -#export TOOLCHAIN_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin" - -# Add the path to the toolchain to the PATH varialble -export PATH="${TOOLCHAIN_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" - -echo "PATH : ${PATH}" diff --git a/nuttx-configs/px4fmu-v1/scripts/gnu-elf.ld b/nuttx-configs/px4fmu-v1/scripts/gnu-elf.ld deleted file mode 100644 index 1f29f02f5b..0000000000 --- a/nuttx-configs/px4fmu-v1/scripts/gnu-elf.ld +++ /dev/null @@ -1,129 +0,0 @@ -/**************************************************************************** - * configs/stm32f4discovery/scripts/gnu-elf.ld - * - * Copyright (C) 2012 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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 NuttX 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. - * - ****************************************************************************/ - -SECTIONS -{ - .text 0x00000000 : - { - _stext = . ; - *(.text) - *(.text.*) - *(.gnu.warning) - *(.stub) - *(.glue_7) - *(.glue_7t) - *(.jcr) - - /* C++ support: The .init and .fini sections contain specific logic - * to manage static constructors and destructors. - */ - - *(.gnu.linkonce.t.*) - *(.init) /* Old ABI */ - *(.fini) /* Old ABI */ - _etext = . ; - } - - .rodata : - { - _srodata = . ; - *(.rodata) - *(.rodata1) - *(.rodata.*) - *(.gnu.linkonce.r*) - _erodata = . ; - } - - .data : - { - _sdata = . ; - *(.data) - *(.data1) - *(.data.*) - *(.gnu.linkonce.d*) - _edata = . ; - } - - /* C++ support. For each global and static local C++ object, - * GCC creates a small subroutine to construct the object. Pointers - * to these routines (not the routines themselves) are stored as - * simple, linear arrays in the .ctors section of the object file. - * Similarly, pointers to global/static destructor routines are - * stored in .dtors. - */ - - .ctors : - { - _sctors = . ; - *(.ctors) /* Old ABI: Unallocated */ - *(.init_array) /* New ABI: Allocated */ - _edtors = . ; - } - - .dtors : - { - _sdtors = . ; - *(.dtors) /* Old ABI: Unallocated */ - *(.fini_array) /* New ABI: Allocated */ - _edtors = . ; - } - - .bss : - { - _sbss = . ; - *(.bss) - *(.bss.*) - *(.sbss) - *(.sbss.*) - *(.gnu.linkonce.b*) - *(COMMON) - _ebss = . ; - } - - /* Stabs debugging sections. */ - - .stab 0 : { *(.stab) } - .stabstr 0 : { *(.stabstr) } - .stab.excl 0 : { *(.stab.excl) } - .stab.exclstr 0 : { *(.stab.exclstr) } - .stab.index 0 : { *(.stab.index) } - .stab.indexstr 0 : { *(.stab.indexstr) } - .comment 0 : { *(.comment) } - .debug_abbrev 0 : { *(.debug_abbrev) } - .debug_info 0 : { *(.debug_info) } - .debug_line 0 : { *(.debug_line) } - .debug_pubnames 0 : { *(.debug_pubnames) } - .debug_aranges 0 : { *(.debug_aranges) } -} diff --git a/nuttx-configs/px4fmu-v1/scripts/ld.script b/nuttx-configs/px4fmu-v1/scripts/ld.script index f6560743bc..ced5b21b7c 100644 --- a/nuttx-configs/px4fmu-v1/scripts/ld.script +++ b/nuttx-configs/px4fmu-v1/scripts/ld.script @@ -1,7 +1,7 @@ /**************************************************************************** - * configs/stm32f4discovery/scripts/ld.script + * configs/px4fmu-v1/scripts/ld.script * - * Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. + * Copyright (C) 2011 Gregory Nutt. All rights reserved. * Author: Gregory Nutt * * Redistribution and use in source and binary forms, with or without @@ -42,41 +42,67 @@ * * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 * where the code expects to begin execution by jumping to the entry point in - * the 0x0800:0000 address - * range. + * the 0x0800:0000 address range. + * + * The first 0x4000 of flash is reserved for the bootloader. */ MEMORY { - flash (rx) : ORIGIN = 0x08004000, LENGTH = 1008K - sram (rwx) : ORIGIN = 0x20000000, LENGTH = 112K + flash (rx) : ORIGIN = 0x08004000, LENGTH = 1008K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 128K + ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K } OUTPUT_ARCH(arm) -EXTERN(_vectors) -ENTRY(_stext) + +ENTRY(__start) /* treat __start as the anchor for dead code stripping */ +EXTERN(_vectors) /* force the vectors to be included in the output */ + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) + SECTIONS { .text : { _stext = ABSOLUTE(.); *(.vectors) - *(.text .text.*) + *(.text .text.*) *(.fixup) *(.gnu.warning) - *(.rodata .rodata.*) + *(.rodata .rodata.*) *(.gnu.linkonce.t.*) - *(.glue_7) - *(.glue_7t) *(.got) *(.gcc_except_table) *(.gnu.linkonce.r.*) _etext = ABSOLUTE(.); + + /* + * This is a hack to make the newlib libm __errno() call + * use the NuttX get_errno_ptr() function. + */ + __errno = get_errno_ptr; } > flash - .init_section : { - _sinit = ABSOLUTE(.); - *(.init_array .init_array.*) - _einit = ABSOLUTE(.); + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + /* + * Construction data for parameters. + */ + __param ALIGN(4): { + __param_start = ABSOLUTE(.); + KEEP(*(__param*)) + __param_end = ABSOLUTE(.); } > flash .ARM.extab : { diff --git a/nuttx-configs/px4fmu-v1/tools/px_mkfw.py b/nuttx-configs/px4fmu-v1/tools/px_mkfw.py deleted file mode 100755 index 9f4ddad62e..0000000000 --- a/nuttx-configs/px4fmu-v1/tools/px_mkfw.py +++ /dev/null @@ -1,110 +0,0 @@ -#!/usr/bin/env python -############################################################################ -# -# Copyright (C) 2012 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. -# -############################################################################ - -# -# PX4 firmware image generator -# -# The PX4 firmware file is a JSON-encoded Python object, containing -# metadata fields and a zlib-compressed base64-encoded firmware image. -# - -import sys -import argparse -import json -import base64 -import zlib -import time -import subprocess - -# -# Construct a basic firmware description -# -def mkdesc(): - proto = {} - proto['magic'] = "PX4FWv1" - proto['board_id'] = 0 - proto['board_revision'] = 0 - proto['version'] = "" - proto['summary'] = "" - proto['description'] = "" - proto['git_identity'] = "" - proto['build_time'] = 0 - proto['image'] = base64.b64encode(bytearray()) - proto['image_size'] = 0 - return proto - -# Parse commandline -parser = argparse.ArgumentParser(description="Firmware generator for the PX autopilot system.") -parser.add_argument("--prototype", action="store", help="read a prototype description from a file") -parser.add_argument("--board_id", action="store", help="set the board ID required") -parser.add_argument("--board_revision", action="store", help="set the board revision required") -parser.add_argument("--version", action="store", help="set a version string") -parser.add_argument("--summary", action="store", help="set a brief description") -parser.add_argument("--description", action="store", help="set a longer description") -parser.add_argument("--git_identity", action="store", help="the working directory to check for git identity") -parser.add_argument("--image", action="store", help="the firmware image") -args = parser.parse_args() - -# Fetch the firmware descriptor prototype if specified -if args.prototype != None: - f = open(args.prototype,"r") - desc = json.load(f) - f.close() -else: - desc = mkdesc() - -desc['build_time'] = int(time.time()) - -if args.board_id != None: - desc['board_id'] = int(args.board_id) -if args.board_revision != None: - desc['board_revision'] = int(args.board_revision) -if args.version != None: - desc['version'] = str(args.version) -if args.summary != None: - desc['summary'] = str(args.summary) -if args.description != None: - desc['description'] = str(args.description) -if args.git_identity != None: - cmd = " ".join(["git", "--git-dir", args.git_identity + "/.git", "describe", "--always", "--dirty"]) - p = subprocess.Popen(cmd, shell=True, stdout=subprocess.PIPE).stdout - desc['git_identity'] = p.read().strip() - p.close() -if args.image != None: - f = open(args.image, "rb") - bytes = f.read() - desc['image_size'] = len(bytes) - desc['image'] = base64.b64encode(zlib.compress(bytes,9)) - -print json.dumps(desc, indent=4) diff --git a/nuttx-configs/px4fmu-v1/tools/px_uploader.py b/nuttx-configs/px4fmu-v1/tools/px_uploader.py deleted file mode 100755 index 3b23f4f83f..0000000000 --- a/nuttx-configs/px4fmu-v1/tools/px_uploader.py +++ /dev/null @@ -1,416 +0,0 @@ -#!/usr/bin/env python -############################################################################ -# -# Copyright (C) 2012 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. -# -############################################################################ - -# -# Serial firmware uploader for the PX4FMU bootloader -# -# The PX4 firmware file is a JSON-encoded Python object, containing -# metadata fields and a zlib-compressed base64-encoded firmware image. -# -# The uploader uses the following fields from the firmware file: -# -# image -# The firmware that will be uploaded. -# image_size -# The size of the firmware in bytes. -# board_id -# The board for which the firmware is intended. -# board_revision -# Currently only used for informational purposes. -# - -import sys -import argparse -import binascii -import serial -import os -import struct -import json -import zlib -import base64 -import time -import array - -from sys import platform as _platform - -class firmware(object): - '''Loads a firmware file''' - - desc = {} - image = bytes() - crctab = array.array('I', [ - 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, - 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, - 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, - 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, - 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, - 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, - 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, - 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, - 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, - 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, - 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, - 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, - 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, - 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, - 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, - 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, - 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, - 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, - 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, - 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, - 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, - 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, - 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, - 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, - 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, - 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, - 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, - 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, - 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, - 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, - 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, - 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d ]) - crcpad = bytearray('\xff\xff\xff\xff') - - def __init__(self, path): - - # read the file - f = open(path, "r") - self.desc = json.load(f) - f.close() - - self.image = bytearray(zlib.decompress(base64.b64decode(self.desc['image']))) - - # pad image to 4-byte length - while ((len(self.image) % 4) != 0): - self.image.append('\xff') - - def property(self, propname): - return self.desc[propname] - - def __crc32(self, bytes, state): - for byte in bytes: - index = (state ^ byte) & 0xff - state = self.crctab[index] ^ (state >> 8) - return state - - def crc(self, padlen): - state = self.__crc32(self.image, int(0)) - for i in range(len(self.image), (padlen - 1), 4): - state = self.__crc32(self.crcpad, state) - return state - -class uploader(object): - '''Uploads a firmware file to the PX FMU bootloader''' - - # protocol bytes - INSYNC = chr(0x12) - EOC = chr(0x20) - - # reply bytes - OK = chr(0x10) - FAILED = chr(0x11) - INVALID = chr(0x13) # rev3+ - - # command bytes - NOP = chr(0x00) # guaranteed to be discarded by the bootloader - GET_SYNC = chr(0x21) - GET_DEVICE = chr(0x22) - CHIP_ERASE = chr(0x23) - CHIP_VERIFY = chr(0x24) # rev2 only - PROG_MULTI = chr(0x27) - READ_MULTI = chr(0x28) # rev2 only - GET_CRC = chr(0x29) # rev3+ - REBOOT = chr(0x30) - - INFO_BL_REV = chr(1) # bootloader protocol revision - BL_REV_MIN = 2 # minimum supported bootloader protocol - BL_REV_MAX = 3 # maximum supported bootloader protocol - INFO_BOARD_ID = chr(2) # board type - INFO_BOARD_REV = chr(3) # board revision - INFO_FLASH_SIZE = chr(4) # max firmware size in bytes - - PROG_MULTI_MAX = 60 # protocol max is 255, must be multiple of 4 - READ_MULTI_MAX = 60 # protocol max is 255, something overflows with >= 64 - - def __init__(self, portname, baudrate): - # open the port, keep the default timeout short so we can poll quickly - self.port = serial.Serial(portname, baudrate, timeout=0.25) - - def close(self): - if self.port is not None: - self.port.close() - - def __send(self, c): -# print("send " + binascii.hexlify(c)) - self.port.write(str(c)) - - def __recv(self, count = 1): - c = self.port.read(count) - if len(c) < 1: - raise RuntimeError("timeout waiting for data") -# print("recv " + binascii.hexlify(c)) - return c - - def __recv_int(self): - raw = self.__recv(4) - val = struct.unpack("= 3: - self.__getSync() - - # split a sequence into a list of size-constrained pieces - def __split_len(self, seq, length): - return [seq[i:i+length] for i in range(0, len(seq), length)] - - # upload code - def __program(self, fw): - code = fw.image - groups = self.__split_len(code, uploader.PROG_MULTI_MAX) - for bytes in groups: - self.__program_multi(bytes) - - # verify code - def __verify_v2(self, fw): - self.__send(uploader.CHIP_VERIFY - + uploader.EOC) - self.__getSync() - code = fw.image - groups = self.__split_len(code, uploader.READ_MULTI_MAX) - for bytes in groups: - if (not self.__verify_multi(bytes)): - raise RuntimeError("Verification failed") - - def __verify_v3(self, fw): - expect_crc = fw.crc(self.fw_maxsize) - self.__send(uploader.GET_CRC - + uploader.EOC) - report_crc = self.__recv_int() - self.__getSync() - if report_crc != expect_crc: - print("Expected 0x%x" % expect_crc) - print("Got 0x%x" % report_crc) - raise RuntimeError("Program CRC failed") - - # get basic data about the board - def identify(self): - # make sure we are in sync before starting - self.__sync() - - # get the bootloader protocol ID first - self.bl_rev = self.__getInfo(uploader.INFO_BL_REV) - if (self.bl_rev < uploader.BL_REV_MIN) or (self.bl_rev > uploader.BL_REV_MAX): - print("Unsupported bootloader protocol %d" % uploader.INFO_BL_REV) - raise RuntimeError("Bootloader protocol mismatch") - - self.board_type = self.__getInfo(uploader.INFO_BOARD_ID) - self.board_rev = self.__getInfo(uploader.INFO_BOARD_REV) - self.fw_maxsize = self.__getInfo(uploader.INFO_FLASH_SIZE) - - # upload the firmware - def upload(self, fw): - # Make sure we are doing the right thing - if self.board_type != fw.property('board_id'): - raise RuntimeError("Firmware not suitable for this board (run 'make configure_px4fmu && make clean' or 'make configure_px4io && make clean' to reconfigure).") - if self.fw_maxsize < fw.property('image_size'): - raise RuntimeError("Firmware image is too large for this board") - - print("erase...") - self.__erase() - - print("program...") - self.__program(fw) - - print("verify...") - if self.bl_rev == 2: - self.__verify_v2(fw) - else: - self.__verify_v3(fw) - - print("done, rebooting.") - self.__reboot() - self.port.close() - - -# Parse commandline arguments -parser = argparse.ArgumentParser(description="Firmware uploader for the PX autopilot system.") -parser.add_argument('--port', action="store", required=True, help="Serial port(s) to which the FMU may be attached") -parser.add_argument('--baud', action="store", type=int, default=115200, help="Baud rate of the serial port (default is 115200), only required for true serial ports.") -parser.add_argument('firmware', action="store", help="Firmware file to be uploaded") -args = parser.parse_args() - -# Load the firmware file -fw = firmware(args.firmware) -print("Loaded firmware for %x,%x, waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision'))) - -# Spin waiting for a device to show up -while True: - for port in args.port.split(","): - - #print("Trying %s" % port) - - # create an uploader attached to the port - try: - if "linux" in _platform: - # Linux, don't open Mac OS and Win ports - if not "COM" in port and not "tty.usb" in port: - up = uploader(port, args.baud) - elif "darwin" in _platform: - # OS X, don't open Windows and Linux ports - if not "COM" in port and not "ACM" in port: - up = uploader(port, args.baud) - elif "win" in _platform: - # Windows, don't open POSIX ports - if not "/" in port: - up = uploader(port, args.baud) - except: - # open failed, rate-limit our attempts - time.sleep(0.05) - - # and loop to the next port - continue - - # port is open, try talking to it - try: - # identify the bootloader - up.identify() - print("Found board %x,%x bootloader rev %x on %s" % (up.board_type, up.board_rev, up.bl_rev, port)) - - except: - # most probably a timeout talking to the port, no bootloader - continue - - try: - # ok, we have a bootloader, try flashing it - up.upload(fw) - - except RuntimeError as ex: - - # print the error - print("ERROR: %s" % ex.args) - - finally: - # always close the port - up.close() - - # we could loop here if we wanted to wait for more boards... - sys.exit(0) diff --git a/nuttx-configs/px4fmu-v1/tools/upload.sh b/nuttx-configs/px4fmu-v1/tools/upload.sh deleted file mode 100755 index 4e6597b3a6..0000000000 --- a/nuttx-configs/px4fmu-v1/tools/upload.sh +++ /dev/null @@ -1,27 +0,0 @@ -#!/bin/sh -# -# Wrapper to upload a PX4 firmware binary -# -TOOLS=`dirname $0` -MKFW=${TOOLS}/px_mkfw.py -UPLOAD=${TOOLS}/px_uploader.py - -BINARY=nuttx.bin -PAYLOAD=nuttx.px4 -PORTS="/dev/tty.usbmodemPX1,/dev/tty.usbmodemPX2,/dev/tty.usbmodemPX3,/dev/tty.usbmodemPX4,/dev/tty.usbmodem1,/dev/tty.usbmodem2,/dev/tty.usbmodem3,/dev/tty.usbmodem4" - -function abort() { - echo "ABORT: $*" - exit 1 -} - -if [ ! -f ${MKFW} -o ! -f ${UPLOAD} ]; then - abort "Missing tools ${MKFW} and/or ${UPLOAD}" -fi -if [ ! -f ${BINARY} ]; then - abort "Missing nuttx binary in current directory." -fi - -rm -f ${PAYLOAD} -${MKFW} --board_id 5 --image ${BINARY} > ${PAYLOAD} -${UPLOAD} --port ${PORTS} ${PAYLOAD} diff --git a/nuttx-configs/px4fmu-v2/Kconfig b/nuttx-configs/px4fmu-v2/Kconfig deleted file mode 100644 index 069b25f9e5..0000000000 --- a/nuttx-configs/px4fmu-v2/Kconfig +++ /dev/null @@ -1,7 +0,0 @@ -# -# For a description of the syntax of this configuration file, -# see misc/tools/kconfig-language.txt. -# - -if ARCH_BOARD_PX4FMU_V2 -endif diff --git a/nuttx-configs/px4fmu-v2/common/Make.defs b/nuttx-configs/px4fmu-v2/common/Make.defs deleted file mode 100644 index be387dce14..0000000000 --- a/nuttx-configs/px4fmu-v2/common/Make.defs +++ /dev/null @@ -1,184 +0,0 @@ -############################################################################ -# configs/px4fmu/common/Make.defs -# -# Copyright (C) 2011 Gregory Nutt. All rights reserved. -# Author: Gregory Nutt -# -# 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 NuttX 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. -# -############################################################################ - -# -# Generic Make.defs for the PX4FMUV2 -# Do not specify/use this file directly - it is included by config-specific -# Make.defs in the per-config directories. -# - -include ${TOPDIR}/tools/Config.mk - -# -# We only support building with the ARM bare-metal toolchain from -# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS. -# -CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI - -include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs - -CC = $(CROSSDEV)gcc -CXX = $(CROSSDEV)g++ -CPP = $(CROSSDEV)gcc -E -LD = $(CROSSDEV)ld -AR = $(CROSSDEV)ar rcs -NM = $(CROSSDEV)nm -OBJCOPY = $(CROSSDEV)objcopy -OBJDUMP = $(CROSSDEV)objdump - -MAXOPTIMIZATION = -O3 -ARCHCPUFLAGS = -mcpu=cortex-m4 \ - -mthumb \ - -march=armv7e-m \ - -mfpu=fpv4-sp-d16 \ - -mfloat-abi=hard - - -# enable precise stack overflow tracking -INSTRUMENTATIONDEFINES = -finstrument-functions \ - -ffixed-r10 - -# pull in *just* libm from the toolchain ... this is grody -LIBM = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}" -EXTRA_LIBS += $(LIBM) - -# use our linker script -LDSCRIPT = ld.script - -ifeq ($(WINTOOL),y) - # Windows-native toolchains - DIRLINK = $(TOPDIR)/tools/copydir.sh - DIRUNLINK = $(TOPDIR)/tools/unlink.sh - MKDEP = $(TOPDIR)/tools/mknulldeps.sh - ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" - ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" - ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT)}" -else - ifeq ($(PX4_WINTOOL),y) - # Windows-native toolchains (MSYS) - DIRLINK = $(TOPDIR)/tools/copydir.sh - DIRUNLINK = $(TOPDIR)/tools/unlink.sh - MKDEP = $(TOPDIR)/tools/mknulldeps.sh - ARCHINCLUDES = -I. -isystem $(TOPDIR)/include - ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx - ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT) - else - # Linux/Cygwin-native toolchain - MKDEP = $(TOPDIR)/tools/mkdeps.sh - ARCHINCLUDES = -I. -isystem $(TOPDIR)/include - ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx - ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT) - endif -endif - -# tool versions -ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} -ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} - -# optimisation flags -ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ - -fno-strict-aliasing \ - -fno-strength-reduce \ - -fomit-frame-pointer \ - -funsafe-math-optimizations \ - -fno-builtin-printf \ - -ffunction-sections \ - -fdata-sections - -ifeq ("${CONFIG_DEBUG_SYMBOLS}","y") -ARCHOPTIMIZATION += -g -endif - -ARCHCFLAGS = -std=gnu99 -ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x -ARCHWARNINGS = -Wall \ - -Wextra \ - -Wdouble-promotion \ - -Wshadow \ - -Wfloat-equal \ - -Wframe-larger-than=1024 \ - -Wpointer-arith \ - -Wlogical-op \ - -Wmissing-declarations \ - -Wpacked \ - -Wno-unused-parameter -# -Wcast-qual - generates spurious noreturn attribute warnings, try again later -# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code -# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives - -ARCHCWARNINGS = $(ARCHWARNINGS) \ - -Wbad-function-cast \ - -Wstrict-prototypes \ - -Wold-style-declaration \ - -Wmissing-parameter-type \ - -Wmissing-prototypes \ - -Wnested-externs \ - -Wunsuffixed-float-constants -ARCHWARNINGSXX = $(ARCHWARNINGS) \ - -Wno-psabi -ARCHDEFINES = -ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 - -# this seems to be the only way to add linker flags -EXTRA_LIBS += --warn-common \ - --gc-sections - -CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common -CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) -CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) -CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -AFLAGS = $(CFLAGS) -D__ASSEMBLY__ - -NXFLATLDFLAGS1 = -r -d -warn-common -NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections -LDNXFLATFLAGS = -e main -s 2048 - -OBJEXT = .o -LIBEXT = .a -EXEEXT = - - -# produce partially-linked $1 from files in $2 -define PRELINK - @echo "PRELINK: $1" - $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 -endef - -HOSTCC = gcc -HOSTINCLUDES = -I. -HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe -HOSTLDFLAGS = - diff --git a/nuttx-configs/px4fmu-v2/nsh/Make.defs b/nuttx-configs/px4fmu-v2/nsh/Make.defs index 00257d5467..e70320aaa4 100644 --- a/nuttx-configs/px4fmu-v2/nsh/Make.defs +++ b/nuttx-configs/px4fmu-v2/nsh/Make.defs @@ -1,3 +1,179 @@ -include ${TOPDIR}/.config +############################################################################ +# configs/px4fmu-v2/nsh/Make.defs +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# +############################################################################ + +include ${TOPDIR}/.config +include ${TOPDIR}/tools/Config.mk + +# +# We only support building with the ARM bare-metal toolchain from +# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS. +# +CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI + +include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs + +CC = $(CROSSDEV)gcc +CXX = $(CROSSDEV)g++ +CPP = $(CROSSDEV)gcc -E +LD = $(CROSSDEV)ld +AR = $(CROSSDEV)ar rcs +NM = $(CROSSDEV)nm +OBJCOPY = $(CROSSDEV)objcopy +OBJDUMP = $(CROSSDEV)objdump + +MAXOPTIMIZATION = -O3 +ARCHCPUFLAGS = -mcpu=cortex-m4 \ + -mthumb \ + -march=armv7e-m \ + -mfpu=fpv4-sp-d16 \ + -mfloat-abi=hard + + +# enable precise stack overflow tracking +INSTRUMENTATIONDEFINES = -finstrument-functions \ + -ffixed-r10 + +# pull in *just* libm from the toolchain ... this is grody +LIBM = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}" +EXTRA_LIBS += $(LIBM) + +# use our linker script +LDSCRIPT = ld.script + +ifeq ($(WINTOOL),y) + # Windows-native toolchains + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" + ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" + ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)}" +else + ifeq ($(PX4_WINTOOL),y) + # Windows-native toolchains (MSYS) + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) + else + # Linux/Cygwin-native toolchain + MKDEP = $(TOPDIR)/tools/mkdeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) + endif +endif + +# tool versions +ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} +ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} + +# optimisation flags +ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ + -fno-strict-aliasing \ + -fno-strength-reduce \ + -fomit-frame-pointer \ + -funsafe-math-optimizations \ + -fno-builtin-printf \ + -ffunction-sections \ + -fdata-sections + +ifeq ("${CONFIG_DEBUG_SYMBOLS}","y") +ARCHOPTIMIZATION += -g +endif + +ARCHCFLAGS = -std=gnu99 +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x +ARCHWARNINGS = -Wall \ + -Wextra \ + -Wdouble-promotion \ + -Wshadow \ + -Wfloat-equal \ + -Wframe-larger-than=1024 \ + -Wpointer-arith \ + -Wlogical-op \ + -Wmissing-declarations \ + -Wpacked \ + -Wno-unused-parameter +# -Wcast-qual - generates spurious noreturn attribute warnings, try again later +# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code +# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives + +ARCHCWARNINGS = $(ARCHWARNINGS) \ + -Wbad-function-cast \ + -Wstrict-prototypes \ + -Wold-style-declaration \ + -Wmissing-parameter-type \ + -Wmissing-prototypes \ + -Wnested-externs \ + -Wunsuffixed-float-constants +ARCHWARNINGSXX = $(ARCHWARNINGS) \ + -Wno-psabi +ARCHDEFINES = +ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 + +# this seems to be the only way to add linker flags +EXTRA_LIBS += --warn-common \ + --gc-sections + +CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common +CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) +CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe +CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) +CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) +AFLAGS = $(CFLAGS) -D__ASSEMBLY__ + +NXFLATLDFLAGS1 = -r -d -warn-common +NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections +LDNXFLATFLAGS = -e main -s 2048 + +OBJEXT = .o +LIBEXT = .a +EXEEXT = + + +# produce partially-linked $1 from files in $2 +define PRELINK + @echo "PRELINK: $1" + $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 +endef + +HOSTCC = gcc +HOSTINCLUDES = -I. +HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe +HOSTLDFLAGS = -include $(TOPDIR)/configs/px4fmu-v2/common/Make.defs diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig.prev b/nuttx-configs/px4fmu-v2/nsh/defconfig.prev deleted file mode 100755 index 42910ce0a8..0000000000 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig.prev +++ /dev/null @@ -1,1067 +0,0 @@ -############################################################################ -# configs/px4fmu/nsh/defconfig -# -# Copyright (C) 2012 PX4 Development Team. All rights reserved. -# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. -# Author: Gregory Nutt -# -# 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 NuttX 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. -# -############################################################################ -# -# architecture selection -# -# CONFIG_ARCH - identifies the arch subdirectory and, hence, the -# processor architecture. -# CONFIG_ARCH_family - for use in C code. This identifies the -# particular chip family that the architecture is implemented -# in. -# CONFIG_ARCH_architecture - for use in C code. This identifies the -# specific architecture within the chip family. -# CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory -# CONFIG_ARCH_CHIP_name - For use in C code -# CONFIG_ARCH_BOARD - identifies the configs subdirectory and, hence, -# the board that supports the particular chip or SoC. -# CONFIG_ARCH_BOARD_name - for use in C code -# CONFIG_ENDIAN_BIG - define if big endian (default is little endian) -# CONFIG_BOARD_LOOPSPERMSEC - for delay loops -# CONFIG_DRAM_SIZE - Describes the installed DRAM. -# CONFIG_DRAM_START - The start address of DRAM (physical) -# CONFIG_ARCH_IRQPRIO - The STM3240xxx supports interrupt prioritization -# CONFIG_ARCH_FPU - The STM3240xxx supports a floating point unit (FPU). -# CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt -# stack. If defined, this symbol is the size of the interrupt -# stack in bytes. If not defined, the user task stacks will be -# used during interrupt handling. -# CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions -# CONFIG_ARCH_BOOTLOADER - Set if you are using a bootloader. -# CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to board architecture. -# CONFIG_ARCH_BUTTONS - Enable support for buttons. Unique to board architecture. -# CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that -# cause a 100 second delay during boot-up. This 100 second delay -# serves no purpose other than it allows you to calibrate -# CONFIG_BOARD_LOOPSPERMSEC. You simply use a stop watch to measure -# the 100 second delay then adjust CONFIG_BOARD_LOOPSPERMSEC until -# the delay actually is 100 seconds. -# CONFIG_ARCH_DMA - Support DMA initialization -# -CONFIG_ARCH="arm" -CONFIG_ARCH_ARM=y -CONFIG_ARCH_CORTEXM4=y -CONFIG_ARCH_CHIP="stm32" -CONFIG_ARCH_CHIP_STM32F427V=y -CONFIG_ARCH_BOARD="px4fmu-v2" -CONFIG_ARCH_BOARD_PX4FMU_V2=y -CONFIG_BOARD_LOOPSPERMSEC=16717 -CONFIG_DRAM_SIZE=0x00040000 -CONFIG_DRAM_START=0x20000000 -CONFIG_ARCH_IRQPRIO=y -CONFIG_ARCH_FPU=y -CONFIG_ARCH_INTERRUPTSTACK=n -CONFIG_ARCH_STACKDUMP=y -CONFIG_ARCH_BOOTLOADER=n -CONFIG_ARCH_LEDS=n -CONFIG_ARCH_BUTTONS=n -CONFIG_ARCH_CALIBRATION=n -CONFIG_ARCH_DMA=y -CONFIG_ARCH_MATH_H=y - -CONFIG_ARMV7M_CMNVECTOR=y -CONFIG_STM32_STM32F427=y - -# -# JTAG Enable settings (by default JTAG-DP and SW-DP are enabled): -# -# CONFIG_STM32_DFU - Use the DFU bootloader, not JTAG (ignored) -# -# JTAG Enable options: -# -# CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) -# CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) -# but without JNTRST. -# CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled -# -CONFIG_STM32_DFU=n -CONFIG_STM32_JTAG_FULL_ENABLE=n -CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n -CONFIG_STM32_JTAG_SW_ENABLE=y - -# -# On-chip CCM SRAM configuration -# -# CONFIG_STM32_CCMEXCLUDE - Exclude CCM SRAM from the HEAP. You would need -# to do this if DMA is enabled to prevent non-DMA-able CCM memory from -# being a part of the stack. -# -CONFIG_STM32_CCMEXCLUDE=y - -# -# On-board FSMC SRAM configuration -# -# CONFIG_STM32_FSMC - Required. See below -# CONFIG_MM_REGIONS - Required. Must be 2 or 3 (see above) -# -# CONFIG_STM32_FSMC_SRAM=y - Indicates that SRAM is available via the -# FSMC (as opposed to an LCD or FLASH). -# CONFIG_HEAP2_BASE - The base address of the SRAM in the FSMC address space -# CONFIG_HEAP2_END - The end (+1) of the SRAM in the FSMC address space -# -#CONFIG_STM32_FSMC_SRAM=n -#CONFIG_HEAP2_BASE=0x64000000 -#CONFIG_HEAP2_END=(0x64000000+(2*1024*1024)) - -# -# Individual subsystems can be enabled: -# -# This set is exhaustive for PX4FMU and should be safe to cut and -# paste into any other config. -# -# AHB1: -CONFIG_STM32_CRC=n -CONFIG_STM32_BKPSRAM=y -CONFIG_STM32_CCMDATARAM=y -CONFIG_STM32_DMA1=y -CONFIG_STM32_DMA2=y -CONFIG_STM32_ETHMAC=n -CONFIG_STM32_OTGHS=n -# AHB2: -CONFIG_STM32_DCMI=n -CONFIG_STM32_CRYP=n -CONFIG_STM32_HASH=n -CONFIG_STM32_RNG=n -CONFIG_STM32_OTGFS=y -# AHB3: -CONFIG_STM32_FSMC=n -# APB1: -CONFIG_STM32_TIM2=n -CONFIG_STM32_TIM3=y -CONFIG_STM32_TIM4=y -CONFIG_STM32_TIM5=n -CONFIG_STM32_TIM6=n -CONFIG_STM32_TIM7=n -CONFIG_STM32_TIM12=n -CONFIG_STM32_TIM13=n -CONFIG_STM32_TIM14=n -CONFIG_STM32_WWDG=y -CONFIG_STM32_IWDG=n -CONFIG_STM32_SPI2=y -CONFIG_STM32_SPI3=n -CONFIG_STM32_USART2=y -CONFIG_STM32_USART3=y -CONFIG_STM32_UART4=y -CONFIG_STM32_UART5=n -CONFIG_STM32_UART7=y -CONFIG_STM32_UART8=y -CONFIG_STM32_I2C1=y -CONFIG_STM32_I2C2=y -CONFIG_STM32_I2C3=n -CONFIG_STM32_CAN1=n -CONFIG_STM32_CAN2=n -CONFIG_STM32_DAC=n -CONFIG_STM32_PWR=y -# APB2: -CONFIG_STM32_TIM1=y -CONFIG_STM32_TIM8=n -CONFIG_STM32_USART1=y -# Mostly owned by the px4io driver, but uploader needs this -CONFIG_STM32_USART6=y -# We use our own driver, but leave this on. -CONFIG_STM32_ADC1=y -CONFIG_STM32_ADC2=n -CONFIG_STM32_ADC3=n -CONFIG_STM32_SDIO=y -CONFIG_STM32_SPI1=y -CONFIG_STM32_SYSCFG=y -CONFIG_STM32_TIM9=y -CONFIG_STM32_TIM10=y -CONFIG_STM32_TIM11=y - -# -# Enable single wire support. If this is not defined, then this mode cannot -# be enabled. -# -CONFIG_STM32_USART_SINGLEWIRE=y - -# -# We want the flash prefetch on for max performance. -# -STM32_FLASH_PREFETCH=y - -# -# STM32F40xxx specific serial device driver settings -# -# CONFIG_SERIAL_TERMIOS - Serial driver supports termios.h interfaces (tcsetattr, -# tcflush, etc.). If this is not defined, then the terminal settings (baud, -# parity, etc.) are not configurable at runtime; serial streams cannot be -# flushed, etc. -# CONFIG_SERIAL_CONSOLE_REINIT - re-initializes the console serial port -# immediately after creating the /dev/console device. This is required -# if the console serial port has RX DMA enabled. -# -# CONFIG_USARTn_SERIAL_CONSOLE - selects the USARTn for the -# console and ttys0 (default is the USART1). -# CONFIG_USARTn_RXBUFSIZE - Characters are buffered as received. -# This specific the size of the receive buffer -# CONFIG_USARTn_TXBUFSIZE - Characters are buffered before -# being sent. This specific the size of the transmit buffer -# CONFIG_USARTn_BAUD - The configure BAUD of the UART. Must be -# CONFIG_USARTn_BITS - The number of bits. Must be either 7 or 8. -# CONFIG_USARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity -# CONFIG_USARTn_2STOP - Two stop bits -# -CONFIG_SERIAL_TERMIOS=y -CONFIG_SERIAL_CONSOLE_REINIT=n -CONFIG_STANDARD_SERIAL=y - -CONFIG_UART8_SERIAL_CONSOLE=y - -#Mavlink messages can be bigger than 128 -CONFIG_USART1_TXBUFSIZE=512 -CONFIG_USART2_TXBUFSIZE=256 -CONFIG_USART3_TXBUFSIZE=256 -CONFIG_UART4_TXBUFSIZE=256 -CONFIG_USART6_TXBUFSIZE=128 -CONFIG_UART7_TXBUFSIZE=256 -CONFIG_UART8_TXBUFSIZE=256 - -CONFIG_USART1_RXBUFSIZE=512 -CONFIG_USART2_RXBUFSIZE=256 -CONFIG_USART3_RXBUFSIZE=256 -CONFIG_UART4_RXBUFSIZE=256 -CONFIG_USART6_RXBUFSIZE=256 -CONFIG_UART7_RXBUFSIZE=256 -CONFIG_UART8_RXBUFSIZE=256 - -CONFIG_USART1_BAUD=115200 -CONFIG_USART2_BAUD=115200 -CONFIG_USART3_BAUD=115200 -CONFIG_UART4_BAUD=115200 -CONFIG_USART6_BAUD=115200 -CONFIG_UART7_BAUD=115200 -CONFIG_UART8_BAUD=57600 - -CONFIG_USART1_BITS=8 -CONFIG_USART2_BITS=8 -CONFIG_USART3_BITS=8 -CONFIG_UART4_BITS=8 -CONFIG_USART6_BITS=8 -CONFIG_UART7_BITS=8 -CONFIG_UART8_BITS=8 - -CONFIG_USART1_PARITY=0 -CONFIG_USART2_PARITY=0 -CONFIG_USART3_PARITY=0 -CONFIG_UART4_PARITY=0 -CONFIG_USART6_PARITY=0 -CONFIG_UART7_PARITY=0 -CONFIG_UART8_PARITY=0 - -CONFIG_USART1_2STOP=0 -CONFIG_USART2_2STOP=0 -CONFIG_USART3_2STOP=0 -CONFIG_UART4_2STOP=0 -CONFIG_USART6_2STOP=0 -CONFIG_UART7_2STOP=0 -CONFIG_UART8_2STOP=0 - -CONFIG_USART1_RXDMA=y -CONFIG_USART2_RXDMA=y -CONFIG_USART3_RXDMA=n -CONFIG_UART4_RXDMA=n -CONFIG_USART6_RXDMA=y -CONFIG_UART7_RXDMA=n -CONFIG_UART8_RXDMA=n - -# -# STM32F40xxx specific SPI device driver settings -# -CONFIG_SPI_EXCHANGE=y -# DMA needs more work, not implemented on STM32F4x yet -#CONFIG_STM32_SPI_DMA=y - -# -# STM32F40xxx specific CAN device driver settings -# -# CONFIG_CAN - Enables CAN support (one or both of CONFIG_STM32_CAN1 or -# CONFIG_STM32_CAN2 must also be defined) -# CONFIG_CAN_EXTID - Enables support for the 29-bit extended ID. Default -# Standard 11-bit IDs. -# CONFIG_CAN_FIFOSIZE - The size of the circular buffer of CAN messages. -# Default: 8 -# CONFIG_CAN_NPENDINGRTR - The size of the list of pending RTR requests. -# Default: 4 -# CONFIG_CAN_LOOPBACK - A CAN driver may or may not support a loopback -# mode for testing. The STM32 CAN driver does support loopback mode. -# CONFIG_CAN1_BAUD - CAN1 BAUD rate. Required if CONFIG_STM32_CAN1 is defined. -# CONFIG_CAN2_BAUD - CAN1 BAUD rate. Required if CONFIG_STM32_CAN2 is defined. -# CONFIG_CAN_TSEG1 - The number of CAN time quanta in segment 1. Default: 6 -# CONFIG_CAN_TSEG2 - the number of CAN time quanta in segment 2. Default: 7 -# -CONFIG_CAN=n -CONFIG_CAN_EXTID=n -#CONFIG_CAN_FIFOSIZE -#CONFIG_CAN_NPENDINGRTR -CONFIG_CAN_LOOPBACK=n -CONFIG_CAN1_BAUD=700000 -CONFIG_CAN2_BAUD=700000 - - -# XXX remove after integration testing -# Allow 180 us per byte, a wide margin for the 400 KHz clock we're using -# e.g. 9.6 ms for an EEPROM page write, 0.9 ms for a MAG update -CONFIG_STM32_I2CTIMEOUS_PER_BYTE=200 -# Constant overhead for generating I2C start / stop conditions -CONFIG_STM32_I2CTIMEOUS_START_STOP=700 -# XXX this is bad and we want it gone -CONFIG_I2C_WRITEREAD=y - -# -# I2C configuration -# -CONFIG_I2C=y -CONFIG_I2C_POLLED=n -CONFIG_I2C_TRANSFER=y -CONFIG_I2C_TRACE=n -CONFIG_I2C_RESET=y -# XXX fixed per-transaction timeout -CONFIG_STM32_I2CTIMEOMS=10 - -# -# MTD support -# -CONFIG_MTD=y - -# XXX re-enable after integration testing - -# -# I2C configuration -# -#CONFIG_I2C=y -#CONFIG_I2C_POLLED=y -#CONFIG_I2C_TRANSFER=y -#CONFIG_I2C_TRACE=n -#CONFIG_I2C_RESET=y - -# Dynamic timeout -#CONFIG_STM32_I2C_DYNTIMEO=y -#CONFIG_STM32_I2C_DYNTIMEO_STARTSTOP=500 -#CONFIG_STM32_I2C_DYNTIMEO_USECPERBYTE=200 - -# Fixed per-transaction timeout -#CONFIG_STM32_I2CTIMEOSEC=0 -#CONFIG_STM32_I2CTIMEOMS=10 - - - - - - -# -# General build options -# -# CONFIG_RRLOAD_BINARY - make the rrload binary format used with -# BSPs from www.ridgerun.com using the tools/mkimage.sh script -# CONFIG_INTELHEX_BINARY - make the Intel HEX binary format -# used with many different loaders using the GNU objcopy program -# Should not be selected if you are not using the GNU toolchain. -# CONFIG_MOTOROLA_SREC - make the Motorola S-Record binary format -# used with many different loaders using the GNU objcopy program -# Should not be selected if you are not using the GNU toolchain. -# CONFIG_RAW_BINARY - make a raw binary format file used with many -# different loaders using the GNU objcopy program. This option -# should not be selected if you are not using the GNU toolchain. -# CONFIG_HAVE_LIBM - toolchain supports libm.a -# -CONFIG_RRLOAD_BINARY=n -CONFIG_INTELHEX_BINARY=n -CONFIG_MOTOROLA_SREC=n -CONFIG_RAW_BINARY=y -CONFIG_HAVE_LIBM=y - -# -# General OS setup -# -# CONFIG_APPS_DIR - Identifies the relative path to the directory -# that builds the application to link with NuttX. Default: ../apps -# CONFIG_DEBUG - enables built-in debug options -# CONFIG_DEBUG_VERBOSE - enables verbose debug output -# CONFIG_DEBUG_SYMBOLS - build without optimization and with -# debug symbols (needed for use with a debugger). -# CONFIG_HAVE_CXX - Enable support for C++ -# CONFIG_HAVE_CXXINITIALIZE - The platform-specific logic includes support -# for initialization of static C++ instances for this architecture -# and for the selected toolchain (via up_cxxinitialize()). -# CONFIG_MM_REGIONS - If the architecture includes multiple -# regions of memory to allocate from, this specifies the -# number of memory regions that the memory manager must -# handle and enables the API mm_addregion(start, end); -# CONFIG_ARCH_LOWPUTC - architecture supports low-level, boot -# time console output -# CONFIG_MSEC_PER_TICK - The default system timer is 100Hz -# or MSEC_PER_TICK=10. This setting may be defined to -# inform NuttX that the processor hardware is providing -# system timer interrupts at some interrupt interval other -# than 10 msec. -# CONFIG_RR_INTERVAL - The round robin timeslice will be set -# this number of milliseconds; Round robin scheduling can -# be disabled by setting this value to zero. -# CONFIG_SCHED_INSTRUMENTATION - enables instrumentation in -# scheduler to monitor system performance -# CONFIG_TASK_NAME_SIZE - Spcifies that maximum size of a -# task name to save in the TCB. Useful if scheduler -# instrumentation is selected. Set to zero to disable. -# CONFIG_START_YEAR, CONFIG_START_MONTH, CONFIG_START_DAY - -# Used to initialize the internal time logic. -# CONFIG_GREGORIAN_TIME - Enables Gregorian time conversions. -# You would only need this if you are concerned about accurate -# time conversions in the past or in the distant future. -# CONFIG_JULIAN_TIME - Enables Julian time conversions. You -# would only need this if you are concerned about accurate -# time conversion in the distand past. You must also define -# CONFIG_GREGORIAN_TIME in order to use Julian time. -# CONFIG_DEV_CONSOLE - Set if architecture-specific logic -# provides /dev/console. Enables stdout, stderr, stdin. -# CONFIG_DEV_LOWCONSOLE - Use the simple, low-level serial console -# driver (minimul support) -# CONFIG_MUTEX_TYPES: Set to enable support for recursive and -# errorcheck mutexes. Enables pthread_mutexattr_settype(). -# CONFIG_PRIORITY_INHERITANCE : Set to enable support for priority -# inheritance on mutexes and semaphores. -# CONFIG_SEM_PREALLOCHOLDERS: This setting is only used if priority -# inheritance is enabled. It defines the maximum number of -# different threads (minus one) that can take counts on a -# semaphore with priority inheritance support. This may be -# set to zero if priority inheritance is disabled OR if you -# are only using semaphores as mutexes (only one holder) OR -# if no more than two threads participate using a counting -# semaphore. -# CONFIG_SEM_NNESTPRIO. If priority inheritance is enabled, -# then this setting is the maximum number of higher priority -# threads (minus 1) than can be waiting for another thread -# to release a count on a semaphore. This value may be set -# to zero if no more than one thread is expected to wait for -# a semaphore. -# CONFIG_FDCLONE_DISABLE. Disable cloning of all file descriptors -# by task_create() when a new task is started. If set, all -# files/drivers will appear to be closed in the new task. -# CONFIG_FDCLONE_STDIO. Disable cloning of all but the first -# three file descriptors (stdin, stdout, stderr) by task_create() -# when a new task is started. If set, all files/drivers will -# appear to be closed in the new task except for stdin, stdout, -# and stderr. -# CONFIG_SDCLONE_DISABLE. Disable cloning of all socket -# desciptors by task_create() when a new task is started. If -# set, all sockets will appear to be closed in the new task. -# CONFIG_SCHED_WORKQUEUE. Create a dedicated "worker" thread to -# handle delayed processing from interrupt handlers. This feature -# is required for some drivers but, if there are not complaints, -# can be safely disabled. The worker thread also performs -# garbage collection -- completing any delayed memory deallocations -# from interrupt handlers. If the worker thread is disabled, -# then that clean will be performed by the IDLE thread instead -# (which runs at the lowest of priority and may not be appropriate -# if memory reclamation is of high priority). If CONFIG_SCHED_WORKQUEUE -# is enabled, then the following options can also be used: -# CONFIG_SCHED_WORKPRIORITY - The execution priority of the worker -# thread. Default: 192 -# CONFIG_SCHED_WORKPERIOD - How often the worker thread checks for -# work in units of microseconds. Default: 50*1000 (50 MS). -# CONFIG_SCHED_WORKSTACKSIZE - The stack size allocated for the worker -# thread. Default: CONFIG_IDLETHREAD_STACKSIZE. -# CONFIG_SIG_SIGWORK - The signal number that will be used to wake-up -# the worker thread. Default: 4 -# -# CONFIG_SCHED_LPWORK. If CONFIG_SCHED_WORKQUEUE is defined, then a single -# work queue is created by default. If CONFIG_SCHED_LPWORK is also defined -# then an additional, lower-priority work queue will also be created. This -# lower priority work queue is better suited for more extended processing -# (such as file system clean-up operations) -# CONFIG_SCHED_LPWORKPRIORITY - The execution priority of the lower priority -# worker thread. Default: 50 -# CONFIG_SCHED_LPWORKPERIOD - How often the lower priority worker thread -# checks for work in units of microseconds. Default: 50*1000 (50 MS). -# CONFIG_SCHED_LPWORKSTACKSIZE - The stack size allocated for the lower -# priority worker thread. Default: CONFIG_IDLETHREAD_STACKSIZE. -# CONFIG_SCHED_WAITPID - Enable the waitpid() API -# CONFIG_SCHED_ATEXIT - Enabled the atexit() API -# -CONFIG_USER_ENTRYPOINT="nsh_main" -#CONFIG_APPS_DIR= -CONFIG_DEBUG=y -CONFIG_DEBUG_VERBOSE=y -CONFIG_DEBUG_SYMBOLS=y -CONFIG_DEBUG_FS=n -CONFIG_DEBUG_GRAPHICS=n -CONFIG_DEBUG_LCD=n -CONFIG_DEBUG_USB=n -CONFIG_DEBUG_NET=n -CONFIG_DEBUG_RTC=n -CONFIG_DEBUG_ANALOG=n -CONFIG_DEBUG_PWM=n -CONFIG_DEBUG_CAN=n -CONFIG_DEBUG_I2C=n -CONFIG_DEBUG_INPUT=n - -CONFIG_HAVE_CXX=y -CONFIG_HAVE_CXXINITIALIZE=y -CONFIG_MM_REGIONS=2 -CONFIG_ARCH_LOWPUTC=y -CONFIG_MSEC_PER_TICK=1 -CONFIG_RR_INTERVAL=0 -CONFIG_SCHED_INSTRUMENTATION=y -CONFIG_TASK_NAME_SIZE=24 -CONFIG_START_YEAR=1970 -CONFIG_START_MONTH=1 -CONFIG_START_DAY=1 -CONFIG_GREGORIAN_TIME=n -CONFIG_JULIAN_TIME=n -CONFIG_DEV_CONSOLE=y -CONFIG_DEV_LOWCONSOLE=y -CONFIG_MUTEX_TYPES=n -CONFIG_PRIORITY_INHERITANCE=y -CONFIG_SEM_PREALLOCHOLDERS=0 -CONFIG_SEM_NNESTPRIO=8 -CONFIG_FDCLONE_DISABLE=n -CONFIG_FDCLONE_STDIO=y -CONFIG_SDCLONE_DISABLE=y -CONFIG_SCHED_WORKQUEUE=y -CONFIG_SCHED_WORKPRIORITY=192 -CONFIG_SCHED_WORKPERIOD=5000 -CONFIG_SCHED_WORKSTACKSIZE=2048 -CONFIG_SCHED_LPWORK=y -CONFIG_SCHED_LPWORKPRIORITY=50 -CONFIG_SCHED_LPWORKPERIOD=50000 -CONFIG_SCHED_LPWORKSTACKSIZE=2048 -CONFIG_SIG_SIGWORK=4 -CONFIG_SCHED_WAITPID=y -CONFIG_SCHED_ATEXIT=n - -# -# System Logging -# -# CONFIG_SYSLOG - Enables the System Logging feature. -# CONFIG_RAMLOG - Enables the RAM logging feature -# CONFIG_RAMLOG_CONSOLE - Use the RAM logging device as a system console. -# If this feature is enabled (along with CONFIG_DEV_CONSOLE), then all -# console output will be re-directed to a circular buffer in RAM. This -# is useful, for example, if the only console is a Telnet console. Then -# in that case, console output from non-Telnet threads will go to the -# circular buffer and can be viewed using the NSH 'dmesg' command. -# CONFIG_RAMLOG_SYSLOG - Use the RAM logging device for the syslogging -# interface. If this feature is enabled (along with CONFIG_SYSLOG), -# then all debug output (only) will be re-directed to the circular -# buffer in RAM. This RAM log can be view from NSH using the 'dmesg' -# command. -# CONFIG_RAMLOG_NPOLLWAITERS - The number of threads than can be waiting -# for this driver on poll(). Default: 4 -# -# If CONFIG_RAMLOG_CONSOLE or CONFIG_RAMLOG_SYSLOG is selected, then the -# following may also be provided: -# -# CONFIG_RAMLOG_CONSOLE_BUFSIZE - Size of the console RAM log. Default: 1024 -# - -CONFIG_SYSLOG=n -CONFIG_RAMLOG=n -CONFIG_RAMLOG_CONSOLE=n -CONFIG_RAMLOG_SYSLOG=n -#CONFIG_RAMLOG_NPOLLWAITERS -#CONFIG_RAMLOG_CONSOLE_BUFSIZE - -# -# The following can be used to disable categories of -# APIs supported by the OS. If the compiler supports -# weak functions, then it should not be necessary to -# disable functions unless you want to restrict usage -# of those APIs. -# -# There are certain dependency relationships in these -# features. -# -# o mq_notify logic depends on signals to awaken tasks -# waiting for queues to become full or empty. -# o pthread_condtimedwait() depends on signals to wake -# up waiting tasks. -# -CONFIG_DISABLE_CLOCK=n -CONFIG_DISABLE_POSIX_TIMERS=n -CONFIG_DISABLE_PTHREAD=n -CONFIG_DISABLE_SIGNALS=n -CONFIG_DISABLE_MQUEUE=n -CONFIG_DISABLE_MOUNTPOINT=n -CONFIG_DISABLE_ENVIRON=n -CONFIG_DISABLE_POLL=n - -# -# Misc libc settings -# -# CONFIG_NOPRINTF_FIELDWIDTH - sprintf-related logic is a -# little smaller if we do not support fieldwidthes -# CONFIG_LIBC_FLOATINGPOINT - Enables printf("%f") -# CONFIG_LIBC_FIXEDPRECISION - Sets 7 digits after dot for printing: -# 5.1234567 -# CONFIG_HAVE_LONG_LONG - Enabled printf("%llu) -# -CONFIG_NOPRINTF_FIELDWIDTH=n -CONFIG_LIBC_FLOATINGPOINT=y -CONFIG_HAVE_LONG_LONG=y - -# -# Allow for architecture optimized implementations -# -# The architecture can provide optimized versions of the -# following to improve system performance -# -CONFIG_ARCH_MEMCPY=y -CONFIG_ARCH_MEMCMP=n -CONFIG_ARCH_MEMMOVE=n -CONFIG_ARCH_MEMSET=n -CONFIG_ARCH_STRCMP=n -CONFIG_ARCH_STRCPY=n -CONFIG_ARCH_STRNCPY=n -CONFIG_ARCH_STRLEN=n -CONFIG_ARCH_STRNLEN=n -CONFIG_ARCH_BZERO=n - -# -# Sizes of configurable things (0 disables) -# -# CONFIG_MAX_TASKS - The maximum number of simultaneously -# active tasks. This value must be a power of two. -# CONFIG_MAX_TASK_ARGS - This controls the maximum number of -# of parameters that a task may receive (i.e., maxmum value -# of 'argc') -# CONFIG_NPTHREAD_KEYS - The number of items of thread- -# specific data that can be retained -# CONFIG_NFILE_DESCRIPTORS - The maximum number of file -# descriptors (one for each open) -# CONFIG_NFILE_STREAMS - The maximum number of streams that -# can be fopen'ed -# CONFIG_NAME_MAX - The maximum size of a file name. -# CONFIG_STDIO_BUFFER_SIZE - Size of the buffer to allocate -# on fopen. (Only if CONFIG_NFILE_STREAMS > 0) -# CONFIG_STDIO_LINEBUFFER - If standard C buffered I/O is enabled -# (CONFIG_STDIO_BUFFER_SIZE > 0), then this option may be added -# to force automatic, line-oriented flushing the output buffer -# for putc(), fputc(), putchar(), puts(), fputs(), printf(), -# fprintf(), and vfprintf(). When a newline is encountered in -# the output string, the output buffer will be flushed. This -# (slightly) increases the NuttX footprint but supports the kind -# of behavior that people expect for printf(). -# CONFIG_NUNGET_CHARS - Number of characters that can be -# buffered by ungetc() (Only if CONFIG_NFILE_STREAMS > 0) -# CONFIG_PREALLOC_MQ_MSGS - The number of pre-allocated message -# structures. The system manages a pool of preallocated -# message structures to minimize dynamic allocations -# CONFIG_MQ_MAXMSGSIZE - Message structures are allocated with -# a fixed payload size given by this settin (does not include -# other message structure overhead. -# CONFIG_MAX_WDOGPARMS - Maximum number of parameters that -# can be passed to a watchdog handler -# CONFIG_PREALLOC_WDOGS - The number of pre-allocated watchdog -# structures. The system manages a pool of preallocated -# watchdog structures to minimize dynamic allocations -# CONFIG_PREALLOC_TIMERS - The number of pre-allocated POSIX -# timer structures. The system manages a pool of preallocated -# timer structures to minimize dynamic allocations. Set to -# zero for all dynamic allocations. -# -CONFIG_MAX_TASKS=32 -CONFIG_MAX_TASK_ARGS=8 -CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=32 -CONFIG_NFILE_STREAMS=25 -CONFIG_NAME_MAX=32 -CONFIG_STDIO_BUFFER_SIZE=256 -CONFIG_STDIO_LINEBUFFER=y -CONFIG_NUNGET_CHARS=2 -CONFIG_PREALLOC_MQ_MSGS=4 -CONFIG_MQ_MAXMSGSIZE=32 -CONFIG_MAX_WDOGPARMS=2 -CONFIG_PREALLOC_WDOGS=50 -CONFIG_PREALLOC_TIMERS=50 - -# -# Filesystem configuration -# -# CONFIG_FS_FAT - Enable FAT filesystem support -# CONFIG_FAT_SECTORSIZE - Max supported sector size -# CONFIG_FAT_LCNAMES - Enable use of the NT-style upper/lower case 8.3 -# file name support. -# CONFIG_FAT_LFN - Enable FAT long file names. NOTE: Microsoft claims -# patents on FAT long file name technology. Please read the -# disclaimer in the top-level COPYING file and only enable this -# feature if you understand these issues. -# CONFIG_FAT_MAXFNAME - If CONFIG_FAT_LFN is defined, then the -# default, maximum long file name is 255 bytes. This can eat up -# a lot of memory (especially stack space). If you are willing -# to live with some non-standard, short long file names, then -# define this value. A good choice would be the same value as -# selected for CONFIG_NAME_MAX which will limit the visibility -# of longer file names anyway. -# CONFIG_FS_NXFFS: Enable NuttX FLASH file system (NXFF) support. -# CONFIG_NXFFS_ERASEDSTATE: The erased state of FLASH. -# This must have one of the values of 0xff or 0x00. -# Default: 0xff. -# CONFIG_NXFFS_PACKTHRESHOLD: When packing flash file data, -# don't both with file chunks smaller than this number of data bytes. -# CONFIG_NXFFS_MAXNAMLEN: The maximum size of an NXFFS file name. -# Default: 255. -# CONFIG_NXFFS_PACKTHRESHOLD: When packing flash file data, -# don't both with file chunks smaller than this number of data bytes. -# Default: 32. -# CONFIG_NXFFS_TAILTHRESHOLD: clean-up can either mean -# packing files together toward the end of the file or, if file are -# deleted at the end of the file, clean up can simply mean erasing -# the end of FLASH memory so that it can be re-used again. However, -# doing this can also harm the life of the FLASH part because it can -# mean that the tail end of the FLASH is re-used too often. This -# threshold determines if/when it is worth erased the tail end of FLASH -# and making it available for re-use (and possible over-wear). -# Default: 8192. -# CONFIG_FS_ROMFS - Enable ROMFS filesystem support -# CONFIG_FS_RAMMAP - For file systems that do not support XIP, this -# option will enable a limited form of memory mapping that is -# implemented by copying whole files into memory. -# -CONFIG_FS_FAT=y -CONFIG_FAT_LCNAMES=y -CONFIG_FAT_LFN=y -CONFIG_FAT_MAXFNAME=32 -CONFIG_FS_NXFFS=y -CONFIG_NXFFS_MAXNAMLEN=32 -CONFIG_NXFFS_TAILTHRESHOLD=2048 -CONFIG_NXFFS_PREALLOCATED=y -CONFIG_FS_ROMFS=y -CONFIG_FS_BINFS=y - -# -# SPI-based MMC/SD driver -# -# CONFIG_MMCSD_NSLOTS -# Number of MMC/SD slots supported by the driver -# CONFIG_MMCSD_READONLY -# Provide read-only access (default is read/write) -# CONFIG_MMCSD_SPICLOCK - Maximum SPI clock to drive MMC/SD card. -# Default is 20MHz, current setting 24 MHz -# -#CONFIG_MMCSD=n -# XXX need to rejig this for SDIO -#CONFIG_MMCSD_SPI=y -#CONFIG_MMCSD_NSLOTS=1 -#CONFIG_MMCSD_READONLY=n -#CONFIG_MMCSD_SPICLOCK=24000000 - -# -# Maintain legacy build behavior (revisit) -# - -CONFIG_MMCSD=y -#CONFIG_MMCSD_SPI=y -CONFIG_MMCSD_SDIO=y -CONFIG_MTD=y - -# -# SPI-based MMC/SD driver -# -#CONFIG_MMCSD_NSLOTS=1 -#CONFIG_MMCSD_READONLY=n -#CONFIG_MMCSD_SPICLOCK=12500000 - -# -# STM32 SDIO-based MMC/SD driver -# -CONFIG_SDIO_DMA=y -#CONFIG_SDIO_PRI=128 -#CONFIG_SDIO_DMAPRIO -#CONFIG_SDIO_WIDTH_D1_ONLY -CONFIG_MMCSD_MULTIBLOCK_DISABLE=y -CONFIG_MMCSD_MMCSUPPORT=n -CONFIG_MMCSD_HAVECARDDETECT=n - -# -# Block driver buffering -# -# CONFIG_FS_READAHEAD -# Enable read-ahead buffering -# CONFIG_FS_WRITEBUFFER -# Enable write buffering -# -CONFIG_FS_READAHEAD=n -CONFIG_FS_WRITEBUFFER=n - -# -# RTC Configuration -# -# CONFIG_RTC - Enables general support for a hardware RTC. Specific -# architectures may require other specific settings. -# CONFIG_RTC_DATETIME - There are two general types of RTC: (1) A simple -# battery backed counter that keeps the time when power is down, and (2) -# A full date / time RTC the provides the date and time information, often -# in BCD format. If CONFIG_RTC_DATETIME is selected, it specifies this -# second kind of RTC. In this case, the RTC is used to "seed" the normal -# NuttX timer and the NuttX system timer provides for higher resoution -# time. -# CONFIG_RTC_HIRES - If CONFIG_RTC_DATETIME not selected, then the simple, -# battery backed counter is used. There are two different implementations -# of such simple counters based on the time resolution of the counter: -# The typical RTC keeps time to resolution of 1 second, usually -# supporting a 32-bit time_t value. In this case, the RTC is used to -# "seed" the normal NuttX timer and the NuttX timer provides for higher -# resoution time. If CONFIG_RTC_HIRES is enabled in the NuttX configuration, -# then the RTC provides higher resolution time and completely replaces the -# system timer for purpose of date and time. -# CONFIG_RTC_FREQUENCY - If CONFIG_RTC_HIRES is defined, then the frequency -# of the high resolution RTC must be provided. If CONFIG_RTC_HIRES is -# not defined, CONFIG_RTC_FREQUENCY is assumed to be one. -# CONFIG_RTC_ALARM - Enable if the RTC hardware supports setting of an -# alarm. A callback function will be executed when the alarm goes off -# -CONFIG_RTC=n -CONFIG_RTC_DATETIME=y -CONFIG_RTC_HIRES=n -CONFIG_RTC_FREQUENCY=n -CONFIG_RTC_ALARM=n - -# -# USB Device Configuration -# -# CONFIG_USBDEV -# Enables USB device support -# CONFIG_USBDEV_ISOCHRONOUS -# Build in extra support for isochronous endpoints -# CONFIG_USBDEV_DUALSPEED -# Hardware handles high and full speed operation (USB 2.0) -# CONFIG_USBDEV_SELFPOWERED -# Will cause USB features to indicate that the device is -# self-powered -# CONFIG_USBDEV_MAXPOWER -# Maximum power consumption in mA -# CONFIG_USBDEV_TRACE -# Enables USB tracing for debug -# CONFIG_USBDEV_TRACE_NRECORDS -# Number of trace entries to remember -# -CONFIG_USBDEV=y -CONFIG_USBDEV_ISOCHRONOUS=n -CONFIG_USBDEV_DUALSPEED=n -CONFIG_USBDEV_SELFPOWERED=y -CONFIG_USBDEV_REMOTEWAKEUP=n -CONFIG_USBDEV_MAXPOWER=500 -CONFIG_USBDEV_TRACE=n -CONFIG_USBDEV_TRACE_NRECORDS=512 - -# -# USB serial device class driver (Standard CDC ACM class) -# -# CONFIG_CDCACM -# Enable compilation of the USB serial driver -# CONFIG_CDCACM_CONSOLE -# Configures the CDC/ACM serial port as the console device. -# CONFIG_CDCACM_EP0MAXPACKET -# Endpoint 0 max packet size. Default 64 -# CONFIG_CDCACM_EPINTIN -# The logical 7-bit address of a hardware endpoint that supports -# interrupt IN operation. Default 2. -# CONFIG_CDCACM_EPINTIN_FSSIZE -# Max package size for the interrupt IN endpoint if full speed mode. -# Default 64. -# CONFIG_CDCACM_EPINTIN_HSSIZE -# Max package size for the interrupt IN endpoint if high speed mode. -# Default 64 -# CONFIG_CDCACM_EPBULKOUT -# The logical 7-bit address of a hardware endpoint that supports -# bulk OUT operation. Default 4. -# CONFIG_CDCACM_EPBULKOUT_FSSIZE -# Max package size for the bulk OUT endpoint if full speed mode. -# Default 64. -# CONFIG_CDCACM_EPBULKOUT_HSSIZE -# Max package size for the bulk OUT endpoint if high speed mode. -# Default 512. -# CONFIG_CDCACM_EPBULKIN -# The logical 7-bit address of a hardware endpoint that supports -# bulk IN operation. Default 3. -# CONFIG_CDCACM_EPBULKIN_FSSIZE -# Max package size for the bulk IN endpoint if full speed mode. -# Default 64. -# CONFIG_CDCACM_EPBULKIN_HSSIZE -# Max package size for the bulk IN endpoint if high speed mode. -# Default 512. -# CONFIG_CDCACM_NWRREQS and CONFIG_CDCACM_NRDREQS -# The number of write/read requests that can be in flight. -# Default 256. -# CONFIG_CDCACM_VENDORID and CONFIG_CDCACM_VENDORSTR -# The vendor ID code/string. Default 0x0525 and "NuttX" -# 0x0525 is the Netchip vendor and should not be used in any -# products. This default VID was selected for compatibility with -# the Linux CDC ACM default VID. -# CONFIG_CDCACM_PRODUCTID and CONFIG_CDCACM_PRODUCTSTR -# The product ID code/string. Default 0xa4a7 and "CDC/ACM Serial" -# 0xa4a7 was selected for compatibility with the Linux CDC ACM -# default PID. -# CONFIG_CDCACM_RXBUFSIZE and CONFIG_CDCACM_TXBUFSIZE -# Size of the serial receive/transmit buffers. Default 256. -# -CONFIG_CDCACM=y -CONFIG_CDCACM_CONSOLE=n -#CONFIG_CDCACM_EP0MAXPACKET -CONFIG_CDCACM_EPINTIN=1 -#CONFIG_CDCACM_EPINTIN_FSSIZE -#CONFIG_CDCACM_EPINTIN_HSSIZE -CONFIG_CDCACM_EPBULKOUT=3 -#CONFIG_CDCACM_EPBULKOUT_FSSIZE -#CONFIG_CDCACM_EPBULKOUT_HSSIZE -CONFIG_CDCACM_EPBULKIN=2 -#CONFIG_CDCACM_EPBULKIN_FSSIZE -#CONFIG_CDCACM_EPBULKIN_HSSIZE -#CONFIG_CDCACM_NWRREQS -#CONFIG_CDCACM_NRDREQS -CONFIG_CDCACM_VENDORID=0x26AC -CONFIG_CDCACM_VENDORSTR="3D Robotics" -CONFIG_CDCACM_PRODUCTID=0x0010 -CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v1.6" -#CONFIG_CDCACM_RXBUFSIZE -#CONFIG_CDCACM_TXBUFSIZE - - -# -# Settings for apps/nshlib -# -# CONFIG_NSH_BUILTIN_APPS - Support external registered, -# "named" applications that can be executed from the NSH -# command line (see apps/README.txt for more information). -# CONFIG_NSH_FILEIOSIZE - Size of a static I/O buffer -# CONFIG_NSH_STRERROR - Use strerror(errno) -# CONFIG_NSH_LINELEN - Maximum length of one command line -# CONFIG_NSH_MAX_ARGUMENTS - Maximum number of arguments for command line -# CONFIG_NSH_NESTDEPTH - Max number of nested if-then[-else]-fi -# CONFIG_NSH_DISABLESCRIPT - Disable scripting support -# CONFIG_NSH_DISABLEBG - Disable background commands -# CONFIG_NSH_ROMFSETC - Use startup script in /etc -# CONFIG_NSH_CONSOLE - Use serial console front end -# CONFIG_NSH_TELNET - Use telnetd console front end -# CONFIG_NSH_ARCHINIT - Platform provides architecture -# specific initialization (nsh_archinitialize()). -# -# If CONFIG_NSH_TELNET is selected: -# CONFIG_NSH_IOBUFFER_SIZE -- Telnetd I/O buffer size -# CONFIG_NSH_DHCPC - Obtain address using DHCP -# CONFIG_NSH_IPADDR - Provides static IP address -# CONFIG_NSH_DRIPADDR - Provides static router IP address -# CONFIG_NSH_NETMASK - Provides static network mask -# CONFIG_NSH_NOMAC - Use a bogus MAC address -# -# If CONFIG_NSH_ROMFSETC is selected: -# CONFIG_NSH_ROMFSMOUNTPT - ROMFS mountpoint -# CONFIG_NSH_INITSCRIPT - Relative path to init script -# CONFIG_NSH_ROMFSDEVNO - ROMFS RAM device minor -# CONFIG_NSH_ROMFSSECTSIZE - ROMF sector size -# CONFIG_NSH_FATDEVNO - FAT FS RAM device minor -# CONFIG_NSH_FATSECTSIZE - FAT FS sector size -# CONFIG_NSH_FATNSECTORS - FAT FS number of sectors -# CONFIG_NSH_FATMOUNTPT - FAT FS mountpoint -# -CONFIG_BUILTIN=y -CONFIG_NSH_BUILTIN_APPS=y -CONFIG_NSH_FILEIOSIZE=512 -CONFIG_NSH_STRERROR=y -CONFIG_NSH_LINELEN=128 -CONFIG_NSH_MAX_ARGUMENTS=12 -CONFIG_NSH_NESTDEPTH=8 -CONFIG_NSH_DISABLESCRIPT=n -CONFIG_NSH_DISABLEBG=n -CONFIG_NSH_ROMFSETC=y -CONFIG_NSH_ARCHROMFS=y -CONFIG_NSH_CONSOLE=y -CONFIG_NSH_USBCONSOLE=n -#CONFIG_NSH_USBCONDEV="/dev/ttyACM0" -CONFIG_NSH_TELNET=n -CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_IOBUFFER_SIZE=512 -CONFIG_NSH_DHCPC=n -CONFIG_NSH_NOMAC=y -CONFIG_NSH_IPADDR=0x0a000002 -CONFIG_NSH_DRIPADDR=0x0a000001 -CONFIG_NSH_NETMASK=0xffffff00 -CONFIG_NSH_ROMFSMOUNTPT="/etc" -CONFIG_NSH_INITSCRIPT="init.d/rcS" -CONFIG_NSH_ROMFSDEVNO=0 -CONFIG_NSH_ROMFSSECTSIZE=128 # Default 64, increased to allow for more than 64 folders on the sdcard -CONFIG_NSH_FATDEVNO=1 -CONFIG_NSH_FATSECTSIZE=512 -CONFIG_NSH_FATNSECTORS=1024 -CONFIG_NSH_FATMOUNTPT=/tmp - -# -# Architecture-specific NSH options -# -#CONFIG_NSH_MMCSDSPIPORTNO=3 -CONFIG_NSH_MMCSDSLOTNO=0 -CONFIG_NSH_MMCSDMINOR=0 - - -# -# Stack and heap information -# -# CONFIG_BOOT_RUNFROMFLASH - Some configurations support XIP -# operation from FLASH but must copy initialized .data sections to RAM. -# (should also be =n for the STM3240G-EVAL which always runs from flash) -# CONFIG_BOOT_COPYTORAM - Some configurations boot in FLASH -# but copy themselves entirely into RAM for better performance. -# CONFIG_CUSTOM_STACK - The up_ implementation will handle -# all stack operations outside of the nuttx model. -# CONFIG_STACK_POINTER - The initial stack pointer (arm7tdmi only) -# CONFIG_IDLETHREAD_STACKSIZE - The size of the initial stack. -# This is the thread that (1) performs the inital boot of the system up -# to the point where user_start() is spawned, and (2) there after is the -# IDLE thread that executes only when there is no other thread ready to -# run. -# CONFIG_USERMAIN_STACKSIZE - The size of the stack to allocate -# for the main user thread that begins at the user_start() entry point. -# CONFIG_PTHREAD_STACK_MIN - Minimum pthread stack size -# CONFIG_PTHREAD_STACK_DEFAULT - Default pthread stack size -# CONFIG_HEAP_BASE - The beginning of the heap -# CONFIG_HEAP_SIZE - The size of the heap -# -CONFIG_BOOT_RUNFROMFLASH=n -CONFIG_BOOT_COPYTORAM=n -CONFIG_CUSTOM_STACK=n -CONFIG_STACK_POINTER= -# Idle thread needs 4096 bytes -# default 1 KB is not enough -# 4096 bytes -CONFIG_IDLETHREAD_STACKSIZE=6000 -# USERMAIN stack size probably needs to be around 4096 bytes -CONFIG_USERMAIN_STACKSIZE=4096 -CONFIG_PTHREAD_STACK_MIN=512 -CONFIG_PTHREAD_STACK_DEFAULT=2048 -CONFIG_HEAP_BASE= -CONFIG_HEAP_SIZE= - -# enable bindir -CONFIG_APPS_BINDIR=y diff --git a/nuttx-configs/px4fmu-v2/common/ld.script b/nuttx-configs/px4fmu-v2/scripts/ld.script similarity index 100% rename from nuttx-configs/px4fmu-v2/common/ld.script rename to nuttx-configs/px4fmu-v2/scripts/ld.script diff --git a/nuttx-configs/px4io-v1/Kconfig b/nuttx-configs/px4io-v1/Kconfig deleted file mode 100644 index fbf74d7f0d..0000000000 --- a/nuttx-configs/px4io-v1/Kconfig +++ /dev/null @@ -1,21 +0,0 @@ -# -# For a description of the syntax of this configuration file, -# see misc/tools/kconfig-language.txt. -# - -if ARCH_BOARD_PX4IO_V1 - -config HRT_TIMER - bool "High resolution timer support" - default y - ---help--- - Enable high resolution timer for PPM capture and system clocks. - -config HRT_PPM - bool "PPM input capture" - default y - depends on HRT_TIMER - ---help--- - Enable PPM input capture via HRT (for CPPM / PPM sum RC inputs) - -endif diff --git a/nuttx-configs/px4io-v1/README.txt b/nuttx-configs/px4io-v1/README.txt deleted file mode 100755 index 9b1615f42d..0000000000 --- a/nuttx-configs/px4io-v1/README.txt +++ /dev/null @@ -1,806 +0,0 @@ -README -====== - -This README discusses issues unique to NuttX configurations for the -STMicro STM3210E-EVAL development board. - -Contents -======== - - - Development Environment - - GNU Toolchain Options - - IDEs - - NuttX buildroot Toolchain - - DFU and JTAG - - OpenOCD - - LEDs - - Temperature Sensor - - RTC - - STM3210E-EVAL-specific Configuration Options - - Configurations - -Development Environment -======================= - - Either Linux or Cygwin on Windows can be used for the development environment. - The source has been built only using the GNU toolchain (see below). Other - toolchains will likely cause problems. Testing was performed using the Cygwin - environment because the Raisonance R-Link emulatator and some RIDE7 development tools - were used and those tools works only under Windows. - -GNU Toolchain Options -===================== - - The NuttX make system has been modified to support the following different - toolchain options. - - 1. The CodeSourcery GNU toolchain, - 2. The devkitARM GNU toolchain, - 3. Raisonance GNU toolchain, or - 4. The NuttX buildroot Toolchain (see below). - - All testing has been conducted using the NuttX buildroot toolchain. However, - the make system is setup to default to use the devkitARM toolchain. To use - the CodeSourcery, devkitARM or Raisonance GNU toolchain, you simply need to - add one of the following configuration options to your .config (or defconfig) - file: - - CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows - CONFIG_STM32_CODESOURCERYL=y : CodeSourcery under Linux - CONFIG_STM32_DEVKITARM=y : devkitARM under Windows - CONFIG_STM32_RAISONANCE=y : Raisonance RIDE7 under Windows - CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin (default) - - If you are not using CONFIG_STM32_BUILDROOT, then you may also have to modify - the PATH in the setenv.h file if your make cannot find the tools. - - NOTE: the CodeSourcery (for Windows), devkitARM, and Raisonance toolchains are - Windows native toolchains. The CodeSourcey (for Linux) and NuttX buildroot - toolchains are Cygwin and/or Linux native toolchains. There are several limitations - to using a Windows based toolchain in a Cygwin environment. The three biggest are: - - 1. The Windows toolchain cannot follow Cygwin paths. Path conversions are - performed automatically in the Cygwin makefiles using the 'cygpath' utility - but you might easily find some new path problems. If so, check out 'cygpath -w' - - 2. Windows toolchains cannot follow Cygwin symbolic links. Many symbolic links - are used in Nuttx (e.g., include/arch). The make system works around these - problems for the Windows tools by copying directories instead of linking them. - But this can also cause some confusion for you: For example, you may edit - a file in a "linked" directory and find that your changes had no effect. - That is because you are building the copy of the file in the "fake" symbolic - directory. If you use a Windows toolchain, you should get in the habit of - making like this: - - make clean_context all - - An alias in your .bashrc file might make that less painful. - - 3. Dependencies are not made when using Windows versions of the GCC. This is - because the dependencies are generated using Windows pathes which do not - work with the Cygwin make. - - Support has been added for making dependencies with the windows-native toolchains. - That support can be enabled by modifying your Make.defs file as follows: - - - MKDEP = $(TOPDIR)/tools/mknulldeps.sh - + MKDEP = $(TOPDIR)/tools/mkdeps.sh --winpaths "$(TOPDIR)" - - If you have problems with the dependency build (for example, if you are not - building on C:), then you may need to modify tools/mkdeps.sh - - NOTE 1: The CodeSourcery toolchain (2009q1) does not work with default optimization - level of -Os (See Make.defs). It will work with -O0, -O1, or -O2, but not with - -Os. - - NOTE 2: The devkitARM toolchain includes a version of MSYS make. Make sure that - the paths to Cygwin's /bin and /usr/bin directories appear BEFORE the devkitARM - path or will get the wrong version of make. - -IDEs -==== - - NuttX is built using command-line make. It can be used with an IDE, but some - effort will be required to create the project (There is a simple RIDE project - in the RIDE subdirectory). - - Makefile Build - -------------- - Under Eclipse, it is pretty easy to set up an "empty makefile project" and - simply use the NuttX makefile to build the system. That is almost for free - under Linux. Under Windows, you will need to set up the "Cygwin GCC" empty - makefile project in order to work with Windows (Google for "Eclipse Cygwin" - - there is a lot of help on the internet). - - Native Build - ------------ - Here are a few tips before you start that effort: - - 1) Select the toolchain that you will be using in your .config file - 2) Start the NuttX build at least one time from the Cygwin command line - before trying to create your project. This is necessary to create - certain auto-generated files and directories that will be needed. - 3) Set up include pathes: You will need include/, arch/arm/src/stm32, - arch/arm/src/common, arch/arm/src/armv7-m, and sched/. - 4) All assembly files need to have the definition option -D __ASSEMBLY__ - on the command line. - - Startup files will probably cause you some headaches. The NuttX startup file - is arch/arm/src/stm32/stm32_vectors.S. With RIDE, I have to build NuttX - one time from the Cygwin command line in order to obtain the pre-built - startup object needed by RIDE. - -NuttX buildroot Toolchain -========================= - - A GNU GCC-based toolchain is assumed. The files */setenv.sh should - be modified to point to the correct path to the Cortex-M3 GCC toolchain (if - different from the default in your PATH variable). - - If you have no Cortex-M3 toolchain, one can be downloaded from the NuttX - SourceForge download site (https://sourceforge.net/project/showfiles.php?group_id=189573). - This GNU toolchain builds and executes in the Linux or Cygwin environment. - - 1. You must have already configured Nuttx in /nuttx. - - cd tools - ./configure.sh stm3210e-eval/ - - 2. Download the latest buildroot package into - - 3. unpack the buildroot tarball. The resulting directory may - have versioning information on it like buildroot-x.y.z. If so, - rename /buildroot-x.y.z to /buildroot. - - 4. cd /buildroot - - 5. cp configs/cortexm3-defconfig-4.3.3 .config - - 6. make oldconfig - - 7. make - - 8. Edit setenv.h, if necessary, so that the PATH variable includes - the path to the newly built binaries. - - See the file configs/README.txt in the buildroot source tree. That has more - detailed PLUS some special instructions that you will need to follow if you are - building a Cortex-M3 toolchain for Cygwin under Windows. - -DFU and JTAG -============ - - Enbling Support for the DFU Bootloader - -------------------------------------- - The linker files in these projects can be configured to indicate that you - will be loading code using STMicro built-in USB Device Firmware Upgrade (DFU) - loader or via some JTAG emulator. You can specify the DFU bootloader by - adding the following line: - - CONFIG_STM32_DFU=y - - to your .config file. Most of the configurations in this directory are set - up to use the DFU loader. - - If CONFIG_STM32_DFU is defined, the code will not be positioned at the beginning - of FLASH (0x08000000) but will be offset to 0x08003000. This offset is needed - to make space for the DFU loader and 0x08003000 is where the DFU loader expects - to find new applications at boot time. If you need to change that origin for some - other bootloader, you will need to edit the file(s) ld.script.dfu for each - configuration. - - The DFU SE PC-based software is available from the STMicro website, - http://www.st.com. General usage instructions: - - 1. Convert the NuttX Intel Hex file (nuttx.ihx) into a special DFU - file (nuttx.dfu)... see below for details. - 2. Connect the STM3210E-EVAL board to your computer using a USB - cable. - 3. Start the DFU loader on the STM3210E-EVAL board. You do this by - resetting the board while holding the "Key" button. Windows should - recognize that the DFU loader has been installed. - 3. Run the DFU SE program to load nuttx.dfu into FLASH. - - What if the DFU loader is not in FLASH? The loader code is available - inside of the Demo dirctory of the USBLib ZIP file that can be downloaded - from the STMicro Website. You can build it using RIDE (or other toolchains); - you will need a JTAG emulator to burn it into FLASH the first time. - - In order to use STMicro's built-in DFU loader, you will have to get - the NuttX binary into a special format with a .dfu extension. The - DFU SE PC_based software installation includes a file "DFU File Manager" - conversion program that a file in Intel Hex format to the special DFU - format. When you successfully build NuttX, you will find a file called - nutt.ihx in the top-level directory. That is the file that you should - provide to the DFU File Manager. You will need to rename it to nuttx.hex - in order to find it with the DFU File Manager. You will end up with - a file called nuttx.dfu that you can use with the STMicro DFU SE program. - - Enabling JTAG - ------------- - If you are not using the DFU, then you will probably also need to enable - JTAG support. By default, all JTAG support is disabled but there NuttX - configuration options to enable JTAG in various different ways. - - These configurations effect the setting of the SWJ_CFG[2:0] bits in the AFIO - MAPR register. These bits are used to configure the SWJ and trace alternate function I/Os. The SWJ (SerialWire JTAG) supports JTAG or SWD access to the - Cortex debug port. The default state in this port is for all JTAG support - to be disable. - - CONFIG_STM32_JTAG_FULL_ENABLE - sets SWJ_CFG[2:0] to 000 which enables full - SWJ (JTAG-DP + SW-DP) - - CONFIG_STM32_JTAG_NOJNTRST_ENABLE - sets SWJ_CFG[2:0] to 001 which enable - full SWJ (JTAG-DP + SW-DP) but without JNTRST. - - CONFIG_STM32_JTAG_SW_ENABLE - sets SWJ_CFG[2:0] to 010 which would set JTAG-DP - disabled and SW-DP enabled - - The default setting (none of the above defined) is SWJ_CFG[2:0] set to 100 - which disable JTAG-DP and SW-DP. - -OpenOCD -======= - -I have also used OpenOCD with the STM3210E-EVAL. In this case, I used -the Olimex USB ARM OCD. See the script in configs/stm3210e-eval/tools/oocd.sh -for more information. Using the script: - -1) Start the OpenOCD GDB server - - cd - configs/stm3210e-eval/tools/oocd.sh $PWD - -2) Load Nuttx - - cd - arm-none-eabi-gdb nuttx - gdb> target remote localhost:3333 - gdb> mon reset - gdb> mon halt - gdb> load nuttx - -3) Running NuttX - - gdb> mon reset - gdb> c - -LEDs -==== - -The STM3210E-EVAL board has four LEDs labeled LD1, LD2, LD3 and LD4 on the -board.. These LEDs are not used by the board port unless CONFIG_ARCH_LEDS is -defined. In that case, the usage by the board port is defined in -include/board.h and src/up_leds.c. The LEDs are used to encode OS-related -events as follows: - - SYMBOL Meaning LED1* LED2 LED3 LED4 - ---------------- ----------------------- ----- ----- ----- ----- - LED_STARTED NuttX has been started ON OFF OFF OFF - LED_HEAPALLOCATE Heap has been allocated OFF ON OFF OFF - LED_IRQSENABLED Interrupts enabled ON ON OFF OFF - LED_STACKCREATED Idle stack created OFF OFF ON OFF - LED_INIRQ In an interrupt** ON N/C N/C OFF - LED_SIGNAL In a signal handler*** N/C ON N/C OFF - LED_ASSERTION An assertion failed ON ON N/C OFF - LED_PANIC The system has crashed N/C N/C N/C ON - LED_IDLE STM32 is is sleep mode (Optional, not used) - - * If LED1, LED2, LED3 are statically on, then NuttX probably failed to boot - and these LEDs will give you some indication of where the failure was - ** The normal state is LED3 ON and LED1 faintly glowing. This faint glow - is because of timer interupts that result in the LED being illuminated - on a small proportion of the time. -*** LED2 may also flicker normally if signals are processed. - -Temperature Sensor -================== - -Support for the on-board LM-75 temperature sensor is available. This supported -has been verified, but has not been included in any of the available the -configurations. To set up the temperature sensor, add the following to the -NuttX configuration file - - CONFIG_I2C=y - CONFIG_I2C_LM75=y - -Then you can implement logic like the following to use the temperature sensor: - - #include - #include - - ret = stm32_lm75initialize("/dev/temp"); /* Register the temperature sensor */ - fd = open("/dev/temp", O_RDONLY); /* Open the temperature sensor device */ - ret = ioctl(fd, SNIOC_FAHRENHEIT, 0); /* Select Fahrenheit */ - bytesread = read(fd, buffer, 8*sizeof(b16_t)); /* Read temperature samples */ - -More complex temperature sensor operations are also available. See the IOCTAL -commands enumerated in include/nuttx/sensors/lm75.h. Also read the descriptions -of the stm32_lm75initialize() and stm32_lm75attach() interfaces in the -arch/board/board.h file (sames as configs/stm3210e-eval/include/board.h). - -RTC -=== - - The STM32 RTC may configured using the following settings. - - CONFIG_RTC - Enables general support for a hardware RTC. Specific - architectures may require other specific settings. - CONFIG_RTC_HIRES - The typical RTC keeps time to resolution of 1 - second, usually supporting a 32-bit time_t value. In this case, - the RTC is used to "seed" the normal NuttX timer and the - NuttX timer provides for higher resoution time. If CONFIG_RTC_HIRES - is enabled in the NuttX configuration, then the RTC provides higher - resolution time and completely replaces the system timer for purpose of - date and time. - CONFIG_RTC_FREQUENCY - If CONFIG_RTC_HIRES is defined, then the - frequency of the high resolution RTC must be provided. If CONFIG_RTC_HIRES - is not defined, CONFIG_RTC_FREQUENCY is assumed to be one. - CONFIG_RTC_ALARM - Enable if the RTC hardware supports setting of an alarm. - A callback function will be executed when the alarm goes off - - In hi-res mode, the STM32 RTC operates only at 16384Hz. Overflow interrupts - are handled when the 32-bit RTC counter overflows every 3 days and 43 minutes. - A BKP register is incremented on each overflow interrupt creating, effectively, - a 48-bit RTC counter. - - In the lo-res mode, the RTC operates at 1Hz. Overflow interrupts are not handled - (because the next overflow is not expected until the year 2106. - - WARNING: Overflow interrupts are lost whenever the STM32 is powered down. The - overflow interrupt may be lost even if the STM32 is powered down only momentarily. - Therefore hi-res solution is only useful in systems where the power is always on. - -STM3210E-EVAL-specific Configuration Options -============================================ - - CONFIG_ARCH - Identifies the arch/ subdirectory. This should - be set to: - - CONFIG_ARCH=arm - - CONFIG_ARCH_family - For use in C code: - - CONFIG_ARCH_ARM=y - - CONFIG_ARCH_architecture - For use in C code: - - CONFIG_ARCH_CORTEXM3=y - - CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory - - CONFIG_ARCH_CHIP=stm32 - - CONFIG_ARCH_CHIP_name - For use in C code to identify the exact - chip: - - CONFIG_ARCH_CHIP_STM32F103ZET6 - - CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG - Enables special STM32 clock - configuration features. - - CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG=n - - CONFIG_ARCH_BOARD - Identifies the configs subdirectory and - hence, the board that supports the particular chip or SoC. - - CONFIG_ARCH_BOARD=stm3210e_eval (for the STM3210E-EVAL development board) - - CONFIG_ARCH_BOARD_name - For use in C code - - CONFIG_ARCH_BOARD_STM3210E_EVAL=y - - CONFIG_ARCH_LOOPSPERMSEC - Must be calibrated for correct operation - of delay loops - - CONFIG_ENDIAN_BIG - define if big endian (default is little - endian) - - CONFIG_DRAM_SIZE - Describes the installed DRAM (SRAM in this case): - - CONFIG_DRAM_SIZE=0x00010000 (64Kb) - - CONFIG_DRAM_START - The start address of installed DRAM - - CONFIG_DRAM_START=0x20000000 - - CONFIG_DRAM_END - Last address+1 of installed RAM - - CONFIG_DRAM_END=(CONFIG_DRAM_START+CONFIG_DRAM_SIZE) - - CONFIG_ARCH_IRQPRIO - The STM32F103Z supports interrupt prioritization - - CONFIG_ARCH_IRQPRIO=y - - CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to boards that - have LEDs - - CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt - stack. If defined, this symbol is the size of the interrupt - stack in bytes. If not defined, the user task stacks will be - used during interrupt handling. - - CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions - - CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to board architecture. - - CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that - cause a 100 second delay during boot-up. This 100 second delay - serves no purpose other than it allows you to calibratre - CONFIG_ARCH_LOOPSPERMSEC. You simply use a stop watch to measure - the 100 second delay then adjust CONFIG_ARCH_LOOPSPERMSEC until - the delay actually is 100 seconds. - - Individual subsystems can be enabled: - AHB - --- - CONFIG_STM32_DMA1 - CONFIG_STM32_DMA2 - CONFIG_STM32_CRC - CONFIG_STM32_FSMC - CONFIG_STM32_SDIO - - APB1 - ---- - CONFIG_STM32_TIM2 - CONFIG_STM32_TIM3 - CONFIG_STM32_TIM4 - CONFIG_STM32_TIM5 - CONFIG_STM32_TIM6 - CONFIG_STM32_TIM7 - CONFIG_STM32_WWDG - CONFIG_STM32_SPI2 - CONFIG_STM32_SPI4 - CONFIG_STM32_USART2 - CONFIG_STM32_USART3 - CONFIG_STM32_UART4 - CONFIG_STM32_UART5 - CONFIG_STM32_I2C1 - CONFIG_STM32_I2C2 - CONFIG_STM32_USB - CONFIG_STM32_CAN - CONFIG_STM32_BKP - CONFIG_STM32_PWR - CONFIG_STM32_DAC1 - CONFIG_STM32_DAC2 - CONFIG_STM32_USB - - APB2 - ---- - CONFIG_STM32_ADC1 - CONFIG_STM32_ADC2 - CONFIG_STM32_TIM1 - CONFIG_STM32_SPI1 - CONFIG_STM32_TIM8 - CONFIG_STM32_USART1 - CONFIG_STM32_ADC3 - - Timer and I2C devices may need to the following to force power to be applied - unconditionally at power up. (Otherwise, the device is powered when it is - initialized). - - CONFIG_STM32_FORCEPOWER - - Timer devices may be used for different purposes. One special purpose is - to generate modulated outputs for such things as motor control. If CONFIG_STM32_TIMn - is defined (as above) then the following may also be defined to indicate that - the timer is intended to be used for pulsed output modulation, ADC conversion, - or DAC conversion. Note that ADC/DAC require two definition: Not only do you have - to assign the timer (n) for used by the ADC or DAC, but then you also have to - configure which ADC or DAC (m) it is assigned to. - - CONFIG_STM32_TIMn_PWM Reserve timer n for use by PWM, n=1,..,8 - CONFIG_STM32_TIMn_ADC Reserve timer n for use by ADC, n=1,..,8 - CONFIG_STM32_TIMn_ADCm Reserve timer n to trigger ADCm, n=1,..,8, m=1,..,3 - CONFIG_STM32_TIMn_DAC Reserve timer n for use by DAC, n=1,..,8 - CONFIG_STM32_TIMn_DACm Reserve timer n to trigger DACm, n=1,..,8, m=1,..,2 - - For each timer that is enabled for PWM usage, we need the following additional - configuration settings: - - CONFIG_STM32_TIMx_CHANNEL - Specifies the timer output channel {1,..,4} - - NOTE: The STM32 timers are each capable of generating different signals on - each of the four channels with different duty cycles. That capability is - not supported by this driver: Only one output channel per timer. - - Alternate pin mappings (should not be used with the STM3210E-EVAL board): - - CONFIG_STM32_TIM1_FULL_REMAP - CONFIG_STM32_TIM1_PARTIAL_REMAP - CONFIG_STM32_TIM2_FULL_REMAP - CONFIG_STM32_TIM2_PARTIAL_REMAP_1 - CONFIG_STM32_TIM2_PARTIAL_REMAP_2 - CONFIG_STM32_TIM3_FULL_REMAP - CONFIG_STM32_TIM3_PARTIAL_REMAP - CONFIG_STM32_TIM4_REMAP - CONFIG_STM32_USART1_REMAP - CONFIG_STM32_USART2_REMAP - CONFIG_STM32_USART3_FULL_REMAP - CONFIG_STM32_USART3_PARTIAL_REMAP - CONFIG_STM32_SPI1_REMAP - CONFIG_STM32_SPI3_REMAP - CONFIG_STM32_I2C1_REMAP - CONFIG_STM32_CAN1_FULL_REMAP - CONFIG_STM32_CAN1_PARTIAL_REMAP - CONFIG_STM32_CAN2_REMAP - - JTAG Enable settings (by default JTAG-DP and SW-DP are disabled): - CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) - CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) - but without JNTRST. - CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled - - STM32F103Z specific device driver settings - - CONFIG_U[S]ARTn_SERIAL_CONSOLE - selects the USARTn (n=1,2,3) or UART - m (m=4,5) for the console and ttys0 (default is the USART1). - CONFIG_U[S]ARTn_RXBUFSIZE - Characters are buffered as received. - This specific the size of the receive buffer - CONFIG_U[S]ARTn_TXBUFSIZE - Characters are buffered before - being sent. This specific the size of the transmit buffer - CONFIG_U[S]ARTn_BAUD - The configure BAUD of the UART. Must be - CONFIG_U[S]ARTn_BITS - The number of bits. Must be either 7 or 8. - CONFIG_U[S]ARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity - CONFIG_U[S]ARTn_2STOP - Two stop bits - - CONFIG_STM32_SPI_INTERRUPTS - Select to enable interrupt driven SPI - support. Non-interrupt-driven, poll-waiting is recommended if the - interrupt rate would be to high in the interrupt driven case. - CONFIG_STM32_SPI_DMA - Use DMA to improve SPI transfer performance. - Cannot be used with CONFIG_STM32_SPI_INTERRUPT. - - CONFIG_SDIO_DMA - Support DMA data transfers. Requires CONFIG_STM32_SDIO - and CONFIG_STM32_DMA2. - CONFIG_SDIO_PRI - Select SDIO interrupt prority. Default: 128 - CONFIG_SDIO_DMAPRIO - Select SDIO DMA interrupt priority. - Default: Medium - CONFIG_SDIO_WIDTH_D1_ONLY - Select 1-bit transfer mode. Default: - 4-bit transfer mode. - - STM3210E-EVAL CAN Configuration - - CONFIG_CAN - Enables CAN support (one or both of CONFIG_STM32_CAN1 or - CONFIG_STM32_CAN2 must also be defined) - CONFIG_CAN_EXTID - Enables support for the 29-bit extended ID. Default - Standard 11-bit IDs. - CONFIG_CAN_FIFOSIZE - The size of the circular buffer of CAN messages. - Default: 8 - CONFIG_CAN_NPENDINGRTR - The size of the list of pending RTR requests. - Default: 4 - CONFIG_CAN_LOOPBACK - A CAN driver may or may not support a loopback - mode for testing. The STM32 CAN driver does support loopback mode. - CONFIG_CAN1_BAUD - CAN1 BAUD rate. Required if CONFIG_STM32_CAN1 is defined. - CONFIG_CAN2_BAUD - CAN1 BAUD rate. Required if CONFIG_STM32_CAN2 is defined. - CONFIG_CAN_TSEG1 - The number of CAN time quanta in segment 1. Default: 6 - CONFIG_CAN_TSEG2 - the number of CAN time quanta in segment 2. Default: 7 - CONFIG_CAN_REGDEBUG - If CONFIG_DEBUG is set, this will generate an - dump of all CAN registers. - - STM3210E-EVAL LCD Hardware Configuration - - CONFIG_LCD_LANDSCAPE - Define for 320x240 display "landscape" - support. Default is this 320x240 "landscape" orientation - (this setting is informative only... not used). - CONFIG_LCD_PORTRAIT - Define for 240x320 display "portrait" - orientation support. In this orientation, the STM3210E-EVAL's - LCD ribbon cable is at the bottom of the display. Default is - 320x240 "landscape" orientation. - CONFIG_LCD_RPORTRAIT - Define for 240x320 display "reverse - portrait" orientation support. In this orientation, the - STM3210E-EVAL's LCD ribbon cable is at the top of the display. - Default is 320x240 "landscape" orientation. - CONFIG_LCD_BACKLIGHT - Define to support a backlight. - CONFIG_LCD_PWM - If CONFIG_STM32_TIM1 is also defined, then an - adjustable backlight will be provided using timer 1 to generate - various pulse widthes. The granularity of the settings is - determined by CONFIG_LCD_MAXPOWER. If CONFIG_LCD_PWM (or - CONFIG_STM32_TIM1) is not defined, then a simple on/off backlight - is provided. - CONFIG_LCD_RDSHIFT - When reading 16-bit gram data, there appears - to be a shift in the returned data. This value fixes the offset. - Default 5. - - The LCD driver dynamically selects the LCD based on the reported LCD - ID value. However, code size can be reduced by suppressing support for - individual LCDs using: - - CONFIG_STM32_AM240320_DISABLE - CONFIG_STM32_SPFD5408B_DISABLE - CONFIG_STM32_R61580_DISABLE - -Configurations -============== - -Each STM3210E-EVAL configuration is maintained in a sudirectory and -can be selected as follow: - - cd tools - ./configure.sh stm3210e-eval/ - cd - - . ./setenv.sh - -Where is one of the following: - - buttons: - -------- - - Uses apps/examples/buttons to exercise STM3210E-EVAL buttons and - button interrupts. - - CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows - - composite - --------- - - This configuration exercises a composite USB interface consisting - of a CDC/ACM device and a USB mass storage device. This configuration - uses apps/examples/composite. - - nsh and nsh2: - ------------ - Configure the NuttShell (nsh) located at examples/nsh. - - Differences between the two NSH configurations: - - =========== ======================= ================================ - nsh nsh2 - =========== ======================= ================================ - Toolchain: NuttX buildroot for Codesourcery for Windows (1) - Linux or Cygwin (1,2) - ----------- ----------------------- -------------------------------- - Loader: DfuSe DfuSe - ----------- ----------------------- -------------------------------- - Serial Debug output: USART1 Debug output: USART1 - Console: NSH output: USART1 NSH output: USART1 (3) - ----------- ----------------------- -------------------------------- - microSD Yes Yes - Support - ----------- ----------------------- -------------------------------- - FAT FS CONFIG_FAT_LCNAME=y CONFIG_FAT_LCNAME=y - Config CONFIG_FAT_LFN=n CONFIG_FAT_LFN=y (4) - ----------- ----------------------- -------------------------------- - Support for No Yes - Built-in - Apps - ----------- ----------------------- -------------------------------- - Built-in None apps/examples/nx - Apps apps/examples/nxhello - apps/examples/usbstorage (5) - =========== ======================= ================================ - - (1) You will probably need to modify nsh/setenv.sh or nsh2/setenv.sh - to set up the correct PATH variable for whichever toolchain you - may use. - (2) Since DfuSe is assumed, this configuration may only work under - Cygwin without modification. - (3) When any other device other than /dev/console is used for a user - interface, (1) linefeeds (\n) will not be expanded to carriage return - / linefeeds \r\n). You will need to configure your terminal program - to account for this. And (2) input is not automatically echoed so - you will have to turn local echo on. - (4) Microsoft holds several patents related to the design of - long file names in the FAT file system. Please refer to the - details in the top-level COPYING file. Please do not use FAT - long file name unless you are familiar with these patent issues. - (5) When built as an NSH add-on command (CONFIG_EXAMPLES_USBMSC_BUILTIN=y), - Caution should be used to assure that the SD drive is not in use when - the USB storage device is configured. Specifically, the SD driver - should be unmounted like: - - nsh> mount -t vfat /dev/mmcsd0 /mnt/sdcard # Card is mounted in NSH - ... - nsh> umount /mnd/sdcard # Unmount before connecting USB!!! - nsh> msconn # Connect the USB storage device - ... - nsh> msdis # Disconnect USB storate device - nsh> mount -t vfat /dev/mmcsd0 /mnt/sdcard # Restore the mount - - Failure to do this could result in corruption of the SD card format. - - nx: - --- - An example using the NuttX graphics system (NX). This example - focuses on general window controls, movement, mouse and keyboard - input. - - CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows - CONFIG_LCD_RPORTRAIT=y : 240x320 reverse portrait - - nxlines: - ------ - Another example using the NuttX graphics system (NX). This - example focuses on placing lines on the background in various - orientations. - - CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows - CONFIG_LCD_RPORTRAIT=y : 240x320 reverse portrait - - nxtext: - ------ - Another example using the NuttX graphics system (NX). This - example focuses on placing text on the background while pop-up - windows occur. Text should continue to update normally with - or without the popup windows present. - - CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin - CONFIG_LCD_RPORTRAIT=y : 240x320 reverse portrait - - NOTE: When I tried building this example with the CodeSourcery - tools, I got a hardfault inside of its libgcc. I haven't - retested since then, but beware if you choose to change the - toolchain. - - ostest: - ------ - This configuration directory, performs a simple OS test using - examples/ostest. By default, this project assumes that you are - using the DFU bootloader. - - CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin - - RIDE - ---- - This configuration builds a trivial bring-up binary. It is - useful only because it words with the RIDE7 IDE and R-Link debugger. - - CONFIG_STM32_RAISONANCE=y : Raisonance RIDE7 under Windows - - usbserial: - --------- - This configuration directory exercises the USB serial class - driver at examples/usbserial. See examples/README.txt for - more information. - - CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin - - USB debug output can be enabled as by changing the following - settings in the configuration file: - - -CONFIG_DEBUG=n - -CONFIG_DEBUG_VERBOSE=n - -CONFIG_DEBUG_USB=n - +CONFIG_DEBUG=y - +CONFIG_DEBUG_VERBOSE=y - +CONFIG_DEBUG_USB=y - - -CONFIG_EXAMPLES_USBSERIAL_TRACEINIT=n - -CONFIG_EXAMPLES_USBSERIAL_TRACECLASS=n - -CONFIG_EXAMPLES_USBSERIAL_TRACETRANSFERS=n - -CONFIG_EXAMPLES_USBSERIAL_TRACECONTROLLER=n - -CONFIG_EXAMPLES_USBSERIAL_TRACEINTERRUPTS=n - +CONFIG_EXAMPLES_USBSERIAL_TRACEINIT=y - +CONFIG_EXAMPLES_USBSERIAL_TRACECLASS=y - +CONFIG_EXAMPLES_USBSERIAL_TRACETRANSFERS=y - +CONFIG_EXAMPLES_USBSERIAL_TRACECONTROLLER=y - +CONFIG_EXAMPLES_USBSERIAL_TRACEINTERRUPTS=y - - By default, the usbserial example uses the Prolific PL2303 - serial/USB converter emulation. The example can be modified - to use the CDC/ACM serial class by making the following changes - to the configuration file: - - -CONFIG_PL2303=y - +CONFIG_PL2303=n - - -CONFIG_CDCACM=n - +CONFIG_CDCACM=y - - The example can also be converted to use the alternative - USB serial example at apps/examples/usbterm by changing the - following: - - -CONFIGURED_APPS += examples/usbserial - +CONFIGURED_APPS += examples/usbterm - - In either the original appconfig file (before configuring) - or in the final apps/.config file (after configuring). - - usbstorage: - ---------- - This configuration directory exercises the USB mass storage - class driver at examples/usbstorage. See examples/README.txt for - more information. - - CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin - diff --git a/nuttx-configs/px4io-v1/common/Make.defs b/nuttx-configs/px4io-v1/common/Make.defs deleted file mode 100644 index 74b183067c..0000000000 --- a/nuttx-configs/px4io-v1/common/Make.defs +++ /dev/null @@ -1,171 +0,0 @@ -############################################################################ -# configs/px4fmu/common/Make.defs -# -# Copyright (C) 2011 Gregory Nutt. All rights reserved. -# Author: Gregory Nutt -# -# 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 NuttX 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. -# -############################################################################ - -# -# Generic Make.defs for the PX4FMU -# Do not specify/use this file directly - it is included by config-specific -# Make.defs in the per-config directories. -# - -include ${TOPDIR}/tools/Config.mk - -# -# We only support building with the ARM bare-metal toolchain from -# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS. -# -CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI - -include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs - -CC = $(CROSSDEV)gcc -CXX = $(CROSSDEV)g++ -CPP = $(CROSSDEV)gcc -E -LD = $(CROSSDEV)ld -AR = $(CROSSDEV)ar rcs -NM = $(CROSSDEV)nm -OBJCOPY = $(CROSSDEV)objcopy -OBJDUMP = $(CROSSDEV)objdump - -MAXOPTIMIZATION = -O3 -ARCHCPUFLAGS = -mcpu=cortex-m3 \ - -mthumb \ - -march=armv7-m - -# use our linker script -LDSCRIPT = ld.script - -ifeq ($(WINTOOL),y) - # Windows-native toolchains - DIRLINK = $(TOPDIR)/tools/copydir.sh - DIRUNLINK = $(TOPDIR)/tools/unlink.sh - MKDEP = $(TOPDIR)/tools/mknulldeps.sh - ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" - ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" - ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT)}" -else - ifeq ($(PX4_WINTOOL),y) - # Windows-native toolchains (MSYS) - DIRLINK = $(TOPDIR)/tools/copydir.sh - DIRUNLINK = $(TOPDIR)/tools/unlink.sh - MKDEP = $(TOPDIR)/tools/mknulldeps.sh - ARCHINCLUDES = -I. -isystem $(TOPDIR)/include - ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx - ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT) - else - # Linux/Cygwin-native toolchain - MKDEP = $(TOPDIR)/tools/mkdeps.sh - ARCHINCLUDES = -I. -isystem $(TOPDIR)/include - ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx - ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT) - endif -endif - -# tool versions -ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} -ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} - -# optimisation flags -ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ - -fno-strict-aliasing \ - -fno-strength-reduce \ - -fomit-frame-pointer \ - -funsafe-math-optimizations \ - -fno-builtin-printf \ - -ffunction-sections \ - -fdata-sections - -ifeq ("${CONFIG_DEBUG_SYMBOLS}","y") -ARCHOPTIMIZATION += -g -endif - -ARCHCFLAGS = -std=gnu99 -ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x -ARCHWARNINGS = -Wall \ - -Wextra \ - -Wdouble-promotion \ - -Wshadow \ - -Wfloat-equal \ - -Wframe-larger-than=1024 \ - -Wpointer-arith \ - -Wlogical-op \ - -Wmissing-declarations \ - -Wpacked \ - -Wno-unused-parameter -# -Wcast-qual - generates spurious noreturn attribute warnings, try again later -# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code -# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives - -ARCHCWARNINGS = $(ARCHWARNINGS) \ - -Wbad-function-cast \ - -Wstrict-prototypes \ - -Wold-style-declaration \ - -Wmissing-parameter-type \ - -Wmissing-prototypes \ - -Wnested-externs \ - -Wunsuffixed-float-constants -ARCHWARNINGSXX = $(ARCHWARNINGS) -ARCHDEFINES = -ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 - -# this seems to be the only way to add linker flags -EXTRA_LIBS += --warn-common \ - --gc-sections - -CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common -CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) -CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) -CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -AFLAGS = $(CFLAGS) -D__ASSEMBLY__ - -NXFLATLDFLAGS1 = -r -d -warn-common -NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections -LDNXFLATFLAGS = -e main -s 2048 - -OBJEXT = .o -LIBEXT = .a -EXEEXT = - -# produce partially-linked $1 from files in $2 -define PRELINK - @echo "PRELINK: $1" - $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 -endef - -HOSTCC = gcc -HOSTINCLUDES = -I. -HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe -HOSTLDFLAGS = - diff --git a/nuttx-configs/px4io-v1/common/setenv.sh b/nuttx-configs/px4io-v1/common/setenv.sh deleted file mode 100755 index ff9a4bf8ae..0000000000 --- a/nuttx-configs/px4io-v1/common/setenv.sh +++ /dev/null @@ -1,47 +0,0 @@ -#!/bin/bash -# configs/stm3210e-eval/dfu/setenv.sh -# -# Copyright (C) 2009 Gregory Nutt. All rights reserved. -# Author: Gregory Nutt -# -# 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 NuttX 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. -# - -if [ "$(basename $0)" = "setenv.sh" ] ; then - echo "You must source this script, not run it!" 1>&2 - exit 1 -fi - -if [ -z "${PATH_ORIG}" ]; then export PATH_ORIG="${PATH}"; fi - -WD=`pwd` -export RIDE_BIN="/cygdrive/c/Program Files/Raisonance/Ride/arm-gcc/bin" -export BUILDROOT_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin" -export PATH="${BUILDROOT_BIN}:${RIDE_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" - -echo "PATH : ${PATH}" diff --git a/nuttx-configs/px4io-v1/include/drv_i2c_device.h b/nuttx-configs/px4io-v1/include/drv_i2c_device.h deleted file mode 100644 index 02582bc092..0000000000 --- a/nuttx-configs/px4io-v1/include/drv_i2c_device.h +++ /dev/null @@ -1,42 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 A simple, polled I2C slave-mode driver. - * - * The master writes to and reads from a byte buffer, which the caller - * can update inbetween calls to the FSM. - */ - -extern void i2c_fsm_init(uint8_t *buffer, size_t buffer_size); -extern bool i2c_fsm(void); diff --git a/nuttx-configs/px4io-v1/nsh/Make.defs b/nuttx-configs/px4io-v1/nsh/Make.defs index c7f6effd9a..712631f471 100644 --- a/nuttx-configs/px4io-v1/nsh/Make.defs +++ b/nuttx-configs/px4io-v1/nsh/Make.defs @@ -1,3 +1,166 @@ -include ${TOPDIR}/.config +############################################################################ +# configs/px4io-v1/nsh/Make.defs +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# +############################################################################ + +include ${TOPDIR}/.config +include ${TOPDIR}/tools/Config.mk + +# +# We only support building with the ARM bare-metal toolchain from +# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS. +# +CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI + +include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs + +CC = $(CROSSDEV)gcc +CXX = $(CROSSDEV)g++ +CPP = $(CROSSDEV)gcc -E +LD = $(CROSSDEV)ld +AR = $(CROSSDEV)ar rcs +NM = $(CROSSDEV)nm +OBJCOPY = $(CROSSDEV)objcopy +OBJDUMP = $(CROSSDEV)objdump + +MAXOPTIMIZATION = -O3 +ARCHCPUFLAGS = -mcpu=cortex-m3 \ + -mthumb \ + -march=armv7-m + +# use our linker script +LDSCRIPT = ld.script + +ifeq ($(WINTOOL),y) + # Windows-native toolchains + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" + ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" + ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)}" +else + ifeq ($(PX4_WINTOOL),y) + # Windows-native toolchains (MSYS) + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) + else + # Linux/Cygwin-native toolchain + MKDEP = $(TOPDIR)/tools/mkdeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) + endif +endif + +# tool versions +ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} +ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} + +# optimisation flags +ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ + -fno-strict-aliasing \ + -fno-strength-reduce \ + -fomit-frame-pointer \ + -funsafe-math-optimizations \ + -fno-builtin-printf \ + -ffunction-sections \ + -fdata-sections + +ifeq ("${CONFIG_DEBUG_SYMBOLS}","y") +ARCHOPTIMIZATION += -g +endif + +ARCHCFLAGS = -std=gnu99 +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x +ARCHWARNINGS = -Wall \ + -Wextra \ + -Wdouble-promotion \ + -Wshadow \ + -Wfloat-equal \ + -Wframe-larger-than=1024 \ + -Wpointer-arith \ + -Wlogical-op \ + -Wmissing-declarations \ + -Wpacked \ + -Wno-unused-parameter +# -Wcast-qual - generates spurious noreturn attribute warnings, try again later +# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code +# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives + +ARCHCWARNINGS = $(ARCHWARNINGS) \ + -Wbad-function-cast \ + -Wstrict-prototypes \ + -Wold-style-declaration \ + -Wmissing-parameter-type \ + -Wmissing-prototypes \ + -Wnested-externs \ + -Wunsuffixed-float-constants +ARCHWARNINGSXX = $(ARCHWARNINGS) +ARCHDEFINES = +ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 + +# this seems to be the only way to add linker flags +EXTRA_LIBS += --warn-common \ + --gc-sections + +CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common +CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) +CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe +CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) +CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) +AFLAGS = $(CFLAGS) -D__ASSEMBLY__ + +NXFLATLDFLAGS1 = -r -d -warn-common +NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections +LDNXFLATFLAGS = -e main -s 2048 + +OBJEXT = .o +LIBEXT = .a +EXEEXT = + +# produce partially-linked $1 from files in $2 +define PRELINK + @echo "PRELINK: $1" + $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 +endef + +HOSTCC = gcc +HOSTINCLUDES = -I. +HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe +HOSTLDFLAGS = -include $(TOPDIR)/configs/px4io-v1/common/Make.defs diff --git a/nuttx-configs/px4io-v1/common/ld.script b/nuttx-configs/px4io-v1/scripts/ld.script similarity index 100% rename from nuttx-configs/px4io-v1/common/ld.script rename to nuttx-configs/px4io-v1/scripts/ld.script diff --git a/nuttx-configs/px4io-v1/src/README.txt b/nuttx-configs/px4io-v1/src/README.txt deleted file mode 100644 index d4eda82fd7..0000000000 --- a/nuttx-configs/px4io-v1/src/README.txt +++ /dev/null @@ -1 +0,0 @@ -This directory contains drivers unique to the STMicro STM3210E-EVAL development board. diff --git a/nuttx-configs/px4io-v1/src/drv_i2c_device.c b/nuttx-configs/px4io-v1/src/drv_i2c_device.c deleted file mode 100644 index 1f5931ae5e..0000000000 --- a/nuttx-configs/px4io-v1/src/drv_i2c_device.c +++ /dev/null @@ -1,618 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 A simple, polled I2C slave-mode driver. - * - * The master writes to and reads from a byte buffer, which the caller - * can update inbetween calls to the FSM. - */ - -#include - -#include "stm32_i2c.h" - -#include - -/* - * I2C register definitions. - */ -#define I2C_BASE STM32_I2C1_BASE - -#define REG(_reg) (*(volatile uint32_t *)(I2C_BASE + _reg)) - -#define rCR1 REG(STM32_I2C_CR1_OFFSET) -#define rCR2 REG(STM32_I2C_CR2_OFFSET) -#define rOAR1 REG(STM32_I2C_OAR1_OFFSET) -#define rOAR2 REG(STM32_I2C_OAR2_OFFSET) -#define rDR REG(STM32_I2C_DR_OFFSET) -#define rSR1 REG(STM32_I2C_SR1_OFFSET) -#define rSR2 REG(STM32_I2C_SR2_OFFSET) -#define rCCR REG(STM32_I2C_CCR_OFFSET) -#define rTRISE REG(STM32_I2C_TRISE_OFFSET) - -/* - * "event" values (cr2 << 16 | cr1) as described in the ST DriverLib - */ -#define I2C_EVENT_SLAVE_RECEIVER_ADDRESS_MATCHED ((uint32_t)0x00020002) /* BUSY and ADDR flags */ -#define I2C_EVENT_SLAVE_TRANSMITTER_ADDRESS_MATCHED ((uint32_t)0x00060082) /* TRA, BUSY, TXE and ADDR flags */ -#define I2C_EVENT_SLAVE_BYTE_RECEIVED ((uint32_t)0x00020040) /* BUSY and RXNE flags */ -#define I2C_EVENT_SLAVE_STOP_DETECTED ((uint32_t)0x00000010) /* STOPF flag */ -#define I2C_EVENT_SLAVE_BYTE_TRANSMITTED ((uint32_t)0x00060084) /* TRA, BUSY, TXE and BTF flags */ -#define I2C_EVENT_SLAVE_BYTE_TRANSMITTING ((uint32_t)0x00060080) /* TRA, BUSY and TXE flags */ -#define I2C_EVENT_SLAVE_ACK_FAILURE ((uint32_t)0x00000400) /* AF flag */ - -/** - * States implemented by the I2C FSM. - */ -enum fsm_state { - BAD_PHASE, // must be zero, default exit on a bad state transition - - WAIT_FOR_MASTER, - - /* write from master */ - WAIT_FOR_COMMAND, - RECEIVE_COMMAND, - RECEIVE_DATA, - HANDLE_COMMAND, - - /* read from master */ - WAIT_TO_SEND, - SEND_STATUS, - SEND_DATA, - - NUM_STATES -}; - -/** - * Events recognised by the I2C FSM. - */ -enum fsm_event { - /* automatic transition */ - AUTO, - - /* write from master */ - ADDRESSED_WRITE, - BYTE_RECEIVED, - STOP_RECEIVED, - - /* read from master */ - ADDRESSED_READ, - BYTE_SENDABLE, - ACK_FAILED, - - NUM_EVENTS -}; - -/** - * Context for the I2C FSM - */ -static struct fsm_context { - enum fsm_state state; - - /* XXX want to eliminate these */ - uint8_t command; - uint8_t status; - - uint8_t *data_ptr; - uint32_t data_count; - - size_t buffer_size; - uint8_t *buffer; -} context; - -/** - * Structure defining one FSM state and its outgoing transitions. - */ -struct fsm_transition { - void (*handler)(void); - enum fsm_state next_state[NUM_EVENTS]; -}; - -static bool i2c_command_received; - -static void fsm_event(enum fsm_event event); - -static void go_bad(void); -static void go_wait_master(void); - -static void go_wait_command(void); -static void go_receive_command(void); -static void go_receive_data(void); -static void go_handle_command(void); - -static void go_wait_send(void); -static void go_send_status(void); -static void go_send_buffer(void); - -/** - * The FSM state graph. - */ -static const struct fsm_transition fsm[NUM_STATES] = { - [BAD_PHASE] = { - .handler = go_bad, - .next_state = { - [AUTO] = WAIT_FOR_MASTER, - }, - }, - - [WAIT_FOR_MASTER] = { - .handler = go_wait_master, - .next_state = { - [ADDRESSED_WRITE] = WAIT_FOR_COMMAND, - [ADDRESSED_READ] = WAIT_TO_SEND, - }, - }, - - /* write from master*/ - [WAIT_FOR_COMMAND] = { - .handler = go_wait_command, - .next_state = { - [BYTE_RECEIVED] = RECEIVE_COMMAND, - [STOP_RECEIVED] = WAIT_FOR_MASTER, - }, - }, - [RECEIVE_COMMAND] = { - .handler = go_receive_command, - .next_state = { - [BYTE_RECEIVED] = RECEIVE_DATA, - [STOP_RECEIVED] = HANDLE_COMMAND, - }, - }, - [RECEIVE_DATA] = { - .handler = go_receive_data, - .next_state = { - [BYTE_RECEIVED] = RECEIVE_DATA, - [STOP_RECEIVED] = HANDLE_COMMAND, - }, - }, - [HANDLE_COMMAND] = { - .handler = go_handle_command, - .next_state = { - [AUTO] = WAIT_FOR_MASTER, - }, - }, - - /* buffer send */ - [WAIT_TO_SEND] = { - .handler = go_wait_send, - .next_state = { - [BYTE_SENDABLE] = SEND_STATUS, - }, - }, - [SEND_STATUS] = { - .handler = go_send_status, - .next_state = { - [BYTE_SENDABLE] = SEND_DATA, - [ACK_FAILED] = WAIT_FOR_MASTER, - }, - }, - [SEND_DATA] = { - .handler = go_send_buffer, - .next_state = { - [BYTE_SENDABLE] = SEND_DATA, - [ACK_FAILED] = WAIT_FOR_MASTER, - }, - }, -}; - - -/* debug support */ -#if 1 -struct fsm_logentry { - char kind; - uint32_t code; -}; - -#define LOG_ENTRIES 32 -static struct fsm_logentry fsm_log[LOG_ENTRIES]; -int fsm_logptr; -#define LOG_NEXT(_x) (((_x) + 1) % LOG_ENTRIES) -#define LOGx(_kind, _code) \ - do { \ - fsm_log[fsm_logptr].kind = _kind; \ - fsm_log[fsm_logptr].code = _code; \ - fsm_logptr = LOG_NEXT(fsm_logptr); \ - fsm_log[fsm_logptr].kind = 0; \ - } while(0) - -#define LOG(_kind, _code) \ - do {\ - if (fsm_logptr < LOG_ENTRIES) { \ - fsm_log[fsm_logptr].kind = _kind; \ - fsm_log[fsm_logptr].code = _code; \ - fsm_logptr++;\ - }\ - }while(0) - -#else -#define LOG(_kind, _code) -#endif - - -static void i2c_setclock(uint32_t frequency); - -/** - * Initialise I2C - * - */ -void -i2c_fsm_init(uint8_t *buffer, size_t buffer_size) -{ - /* save the buffer */ - context.buffer = buffer; - context.buffer_size = buffer_size; - - // initialise the FSM - context.status = 0; - context.command = 0; - context.state = BAD_PHASE; - fsm_event(AUTO); - -#if 0 - // enable the i2c block clock and reset it - modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_I2C1EN); - modifyreg32(STM32_RCC_APB1RSTR, 0, RCC_APB1RSTR_I2C1RST); - modifyreg32(STM32_RCC_APB1RSTR, RCC_APB1RSTR_I2C1RST, 0); - - // configure the i2c GPIOs - stm32_configgpio(GPIO_I2C1_SCL); - stm32_configgpio(GPIO_I2C1_SDA); - - // set the peripheral clock to match the APB clock - rCR2 = STM32_PCLK1_FREQUENCY / 1000000; - - // configure for 100kHz operation - i2c_setclock(100000); - - // enable i2c - rCR1 = I2C_CR1_PE; -#endif -} - -/** - * Run the I2C FSM for some period. - * - * @return True if the buffer has been updated by a command. - */ -bool -i2c_fsm(void) -{ - uint32_t event; - int idle_iterations = 0; - - for (;;) { - // handle bus error states by discarding the current operation - if (rSR1 & I2C_SR1_BERR) { - context.state = WAIT_FOR_MASTER; - rSR1 = ~I2C_SR1_BERR; - } - - // we do not anticipate over/underrun errors as clock-stretching is enabled - - // fetch the most recent event - event = ((rSR2 << 16) | rSR1) & 0x00ffffff; - - // generate FSM events based on I2C events - switch (event) { - case I2C_EVENT_SLAVE_RECEIVER_ADDRESS_MATCHED: - LOG('w', 0); - fsm_event(ADDRESSED_WRITE); - break; - - case I2C_EVENT_SLAVE_TRANSMITTER_ADDRESS_MATCHED: - LOG('r', 0); - fsm_event(ADDRESSED_READ); - break; - - case I2C_EVENT_SLAVE_BYTE_RECEIVED: - LOG('R', 0); - fsm_event(BYTE_RECEIVED); - break; - - case I2C_EVENT_SLAVE_STOP_DETECTED: - LOG('s', 0); - fsm_event(STOP_RECEIVED); - break; - - case I2C_EVENT_SLAVE_BYTE_TRANSMITTING: - //case I2C_EVENT_SLAVE_BYTE_TRANSMITTED: - LOG('T', 0); - fsm_event(BYTE_SENDABLE); - break; - - case I2C_EVENT_SLAVE_ACK_FAILURE: - LOG('a', 0); - fsm_event(ACK_FAILED); - break; - - default: - idle_iterations++; -// if ((event) && (event != 0x00020000)) -// LOG('e', event); - break; - } - - /* if we have just received something, drop out and let the caller handle it */ - if (i2c_command_received) { - i2c_command_received = false; - return true; - } - - /* if we have done nothing recently, drop out and let the caller have a slice */ - if (idle_iterations > 1000) - return false; - } -} - -/** - * Update the FSM with an event - * - * @param event New event. - */ -static void -fsm_event(enum fsm_event event) -{ - // move to the next state - context.state = fsm[context.state].next_state[event]; - - LOG('f', context.state); - - // call the state entry handler - if (fsm[context.state].handler) { - fsm[context.state].handler(); - } -} - -static void -go_bad() -{ - LOG('B', 0); - fsm_event(AUTO); -} - -/** - * Wait for the master to address us. - * - */ -static void -go_wait_master() -{ - // We currently don't have a command byte. - // - context.command = '\0'; - - // The data pointer starts pointing to the start of the data buffer. - // - context.data_ptr = context.buffer; - - // The data count is either: - // - the size of the data buffer - // - some value less than or equal the size of the data buffer during a write or a read - // - context.data_count = context.buffer_size; - - // (re)enable the peripheral, clear the stop event flag in - // case we just finished receiving data - rCR1 |= I2C_CR1_PE; - - // clear the ACK failed flag in case we just finished sending data - rSR1 = ~I2C_SR1_AF; -} - -/** - * Prepare to receive a command byte. - * - */ -static void -go_wait_command() -{ - // NOP -} - -/** - * Command byte has been received, save it and prepare to handle the data. - * - */ -static void -go_receive_command() -{ - - // fetch the command byte - context.command = (uint8_t)rDR; - LOG('c', context.command); - -} - -/** - * Receive a data byte. - * - */ -static void -go_receive_data() -{ - uint8_t d; - - // fetch the byte - d = (uint8_t)rDR; - LOG('d', d); - - // if we have somewhere to put it, do so - if (context.data_count) { - *context.data_ptr++ = d; - context.data_count--; - } -} - -/** - * Handle a command once the host is done sending it to us. - * - */ -static void -go_handle_command() -{ - // presume we are happy with the command - context.status = 0; - - // make a note that the buffer contains a fresh command - i2c_command_received = true; - - // kick along to the next state - fsm_event(AUTO); -} - -/** - * Wait to be able to send the status byte. - * - */ -static void -go_wait_send() -{ - // NOP -} - -/** - * Send the status byte. - * - */ -static void -go_send_status() -{ - rDR = context.status; - LOG('?', context.status); -} - -/** - * Send a data or pad byte. - * - */ -static void -go_send_buffer() -{ - if (context.data_count) { - LOG('D', *context.data_ptr); - rDR = *(context.data_ptr++); - context.data_count--; - } else { - LOG('-', 0); - rDR = 0xff; - } -} - -/* cribbed directly from the NuttX master driver */ -static void -i2c_setclock(uint32_t frequency) -{ - uint16_t cr1; - uint16_t ccr; - uint16_t trise; - uint16_t freqmhz; - uint16_t speed; - - /* Disable the selected I2C peripheral to configure TRISE */ - - cr1 = rCR1; - rCR1 &= ~I2C_CR1_PE; - - /* Update timing and control registers */ - - freqmhz = (uint16_t)(STM32_PCLK1_FREQUENCY / 1000000); - ccr = 0; - - /* Configure speed in standard mode */ - - if (frequency <= 100000) { - /* Standard mode speed calculation */ - - speed = (uint16_t)(STM32_PCLK1_FREQUENCY / (frequency << 1)); - - /* The CCR fault must be >= 4 */ - - if (speed < 4) { - /* Set the minimum allowed value */ - - speed = 4; - } - ccr |= speed; - - /* Set Maximum Rise Time for standard mode */ - - trise = freqmhz + 1; - - /* Configure speed in fast mode */ - } else { /* (frequency <= 400000) */ - /* Fast mode speed calculation with Tlow/Thigh = 16/9 */ - -#ifdef CONFIG_I2C_DUTY16_9 - speed = (uint16_t)(STM32_PCLK1_FREQUENCY / (frequency * 25)); - - /* Set DUTY and fast speed bits */ - - ccr |= (I2C_CCR_DUTY|I2C_CCR_FS); -#else - /* Fast mode speed calculation with Tlow/Thigh = 2 */ - - speed = (uint16_t)(STM32_PCLK1_FREQUENCY / (frequency * 3)); - - /* Set fast speed bit */ - - ccr |= I2C_CCR_FS; -#endif - - /* Verify that the CCR speed value is nonzero */ - - if (speed < 1) { - /* Set the minimum allowed value */ - - speed = 1; - } - ccr |= speed; - - /* Set Maximum Rise Time for fast mode */ - - trise = (uint16_t)(((freqmhz * 300) / 1000) + 1); - } - - /* Write the new values of the CCR and TRISE registers */ - - rCCR = ccr; - rTRISE = trise; - - /* Bit 14 of OAR1 must be configured and kept at 1 */ - - rOAR1 = I2C_OAR1_ONE); - - /* Re-enable the peripheral (or not) */ - - rCR1 = cr1; -} diff --git a/nuttx-configs/px4io-v2/README.txt b/nuttx-configs/px4io-v2/README.txt deleted file mode 100755 index 9b1615f42d..0000000000 --- a/nuttx-configs/px4io-v2/README.txt +++ /dev/null @@ -1,806 +0,0 @@ -README -====== - -This README discusses issues unique to NuttX configurations for the -STMicro STM3210E-EVAL development board. - -Contents -======== - - - Development Environment - - GNU Toolchain Options - - IDEs - - NuttX buildroot Toolchain - - DFU and JTAG - - OpenOCD - - LEDs - - Temperature Sensor - - RTC - - STM3210E-EVAL-specific Configuration Options - - Configurations - -Development Environment -======================= - - Either Linux or Cygwin on Windows can be used for the development environment. - The source has been built only using the GNU toolchain (see below). Other - toolchains will likely cause problems. Testing was performed using the Cygwin - environment because the Raisonance R-Link emulatator and some RIDE7 development tools - were used and those tools works only under Windows. - -GNU Toolchain Options -===================== - - The NuttX make system has been modified to support the following different - toolchain options. - - 1. The CodeSourcery GNU toolchain, - 2. The devkitARM GNU toolchain, - 3. Raisonance GNU toolchain, or - 4. The NuttX buildroot Toolchain (see below). - - All testing has been conducted using the NuttX buildroot toolchain. However, - the make system is setup to default to use the devkitARM toolchain. To use - the CodeSourcery, devkitARM or Raisonance GNU toolchain, you simply need to - add one of the following configuration options to your .config (or defconfig) - file: - - CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows - CONFIG_STM32_CODESOURCERYL=y : CodeSourcery under Linux - CONFIG_STM32_DEVKITARM=y : devkitARM under Windows - CONFIG_STM32_RAISONANCE=y : Raisonance RIDE7 under Windows - CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin (default) - - If you are not using CONFIG_STM32_BUILDROOT, then you may also have to modify - the PATH in the setenv.h file if your make cannot find the tools. - - NOTE: the CodeSourcery (for Windows), devkitARM, and Raisonance toolchains are - Windows native toolchains. The CodeSourcey (for Linux) and NuttX buildroot - toolchains are Cygwin and/or Linux native toolchains. There are several limitations - to using a Windows based toolchain in a Cygwin environment. The three biggest are: - - 1. The Windows toolchain cannot follow Cygwin paths. Path conversions are - performed automatically in the Cygwin makefiles using the 'cygpath' utility - but you might easily find some new path problems. If so, check out 'cygpath -w' - - 2. Windows toolchains cannot follow Cygwin symbolic links. Many symbolic links - are used in Nuttx (e.g., include/arch). The make system works around these - problems for the Windows tools by copying directories instead of linking them. - But this can also cause some confusion for you: For example, you may edit - a file in a "linked" directory and find that your changes had no effect. - That is because you are building the copy of the file in the "fake" symbolic - directory. If you use a Windows toolchain, you should get in the habit of - making like this: - - make clean_context all - - An alias in your .bashrc file might make that less painful. - - 3. Dependencies are not made when using Windows versions of the GCC. This is - because the dependencies are generated using Windows pathes which do not - work with the Cygwin make. - - Support has been added for making dependencies with the windows-native toolchains. - That support can be enabled by modifying your Make.defs file as follows: - - - MKDEP = $(TOPDIR)/tools/mknulldeps.sh - + MKDEP = $(TOPDIR)/tools/mkdeps.sh --winpaths "$(TOPDIR)" - - If you have problems with the dependency build (for example, if you are not - building on C:), then you may need to modify tools/mkdeps.sh - - NOTE 1: The CodeSourcery toolchain (2009q1) does not work with default optimization - level of -Os (See Make.defs). It will work with -O0, -O1, or -O2, but not with - -Os. - - NOTE 2: The devkitARM toolchain includes a version of MSYS make. Make sure that - the paths to Cygwin's /bin and /usr/bin directories appear BEFORE the devkitARM - path or will get the wrong version of make. - -IDEs -==== - - NuttX is built using command-line make. It can be used with an IDE, but some - effort will be required to create the project (There is a simple RIDE project - in the RIDE subdirectory). - - Makefile Build - -------------- - Under Eclipse, it is pretty easy to set up an "empty makefile project" and - simply use the NuttX makefile to build the system. That is almost for free - under Linux. Under Windows, you will need to set up the "Cygwin GCC" empty - makefile project in order to work with Windows (Google for "Eclipse Cygwin" - - there is a lot of help on the internet). - - Native Build - ------------ - Here are a few tips before you start that effort: - - 1) Select the toolchain that you will be using in your .config file - 2) Start the NuttX build at least one time from the Cygwin command line - before trying to create your project. This is necessary to create - certain auto-generated files and directories that will be needed. - 3) Set up include pathes: You will need include/, arch/arm/src/stm32, - arch/arm/src/common, arch/arm/src/armv7-m, and sched/. - 4) All assembly files need to have the definition option -D __ASSEMBLY__ - on the command line. - - Startup files will probably cause you some headaches. The NuttX startup file - is arch/arm/src/stm32/stm32_vectors.S. With RIDE, I have to build NuttX - one time from the Cygwin command line in order to obtain the pre-built - startup object needed by RIDE. - -NuttX buildroot Toolchain -========================= - - A GNU GCC-based toolchain is assumed. The files */setenv.sh should - be modified to point to the correct path to the Cortex-M3 GCC toolchain (if - different from the default in your PATH variable). - - If you have no Cortex-M3 toolchain, one can be downloaded from the NuttX - SourceForge download site (https://sourceforge.net/project/showfiles.php?group_id=189573). - This GNU toolchain builds and executes in the Linux or Cygwin environment. - - 1. You must have already configured Nuttx in /nuttx. - - cd tools - ./configure.sh stm3210e-eval/ - - 2. Download the latest buildroot package into - - 3. unpack the buildroot tarball. The resulting directory may - have versioning information on it like buildroot-x.y.z. If so, - rename /buildroot-x.y.z to /buildroot. - - 4. cd /buildroot - - 5. cp configs/cortexm3-defconfig-4.3.3 .config - - 6. make oldconfig - - 7. make - - 8. Edit setenv.h, if necessary, so that the PATH variable includes - the path to the newly built binaries. - - See the file configs/README.txt in the buildroot source tree. That has more - detailed PLUS some special instructions that you will need to follow if you are - building a Cortex-M3 toolchain for Cygwin under Windows. - -DFU and JTAG -============ - - Enbling Support for the DFU Bootloader - -------------------------------------- - The linker files in these projects can be configured to indicate that you - will be loading code using STMicro built-in USB Device Firmware Upgrade (DFU) - loader or via some JTAG emulator. You can specify the DFU bootloader by - adding the following line: - - CONFIG_STM32_DFU=y - - to your .config file. Most of the configurations in this directory are set - up to use the DFU loader. - - If CONFIG_STM32_DFU is defined, the code will not be positioned at the beginning - of FLASH (0x08000000) but will be offset to 0x08003000. This offset is needed - to make space for the DFU loader and 0x08003000 is where the DFU loader expects - to find new applications at boot time. If you need to change that origin for some - other bootloader, you will need to edit the file(s) ld.script.dfu for each - configuration. - - The DFU SE PC-based software is available from the STMicro website, - http://www.st.com. General usage instructions: - - 1. Convert the NuttX Intel Hex file (nuttx.ihx) into a special DFU - file (nuttx.dfu)... see below for details. - 2. Connect the STM3210E-EVAL board to your computer using a USB - cable. - 3. Start the DFU loader on the STM3210E-EVAL board. You do this by - resetting the board while holding the "Key" button. Windows should - recognize that the DFU loader has been installed. - 3. Run the DFU SE program to load nuttx.dfu into FLASH. - - What if the DFU loader is not in FLASH? The loader code is available - inside of the Demo dirctory of the USBLib ZIP file that can be downloaded - from the STMicro Website. You can build it using RIDE (or other toolchains); - you will need a JTAG emulator to burn it into FLASH the first time. - - In order to use STMicro's built-in DFU loader, you will have to get - the NuttX binary into a special format with a .dfu extension. The - DFU SE PC_based software installation includes a file "DFU File Manager" - conversion program that a file in Intel Hex format to the special DFU - format. When you successfully build NuttX, you will find a file called - nutt.ihx in the top-level directory. That is the file that you should - provide to the DFU File Manager. You will need to rename it to nuttx.hex - in order to find it with the DFU File Manager. You will end up with - a file called nuttx.dfu that you can use with the STMicro DFU SE program. - - Enabling JTAG - ------------- - If you are not using the DFU, then you will probably also need to enable - JTAG support. By default, all JTAG support is disabled but there NuttX - configuration options to enable JTAG in various different ways. - - These configurations effect the setting of the SWJ_CFG[2:0] bits in the AFIO - MAPR register. These bits are used to configure the SWJ and trace alternate function I/Os. The SWJ (SerialWire JTAG) supports JTAG or SWD access to the - Cortex debug port. The default state in this port is for all JTAG support - to be disable. - - CONFIG_STM32_JTAG_FULL_ENABLE - sets SWJ_CFG[2:0] to 000 which enables full - SWJ (JTAG-DP + SW-DP) - - CONFIG_STM32_JTAG_NOJNTRST_ENABLE - sets SWJ_CFG[2:0] to 001 which enable - full SWJ (JTAG-DP + SW-DP) but without JNTRST. - - CONFIG_STM32_JTAG_SW_ENABLE - sets SWJ_CFG[2:0] to 010 which would set JTAG-DP - disabled and SW-DP enabled - - The default setting (none of the above defined) is SWJ_CFG[2:0] set to 100 - which disable JTAG-DP and SW-DP. - -OpenOCD -======= - -I have also used OpenOCD with the STM3210E-EVAL. In this case, I used -the Olimex USB ARM OCD. See the script in configs/stm3210e-eval/tools/oocd.sh -for more information. Using the script: - -1) Start the OpenOCD GDB server - - cd - configs/stm3210e-eval/tools/oocd.sh $PWD - -2) Load Nuttx - - cd - arm-none-eabi-gdb nuttx - gdb> target remote localhost:3333 - gdb> mon reset - gdb> mon halt - gdb> load nuttx - -3) Running NuttX - - gdb> mon reset - gdb> c - -LEDs -==== - -The STM3210E-EVAL board has four LEDs labeled LD1, LD2, LD3 and LD4 on the -board.. These LEDs are not used by the board port unless CONFIG_ARCH_LEDS is -defined. In that case, the usage by the board port is defined in -include/board.h and src/up_leds.c. The LEDs are used to encode OS-related -events as follows: - - SYMBOL Meaning LED1* LED2 LED3 LED4 - ---------------- ----------------------- ----- ----- ----- ----- - LED_STARTED NuttX has been started ON OFF OFF OFF - LED_HEAPALLOCATE Heap has been allocated OFF ON OFF OFF - LED_IRQSENABLED Interrupts enabled ON ON OFF OFF - LED_STACKCREATED Idle stack created OFF OFF ON OFF - LED_INIRQ In an interrupt** ON N/C N/C OFF - LED_SIGNAL In a signal handler*** N/C ON N/C OFF - LED_ASSERTION An assertion failed ON ON N/C OFF - LED_PANIC The system has crashed N/C N/C N/C ON - LED_IDLE STM32 is is sleep mode (Optional, not used) - - * If LED1, LED2, LED3 are statically on, then NuttX probably failed to boot - and these LEDs will give you some indication of where the failure was - ** The normal state is LED3 ON and LED1 faintly glowing. This faint glow - is because of timer interupts that result in the LED being illuminated - on a small proportion of the time. -*** LED2 may also flicker normally if signals are processed. - -Temperature Sensor -================== - -Support for the on-board LM-75 temperature sensor is available. This supported -has been verified, but has not been included in any of the available the -configurations. To set up the temperature sensor, add the following to the -NuttX configuration file - - CONFIG_I2C=y - CONFIG_I2C_LM75=y - -Then you can implement logic like the following to use the temperature sensor: - - #include - #include - - ret = stm32_lm75initialize("/dev/temp"); /* Register the temperature sensor */ - fd = open("/dev/temp", O_RDONLY); /* Open the temperature sensor device */ - ret = ioctl(fd, SNIOC_FAHRENHEIT, 0); /* Select Fahrenheit */ - bytesread = read(fd, buffer, 8*sizeof(b16_t)); /* Read temperature samples */ - -More complex temperature sensor operations are also available. See the IOCTAL -commands enumerated in include/nuttx/sensors/lm75.h. Also read the descriptions -of the stm32_lm75initialize() and stm32_lm75attach() interfaces in the -arch/board/board.h file (sames as configs/stm3210e-eval/include/board.h). - -RTC -=== - - The STM32 RTC may configured using the following settings. - - CONFIG_RTC - Enables general support for a hardware RTC. Specific - architectures may require other specific settings. - CONFIG_RTC_HIRES - The typical RTC keeps time to resolution of 1 - second, usually supporting a 32-bit time_t value. In this case, - the RTC is used to "seed" the normal NuttX timer and the - NuttX timer provides for higher resoution time. If CONFIG_RTC_HIRES - is enabled in the NuttX configuration, then the RTC provides higher - resolution time and completely replaces the system timer for purpose of - date and time. - CONFIG_RTC_FREQUENCY - If CONFIG_RTC_HIRES is defined, then the - frequency of the high resolution RTC must be provided. If CONFIG_RTC_HIRES - is not defined, CONFIG_RTC_FREQUENCY is assumed to be one. - CONFIG_RTC_ALARM - Enable if the RTC hardware supports setting of an alarm. - A callback function will be executed when the alarm goes off - - In hi-res mode, the STM32 RTC operates only at 16384Hz. Overflow interrupts - are handled when the 32-bit RTC counter overflows every 3 days and 43 minutes. - A BKP register is incremented on each overflow interrupt creating, effectively, - a 48-bit RTC counter. - - In the lo-res mode, the RTC operates at 1Hz. Overflow interrupts are not handled - (because the next overflow is not expected until the year 2106. - - WARNING: Overflow interrupts are lost whenever the STM32 is powered down. The - overflow interrupt may be lost even if the STM32 is powered down only momentarily. - Therefore hi-res solution is only useful in systems where the power is always on. - -STM3210E-EVAL-specific Configuration Options -============================================ - - CONFIG_ARCH - Identifies the arch/ subdirectory. This should - be set to: - - CONFIG_ARCH=arm - - CONFIG_ARCH_family - For use in C code: - - CONFIG_ARCH_ARM=y - - CONFIG_ARCH_architecture - For use in C code: - - CONFIG_ARCH_CORTEXM3=y - - CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory - - CONFIG_ARCH_CHIP=stm32 - - CONFIG_ARCH_CHIP_name - For use in C code to identify the exact - chip: - - CONFIG_ARCH_CHIP_STM32F103ZET6 - - CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG - Enables special STM32 clock - configuration features. - - CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG=n - - CONFIG_ARCH_BOARD - Identifies the configs subdirectory and - hence, the board that supports the particular chip or SoC. - - CONFIG_ARCH_BOARD=stm3210e_eval (for the STM3210E-EVAL development board) - - CONFIG_ARCH_BOARD_name - For use in C code - - CONFIG_ARCH_BOARD_STM3210E_EVAL=y - - CONFIG_ARCH_LOOPSPERMSEC - Must be calibrated for correct operation - of delay loops - - CONFIG_ENDIAN_BIG - define if big endian (default is little - endian) - - CONFIG_DRAM_SIZE - Describes the installed DRAM (SRAM in this case): - - CONFIG_DRAM_SIZE=0x00010000 (64Kb) - - CONFIG_DRAM_START - The start address of installed DRAM - - CONFIG_DRAM_START=0x20000000 - - CONFIG_DRAM_END - Last address+1 of installed RAM - - CONFIG_DRAM_END=(CONFIG_DRAM_START+CONFIG_DRAM_SIZE) - - CONFIG_ARCH_IRQPRIO - The STM32F103Z supports interrupt prioritization - - CONFIG_ARCH_IRQPRIO=y - - CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to boards that - have LEDs - - CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt - stack. If defined, this symbol is the size of the interrupt - stack in bytes. If not defined, the user task stacks will be - used during interrupt handling. - - CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions - - CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to board architecture. - - CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that - cause a 100 second delay during boot-up. This 100 second delay - serves no purpose other than it allows you to calibratre - CONFIG_ARCH_LOOPSPERMSEC. You simply use a stop watch to measure - the 100 second delay then adjust CONFIG_ARCH_LOOPSPERMSEC until - the delay actually is 100 seconds. - - Individual subsystems can be enabled: - AHB - --- - CONFIG_STM32_DMA1 - CONFIG_STM32_DMA2 - CONFIG_STM32_CRC - CONFIG_STM32_FSMC - CONFIG_STM32_SDIO - - APB1 - ---- - CONFIG_STM32_TIM2 - CONFIG_STM32_TIM3 - CONFIG_STM32_TIM4 - CONFIG_STM32_TIM5 - CONFIG_STM32_TIM6 - CONFIG_STM32_TIM7 - CONFIG_STM32_WWDG - CONFIG_STM32_SPI2 - CONFIG_STM32_SPI4 - CONFIG_STM32_USART2 - CONFIG_STM32_USART3 - CONFIG_STM32_UART4 - CONFIG_STM32_UART5 - CONFIG_STM32_I2C1 - CONFIG_STM32_I2C2 - CONFIG_STM32_USB - CONFIG_STM32_CAN - CONFIG_STM32_BKP - CONFIG_STM32_PWR - CONFIG_STM32_DAC1 - CONFIG_STM32_DAC2 - CONFIG_STM32_USB - - APB2 - ---- - CONFIG_STM32_ADC1 - CONFIG_STM32_ADC2 - CONFIG_STM32_TIM1 - CONFIG_STM32_SPI1 - CONFIG_STM32_TIM8 - CONFIG_STM32_USART1 - CONFIG_STM32_ADC3 - - Timer and I2C devices may need to the following to force power to be applied - unconditionally at power up. (Otherwise, the device is powered when it is - initialized). - - CONFIG_STM32_FORCEPOWER - - Timer devices may be used for different purposes. One special purpose is - to generate modulated outputs for such things as motor control. If CONFIG_STM32_TIMn - is defined (as above) then the following may also be defined to indicate that - the timer is intended to be used for pulsed output modulation, ADC conversion, - or DAC conversion. Note that ADC/DAC require two definition: Not only do you have - to assign the timer (n) for used by the ADC or DAC, but then you also have to - configure which ADC or DAC (m) it is assigned to. - - CONFIG_STM32_TIMn_PWM Reserve timer n for use by PWM, n=1,..,8 - CONFIG_STM32_TIMn_ADC Reserve timer n for use by ADC, n=1,..,8 - CONFIG_STM32_TIMn_ADCm Reserve timer n to trigger ADCm, n=1,..,8, m=1,..,3 - CONFIG_STM32_TIMn_DAC Reserve timer n for use by DAC, n=1,..,8 - CONFIG_STM32_TIMn_DACm Reserve timer n to trigger DACm, n=1,..,8, m=1,..,2 - - For each timer that is enabled for PWM usage, we need the following additional - configuration settings: - - CONFIG_STM32_TIMx_CHANNEL - Specifies the timer output channel {1,..,4} - - NOTE: The STM32 timers are each capable of generating different signals on - each of the four channels with different duty cycles. That capability is - not supported by this driver: Only one output channel per timer. - - Alternate pin mappings (should not be used with the STM3210E-EVAL board): - - CONFIG_STM32_TIM1_FULL_REMAP - CONFIG_STM32_TIM1_PARTIAL_REMAP - CONFIG_STM32_TIM2_FULL_REMAP - CONFIG_STM32_TIM2_PARTIAL_REMAP_1 - CONFIG_STM32_TIM2_PARTIAL_REMAP_2 - CONFIG_STM32_TIM3_FULL_REMAP - CONFIG_STM32_TIM3_PARTIAL_REMAP - CONFIG_STM32_TIM4_REMAP - CONFIG_STM32_USART1_REMAP - CONFIG_STM32_USART2_REMAP - CONFIG_STM32_USART3_FULL_REMAP - CONFIG_STM32_USART3_PARTIAL_REMAP - CONFIG_STM32_SPI1_REMAP - CONFIG_STM32_SPI3_REMAP - CONFIG_STM32_I2C1_REMAP - CONFIG_STM32_CAN1_FULL_REMAP - CONFIG_STM32_CAN1_PARTIAL_REMAP - CONFIG_STM32_CAN2_REMAP - - JTAG Enable settings (by default JTAG-DP and SW-DP are disabled): - CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) - CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) - but without JNTRST. - CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled - - STM32F103Z specific device driver settings - - CONFIG_U[S]ARTn_SERIAL_CONSOLE - selects the USARTn (n=1,2,3) or UART - m (m=4,5) for the console and ttys0 (default is the USART1). - CONFIG_U[S]ARTn_RXBUFSIZE - Characters are buffered as received. - This specific the size of the receive buffer - CONFIG_U[S]ARTn_TXBUFSIZE - Characters are buffered before - being sent. This specific the size of the transmit buffer - CONFIG_U[S]ARTn_BAUD - The configure BAUD of the UART. Must be - CONFIG_U[S]ARTn_BITS - The number of bits. Must be either 7 or 8. - CONFIG_U[S]ARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity - CONFIG_U[S]ARTn_2STOP - Two stop bits - - CONFIG_STM32_SPI_INTERRUPTS - Select to enable interrupt driven SPI - support. Non-interrupt-driven, poll-waiting is recommended if the - interrupt rate would be to high in the interrupt driven case. - CONFIG_STM32_SPI_DMA - Use DMA to improve SPI transfer performance. - Cannot be used with CONFIG_STM32_SPI_INTERRUPT. - - CONFIG_SDIO_DMA - Support DMA data transfers. Requires CONFIG_STM32_SDIO - and CONFIG_STM32_DMA2. - CONFIG_SDIO_PRI - Select SDIO interrupt prority. Default: 128 - CONFIG_SDIO_DMAPRIO - Select SDIO DMA interrupt priority. - Default: Medium - CONFIG_SDIO_WIDTH_D1_ONLY - Select 1-bit transfer mode. Default: - 4-bit transfer mode. - - STM3210E-EVAL CAN Configuration - - CONFIG_CAN - Enables CAN support (one or both of CONFIG_STM32_CAN1 or - CONFIG_STM32_CAN2 must also be defined) - CONFIG_CAN_EXTID - Enables support for the 29-bit extended ID. Default - Standard 11-bit IDs. - CONFIG_CAN_FIFOSIZE - The size of the circular buffer of CAN messages. - Default: 8 - CONFIG_CAN_NPENDINGRTR - The size of the list of pending RTR requests. - Default: 4 - CONFIG_CAN_LOOPBACK - A CAN driver may or may not support a loopback - mode for testing. The STM32 CAN driver does support loopback mode. - CONFIG_CAN1_BAUD - CAN1 BAUD rate. Required if CONFIG_STM32_CAN1 is defined. - CONFIG_CAN2_BAUD - CAN1 BAUD rate. Required if CONFIG_STM32_CAN2 is defined. - CONFIG_CAN_TSEG1 - The number of CAN time quanta in segment 1. Default: 6 - CONFIG_CAN_TSEG2 - the number of CAN time quanta in segment 2. Default: 7 - CONFIG_CAN_REGDEBUG - If CONFIG_DEBUG is set, this will generate an - dump of all CAN registers. - - STM3210E-EVAL LCD Hardware Configuration - - CONFIG_LCD_LANDSCAPE - Define for 320x240 display "landscape" - support. Default is this 320x240 "landscape" orientation - (this setting is informative only... not used). - CONFIG_LCD_PORTRAIT - Define for 240x320 display "portrait" - orientation support. In this orientation, the STM3210E-EVAL's - LCD ribbon cable is at the bottom of the display. Default is - 320x240 "landscape" orientation. - CONFIG_LCD_RPORTRAIT - Define for 240x320 display "reverse - portrait" orientation support. In this orientation, the - STM3210E-EVAL's LCD ribbon cable is at the top of the display. - Default is 320x240 "landscape" orientation. - CONFIG_LCD_BACKLIGHT - Define to support a backlight. - CONFIG_LCD_PWM - If CONFIG_STM32_TIM1 is also defined, then an - adjustable backlight will be provided using timer 1 to generate - various pulse widthes. The granularity of the settings is - determined by CONFIG_LCD_MAXPOWER. If CONFIG_LCD_PWM (or - CONFIG_STM32_TIM1) is not defined, then a simple on/off backlight - is provided. - CONFIG_LCD_RDSHIFT - When reading 16-bit gram data, there appears - to be a shift in the returned data. This value fixes the offset. - Default 5. - - The LCD driver dynamically selects the LCD based on the reported LCD - ID value. However, code size can be reduced by suppressing support for - individual LCDs using: - - CONFIG_STM32_AM240320_DISABLE - CONFIG_STM32_SPFD5408B_DISABLE - CONFIG_STM32_R61580_DISABLE - -Configurations -============== - -Each STM3210E-EVAL configuration is maintained in a sudirectory and -can be selected as follow: - - cd tools - ./configure.sh stm3210e-eval/ - cd - - . ./setenv.sh - -Where is one of the following: - - buttons: - -------- - - Uses apps/examples/buttons to exercise STM3210E-EVAL buttons and - button interrupts. - - CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows - - composite - --------- - - This configuration exercises a composite USB interface consisting - of a CDC/ACM device and a USB mass storage device. This configuration - uses apps/examples/composite. - - nsh and nsh2: - ------------ - Configure the NuttShell (nsh) located at examples/nsh. - - Differences between the two NSH configurations: - - =========== ======================= ================================ - nsh nsh2 - =========== ======================= ================================ - Toolchain: NuttX buildroot for Codesourcery for Windows (1) - Linux or Cygwin (1,2) - ----------- ----------------------- -------------------------------- - Loader: DfuSe DfuSe - ----------- ----------------------- -------------------------------- - Serial Debug output: USART1 Debug output: USART1 - Console: NSH output: USART1 NSH output: USART1 (3) - ----------- ----------------------- -------------------------------- - microSD Yes Yes - Support - ----------- ----------------------- -------------------------------- - FAT FS CONFIG_FAT_LCNAME=y CONFIG_FAT_LCNAME=y - Config CONFIG_FAT_LFN=n CONFIG_FAT_LFN=y (4) - ----------- ----------------------- -------------------------------- - Support for No Yes - Built-in - Apps - ----------- ----------------------- -------------------------------- - Built-in None apps/examples/nx - Apps apps/examples/nxhello - apps/examples/usbstorage (5) - =========== ======================= ================================ - - (1) You will probably need to modify nsh/setenv.sh or nsh2/setenv.sh - to set up the correct PATH variable for whichever toolchain you - may use. - (2) Since DfuSe is assumed, this configuration may only work under - Cygwin without modification. - (3) When any other device other than /dev/console is used for a user - interface, (1) linefeeds (\n) will not be expanded to carriage return - / linefeeds \r\n). You will need to configure your terminal program - to account for this. And (2) input is not automatically echoed so - you will have to turn local echo on. - (4) Microsoft holds several patents related to the design of - long file names in the FAT file system. Please refer to the - details in the top-level COPYING file. Please do not use FAT - long file name unless you are familiar with these patent issues. - (5) When built as an NSH add-on command (CONFIG_EXAMPLES_USBMSC_BUILTIN=y), - Caution should be used to assure that the SD drive is not in use when - the USB storage device is configured. Specifically, the SD driver - should be unmounted like: - - nsh> mount -t vfat /dev/mmcsd0 /mnt/sdcard # Card is mounted in NSH - ... - nsh> umount /mnd/sdcard # Unmount before connecting USB!!! - nsh> msconn # Connect the USB storage device - ... - nsh> msdis # Disconnect USB storate device - nsh> mount -t vfat /dev/mmcsd0 /mnt/sdcard # Restore the mount - - Failure to do this could result in corruption of the SD card format. - - nx: - --- - An example using the NuttX graphics system (NX). This example - focuses on general window controls, movement, mouse and keyboard - input. - - CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows - CONFIG_LCD_RPORTRAIT=y : 240x320 reverse portrait - - nxlines: - ------ - Another example using the NuttX graphics system (NX). This - example focuses on placing lines on the background in various - orientations. - - CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows - CONFIG_LCD_RPORTRAIT=y : 240x320 reverse portrait - - nxtext: - ------ - Another example using the NuttX graphics system (NX). This - example focuses on placing text on the background while pop-up - windows occur. Text should continue to update normally with - or without the popup windows present. - - CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin - CONFIG_LCD_RPORTRAIT=y : 240x320 reverse portrait - - NOTE: When I tried building this example with the CodeSourcery - tools, I got a hardfault inside of its libgcc. I haven't - retested since then, but beware if you choose to change the - toolchain. - - ostest: - ------ - This configuration directory, performs a simple OS test using - examples/ostest. By default, this project assumes that you are - using the DFU bootloader. - - CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin - - RIDE - ---- - This configuration builds a trivial bring-up binary. It is - useful only because it words with the RIDE7 IDE and R-Link debugger. - - CONFIG_STM32_RAISONANCE=y : Raisonance RIDE7 under Windows - - usbserial: - --------- - This configuration directory exercises the USB serial class - driver at examples/usbserial. See examples/README.txt for - more information. - - CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin - - USB debug output can be enabled as by changing the following - settings in the configuration file: - - -CONFIG_DEBUG=n - -CONFIG_DEBUG_VERBOSE=n - -CONFIG_DEBUG_USB=n - +CONFIG_DEBUG=y - +CONFIG_DEBUG_VERBOSE=y - +CONFIG_DEBUG_USB=y - - -CONFIG_EXAMPLES_USBSERIAL_TRACEINIT=n - -CONFIG_EXAMPLES_USBSERIAL_TRACECLASS=n - -CONFIG_EXAMPLES_USBSERIAL_TRACETRANSFERS=n - -CONFIG_EXAMPLES_USBSERIAL_TRACECONTROLLER=n - -CONFIG_EXAMPLES_USBSERIAL_TRACEINTERRUPTS=n - +CONFIG_EXAMPLES_USBSERIAL_TRACEINIT=y - +CONFIG_EXAMPLES_USBSERIAL_TRACECLASS=y - +CONFIG_EXAMPLES_USBSERIAL_TRACETRANSFERS=y - +CONFIG_EXAMPLES_USBSERIAL_TRACECONTROLLER=y - +CONFIG_EXAMPLES_USBSERIAL_TRACEINTERRUPTS=y - - By default, the usbserial example uses the Prolific PL2303 - serial/USB converter emulation. The example can be modified - to use the CDC/ACM serial class by making the following changes - to the configuration file: - - -CONFIG_PL2303=y - +CONFIG_PL2303=n - - -CONFIG_CDCACM=n - +CONFIG_CDCACM=y - - The example can also be converted to use the alternative - USB serial example at apps/examples/usbterm by changing the - following: - - -CONFIGURED_APPS += examples/usbserial - +CONFIGURED_APPS += examples/usbterm - - In either the original appconfig file (before configuring) - or in the final apps/.config file (after configuring). - - usbstorage: - ---------- - This configuration directory exercises the USB mass storage - class driver at examples/usbstorage. See examples/README.txt for - more information. - - CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin - diff --git a/nuttx-configs/px4io-v2/common/Make.defs b/nuttx-configs/px4io-v2/common/Make.defs deleted file mode 100644 index 7f782b5b22..0000000000 --- a/nuttx-configs/px4io-v2/common/Make.defs +++ /dev/null @@ -1,175 +0,0 @@ -############################################################################ -# configs/px4fmu/common/Make.defs -# -# Copyright (C) 2011 Gregory Nutt. All rights reserved. -# Author: Gregory Nutt -# -# 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 NuttX 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. -# -############################################################################ - -# -# Generic Make.defs for the PX4FMU -# Do not specify/use this file directly - it is included by config-specific -# Make.defs in the per-config directories. -# - -include ${TOPDIR}/tools/Config.mk - -# -# We only support building with the ARM bare-metal toolchain from -# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS. -# -CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI - -include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs - -CC = $(CROSSDEV)gcc -CXX = $(CROSSDEV)g++ -CPP = $(CROSSDEV)gcc -E -LD = $(CROSSDEV)ld -AR = $(CROSSDEV)ar rcs -NM = $(CROSSDEV)nm -OBJCOPY = $(CROSSDEV)objcopy -OBJDUMP = $(CROSSDEV)objdump - -MAXOPTIMIZATION = -O3 -ARCHCPUFLAGS = -mcpu=cortex-m3 \ - -mthumb \ - -march=armv7-m - -# enable precise stack overflow tracking -#INSTRUMENTATIONDEFINES = -finstrument-functions \ -# -ffixed-r10 - -# use our linker script -LDSCRIPT = ld.script - -ifeq ($(WINTOOL),y) - # Windows-native toolchains - DIRLINK = $(TOPDIR)/tools/copydir.sh - DIRUNLINK = $(TOPDIR)/tools/unlink.sh - MKDEP = $(TOPDIR)/tools/mknulldeps.sh - ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" - ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" - ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT)}" -else - ifeq ($(PX4_WINTOOL),y) - # Windows-native toolchains (MSYS) - DIRLINK = $(TOPDIR)/tools/copydir.sh - DIRUNLINK = $(TOPDIR)/tools/unlink.sh - MKDEP = $(TOPDIR)/tools/mknulldeps.sh - ARCHINCLUDES = -I. -isystem $(TOPDIR)/include - ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx - ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT) - else - # Linux/Cygwin-native toolchain - MKDEP = $(TOPDIR)/tools/mkdeps.sh - ARCHINCLUDES = -I. -isystem $(TOPDIR)/include - ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx - ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT) - endif -endif - -# tool versions -ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} -ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} - -# optimisation flags -ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ - -fno-strict-aliasing \ - -fno-strength-reduce \ - -fomit-frame-pointer \ - -funsafe-math-optimizations \ - -fno-builtin-printf \ - -ffunction-sections \ - -fdata-sections - -ifeq ("${CONFIG_DEBUG_SYMBOLS}","y") -ARCHOPTIMIZATION += -g -endif - -ARCHCFLAGS = -std=gnu99 -ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x -ARCHWARNINGS = -Wall \ - -Wextra \ - -Wdouble-promotion \ - -Wshadow \ - -Wfloat-equal \ - -Wframe-larger-than=1024 \ - -Wpointer-arith \ - -Wlogical-op \ - -Wmissing-declarations \ - -Wpacked \ - -Wno-unused-parameter -# -Wcast-qual - generates spurious noreturn attribute warnings, try again later -# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code -# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives - -ARCHCWARNINGS = $(ARCHWARNINGS) \ - -Wbad-function-cast \ - -Wstrict-prototypes \ - -Wold-style-declaration \ - -Wmissing-parameter-type \ - -Wmissing-prototypes \ - -Wnested-externs \ - -Wunsuffixed-float-constants -ARCHWARNINGSXX = $(ARCHWARNINGS) -ARCHDEFINES = -ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 - -# this seems to be the only way to add linker flags -EXTRA_LIBS += --warn-common \ - --gc-sections - -CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common -CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) -CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) -CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -AFLAGS = $(CFLAGS) -D__ASSEMBLY__ - -NXFLATLDFLAGS1 = -r -d -warn-common -NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections -LDNXFLATFLAGS = -e main -s 2048 - -OBJEXT = .o -LIBEXT = .a -EXEEXT = - -# produce partially-linked $1 from files in $2 -define PRELINK - @echo "PRELINK: $1" - $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 -endef - -HOSTCC = gcc -HOSTINCLUDES = -I. -HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe -HOSTLDFLAGS = - diff --git a/nuttx-configs/px4io-v2/common/setenv.sh b/nuttx-configs/px4io-v2/common/setenv.sh deleted file mode 100755 index ff9a4bf8ae..0000000000 --- a/nuttx-configs/px4io-v2/common/setenv.sh +++ /dev/null @@ -1,47 +0,0 @@ -#!/bin/bash -# configs/stm3210e-eval/dfu/setenv.sh -# -# Copyright (C) 2009 Gregory Nutt. All rights reserved. -# Author: Gregory Nutt -# -# 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 NuttX 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. -# - -if [ "$(basename $0)" = "setenv.sh" ] ; then - echo "You must source this script, not run it!" 1>&2 - exit 1 -fi - -if [ -z "${PATH_ORIG}" ]; then export PATH_ORIG="${PATH}"; fi - -WD=`pwd` -export RIDE_BIN="/cygdrive/c/Program Files/Raisonance/Ride/arm-gcc/bin" -export BUILDROOT_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin" -export PATH="${BUILDROOT_BIN}:${RIDE_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" - -echo "PATH : ${PATH}" diff --git a/nuttx-configs/px4io-v2/include/README.txt b/nuttx-configs/px4io-v2/include/README.txt deleted file mode 100755 index 2264a80aa8..0000000000 --- a/nuttx-configs/px4io-v2/include/README.txt +++ /dev/null @@ -1 +0,0 @@ -This directory contains header files unique to the PX4IO board. diff --git a/nuttx-configs/px4io-v2/nsh/Make.defs b/nuttx-configs/px4io-v2/nsh/Make.defs index bdfc4e3e2c..cd2d8eba3b 100644 --- a/nuttx-configs/px4io-v2/nsh/Make.defs +++ b/nuttx-configs/px4io-v2/nsh/Make.defs @@ -1,3 +1,170 @@ -include ${TOPDIR}/.config +############################################################################ +# configs/px4io-v2/nsh/Make.defs +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# +############################################################################ + +include ${TOPDIR}/.config +include ${TOPDIR}/tools/Config.mk + +# +# We only support building with the ARM bare-metal toolchain from +# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS. +# +CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI + +include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs + +CC = $(CROSSDEV)gcc +CXX = $(CROSSDEV)g++ +CPP = $(CROSSDEV)gcc -E +LD = $(CROSSDEV)ld +AR = $(CROSSDEV)ar rcs +NM = $(CROSSDEV)nm +OBJCOPY = $(CROSSDEV)objcopy +OBJDUMP = $(CROSSDEV)objdump + +MAXOPTIMIZATION = -O3 +ARCHCPUFLAGS = -mcpu=cortex-m3 \ + -mthumb \ + -march=armv7-m + +# enable precise stack overflow tracking +#INSTRUMENTATIONDEFINES = -finstrument-functions \ +# -ffixed-r10 + +# use our linker script +LDSCRIPT = ld.script + +ifeq ($(WINTOOL),y) + # Windows-native toolchains + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" + ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" + ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)}" +else + ifeq ($(PX4_WINTOOL),y) + # Windows-native toolchains (MSYS) + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) + else + # Linux/Cygwin-native toolchain + MKDEP = $(TOPDIR)/tools/mkdeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) + endif +endif + +# tool versions +ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} +ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} + +# optimisation flags +ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ + -fno-strict-aliasing \ + -fno-strength-reduce \ + -fomit-frame-pointer \ + -funsafe-math-optimizations \ + -fno-builtin-printf \ + -ffunction-sections \ + -fdata-sections + +ifeq ("${CONFIG_DEBUG_SYMBOLS}","y") +ARCHOPTIMIZATION += -g +endif + +ARCHCFLAGS = -std=gnu99 +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x +ARCHWARNINGS = -Wall \ + -Wextra \ + -Wdouble-promotion \ + -Wshadow \ + -Wfloat-equal \ + -Wframe-larger-than=1024 \ + -Wpointer-arith \ + -Wlogical-op \ + -Wmissing-declarations \ + -Wpacked \ + -Wno-unused-parameter +# -Wcast-qual - generates spurious noreturn attribute warnings, try again later +# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code +# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives + +ARCHCWARNINGS = $(ARCHWARNINGS) \ + -Wbad-function-cast \ + -Wstrict-prototypes \ + -Wold-style-declaration \ + -Wmissing-parameter-type \ + -Wmissing-prototypes \ + -Wnested-externs \ + -Wunsuffixed-float-constants +ARCHWARNINGSXX = $(ARCHWARNINGS) +ARCHDEFINES = +ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 + +# this seems to be the only way to add linker flags +EXTRA_LIBS += --warn-common \ + --gc-sections + +CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common +CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) +CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe +CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) +CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) +AFLAGS = $(CFLAGS) -D__ASSEMBLY__ + +NXFLATLDFLAGS1 = -r -d -warn-common +NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections +LDNXFLATFLAGS = -e main -s 2048 + +OBJEXT = .o +LIBEXT = .a +EXEEXT = + +# produce partially-linked $1 from files in $2 +define PRELINK + @echo "PRELINK: $1" + $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 +endef + +HOSTCC = gcc +HOSTINCLUDES = -I. +HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe +HOSTLDFLAGS = -include $(TOPDIR)/configs/px4io-v2/common/Make.defs diff --git a/nuttx-configs/px4io-v2/common/ld.script b/nuttx-configs/px4io-v2/scripts/ld.script similarity index 100% rename from nuttx-configs/px4io-v2/common/ld.script rename to nuttx-configs/px4io-v2/scripts/ld.script diff --git a/nuttx-configs/px4io-v2/src/README.txt b/nuttx-configs/px4io-v2/src/README.txt deleted file mode 100644 index d4eda82fd7..0000000000 --- a/nuttx-configs/px4io-v2/src/README.txt +++ /dev/null @@ -1 +0,0 @@ -This directory contains drivers unique to the STMicro STM3210E-EVAL development board. diff --git a/nuttx-configs/px4io-v2/test/Make.defs b/nuttx-configs/px4io-v2/test/Make.defs deleted file mode 100644 index 87508e22ec..0000000000 --- a/nuttx-configs/px4io-v2/test/Make.defs +++ /dev/null @@ -1,3 +0,0 @@ -include ${TOPDIR}/.config - -include $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/Make.defs diff --git a/nuttx-configs/px4io-v2/test/appconfig b/nuttx-configs/px4io-v2/test/appconfig deleted file mode 100644 index 3cfc41b437..0000000000 --- a/nuttx-configs/px4io-v2/test/appconfig +++ /dev/null @@ -1,44 +0,0 @@ -############################################################################ -# configs/stm3210e-eval/nsh/appconfig -# -# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. -# Author: Gregory Nutt -# -# 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 NuttX 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. -# -############################################################################ - -# Path to example in apps/examples containing the user_start entry point - -CONFIGURED_APPS += examples/nsh - -CONFIGURED_APPS += system/readline -CONFIGURED_APPS += nshlib -CONFIGURED_APPS += reboot - -CONFIGURED_APPS += drivers/boards/px4iov2 diff --git a/nuttx-configs/px4io-v2/test/defconfig b/nuttx-configs/px4io-v2/test/defconfig deleted file mode 100755 index 132c40d806..0000000000 --- a/nuttx-configs/px4io-v2/test/defconfig +++ /dev/null @@ -1,566 +0,0 @@ -############################################################################ -# configs/px4io/nsh/defconfig -# -# Copyright (C) 2009-2012 Gregory Nutt. All rights reserved. -# Author: Gregory Nutt -# -# 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 NuttX 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. -# -############################################################################ -# -# architecture selection -# -# CONFIG_ARCH - identifies the arch subdirectory and, hence, the -# processor architecture. -# CONFIG_ARCH_family - for use in C code. This identifies the -# particular chip family that the architecture is implemented -# in. -# CONFIG_ARCH_architecture - for use in C code. This identifies the -# specific architecture within the chip familyl. -# CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory -# CONFIG_ARCH_CHIP_name - For use in C code -# CONFIG_ARCH_BOARD - identifies the configs subdirectory and, hence, -# the board that supports the particular chip or SoC. -# CONFIG_ARCH_BOARD_name - for use in C code -# CONFIG_ENDIAN_BIG - define if big endian (default is little endian) -# CONFIG_BOARD_LOOPSPERMSEC - for delay loops -# CONFIG_DRAM_SIZE - Describes the installed DRAM. -# CONFIG_DRAM_START - The start address of DRAM (physical) -# CONFIG_DRAM_END - Last address+1 of installed RAM -# CONFIG_ARCH_IRQPRIO - The ST32F100CB supports interrupt prioritization -# CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt -# stack. If defined, this symbol is the size of the interrupt -# stack in bytes. If not defined, the user task stacks will be -# used during interrupt handling. -# CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions -# CONFIG_ARCH_BOOTLOADER - Set if you are using a bootloader. -# CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to board architecture. -# CONFIG_ARCH_BUTTONS - Enable support for buttons. Unique to board architecture. -# CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that -# cause a 100 second delay during boot-up. This 100 second delay -# serves no purpose other than it allows you to calibrate -# CONFIG_BOARD_LOOPSPERMSEC. You simply use a stop watch to measure -# the 100 second delay then adjust CONFIG_BOARD_LOOPSPERMSEC until -# the delay actually is 100 seconds. -# CONFIG_ARCH_DMA - Support DMA initialization -# -CONFIG_ARCH=arm -CONFIG_ARCH_ARM=y -CONFIG_ARCH_CORTEXM3=y -CONFIG_ARCH_CHIP=stm32 -CONFIG_ARCH_CHIP_STM32F100C8=y -CONFIG_ARCH_BOARD=px4iov2 -CONFIG_ARCH_BOARD_PX4IO_V2=y -CONFIG_BOARD_LOOPSPERMSEC=24000 -CONFIG_DRAM_SIZE=0x00002000 -CONFIG_DRAM_START=0x20000000 -CONFIG_DRAM_END=(CONFIG_DRAM_START+CONFIG_DRAM_SIZE) -CONFIG_ARCH_IRQPRIO=y -CONFIG_ARCH_INTERRUPTSTACK=n -CONFIG_ARCH_STACKDUMP=y -CONFIG_ARCH_BOOTLOADER=n -CONFIG_ARCH_LEDS=n -CONFIG_ARCH_BUTTONS=y -CONFIG_ARCH_CALIBRATION=n -CONFIG_ARCH_DMA=n -CONFIG_ARMV7M_CMNVECTOR=y - -# -# JTAG Enable settings (by default JTAG-DP and SW-DP are disabled): -# -# CONFIG_STM32_DFU - Use the DFU bootloader, not JTAG -# -# JTAG Enable options: -# -# CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) -# CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) -# but without JNTRST. -# CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled -# -CONFIG_STM32_DFU=n -CONFIG_STM32_JTAG_FULL_ENABLE=y -CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n -CONFIG_STM32_JTAG_SW_ENABLE=n - -# -# Individual subsystems can be enabled: -# AHB: -CONFIG_STM32_DMA1=n -CONFIG_STM32_DMA2=n -CONFIG_STM32_CRC=n -# APB1: -# Timers 2,3 and 4 are owned by the PWM driver -CONFIG_STM32_TIM2=n -CONFIG_STM32_TIM3=n -CONFIG_STM32_TIM4=n -CONFIG_STM32_TIM5=n -CONFIG_STM32_TIM6=n -CONFIG_STM32_TIM7=n -CONFIG_STM32_WWDG=n -CONFIG_STM32_SPI2=n -CONFIG_STM32_USART2=y -CONFIG_STM32_USART3=y -CONFIG_STM32_I2C1=y -CONFIG_STM32_I2C2=n -CONFIG_STM32_BKP=n -CONFIG_STM32_PWR=n -CONFIG_STM32_DAC=n -# APB2: -CONFIG_STM32_ADC1=y -CONFIG_STM32_ADC2=n -# TIM1 is owned by the HRT -CONFIG_STM32_TIM1=n -CONFIG_STM32_SPI1=n -CONFIG_STM32_TIM8=n -CONFIG_STM32_USART1=y -CONFIG_STM32_ADC3=n - -# -# Timer and I2C devices may need to the following to force power to be applied: -# -#CONFIG_STM32_FORCEPOWER=y - -# -# STM32F100 specific serial device driver settings -# -# CONFIG_USARTn_SERIAL_CONSOLE - selects the USARTn for the -# console and ttys0 (default is the USART1). -# CONFIG_USARTn_RXBUFSIZE - Characters are buffered as received. -# This specific the size of the receive buffer -# CONFIG_USARTn_TXBUFSIZE - Characters are buffered before -# being sent. This specific the size of the transmit buffer -# CONFIG_USARTn_BAUD - The configure BAUD of the UART. Must be -# CONFIG_USARTn_BITS - The number of bits. Must be either 7 or 8. -# CONFIG_USARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity -# CONFIG_USARTn_2STOP - Two stop bits -# -CONFIG_USART1_SERIAL_CONSOLE=y -CONFIG_USART2_SERIAL_CONSOLE=n -CONFIG_USART3_SERIAL_CONSOLE=n - -CONFIG_USART1_TXBUFSIZE=64 -CONFIG_USART2_TXBUFSIZE=64 -CONFIG_USART3_TXBUFSIZE=64 - -CONFIG_USART1_RXBUFSIZE=64 -CONFIG_USART2_RXBUFSIZE=64 -CONFIG_USART3_RXBUFSIZE=64 - -CONFIG_USART1_BAUD=57600 -CONFIG_USART2_BAUD=115200 -CONFIG_USART3_BAUD=115200 - -CONFIG_USART1_BITS=8 -CONFIG_USART2_BITS=8 -CONFIG_USART3_BITS=8 - -CONFIG_USART1_PARITY=0 -CONFIG_USART2_PARITY=0 -CONFIG_USART3_PARITY=0 - -CONFIG_USART1_2STOP=0 -CONFIG_USART2_2STOP=0 -CONFIG_USART3_2STOP=0 - -# -# PX4IO specific driver settings -# -# CONFIG_HRT_TIMER -# Enables the high-resolution timer. The board definition must -# set HRT_TIMER and HRT_TIMER_CHANNEL to the timer and capture/ -# compare channels to be used. -# CONFIG_HRT_PPM -# Enables R/C PPM input using the HRT. The board definition must -# set HRT_PPM_CHANNEL to the timer capture/compare channel to be -# used, and define GPIO_PPM_IN to configure the appropriate timer -# GPIO. -# CONFIG_PWM_SERVO -# Enables the PWM servo driver. The driver configuration must be -# supplied by the board support at initialisation time. -# Note that USART2 must be disabled on the PX4 board for this to -# be available. -# -# -CONFIG_HRT_TIMER=y -CONFIG_HRT_PPM=y - -# -# General build options -# -# CONFIG_RRLOAD_BINARY - make the rrload binary format used with -# BSPs from www.ridgerun.com using the tools/mkimage.sh script -# CONFIG_INTELHEX_BINARY - make the Intel HEX binary format -# used with many different loaders using the GNU objcopy program -# Should not be selected if you are not using the GNU toolchain. -# CONFIG_MOTOROLA_SREC - make the Motorola S-Record binary format -# used with many different loaders using the GNU objcopy program -# Should not be selected if you are not using the GNU toolchain. -# CONFIG_RAW_BINARY - make a raw binary format file used with many -# different loaders using the GNU objcopy program. This option -# should not be selected if you are not using the GNU toolchain. -# CONFIG_HAVE_LIBM - toolchain supports libm.a -# -CONFIG_RRLOAD_BINARY=n -CONFIG_INTELHEX_BINARY=n -CONFIG_MOTOROLA_SREC=n -CONFIG_RAW_BINARY=y -CONFIG_HAVE_LIBM=n - -# -# General OS setup -# -# CONFIG_APPS_DIR - Identifies the relative path to the directory -# that builds the application to link with NuttX. Default: ../apps -# CONFIG_DEBUG - enables built-in debug options -# CONFIG_DEBUG_VERBOSE - enables verbose debug output -# CONFIG_DEBUG_SYMBOLS - build without optimization and with -# debug symbols (needed for use with a debugger). -# CONFIG_HAVE_CXX - Enable support for C++ -# CONFIG_HAVE_CXXINITIALIZE - The platform-specific logic includes support -# for initialization of static C++ instances for this architecture -# and for the selected toolchain (via up_cxxinitialize()). -# CONFIG_MM_REGIONS - If the architecture includes multiple -# regions of memory to allocate from, this specifies the -# number of memory regions that the memory manager must -# handle and enables the API mm_addregion(start, end); -# CONFIG_ARCH_LOWPUTC - architecture supports low-level, boot -# time console output -# CONFIG_MSEC_PER_TICK - The default system timer is 100Hz -# or MSEC_PER_TICK=10. This setting may be defined to -# inform NuttX that the processor hardware is providing -# system timer interrupts at some interrupt interval other -# than 10 msec. -# CONFIG_RR_INTERVAL - The round robin timeslice will be set -# this number of milliseconds; Round robin scheduling can -# be disabled by setting this value to zero. -# CONFIG_SCHED_INSTRUMENTATION - enables instrumentation in -# scheduler to monitor system performance -# CONFIG_TASK_NAME_SIZE - Spcifies that maximum size of a -# task name to save in the TCB. Useful if scheduler -# instrumentation is selected. Set to zero to disable. -# CONFIG_START_YEAR, CONFIG_START_MONTH, CONFIG_START_DAY - -# Used to initialize the internal time logic. -# CONFIG_GREGORIAN_TIME - Enables Gregorian time conversions. -# You would only need this if you are concerned about accurate -# time conversions in the past or in the distant future. -# CONFIG_JULIAN_TIME - Enables Julian time conversions. You -# would only need this if you are concerned about accurate -# time conversion in the distand past. You must also define -# CONFIG_GREGORIAN_TIME in order to use Julian time. -# CONFIG_DEV_CONSOLE - Set if architecture-specific logic -# provides /dev/console. Enables stdout, stderr, stdin. -# CONFIG_DEV_LOWCONSOLE - Use the simple, low-level serial console -# driver (minimul support) -# CONFIG_MUTEX_TYPES: Set to enable support for recursive and -# errorcheck mutexes. Enables pthread_mutexattr_settype(). -# CONFIG_PRIORITY_INHERITANCE : Set to enable support for priority -# inheritance on mutexes and semaphores. -# CONFIG_SEM_PREALLOCHOLDERS: This setting is only used if priority -# inheritance is enabled. It defines the maximum number of -# different threads (minus one) that can take counts on a -# semaphore with priority inheritance support. This may be -# set to zero if priority inheritance is disabled OR if you -# are only using semaphores as mutexes (only one holder) OR -# if no more than two threads participate using a counting -# semaphore. -# CONFIG_SEM_NNESTPRIO. If priority inheritance is enabled, -# then this setting is the maximum number of higher priority -# threads (minus 1) than can be waiting for another thread -# to release a count on a semaphore. This value may be set -# to zero if no more than one thread is expected to wait for -# a semaphore. -# CONFIG_FDCLONE_DISABLE. Disable cloning of all file descriptors -# by task_create() when a new task is started. If set, all -# files/drivers will appear to be closed in the new task. -# CONFIG_FDCLONE_STDIO. Disable cloning of all but the first -# three file descriptors (stdin, stdout, stderr) by task_create() -# when a new task is started. If set, all files/drivers will -# appear to be closed in the new task except for stdin, stdout, -# and stderr. -# CONFIG_SDCLONE_DISABLE. Disable cloning of all socket -# desciptors by task_create() when a new task is started. If -# set, all sockets will appear to be closed in the new task. -# CONFIG_NXFLAT. Enable support for the NXFLAT binary format. -# This format will support execution of NuttX binaries located -# in a ROMFS filesystem (see examples/nxflat). -# CONFIG_SCHED_WORKQUEUE. Create a dedicated "worker" thread to -# handle delayed processing from interrupt handlers. This feature -# is required for some drivers but, if there are not complaints, -# can be safely disabled. The worker thread also performs -# garbage collection -- completing any delayed memory deallocations -# from interrupt handlers. If the worker thread is disabled, -# then that clean will be performed by the IDLE thread instead -# (which runs at the lowest of priority and may not be appropriate -# if memory reclamation is of high priority). If CONFIG_SCHED_WORKQUEUE -# is enabled, then the following options can also be used: -# CONFIG_SCHED_WORKPRIORITY - The execution priority of the worker -# thread. Default: 50 -# CONFIG_SCHED_WORKPERIOD - How often the worker thread checks for -# work in units of microseconds. Default: 50*1000 (50 MS). -# CONFIG_SCHED_WORKSTACKSIZE - The stack size allocated for the worker -# thread. Default: CONFIG_IDLETHREAD_STACKSIZE. -# CONFIG_SIG_SIGWORK - The signal number that will be used to wake-up -# the worker thread. Default: 4 -# -#CONFIG_APPS_DIR= -CONFIG_DEBUG=n -CONFIG_DEBUG_VERBOSE=n -CONFIG_DEBUG_SYMBOLS=y -CONFIG_HAVE_CXX=y -CONFIG_HAVE_CXXINITIALIZE=n -CONFIG_MM_REGIONS=1 -CONFIG_MM_SMALL=y -CONFIG_ARCH_LOWPUTC=y -CONFIG_RR_INTERVAL=200 -CONFIG_SCHED_INSTRUMENTATION=n -CONFIG_TASK_NAME_SIZE=0 -CONFIG_START_YEAR=2009 -CONFIG_START_MONTH=9 -CONFIG_START_DAY=21 -CONFIG_GREGORIAN_TIME=n -CONFIG_JULIAN_TIME=n -CONFIG_DEV_CONSOLE=y -CONFIG_DEV_LOWCONSOLE=n -CONFIG_MUTEX_TYPES=n -CONFIG_PRIORITY_INHERITANCE=n -CONFIG_SEM_PREALLOCHOLDERS=0 -CONFIG_SEM_NNESTPRIO=0 -CONFIG_FDCLONE_DISABLE=n -CONFIG_FDCLONE_STDIO=y -CONFIG_SDCLONE_DISABLE=y -CONFIG_NXFLAT=n -CONFIG_SCHED_WORKQUEUE=n -CONFIG_SCHED_WORKPRIORITY=50 -CONFIG_SCHED_WORKPERIOD=(50*1000) -CONFIG_SCHED_WORKSTACKSIZE=512 -CONFIG_SIG_SIGWORK=4 - -CONFIG_USER_ENTRYPOINT="nsh_main" -# -# The following can be used to disable categories of -# APIs supported by the OS. If the compiler supports -# weak functions, then it should not be necessary to -# disable functions unless you want to restrict usage -# of those APIs. -# -# There are certain dependency relationships in these -# features. -# -# o mq_notify logic depends on signals to awaken tasks -# waiting for queues to become full or empty. -# o pthread_condtimedwait() depends on signals to wake -# up waiting tasks. -# -CONFIG_DISABLE_CLOCK=n -CONFIG_DISABLE_POSIX_TIMERS=y -CONFIG_DISABLE_PTHREAD=n -CONFIG_DISABLE_SIGNALS=n -CONFIG_DISABLE_MQUEUE=y -CONFIG_DISABLE_MOUNTPOINT=y -CONFIG_DISABLE_ENVIRON=y -CONFIG_DISABLE_POLL=y - -# -# Misc libc settings -# -# CONFIG_NOPRINTF_FIELDWIDTH - sprintf-related logic is a -# little smaller if we do not support fieldwidthes -# -CONFIG_NOPRINTF_FIELDWIDTH=n - -# -# Allow for architecture optimized implementations -# -# The architecture can provide optimized versions of the -# following to improve system performance -# -CONFIG_ARCH_MEMCPY=n -CONFIG_ARCH_MEMCMP=n -CONFIG_ARCH_MEMMOVE=n -CONFIG_ARCH_MEMSET=n -CONFIG_ARCH_STRCMP=n -CONFIG_ARCH_STRCPY=n -CONFIG_ARCH_STRNCPY=n -CONFIG_ARCH_STRLEN=n -CONFIG_ARCH_STRNLEN=n -CONFIG_ARCH_BZERO=n - -# -# Sizes of configurable things (0 disables) -# -# CONFIG_MAX_TASKS - The maximum number of simultaneously -# active tasks. This value must be a power of two. -# CONFIG_MAX_TASK_ARGS - This controls the maximum number of -# of parameters that a task may receive (i.e., maxmum value -# of 'argc') -# CONFIG_NPTHREAD_KEYS - The number of items of thread- -# specific data that can be retained -# CONFIG_NFILE_DESCRIPTORS - The maximum number of file -# descriptors (one for each open) -# CONFIG_NFILE_STREAMS - The maximum number of streams that -# can be fopen'ed -# CONFIG_NAME_MAX - The maximum size of a file name. -# CONFIG_STDIO_BUFFER_SIZE - Size of the buffer to allocate -# on fopen. (Only if CONFIG_NFILE_STREAMS > 0) -# CONFIG_NUNGET_CHARS - Number of characters that can be -# buffered by ungetc() (Only if CONFIG_NFILE_STREAMS > 0) -# CONFIG_PREALLOC_MQ_MSGS - The number of pre-allocated message -# structures. The system manages a pool of preallocated -# message structures to minimize dynamic allocations -# CONFIG_MQ_MAXMSGSIZE - Message structures are allocated with -# a fixed payload size given by this settin (does not include -# other message structure overhead. -# CONFIG_MAX_WDOGPARMS - Maximum number of parameters that -# can be passed to a watchdog handler -# CONFIG_PREALLOC_WDOGS - The number of pre-allocated watchdog -# structures. The system manages a pool of preallocated -# watchdog structures to minimize dynamic allocations -# CONFIG_PREALLOC_TIMERS - The number of pre-allocated POSIX -# timer structures. The system manages a pool of preallocated -# timer structures to minimize dynamic allocations. Set to -# zero for all dynamic allocations. -# -CONFIG_MAX_TASKS=4 -CONFIG_MAX_TASK_ARGS=4 -CONFIG_NPTHREAD_KEYS=2 -CONFIG_NFILE_DESCRIPTORS=6 -CONFIG_NFILE_STREAMS=4 -CONFIG_NAME_MAX=32 -CONFIG_STDIO_BUFFER_SIZE=64 -CONFIG_NUNGET_CHARS=2 -CONFIG_PREALLOC_MQ_MSGS=1 -CONFIG_MQ_MAXMSGSIZE=32 -CONFIG_MAX_WDOGPARMS=2 -CONFIG_PREALLOC_WDOGS=3 -CONFIG_PREALLOC_TIMERS=1 - - -# -# Settings for apps/nshlib -# -# CONFIG_NSH_BUILTIN_APPS - Support external registered, -# "named" applications that can be executed from the NSH -# command line (see apps/README.txt for more information). -# CONFIG_NSH_FILEIOSIZE - Size of a static I/O buffer -# CONFIG_NSH_STRERROR - Use strerror(errno) -# CONFIG_NSH_LINELEN - Maximum length of one command line -# CONFIG_NSH_NESTDEPTH - Max number of nested if-then[-else]-fi -# CONFIG_NSH_DISABLESCRIPT - Disable scripting support -# CONFIG_NSH_DISABLEBG - Disable background commands -# CONFIG_NSH_ROMFSETC - Use startup script in /etc -# CONFIG_NSH_CONSOLE - Use serial console front end -# CONFIG_NSH_TELNET - Use telnetd console front end -# CONFIG_NSH_ARCHINIT - Platform provides architecture -# specific initialization (nsh_archinitialize()). -# -# If CONFIG_NSH_TELNET is selected: -# CONFIG_NSH_IOBUFFER_SIZE -- Telnetd I/O buffer size -# CONFIG_NSH_DHCPC - Obtain address using DHCP -# CONFIG_NSH_IPADDR - Provides static IP address -# CONFIG_NSH_DRIPADDR - Provides static router IP address -# CONFIG_NSH_NETMASK - Provides static network mask -# CONFIG_NSH_NOMAC - Use a bogus MAC address -# -# If CONFIG_NSH_ROMFSETC is selected: -# CONFIG_NSH_ROMFSMOUNTPT - ROMFS mountpoint -# CONFIG_NSH_INITSCRIPT - Relative path to init script -# CONFIG_NSH_ROMFSDEVNO - ROMFS RAM device minor -# CONFIG_NSH_ROMFSSECTSIZE - ROMF sector size -# CONFIG_NSH_FATDEVNO - FAT FS RAM device minor -# CONFIG_NSH_FATSECTSIZE - FAT FS sector size -# CONFIG_NSH_FATNSECTORS - FAT FS number of sectors -# CONFIG_NSH_FATMOUNTPT - FAT FS mountpoint -# -CONFIG_BUILTIN=y -CONFIG_NSH_BUILTIN_APPS=y -CONFIG_NSH_FILEIOSIZE=64 -CONFIG_NSH_STRERROR=n -CONFIG_NSH_LINELEN=64 -CONFIG_NSH_NESTDEPTH=1 -CONFIG_NSH_DISABLESCRIPT=y -CONFIG_NSH_DISABLEBG=n -CONFIG_NSH_ROMFSETC=n -CONFIG_NSH_CONSOLE=y -CONFIG_NSH_TELNET=n -CONFIG_NSH_ARCHINIT=n -CONFIG_NSH_IOBUFFER_SIZE=256 -CONFIG_NSH_STACKSIZE=1024 -CONFIG_NSH_DHCPC=n -CONFIG_NSH_NOMAC=n -CONFIG_NSH_IPADDR=(10<<24|0<<16|0<<8|2) -CONFIG_NSH_DRIPADDR=(10<<24|0<<16|0<<8|1) -CONFIG_NSH_NETMASK=(255<<24|255<<16|255<<8|0) -CONFIG_NSH_ROMFSMOUNTPT="/etc" -CONFIG_NSH_INITSCRIPT="init.d/rcS" -CONFIG_NSH_ROMFSDEVNO=0 -CONFIG_NSH_ROMFSSECTSIZE=64 -CONFIG_NSH_FATDEVNO=1 -CONFIG_NSH_FATSECTSIZE=512 -CONFIG_NSH_FATNSECTORS=1024 -CONFIG_NSH_FATMOUNTPT=/tmp - -# -# Architecture-specific NSH options -# -CONFIG_NSH_MMCSDSPIPORTNO=0 -CONFIG_NSH_MMCSDSLOTNO=0 -CONFIG_NSH_MMCSDMINOR=0 - -# -# Stack and heap information -# -# CONFIG_BOOT_RUNFROMFLASH - Some configurations support XIP -# operation from FLASH but must copy initialized .data sections to RAM. -# (should also be =n for the STM3210E-EVAL which always runs from flash) -# CONFIG_BOOT_COPYTORAM - Some configurations boot in FLASH -# but copy themselves entirely into RAM for better performance. -# CONFIG_CUSTOM_STACK - The up_ implementation will handle -# all stack operations outside of the nuttx model. -# CONFIG_STACK_POINTER - The initial stack pointer (arm7tdmi only) -# CONFIG_IDLETHREAD_STACKSIZE - The size of the initial stack. -# This is the thread that (1) performs the inital boot of the system up -# to the point where user_start() is spawned, and (2) there after is the -# IDLE thread that executes only when there is no other thread ready to -# run. -# CONFIG_USERMAIN_STACKSIZE - The size of the stack to allocate -# for the main user thread that begins at the user_start() entry point. -# CONFIG_PTHREAD_STACK_MIN - Minimum pthread stack size -# CONFIG_PTHREAD_STACK_DEFAULT - Default pthread stack size -# CONFIG_HEAP_BASE - The beginning of the heap -# CONFIG_HEAP_SIZE - The size of the heap -# -CONFIG_BOOT_RUNFROMFLASH=n -CONFIG_BOOT_COPYTORAM=n -CONFIG_CUSTOM_STACK=n -CONFIG_STACK_POINTER= -CONFIG_IDLETHREAD_STACKSIZE=800 -CONFIG_USERMAIN_STACKSIZE=1024 -CONFIG_PTHREAD_STACK_MIN=256 -CONFIG_PTHREAD_STACK_DEFAULT=512 -CONFIG_HEAP_BASE= -CONFIG_HEAP_SIZE= diff --git a/nuttx-configs/px4io-v2/test/setenv.sh b/nuttx-configs/px4io-v2/test/setenv.sh deleted file mode 100755 index d836851921..0000000000 --- a/nuttx-configs/px4io-v2/test/setenv.sh +++ /dev/null @@ -1,47 +0,0 @@ -#!/bin/bash -# configs/stm3210e-eval/dfu/setenv.sh -# -# Copyright (C) 2009 Gregory Nutt. All rights reserved. -# Author: Gregory Nutt -# -# 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 NuttX 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. -# - -if [ "$(basename $0)" = "setenv.sh" ] ; then - echo "You must source this script, not run it!" 1>&2 - exit 1 -fi - -if [ -z "${PATH_ORIG}" ]; then export PATH_ORIG="${PATH}"; fi - -WD=`pwd` -export RIDE_BIN="/cygdrive/c/Program Files/Raisonance/Ride/arm-gcc/bin" -export BUILDROOT_BIN="${WD}/../buildroot/build_arm_nofpu/staging_dir/bin" -export PATH="${BUILDROOT_BIN}:${RIDE_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" - -echo "PATH : ${PATH}" From 4708693f86858772e88d7736a4996023d4f57327 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 3 Aug 2013 18:54:03 +1000 Subject: [PATCH 322/486] nshterm: start a nsh console on any device this is used in APM startup to fallback to a console on ttyACM0 if startup fails for any reason --- src/systemcmds/nshterm/module.mk | 41 +++++++++++++++ src/systemcmds/nshterm/nshterm.c | 90 ++++++++++++++++++++++++++++++++ 2 files changed, 131 insertions(+) create mode 100644 src/systemcmds/nshterm/module.mk create mode 100644 src/systemcmds/nshterm/nshterm.c diff --git a/src/systemcmds/nshterm/module.mk b/src/systemcmds/nshterm/module.mk new file mode 100644 index 0000000000..a485885354 --- /dev/null +++ b/src/systemcmds/nshterm/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Build nshterm utility +# + +MODULE_COMMAND = nshterm +SRCS = nshterm.c + +MODULE_STACKSIZE = 3000 diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c new file mode 100644 index 0000000000..bde0e78411 --- /dev/null +++ b/src/systemcmds/nshterm/nshterm.c @@ -0,0 +1,90 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: Andrew Tridgell + * + * 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 nshterm.c + * start a nsh terminal on a given port. This can be useful for error + * handling in startup scripts to start a nsh shell on /dev/ttyACM0 + * for diagnostics + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +__EXPORT int nshterm_main(int argc, char *argv[]); + +int +nshterm_main(int argc, char *argv[]) +{ + if (argc < 2) { + printf("Usage: nshterm \n"); + exit(1); + } + uint8_t retries = 0; + int fd = -1; + while (retries < 5) { + // the retries are to cope with the behaviour of /dev/ttyACM0 + // which may not be ready immediately. + fd = open(argv[1], O_RDWR); + if (fd != -1) { + break; + } + usleep(100000); + retries++; + } + if (fd == -1) { + perror(argv[1]); + exit(1); + } + // setup standard file descriptors + close(0); + close(1); + close(2); + dup2(fd, 0); + dup2(fd, 1); + dup2(fd, 2); + + nsh_consolemain(0, NULL); + + close(fd); + + return OK; +} From 8e599e4a3cf1d89c5ce4d1e8f1020fe047e0a55c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 3 Aug 2013 12:39:40 +1000 Subject: [PATCH 323/486] Merged USB ID changes to match bootloader --- nuttx-configs/px4fmu-v2/nsh/defconfig | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 2e73a5a07f..fcc7c7df91 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -652,10 +652,12 @@ CONFIG_USBDEV_MAXPOWER=500 CONFIG_CDCACM=y CONFIG_CDCACM_CONSOLE=y CONFIG_CDCACM_EP0MAXPACKET=64 -CONFIG_CDCACM_EPINTIN=1 +# the endpoint addresses are chosen to match the +# bootloader for the FMUv2 +CONFIG_CDCACM_EPINTIN=3 CONFIG_CDCACM_EPINTIN_FSSIZE=64 CONFIG_CDCACM_EPINTIN_HSSIZE=64 -CONFIG_CDCACM_EPBULKOUT=3 +CONFIG_CDCACM_EPBULKOUT=1 CONFIG_CDCACM_EPBULKOUT_FSSIZE=64 CONFIG_CDCACM_EPBULKOUT_HSSIZE=512 CONFIG_CDCACM_EPBULKIN=2 @@ -664,12 +666,15 @@ CONFIG_CDCACM_EPBULKIN_HSSIZE=512 CONFIG_CDCACM_NWRREQS=4 CONFIG_CDCACM_NRDREQS=4 CONFIG_CDCACM_BULKIN_REQLEN=96 -CONFIG_CDCACM_RXBUFSIZE=256 -CONFIG_CDCACM_TXBUFSIZE=256 +CONFIG_CDCACM_RXBUFSIZE=512 +CONFIG_CDCACM_TXBUFSIZE=512 +# these IDs are chosen to match the bootloader, to prevent +# windows from seeing it as a new device when switching from +# bootloader mode to flight mode CONFIG_CDCACM_VENDORID=0x26ac -CONFIG_CDCACM_PRODUCTID=0x0010 +CONFIG_CDCACM_PRODUCTID=0x0016 CONFIG_CDCACM_VENDORSTR="3D Robotics" -CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v2.x" +CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v2,0" # CONFIG_USBMSC is not set # CONFIG_USBHOST is not set # CONFIG_WIRELESS is not set From 6cf24ac106688d70bdeda9d13fa252246f599b5a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Aug 2013 00:16:59 +0200 Subject: [PATCH 324/486] Increased comm buf size to better deal with higher-speed MAVLink transfers --- nuttx-configs/px4fmu-v1/nsh/defconfig | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index eb225e22c5..dc884413a0 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -559,8 +559,8 @@ CONFIG_CDCACM_EPBULKIN_FSSIZE=64 CONFIG_CDCACM_EPBULKIN_HSSIZE=512 CONFIG_CDCACM_NWRREQS=4 CONFIG_CDCACM_NRDREQS=4 -CONFIG_CDCACM_RXBUFSIZE=256 -CONFIG_CDCACM_TXBUFSIZE=256 +CONFIG_CDCACM_RXBUFSIZE=512 +CONFIG_CDCACM_TXBUFSIZE=512 CONFIG_CDCACM_VENDORID=0x26ac CONFIG_CDCACM_PRODUCTID=0x0010 CONFIG_CDCACM_VENDORSTR="3D Robotics" From 9499e48a524941d955c1b092c7b4e8ebdc1cc807 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Aug 2013 01:52:22 +0200 Subject: [PATCH 325/486] Fixed setting mag queue depth --- src/drivers/lsm303d/lsm303d.cpp | 28 +++++++++++++++++++++++++++- 1 file changed, 27 insertions(+), 1 deletion(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index d9801f88ff..7a65f644a2 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -811,8 +811,34 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) return SENSOR_POLLRATE_MANUAL; return 1000000 / _call_mag_interval; - case SENSORIOCSQUEUEDEPTH: + + case SENSORIOCSQUEUEDEPTH: { + /* account for sentinel in the ring */ + arg++; + + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 2) || (arg > 100)) + return -EINVAL; + + /* allocate new buffer */ + struct mag_report *buf = new struct mag_report[arg]; + + if (nullptr == buf) + return -ENOMEM; + + /* reset the measurement state machine with the new buffer, free the old */ + stop(); + delete[] _mag_reports; + _num_mag_reports = arg; + _mag_reports = buf; + start(); + + return OK; + } + case SENSORIOCGQUEUEDEPTH: + return _num_mag_reports - 1; + case SENSORIOCRESET: return ioctl(filp, cmd, arg); From fbd5aae8c67ef7695cc31aa3c9d450a3e0ce46cb Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 3 Aug 2013 19:41:37 -0700 Subject: [PATCH 326/486] Revert "Merged USB ID changes to match bootloader" This reverts commit 8e599e4a3cf1d89c5ce4d1e8f1020fe047e0a55c. --- nuttx-configs/px4fmu-v2/nsh/defconfig | 17 ++++++----------- 1 file changed, 6 insertions(+), 11 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index fcc7c7df91..2e73a5a07f 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -652,12 +652,10 @@ CONFIG_USBDEV_MAXPOWER=500 CONFIG_CDCACM=y CONFIG_CDCACM_CONSOLE=y CONFIG_CDCACM_EP0MAXPACKET=64 -# the endpoint addresses are chosen to match the -# bootloader for the FMUv2 -CONFIG_CDCACM_EPINTIN=3 +CONFIG_CDCACM_EPINTIN=1 CONFIG_CDCACM_EPINTIN_FSSIZE=64 CONFIG_CDCACM_EPINTIN_HSSIZE=64 -CONFIG_CDCACM_EPBULKOUT=1 +CONFIG_CDCACM_EPBULKOUT=3 CONFIG_CDCACM_EPBULKOUT_FSSIZE=64 CONFIG_CDCACM_EPBULKOUT_HSSIZE=512 CONFIG_CDCACM_EPBULKIN=2 @@ -666,15 +664,12 @@ CONFIG_CDCACM_EPBULKIN_HSSIZE=512 CONFIG_CDCACM_NWRREQS=4 CONFIG_CDCACM_NRDREQS=4 CONFIG_CDCACM_BULKIN_REQLEN=96 -CONFIG_CDCACM_RXBUFSIZE=512 -CONFIG_CDCACM_TXBUFSIZE=512 -# these IDs are chosen to match the bootloader, to prevent -# windows from seeing it as a new device when switching from -# bootloader mode to flight mode +CONFIG_CDCACM_RXBUFSIZE=256 +CONFIG_CDCACM_TXBUFSIZE=256 CONFIG_CDCACM_VENDORID=0x26ac -CONFIG_CDCACM_PRODUCTID=0x0016 +CONFIG_CDCACM_PRODUCTID=0x0010 CONFIG_CDCACM_VENDORSTR="3D Robotics" -CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v2,0" +CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v2.x" # CONFIG_USBMSC is not set # CONFIG_USBHOST is not set # CONFIG_WIRELESS is not set From e931d3b9cda723d63bf352ca866dc499d8df21f5 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 3 Aug 2013 22:35:18 -0700 Subject: [PATCH 327/486] Add an option to the systemreset() call and to the reboot command (-b) to reboot into the bootloader. The system will remain in the bootloader until it's reset, or until an upload is completed. --- src/modules/commander/state_machine_helper.c | 2 +- src/modules/systemlib/systemlib.c | 12 +++++++++++- src/modules/systemlib/systemlib.h | 2 +- src/systemcmds/reboot/reboot.c | 19 ++++++++++++++++++- 4 files changed, 31 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index c264787852..9b6527c330 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -141,7 +141,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con current_status->flag_system_armed = false; mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); usleep(500000); - systemreset(); + systemreset(false); /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ } else { diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c index 96276b56a3..57a751e1c3 100644 --- a/src/modules/systemlib/systemlib.c +++ b/src/modules/systemlib/systemlib.c @@ -50,9 +50,19 @@ #include #include +#include + #include "systemlib.h" -__EXPORT extern void systemreset(void) { +void +systemreset(bool to_bootloader) +{ + if (to_bootloader) { + stm32_pwr_enablebkp(); + + /* XXX wow, this is evil - write a magic number into backup register zero */ + *(uint32_t *)0x40002850 = 0xb007b007; + } up_systemreset(); } diff --git a/src/modules/systemlib/systemlib.h b/src/modules/systemlib/systemlib.h index 77fdfe08af..3728f20672 100644 --- a/src/modules/systemlib/systemlib.h +++ b/src/modules/systemlib/systemlib.h @@ -45,7 +45,7 @@ __BEGIN_DECLS /** Reboots the board */ -__EXPORT void systemreset(void) noreturn_function; +__EXPORT void systemreset(bool to_bootloader) noreturn_function; /** Sends SIGUSR1 to all processes */ __EXPORT void killall(void); diff --git a/src/systemcmds/reboot/reboot.c b/src/systemcmds/reboot/reboot.c index 0fd1e27244..91a2c2eb8d 100644 --- a/src/systemcmds/reboot/reboot.c +++ b/src/systemcmds/reboot/reboot.c @@ -40,14 +40,31 @@ #include #include #include +#include #include +#include __EXPORT int reboot_main(int argc, char *argv[]); int reboot_main(int argc, char *argv[]) { - systemreset(); + int ch; + bool to_bootloader = false; + + while ((ch = getopt(argc, argv, "b")) != -1) { + switch (ch) { + case 'b': + to_bootloader = true; + break; + default: + errx(1, "usage: reboot [-b]\n" + " -b reboot into the bootloader"); + + } + } + + systemreset(to_bootloader); } From 7ddd88623efcdc83eeb93bbb592b936ac75096f0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 4 Aug 2013 15:09:16 +1000 Subject: [PATCH 328/486] mathlib: added LowPassFilter2p object used in gyro and accel drivers for better filtering --- .../mathlib/math/filter/LowPassFilter2p.cpp | 77 ++++++++++++++++++ .../mathlib/math/filter/LowPassFilter2p.hpp | 78 +++++++++++++++++++ src/modules/mathlib/math/filter/module.mk | 44 +++++++++++ 3 files changed, 199 insertions(+) create mode 100644 src/modules/mathlib/math/filter/LowPassFilter2p.cpp create mode 100644 src/modules/mathlib/math/filter/LowPassFilter2p.hpp create mode 100644 src/modules/mathlib/math/filter/module.mk diff --git a/src/modules/mathlib/math/filter/LowPassFilter2p.cpp b/src/modules/mathlib/math/filter/LowPassFilter2p.cpp new file mode 100644 index 0000000000..efb17225d2 --- /dev/null +++ b/src/modules/mathlib/math/filter/LowPassFilter2p.cpp @@ -0,0 +1,77 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/**************************************************************************** + * + * Copyright (C) 2012 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 LowPassFilter.cpp +/// @brief A class to implement a second order low pass filter +/// Author: Leonard Hall + +#include "LowPassFilter2p.hpp" +#include "math.h" + +namespace math +{ + +void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq) +{ + _cutoff_freq = cutoff_freq; + float fr = sample_freq/_cutoff_freq; + float ohm = tanf(M_PI_F/fr); + float c = 1.0f+2.0f*cosf(M_PI_F/4.0f)*ohm + ohm*ohm; + _b0 = ohm*ohm/c; + _b1 = 2.0f*_b0; + _b2 = _b0; + _a1 = 2.0f*(ohm*ohm-1.0f)/c; + _a2 = (1.0f-2.0f*cosf(M_PI_F/4.0f)*ohm+ohm*ohm)/c; +} + +float LowPassFilter2p::apply(float sample) +{ + // do the filtering + float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2; + if (isnan(delay_element_0) || isinf(delay_element_0)) { + // don't allow bad values to propogate via the filter + delay_element_0 = sample; + } + float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2; + + _delay_element_2 = _delay_element_1; + _delay_element_1 = delay_element_0; + + // return the value. Should be no need to check limits + return output; +} + +} // namespace math + diff --git a/src/modules/mathlib/math/filter/LowPassFilter2p.hpp b/src/modules/mathlib/math/filter/LowPassFilter2p.hpp new file mode 100644 index 0000000000..208ec98d4e --- /dev/null +++ b/src/modules/mathlib/math/filter/LowPassFilter2p.hpp @@ -0,0 +1,78 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/**************************************************************************** + * + * Copyright (C) 2012 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 LowPassFilter.h +/// @brief A class to implement a second order low pass filter +/// Author: Leonard Hall +/// Adapted for PX4 by Andrew Tridgell + +#pragma once + +namespace math +{ +class __EXPORT LowPassFilter2p +{ +public: + // constructor + LowPassFilter2p(float sample_freq, float cutoff_freq) { + // set initial parameters + set_cutoff_frequency(sample_freq, cutoff_freq); + _delay_element_1 = _delay_element_2 = 0; + } + + // change parameters + void set_cutoff_frequency(float sample_freq, float cutoff_freq); + + // apply - Add a new raw value to the filter + // and retrieve the filtered result + float apply(float sample); + + // return the cutoff frequency + float get_cutoff_freq(void) const { + return _cutoff_freq; + } + +private: + float _cutoff_freq; + float _a1; + float _a2; + float _b0; + float _b1; + float _b2; + float _delay_element_1; // buffered sample -1 + float _delay_element_2; // buffered sample -2 +}; + +} // namespace math diff --git a/src/modules/mathlib/math/filter/module.mk b/src/modules/mathlib/math/filter/module.mk new file mode 100644 index 0000000000..fe92c8c70f --- /dev/null +++ b/src/modules/mathlib/math/filter/module.mk @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# filter library +# +SRCS = LowPassFilter2p.cpp + +# +# In order to include .config we first have to save off the +# current makefile name, since app.mk needs it. +# +APP_MAKEFILE := $(lastword $(MAKEFILE_LIST)) +-include $(TOPDIR)/.config From a87690d0e279bfe273465dc34ad0058bdaabd015 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 4 Aug 2013 15:10:01 +1000 Subject: [PATCH 329/486] Added L3GD20 lowpass --- src/drivers/l3gd20/l3gd20.cpp | 35 +++++++++++++++++++++++++++++++---- 1 file changed, 31 insertions(+), 4 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 3bb9a77648..9b4bda0ae5 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -63,6 +63,7 @@ #include #include +#include /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -188,6 +189,10 @@ private: perf_counter_t _sample_perf; + math::LowPassFilter2p _gyro_filter_x; + math::LowPassFilter2p _gyro_filter_y; + math::LowPassFilter2p _gyro_filter_z; + /** * Start automatic measurement. */ @@ -278,7 +283,10 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : _gyro_topic(-1), _current_rate(0), _current_range(0), - _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")) + _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")), + _gyro_filter_x(250, 30), + _gyro_filter_y(250, 30), + _gyro_filter_z(250, 30) { // enable debug() calls _debug_enabled = true; @@ -441,6 +449,13 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) /* XXX this is a bit shady, but no other way to adjust... */ _call.period = _call_interval = ticks; + // adjust filters + float cutoff_freq_hz = _gyro_filter_x.get_cutoff_freq(); + float sample_rate = 1.0e6f/ticks; + _gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + _gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + _gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + /* if we need to start the poll state machine, do it */ if (want_start) start(); @@ -493,10 +508,17 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) case GYROIOCGSAMPLERATE: return _current_rate; - case GYROIOCSLOWPASS: + case GYROIOCSLOWPASS: { + float cutoff_freq_hz = arg; + float sample_rate = 1.0e6f / _call_interval; + _gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + _gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + _gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + return OK; + } + case GYROIOCGLOWPASS: - /* XXX not implemented due to wacky interaction with samplerate */ - return -EINVAL; + return _gyro_filter_x.get_cutoff_freq(); case GYROIOCSSCALE: /* copy scale in */ @@ -699,6 +721,11 @@ L3GD20::measure() report->x = ((report->x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; report->y = ((report->y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; report->z = ((report->z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; + + report->x = _gyro_filter_x.apply(report->x); + report->y = _gyro_filter_y.apply(report->y); + report->z = _gyro_filter_z.apply(report->z); + report->scaling = _gyro_range_scale; report->range_rad_s = _gyro_range_rad_s; From 686edfefb8b8bb90e630cac166ad06776229f55a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 4 Aug 2013 13:46:30 +1000 Subject: [PATCH 330/486] Removed LSM303D filter --- src/drivers/lsm303d/iirFilter.c | 255 -------------------------------- src/drivers/lsm303d/iirFilter.h | 59 -------- src/drivers/lsm303d/lsm303d.cpp | 22 --- src/drivers/lsm303d/module.mk | 5 +- 4 files changed, 3 insertions(+), 338 deletions(-) delete mode 100644 src/drivers/lsm303d/iirFilter.c delete mode 100644 src/drivers/lsm303d/iirFilter.h diff --git a/src/drivers/lsm303d/iirFilter.c b/src/drivers/lsm303d/iirFilter.c deleted file mode 100644 index 8311f14d6d..0000000000 --- a/src/drivers/lsm303d/iirFilter.c +++ /dev/null @@ -1,255 +0,0 @@ -#include "math.h" -#include "string.h" -#include "iirFilter.h" - -/////////////////////////////////////////////////////////////////////////////// -// Internal function prototypes - -int btZpgcToZpgd(const TF_ZPG_t *pkZpgc, double Ts, TF_ZPG_t *pZpgd); - -int btDifcToZpgd(const TF_DIF_t *pkDifc, double Ts, TF_ZPG_t *pZpgd); - -int tPolydToFil(const TF_POLY_t *pkPolyd, FIL_T *pFilt); - -int tZpgxToPolyx(const TF_ZPG_t *pkZpg, TF_POLY_t *pPoly); - -/////////////////////////////////////////////////////////////////////////////// -// external functions - -int testFunction() -{ - printf("TEST\n"); - return 1; -} - -int initFilter(const TF_DIF_t *pDifc, double Ts, FIL_T *pFilt) -{ - TF_POLY_t polyd; - TF_ZPG_t zpgd; - - memset(pFilt, 0, sizeof(FIL_T)); - - // perform bilinear transform with frequency pre warping - btDifcToZpgd(pDifc, Ts, &zpgd); - - // calculate polynom - tZpgxToPolyx(&zpgd, &polyd); - - // set the filter parameters - tPolydToFil(&polyd, pFilt); - - return 1; -} - -// run filter using inp, return output of the filter -float updateFilter(FIL_T *pFilt, float inp) -{ - float outp = 0; - int idx; // index used for different things - int i; // loop variable - - // Store the input to the input array - idx = pFilt->inpCnt % MAX_LENGTH; - pFilt->inpData[idx] = inp; - - // calculate numData * inpData - for (i = 0; i < pFilt->numLength; i++) - { - // index of inp array - idx = (pFilt->inpCnt + i - pFilt->numLength + 1) % MAX_LENGTH; - outp += pFilt->numData[i] * pFilt->inpData[idx]; - } - - // calculate denData * outData - for (i = 0; i < pFilt->denLength; i++) - { - // index of inp array - idx = (pFilt->inpCnt + i - pFilt->denLength) % MAX_LENGTH; - outp -= pFilt->denData[i] * pFilt->outData[idx]; - } - - // store the ouput data to the output array - idx = pFilt->inpCnt % MAX_LENGTH; - pFilt->outData[idx] = outp; - - pFilt->inpCnt += 1; - - return outp; -} - -/////////////////////////////////////////////////////////////////////////////// -// Internal functions - -int tPolydToFil(const TF_POLY_t *pkPolyd, FIL_T *pFilt) -{ - double gain; - int i; - - if (pkPolyd->Ts < 0) - { - return 0; - } - - // initialize filter to 0 - memset(pFilt, 0, sizeof(FIL_T)); - - gain = pkPolyd->denData[pkPolyd->denLength - 1]; - pFilt->Ts = pkPolyd->Ts; - - pFilt->denLength = pkPolyd->denLength - 1; - pFilt->numLength = pkPolyd->numLength; - - for (i = 0; i < pkPolyd->numLength; i++) - { - pFilt->numData[i] = pkPolyd->numData[i] / gain; - } - - for (i = 0; i < (pkPolyd->denLength - 1); i++) - { - pFilt->denData[i] = pkPolyd->denData[i] / gain; - } -} - -// bilinear transformation of poles and zeros -int btDifcToZpgd(const TF_DIF_t *pkDifc, - double Ts, - TF_ZPG_t *pZpgd) -{ - TF_ZPG_t zpgc; - int totDiff; - int i; - - zpgc.zerosLength = 0; - zpgc.polesLength = 0; - zpgc.gain = pkDifc->gain; - zpgc.Ts = pkDifc->Ts; - - // Total number of differentiators / integerators - // positive diff, negative integ, 0 for no int/diff - totDiff = pkDifc->numDiff - pkDifc->numInt + pkDifc->highpassLength; - - while (zpgc.zerosLength < totDiff) - { - zpgc.zerosData[zpgc.zerosLength] = 0; - zpgc.zerosLength++; - } - while (zpgc.polesLength < -totDiff) - { - zpgc.polesData[zpgc.polesLength] = 0; - zpgc.polesLength++; - } - - // The next step is to calculate the poles - // This has to be done for the highpass and lowpass filters - // As we are using bilinear transformation there will be frequency - // warping which has to be accounted for - for (i = 0; i < pkDifc->lowpassLength; i++) - { - // pre-warping: - double freq = 2.0 / Ts * tan(pkDifc->lowpassData[i] * 2.0 * M_PI * Ts / 2.0); - // calculate pole: - zpgc.polesData[zpgc.polesLength] = -freq; - // adjust gain such that lp has gain = 1 for low frequencies - zpgc.gain *= freq; - zpgc.polesLength++; - } - for (i = 0; i < pkDifc->highpassLength; i++) - { - // pre-warping - double freq = 2.0 / Ts * tan(pkDifc->highpassData[i] * 2.0 * M_PI * Ts / 2.0); - // calculate pole: - zpgc.polesData[zpgc.polesLength] = -freq; - // gain does not have to be adjusted - zpgc.polesLength++; - } - - return btZpgcToZpgd(&zpgc, Ts, pZpgd); -} - -// bilinear transformation of poles and zeros -int btZpgcToZpgd(const TF_ZPG_t *pkZpgc, - double Ts, - TF_ZPG_t *pZpgd) -{ - int i; - - // init digital gain - pZpgd->gain = pkZpgc->gain; - - // transform the poles - pZpgd->polesLength = pkZpgc->polesLength; - for (i = 0; i < pkZpgc->polesLength; i++) - { - pZpgd->polesData[i] = (2.0 / Ts + pkZpgc->polesData[i]) / (2.0 / Ts - pkZpgc->polesData[i]); - pZpgd->gain /= (2.0 / Ts - pkZpgc->polesData[i]); - } - - // transform the zeros - pZpgd->zerosLength = pkZpgc->zerosLength; - for (i = 0; i < pkZpgc->zerosLength; i++) - { - pZpgd->zerosData[i] = (2.0 / Ts + pkZpgc->zerosData[i]) / (2.0 / Ts + pkZpgc->zerosData[i]); - pZpgd->gain *= (2.0 / Ts - pkZpgc->zerosData[i]); - } - - // if we don't have the same number of poles as zeros we have to add - // poles or zeros due to the bilinear transformation - while (pZpgd->zerosLength < pZpgd->polesLength) - { - pZpgd->zerosData[pZpgd->zerosLength] = -1.0; - pZpgd->zerosLength++; - } - while (pZpgd->zerosLength > pZpgd->polesLength) - { - pZpgd->polesData[pZpgd->polesLength] = -1.0; - pZpgd->polesLength++; - } - - pZpgd->Ts = Ts; - - return 1; -} - -// calculate polynom from zero, pole, gain form -int tZpgxToPolyx(const TF_ZPG_t *pkZpg, TF_POLY_t *pPoly) -{ - int i, j; // counter variable - double tmp0, tmp1, gain; - - // copy Ts - pPoly->Ts = pkZpg->Ts; - - // calculate denominator polynom - pPoly->denLength = 1; - pPoly->denData[0] = 1.0; - for (i = 0; i < pkZpg->polesLength; i++) - { - // init temporary variable - tmp0 = 0.0; - // increase the polynom by 1 - pPoly->denData[pPoly->denLength] = 0; - pPoly->denLength++; - for (j = 0; j < pPoly->denLength; j++) - { - tmp1 = pPoly->denData[j]; - pPoly->denData[j] = tmp1 * -pkZpg->polesData[i] + tmp0; - tmp0 = tmp1; - } - } - - // calculate numerator polynom - pPoly->numLength = 1; - pPoly->numData[0] = pkZpg->gain; - for (i = 0; i < pkZpg->zerosLength; i++) - { - tmp0 = 0.0; - pPoly->numData[pPoly->numLength] = 0; - pPoly->numLength++; - for (j = 0; j < pPoly->numLength; j++) - { - tmp1 = pPoly->numData[j]; - pPoly->numData[j] = tmp1 * -pkZpg->zerosData[i] + tmp0; - tmp0 = tmp1; - } - } -} diff --git a/src/drivers/lsm303d/iirFilter.h b/src/drivers/lsm303d/iirFilter.h deleted file mode 100644 index ab4223a8ed..0000000000 --- a/src/drivers/lsm303d/iirFilter.h +++ /dev/null @@ -1,59 +0,0 @@ -#ifndef IIRFILTER__H__ -#define IIRFILTER__H__ - -__BEGIN_DECLS - -#define MAX_LENGTH 4 - -typedef struct FILTER_s -{ - float denData[MAX_LENGTH]; - float numData[MAX_LENGTH]; - int denLength; - int numLength; - float Ts; - float inpData[MAX_LENGTH]; - float outData[MAX_LENGTH]; - unsigned int inpCnt; -} FIL_T; - -typedef struct TF_ZPG_s -{ - int zerosLength; - double zerosData[MAX_LENGTH + 1]; - int polesLength; - double polesData[MAX_LENGTH + 1]; - double gain; - double Ts; -} TF_ZPG_t; - -typedef struct TF_POLY_s -{ - int numLength; - double numData[MAX_LENGTH]; - int denLength; - double denData[MAX_LENGTH]; - double Ts; -} TF_POLY_t; - -typedef struct TF_DIF_s -{ - int numInt; - int numDiff; - int lowpassLength; - int highpassLength; - double lowpassData[MAX_LENGTH]; - int highpassData[MAX_LENGTH]; - double gain; - double Ts; -} TF_DIF_t; - -__EXPORT int testFunction(void); - -__EXPORT float updateFilter(FIL_T *pFilt, float inp); - -__EXPORT int initFilter(const TF_DIF_t *pDifc, double Ts, FIL_T *pFilt); - -__END_DECLS - -#endif diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 7a65f644a2..9ebffcac91 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -65,8 +65,6 @@ #include -#include "iirFilter.h" - /* oddly, ERROR is not defined for c++ */ #ifdef ERROR # undef ERROR @@ -221,10 +219,6 @@ private: unsigned _current_samplerate; -// FIL_T _filter_x; -// FIL_T _filter_y; -// FIL_T _filter_z; - unsigned _num_mag_reports; volatile unsigned _next_mag_report; volatile unsigned _oldest_mag_report; @@ -494,22 +488,6 @@ LSM303D::init() set_antialias_filter_bandwidth(50); /* available bandwidths: 50, 194, 362 or 773 Hz */ set_samplerate(400); /* max sample rate */ - /* initiate filter */ -// TF_DIF_t tf_filter; -// tf_filter.numInt = 0; -// tf_filter.numDiff = 0; -// tf_filter.lowpassLength = 2; -// tf_filter.highpassLength = 0; -// tf_filter.lowpassData[0] = 10; -// tf_filter.lowpassData[1] = 10; -// //tf_filter.highpassData[0] = ; -// tf_filter.gain = 1; -// tf_filter.Ts = 1/_current_samplerate; -// -// initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_x); -// initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_y); -// initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_z); - mag_set_range(4); /* XXX: take highest sensor range of 12GA? */ mag_set_samplerate(100); diff --git a/src/drivers/lsm303d/module.mk b/src/drivers/lsm303d/module.mk index 8fd5679c9f..e40f718c54 100644 --- a/src/drivers/lsm303d/module.mk +++ b/src/drivers/lsm303d/module.mk @@ -3,5 +3,6 @@ # MODULE_COMMAND = lsm303d -SRCS = lsm303d.cpp \ - iirFilter.c +SRCS = lsm303d.cpp + + From 26204496b69740bddfd6f8ddbdd71ee4e755008c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 4 Aug 2013 15:10:42 +1000 Subject: [PATCH 331/486] Merged LSM303D lowpass --- src/drivers/lsm303d/lsm303d.cpp | 53 ++++++++++++++++++++------------- 1 file changed, 32 insertions(+), 21 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 9ebffcac91..56400c8438 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -64,6 +64,7 @@ #include #include +#include /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -232,6 +233,10 @@ private: perf_counter_t _accel_sample_perf; perf_counter_t _mag_sample_perf; + math::LowPassFilter2p _accel_filter_x; + math::LowPassFilter2p _accel_filter_y; + math::LowPassFilter2p _accel_filter_z; + /** * Start automatic measurement. */ @@ -402,7 +407,10 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _mag_range_scale(0.0f), _mag_range_ga(0.0f), _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), - _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")) + _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")), + _accel_filter_x(800, 30), + _accel_filter_y(800, 30), + _accel_filter_z(800, 30) { // enable debug() calls _debug_enabled = true; @@ -446,7 +454,6 @@ LSM303D::init() { int ret = ERROR; int mag_ret; - int fd_mag; /* do SPI init (and probe) first */ if (SPI::init() != OK) @@ -622,7 +629,6 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: - return ioctl(filp, SENSORIOCSPOLLRATE, 1600); case SENSOR_POLLRATE_DEFAULT: @@ -645,6 +651,13 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) /* adjust sample rate of sensor */ set_samplerate(arg); + // adjust filters + float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq(); + float sample_rate = 1.0e6f/ticks; + _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + _accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ _accel_call.period = _call_accel_interval = ticks; @@ -695,15 +708,17 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) /* XXX implement */ return -EINVAL; -// case ACCELIOCSLOWPASS: + case ACCELIOCSLOWPASS: { + float cutoff_freq_hz = arg; + float sample_rate = 1.0e6f / _call_accel_interval; + _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + _accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + return OK; + } + case ACCELIOCGLOWPASS: - - unsigned bandwidth; - - if (OK == get_antialias_filter_bandwidth(bandwidth)) - return bandwidth; - else - return -EINVAL; + return _accel_filter_x.get_cutoff_freq(); case ACCELIOCSSCALE: { @@ -1186,17 +1201,13 @@ LSM303D::measure() accel_report->y_raw = raw_accel_report.y; accel_report->z_raw = raw_accel_report.z; -// float x_in_new = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; -// float y_in_new = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; -// float z_in_new = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; -// -// accel_report->x = updateFilter(&_filter_x, x_in_new); -// accel_report->y = updateFilter(&_filter_y, y_in_new); -// accel_report->z = updateFilter(&_filter_z, z_in_new); + float x_in_new = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + float y_in_new = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + float z_in_new = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; - accel_report->x = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - accel_report->y = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - accel_report->z = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + accel_report->x = _accel_filter_x.apply(x_in_new); + accel_report->y = _accel_filter_y.apply(y_in_new); + accel_report->z = _accel_filter_z.apply(z_in_new); accel_report->scaling = _accel_range_scale; accel_report->range_m_s2 = _accel_range_m_s2; From c84cdf2ff627fd84c901c869a84d17751323eb97 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Aug 2013 16:03:50 +0200 Subject: [PATCH 332/486] Enabled filter in makefile --- makefiles/config_px4fmu-v1_default.mk | 1 + makefiles/config_px4fmu-v2_default.mk | 1 + 2 files changed, 2 insertions(+) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 255093e672..f1cfc45e66 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -94,6 +94,7 @@ MODULES += modules/sdlog2 MODULES += modules/systemlib MODULES += modules/systemlib/mixer MODULES += modules/mathlib +MODULES += modules/mathlib/math/filter MODULES += modules/controllib MODULES += modules/uORB diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 75573e2c27..ae61802c95 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -88,6 +88,7 @@ MODULES += modules/sdlog MODULES += modules/systemlib MODULES += modules/systemlib/mixer MODULES += modules/mathlib +MODULES += modules/mathlib/math/filter MODULES += modules/controllib MODULES += modules/uORB From 83189a85dacbd56c3aba6ef704d9dd25a85d33da Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 2 Aug 2013 20:25:26 +1000 Subject: [PATCH 333/486] l3gd20: disable the FIFO the FIFO was not gaining us anything, and was adding latency. If we use the FIFO we'd need to do multiple SPI transfers to ensure it is drained --- src/drivers/l3gd20/l3gd20.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 9b4bda0ae5..ce381807f5 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -342,7 +342,11 @@ L3GD20::init() write_reg(ADDR_CTRL_REG5, 0); write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */ - write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_STREAM_MODE); /* Enable FIFO, old data is overwritten */ + + /* disable FIFO. This makes things simpler and ensures we + * aren't getting stale data. It means we must run the hrt + * callback fast enough to not miss data. */ + write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE); set_range(2000); /* default to 2000dps */ set_samplerate(0); /* max sample rate */ From 17da1e3f363b0ca79250a2f34588558ceb0146c9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Aug 2013 19:19:48 +0200 Subject: [PATCH 334/486] Fixed MS5611 startup on V1 boards --- src/drivers/ms5611/ms5611_i2c.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/ms5611/ms5611_i2c.cpp b/src/drivers/ms5611/ms5611_i2c.cpp index 06867cc9df..87d9b94a6e 100644 --- a/src/drivers/ms5611/ms5611_i2c.cpp +++ b/src/drivers/ms5611/ms5611_i2c.cpp @@ -54,6 +54,8 @@ #include "ms5611.h" +#include "board_config.h" + #ifdef PX4_I2C_OBDEV_MS5611 #ifndef PX4_I2C_BUS_ONBOARD From c2ee4437e0fd362ed8d73203394d34802e9eb48d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Aug 2013 19:20:55 +0200 Subject: [PATCH 335/486] Rate limit sensors, as the in-sensor lowpass allows us to operate at 200-250 Hz --- src/modules/sensors/sensors.cpp | 35 ++++++++++++++++++++++++++++++++- 1 file changed, 34 insertions(+), 1 deletion(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index d9185891b7..0f1782fca6 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -758,12 +758,27 @@ Sensors::accel_init() errx(1, "FATAL: no accelerometer found"); } else { + + // XXX do the check more elegantly + + #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + /* set the accel internal sampling rate up to at leat 500Hz */ ioctl(fd, ACCELIOCSSAMPLERATE, 500); /* set the driver to poll at 500Hz */ ioctl(fd, SENSORIOCSPOLLRATE, 500); + #else + + /* set the accel internal sampling rate up to at leat 800Hz */ + ioctl(fd, ACCELIOCSSAMPLERATE, 800); + + /* set the driver to poll at 800Hz */ + ioctl(fd, SENSORIOCSPOLLRATE, 800); + + #endif + warnx("using system accel"); close(fd); } @@ -781,12 +796,27 @@ Sensors::gyro_init() errx(1, "FATAL: no gyro found"); } else { + + // XXX do the check more elegantly + + #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + /* set the gyro internal sampling rate up to at leat 500Hz */ ioctl(fd, GYROIOCSSAMPLERATE, 500); /* set the driver to poll at 500Hz */ ioctl(fd, SENSORIOCSPOLLRATE, 500); + #else + + /* set the gyro internal sampling rate up to at leat 800Hz */ + ioctl(fd, GYROIOCSSAMPLERATE, 800); + + /* set the driver to poll at 800Hz */ + ioctl(fd, SENSORIOCSPOLLRATE, 800); + + #endif + warnx("using system gyro"); close(fd); } @@ -1387,6 +1417,9 @@ Sensors::task_main() /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vstatus_sub, 200); + /* rate limit gyro to 250 Hz (the gyro signal is lowpassed accordingly earlier) */ + orb_set_interval(_gyro_sub, 4); + /* * do advertisements */ @@ -1422,7 +1455,7 @@ Sensors::task_main() while (!_task_should_exit) { - /* wait for up to 500ms for data */ + /* wait for up to 100ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit, etc. */ From 8b4413f3b1309deff41ff98c457d2e9abe7817d1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Aug 2013 19:09:10 +0200 Subject: [PATCH 336/486] Hotfix for attitude estimator EKF init --- .../attitude_estimator_ekf_main.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 1eff60e88f..9e533ccdfb 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -224,8 +224,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* subscribe to raw data */ int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); - /* rate-limit raw data updates to 200Hz */ - orb_set_interval(sub_raw, 4); + /* rate-limit raw data updates to 333 Hz (sensors app publishes at 200, so this is just paranoid) */ + orb_set_interval(sub_raw, 3); /* subscribe to param changes */ int sub_params = orb_subscribe(ORB_ID(parameter_update)); @@ -236,7 +236,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* advertise attitude */ orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); - int loopcounter = 0; int printcounter = 0; @@ -384,7 +383,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds static bool const_initialized = false; /* initialize with good values once we have a reasonable dt estimate */ - if (!const_initialized && dt < 0.05f && dt > 0.005f) { + if (!const_initialized && dt < 0.05f && dt > 0.001f) { dt = 0.005f; parameters_update(&ekf_param_handles, &ekf_params); @@ -424,7 +423,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds continue; } - if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data); + if (last_data > 0 && raw.timestamp - last_data > 12000) + printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data); last_data = raw.timestamp; From b911d37975244b9e7a5825d8fbff6cc913dc3050 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 4 Aug 2013 19:37:08 -0700 Subject: [PATCH 337/486] Merge a path definition from the mainline. --- makefiles/setup.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/makefiles/setup.mk b/makefiles/setup.mk index fdb1612014..168e41a5cb 100644 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -45,6 +45,7 @@ export PX4_INCLUDE_DIR = $(abspath $(PX4_BASE)/src/include)/ export PX4_MODULE_SRC = $(abspath $(PX4_BASE)/src)/ export PX4_MK_DIR = $(abspath $(PX4_BASE)/makefiles)/ export NUTTX_SRC = $(abspath $(PX4_BASE)/NuttX/nuttx)/ +export NUTTX_APP_SRC = $(abspath $(PX4_BASE)/NuttX/apps)/ export MAVLINK_SRC = $(abspath $(PX4_BASE)/mavlink)/ export ROMFS_SRC = $(abspath $(PX4_BASE)/ROMFS)/ export IMAGE_DIR = $(abspath $(PX4_BASE)/Images)/ From 58a4c5d5449eeffa0c296cbf5c13058d7412e76b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 5 Aug 2013 16:32:39 +0200 Subject: [PATCH 338/486] Added missing include for MS5611 --- src/drivers/ms5611/ms5611_spi.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 1ea6989224..f6c6243408 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -53,6 +53,7 @@ #include #include "ms5611.h" +#include "board_config.h" /* SPI protocol address bits */ #define DIR_READ (1<<7) From ca9a11a586e8c4994e8be28e1b0e23c3f0d341ed Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 5 Aug 2013 23:09:02 +0200 Subject: [PATCH 339/486] Indendation fixes --- src/drivers/l3gd20/l3gd20.cpp | 6 +++--- src/drivers/lsm303d/lsm303d.cpp | 14 +++++++------- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index ce381807f5..fde201351e 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -189,9 +189,9 @@ private: perf_counter_t _sample_perf; - math::LowPassFilter2p _gyro_filter_x; - math::LowPassFilter2p _gyro_filter_y; - math::LowPassFilter2p _gyro_filter_z; + math::LowPassFilter2p _gyro_filter_x; + math::LowPassFilter2p _gyro_filter_y; + math::LowPassFilter2p _gyro_filter_z; /** * Start automatic measurement. diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 56400c8438..a95b62468f 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -233,9 +233,9 @@ private: perf_counter_t _accel_sample_perf; perf_counter_t _mag_sample_perf; - math::LowPassFilter2p _accel_filter_x; - math::LowPassFilter2p _accel_filter_y; - math::LowPassFilter2p _accel_filter_z; + math::LowPassFilter2p _accel_filter_x; + math::LowPassFilter2p _accel_filter_y; + math::LowPassFilter2p _accel_filter_z; /** * Start automatic measurement. @@ -407,10 +407,10 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _mag_range_scale(0.0f), _mag_range_ga(0.0f), _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), - _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")), - _accel_filter_x(800, 30), - _accel_filter_y(800, 30), - _accel_filter_z(800, 30) + _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")), + _accel_filter_x(800, 30), + _accel_filter_y(800, 30), + _accel_filter_z(800, 30) { // enable debug() calls _debug_enabled = true; From ec2e02d50ea2f518051b50e4e83e12736526fbc2 Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 5 Aug 2013 21:05:53 -0700 Subject: [PATCH 340/486] Fix CAN2 pinout selection thanks to heads-up from Joe van Niekerk --- nuttx-configs/px4fmu-v1/include/board.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/include/board.h b/nuttx-configs/px4fmu-v1/include/board.h index 1921f7715b..27ace4b7db 100644 --- a/nuttx-configs/px4fmu-v1/include/board.h +++ b/nuttx-configs/px4fmu-v1/include/board.h @@ -216,8 +216,8 @@ * CAN2 is routed to the expansion connector. */ -#define GPIO_CAN2_RX GPIO_CAN2_RX_2 -#define GPIO_CAN2_TX GPIO_CAN2_TX_2 +#define GPIO_CAN2_RX GPIO_CAN2_RX_1 +#define GPIO_CAN2_TX GPIO_CAN2_TX_1 /* * I2C From 46fd904b5a6002f2fea0495db88c44f3c6a6ee38 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 6 Aug 2013 10:30:32 +1000 Subject: [PATCH 341/486] px4io: include board_config.h without this we don't get the I2C interface built for PX4IO --- src/drivers/px4io/px4io_i2c.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io_i2c.cpp b/src/drivers/px4io/px4io_i2c.cpp index e0c3e56083..19776c40a2 100644 --- a/src/drivers/px4io/px4io_i2c.cpp +++ b/src/drivers/px4io/px4io_i2c.cpp @@ -49,6 +49,7 @@ #include #include +#include #include @@ -165,4 +166,4 @@ PX4IO_I2C::read(unsigned address, void *data, unsigned count) return ret; } -#endif /* PX4_I2C_OBDEV_PX4IO */ \ No newline at end of file +#endif /* PX4_I2C_OBDEV_PX4IO */ From 40c56ab61e04fe73aff3a84d20ffc81e102373f3 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 6 Aug 2013 21:10:41 +0200 Subject: [PATCH 342/486] Corrected bug in px4io driver that lead to hang of FMU-IO communication --- src/drivers/px4io/px4io.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 827b0bb009..bc3f1dcc68 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1008,8 +1008,6 @@ PX4IO::io_handle_status(uint16_t status) struct safety_s safety; safety.timestamp = hrt_absolute_time(); - orb_copy(ORB_ID(safety), _to_safety, &safety); - if (status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { safety.safety_off = true; safety.safety_switch_available = true; From 32439d748ad169f6f9956fb3248535730e0374a4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 7 Aug 2013 20:21:49 +0200 Subject: [PATCH 343/486] commander: set mode using base_mode and custom_mode --- src/modules/commander/commander.cpp | 58 ++++++++++++++++++++--------- 1 file changed, 41 insertions(+), 17 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2012abcc09..7ede3e1e64 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -126,10 +126,6 @@ extern struct system_load_s system_load; #define PRINT_INTERVAL 5000000 #define PRINT_MODE_REJECT_INTERVAL 2000000 -// TODO include mavlink headers -/** @brief These flags encode the MAV mode. */ -#ifndef HAVE_ENUM_MAV_MODE_FLAG -#define HAVE_ENUM_MAV_MODE_FLAG enum MAV_MODE_FLAG { MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */ MAV_MODE_FLAG_TEST_ENABLED = 2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */ @@ -141,7 +137,13 @@ enum MAV_MODE_FLAG { MAV_MODE_FLAG_SAFETY_ARMED = 128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. | */ MAV_MODE_FLAG_ENUM_END = 129, /* | */ }; -#endif + +enum PX4_CUSTOM_MODE { + PX4_CUSTOM_MODE_MANUAL = 1, + PX4_CUSTOM_MODE_SEATBELT, + PX4_CUSTOM_MODE_EASY, + PX4_CUSTOM_MODE_AUTO, +}; /* Mavlink file descriptors */ static int mavlink_fd; @@ -277,8 +279,9 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode /* request to set different system mode */ switch (cmd->command) { case VEHICLE_CMD_DO_SET_MODE: { - uint8_t base_mode = (int) cmd->param1; - uint8_t custom_mode = (int) cmd->param2; + uint8_t base_mode = (uint8_t) cmd->param1; + uint32_t custom_mode = (uint32_t) cmd->param2; + // TODO remove debug code mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_mode); /* set arming state */ @@ -286,6 +289,7 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { arming_res = arming_state_transition(status, ARMING_STATE_ARMED, armed); + if (arming_res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_fd, "[cmd] ARMED by command"); } @@ -294,9 +298,11 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) { arming_state_t new_arming_state = (status->arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); arming_res = arming_state_transition(status, new_arming_state, armed); + if (arming_res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command"); } + } else { arming_res = TRANSITION_NOT_CHANGED; } @@ -305,21 +311,37 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode /* set main state */ transition_result_t main_res = TRANSITION_DENIED; - if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { - /* AUTO */ - main_res = main_state_transition(status, MAIN_STATE_AUTO); + if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { + /* use autopilot-specific mode */ + if (custom_mode == PX4_CUSTOM_MODE_MANUAL) { + /* MANUAL */ + main_res = main_state_transition(status, MAIN_STATE_MANUAL); - } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { - if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { + } else if (custom_mode == PX4_CUSTOM_MODE_SEATBELT) { + /* SEATBELT */ + main_res = main_state_transition(status, MAIN_STATE_SEATBELT); + + } else if (custom_mode == PX4_CUSTOM_MODE_EASY) { /* EASY */ main_res = main_state_transition(status, MAIN_STATE_EASY); - } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { - if (custom_mode & 1) { // TODO add custom mode flags definition - /* SEATBELT */ - main_res = main_state_transition(status, MAIN_STATE_SEATBELT); + } else if (custom_mode == PX4_CUSTOM_MODE_AUTO) { + /* AUTO */ + main_res = main_state_transition(status, MAIN_STATE_AUTO); + } - } else { + } else { + /* use base mode */ + if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { + /* AUTO */ + main_res = main_state_transition(status, MAIN_STATE_AUTO); + + } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { + if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { + /* EASY */ + main_res = main_state_transition(status, MAIN_STATE_EASY); + + } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { /* MANUAL */ main_res = main_state_transition(status, MAIN_STATE_MANUAL); } @@ -1534,6 +1556,7 @@ void *commander_low_prio_loop(void *arg) mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); tune_error(); } + break; case LOW_PRIO_TASK_PARAM_SAVE: @@ -1545,6 +1568,7 @@ void *commander_low_prio_loop(void *arg) mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); tune_error(); } + break; case LOW_PRIO_TASK_GYRO_CALIBRATION: From 687273ae6fe65fd006492844509a6cd1e25115b6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 6 Aug 2013 10:30:32 +1000 Subject: [PATCH 344/486] Enable the dedicated interrupt stack for both v1 and v2 boards. This will help save us from threads that are under-provisioned stack-wise. --- nuttx-configs/px4fmu-v1/nsh/defconfig | 2 +- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index 129d921492..bcf456c560 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -328,7 +328,7 @@ CONFIG_BOARD_LOOPSPERMSEC=16717 CONFIG_DRAM_START=0x20000000 CONFIG_DRAM_SIZE=196608 CONFIG_ARCH_HAVE_INTERRUPTSTACK=y -CONFIG_ARCH_INTERRUPTSTACK=0 +CONFIG_ARCH_INTERRUPTSTACK=2048 # # Boot options diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 2e73a5a07f..ebbbd8f469 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -380,7 +380,7 @@ CONFIG_BOARD_LOOPSPERMSEC=16717 CONFIG_DRAM_START=0x20000000 CONFIG_DRAM_SIZE=262144 CONFIG_ARCH_HAVE_INTERRUPTSTACK=y -CONFIG_ARCH_INTERRUPTSTACK=0 +CONFIG_ARCH_INTERRUPTSTACK=2048 # # Boot options From a0235bd5071bc8488c585588241422128b1d3361 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 9 Aug 2013 08:36:18 +0200 Subject: [PATCH 345/486] Increased buffer sizes for telemetry, set USB PID correctly according to new scheme --- nuttx-configs/px4fmu-v2/nsh/defconfig | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index ebbbd8f469..6e8e239bc1 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -664,10 +664,10 @@ CONFIG_CDCACM_EPBULKIN_HSSIZE=512 CONFIG_CDCACM_NWRREQS=4 CONFIG_CDCACM_NRDREQS=4 CONFIG_CDCACM_BULKIN_REQLEN=96 -CONFIG_CDCACM_RXBUFSIZE=256 -CONFIG_CDCACM_TXBUFSIZE=256 +CONFIG_CDCACM_RXBUFSIZE=512 +CONFIG_CDCACM_TXBUFSIZE=512 CONFIG_CDCACM_VENDORID=0x26ac -CONFIG_CDCACM_PRODUCTID=0x0010 +CONFIG_CDCACM_PRODUCTID=0x0011 CONFIG_CDCACM_VENDORSTR="3D Robotics" CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v2.x" # CONFIG_USBMSC is not set From 91e54aa5be9930441c85c0585494f8ac8f514681 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 10 Aug 2013 15:51:00 +1000 Subject: [PATCH 346/486] add .gdbinit as Debug/dot.gdbinit very useful for JTAG debugging --- Debug/dot.gdbinit | 13 +++++++++++++ 1 file changed, 13 insertions(+) create mode 100644 Debug/dot.gdbinit diff --git a/Debug/dot.gdbinit b/Debug/dot.gdbinit new file mode 100644 index 0000000000..d70410bc78 --- /dev/null +++ b/Debug/dot.gdbinit @@ -0,0 +1,13 @@ +# copy the file to .gdbinit in your Firmware tree, and adjust the path +# below to match your system +# For example: +# target extended /dev/serial/by-id/usb-Black_Sphere_Technologies_Black_Magic_Probe_DDE5A1C4-if00 +# target extended /dev/ttyACM4 + + +monitor swdp_scan +attach 1 +monitor vector_catch disable hard +set mem inaccessible-by-default off +set print pretty +source Debug/PX4 From 910cf4e5ba20bfa1a18f5bde67c65e83a352f9f7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 10 Aug 2013 15:50:26 +1000 Subject: [PATCH 347/486] dot.gdbinit should not be ignored --- .gitignore | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/.gitignore b/.gitignore index 5a4f7683cf..3c825ca642 100644 --- a/.gitignore +++ b/.gitignore @@ -19,7 +19,6 @@ Archives/* Build/* core cscope.out -dot.gdbinit Firmware.sublime-workspace Images/*.bin Images/*.px4 @@ -30,4 +29,4 @@ nuttx-configs/px4io-v2/src/Make.dep nuttx-configs/px4io-v2/src/libboard.a nuttx-configs/px4io-v1/src/.depend nuttx-configs/px4io-v1/src/Make.dep -nuttx-configs/px4io-v1/src/libboard.a \ No newline at end of file +nuttx-configs/px4io-v1/src/libboard.a From cbb5ce8f59a34615fe0d702041c497efe40edb56 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 11 Aug 2013 16:54:00 +0200 Subject: [PATCH 348/486] Fixed startup behavior for PX4 autostart --- ROMFS/px4fmu_common/init.d/10_io_f330 | 10 ++++------ ROMFS/px4fmu_common/init.d/rc.sensors | 3 --- ROMFS/px4fmu_common/init.d/rc.usb | 14 ++++++++++++++ src/drivers/l3gd20/l3gd20.cpp | 2 +- src/drivers/lsm303d/lsm303d.cpp | 2 +- 5 files changed, 20 insertions(+), 11 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_io_f330 index 4450eb50db..4083bb905d 100644 --- a/ROMFS/px4fmu_common/init.d/10_io_f330 +++ b/ROMFS/px4fmu_common/init.d/10_io_f330 @@ -75,6 +75,8 @@ px4io min 1200 1200 1200 1200 # Upper limits could be higher, this is on the safe side # px4io max 1800 1800 1800 1800 + +mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix # # Start the sensors (depends on orb, px4io) @@ -95,17 +97,13 @@ gps start # Start the attitude estimator (depends on orb) # attitude_estimator_ekf start - -# -# Load mixer and start controllers (depends on px4io) -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix + multirotor_att_control start # # Start logging # -sdlog2 start -r 20 -a -b 16 +sdlog2 start -r 50 -a -b 16 # # Start system state diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 5e80ddc2f4..26b561571b 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -36,8 +36,5 @@ fi # if sensors start then - # - # Check sensors - run AFTER 'sensors start' - # preflight_check & fi diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 5b1bd272e9..abeed0ca3c 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -16,12 +16,26 @@ fi sleep 2 mavlink start -b 230400 -d /dev/ttyACM0 +# Stop commander +if commander stop +then + echo "Commander stopped" +fi +sleep 1 + # Start the commander if commander start then echo "Commander started" fi +# Stop px4io +if px4io stop +then + echo "PX4IO stopped" +fi +sleep 1 + # Start px4io if present if px4io start then diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index fde201351e..42a0c264c4 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -780,7 +780,7 @@ start() int fd; if (g_dev != nullptr) - errx(1, "already started"); + errx(0, "already started"); /* create the driver */ g_dev = new L3GD20(1 /* XXX magic number */, GYRO_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO); diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index a95b62468f..117583fafe 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1370,7 +1370,7 @@ start() int fd, fd_mag; if (g_dev != nullptr) - errx(1, "already started"); + errx(0, "already started"); /* create the driver */ g_dev = new LSM303D(1 /* XXX magic number */, ACCEL_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); From 42b496178136a398447742f0efc81348845087e4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 11 Aug 2013 16:54:24 +0200 Subject: [PATCH 349/486] Reduced excessive IO stack size (had 4k, is using 0.7k, has now 2k) --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 281debe5c0..1122195d4f 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -558,7 +558,7 @@ PX4IO::init() } /* start the IO interface task */ - _task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr); + _task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 2048, (main_t)&PX4IO::task_main_trampoline, nullptr); if (_task < 0) { debug("task start failed: %d", errno); From 66d294b5bf3972c1f49ea452964f32e18b25b2d3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 11 Aug 2013 17:39:10 +0200 Subject: [PATCH 350/486] Fixed to FMUv2 autostart and config --- ROMFS/px4fmu_common/init.d/10_io_f330 | 30 ++++++++++++++------------- ROMFS/px4fmu_common/init.d/rc.sensors | 2 ++ makefiles/config_px4fmu-v2_default.mk | 2 +- 3 files changed, 19 insertions(+), 15 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_io_f330 index 4083bb905d..13272426e8 100644 --- a/ROMFS/px4fmu_common/init.d/10_io_f330 +++ b/ROMFS/px4fmu_common/init.d/10_io_f330 @@ -56,11 +56,6 @@ pwm -u 400 -m 0xff # this is very unlikely, but quite safe and robust. px4io recovery -# -# Disable px4io topic limiting -# -px4io limit 200 - # # This sets a PWM right after startup (regardless of safety button) # @@ -99,16 +94,23 @@ gps start attitude_estimator_ekf start multirotor_att_control start - + # -# Start logging +# Disable px4io topic limiting and start logging # -sdlog2 start -r 50 -a -b 16 - -# -# Start system state -# -if blinkm start +if [ $BOARD == fmuv1 ] then - blinkm systemstate + px4io limit 200 + sdlog2 start -r 50 -a -b 16 + if blinkm start + then + blinkm systemstate + fi +else + px4io limit 400 + sdlog2 start -r 100 -a -b 16 + if rgbled start + then + #rgbled systemstate + fi fi diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 26b561571b..17591be5b3 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -22,10 +22,12 @@ if mpu6000 start then echo "using MPU6000 and HMC5883L" hmc5883 start + set BOARD fmuv1 else echo "using L3GD20 and LSM303D" l3gd20 start lsm303d start + set BOARD fmuv2 fi # diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index ae61802c95..cc182e6af5 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -80,7 +80,7 @@ MODULES += modules/multirotor_pos_control # # Logging # -MODULES += modules/sdlog +MODULES += modules/sdlog2 # # Library modules From 083cc60acb8b602864d0727e09218eeeca5eb980 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 11 Aug 2013 18:42:20 +0200 Subject: [PATCH 351/486] Increased logging to 200 Hz in F330 startup for v2, allowed to set up to 333 Hz update rate in IO driver for v2 link --- ROMFS/px4fmu_common/init.d/10_io_f330 | 2 +- src/drivers/px4io/px4io.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_io_f330 index 13272426e8..b3fb027573 100644 --- a/ROMFS/px4fmu_common/init.d/10_io_f330 +++ b/ROMFS/px4fmu_common/init.d/10_io_f330 @@ -108,7 +108,7 @@ then fi else px4io limit 400 - sdlog2 start -r 100 -a -b 16 + sdlog2 start -r 200 -a -b 16 if rgbled start then #rgbled systemstate diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 1122195d4f..ae5d6ce196 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1628,8 +1628,8 @@ int PX4IO::set_update_rate(int rate) { int interval_ms = 1000 / rate; - if (interval_ms < 5) { - interval_ms = 5; + if (interval_ms < 3) { + interval_ms = 3; warnx("update rate too high, limiting interval to %d ms (%d Hz).", interval_ms, 1000 / interval_ms); } @@ -1956,7 +1956,7 @@ px4io_main(int argc, char *argv[]) if ((argc > 2)) { g_dev->set_update_rate(atoi(argv[2])); } else { - errx(1, "missing argument (50 - 200 Hz)"); + errx(1, "missing argument (50 - 400 Hz)"); return 1; } exit(0); From f36a2ff45a9f24637ea360cdadd5f168c5f50946 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 10 Aug 2013 12:39:58 -0700 Subject: [PATCH 352/486] Add a 'menuconfig' target that makes it possible to use the NuttX menuconfig tool on the PX4 config files. --- Makefile | 28 +++++++++++++++++++++++++++- 1 file changed, 27 insertions(+), 1 deletion(-) diff --git a/Makefile b/Makefile index 82a5ffd27c..9ef30c2df7 100644 --- a/Makefile +++ b/Makefile @@ -148,7 +148,7 @@ $(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) @$(ECHO) %% Configuring NuttX for $(board) $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) $(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean - $(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)/nuttx-configs/$(board) .) + $(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(board) .) $(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(board)/$(configuration)) @$(ECHO) %% Exporting NuttX for $(board) $(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) export @@ -156,6 +156,32 @@ $(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) $(Q) $(COPY) $(NUTTX_SRC)nuttx-export.zip $@ $(Q) (cd $(NUTTX_SRC)/configs && $(RMDIR) $(board)) +# +# The user can run the NuttX 'menuconfig' tool for a single board configuration with +# make BOARDS= menuconfig +# +ifeq ($(MAKECMDGOALS),menuconfig) +ifneq ($(words $(BOARDS)),1) +$(error BOARDS must specify exactly one board for the menuconfig goal) +endif +BOARD = $(BOARDS) +menuconfig: $(NUTTX_SRC) + @$(ECHO) %% Configuring NuttX for $(BOARD) + $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) + $(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean + $(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(BOARD) .) + $(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(BOARD)/nsh) + @$(ECHO) %% Running menuconfig for $(BOARD) + $(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) menuconfig + @$(ECHO) %% Saving configuration file + $(Q)$(COPY) $(NUTTX_SRC).config $(PX4_BASE)nuttx-configs/$(BOARD)/nsh/defconfig +else +menuconfig: + @$(ECHO) "" + @$(ECHO) "The menuconfig goal must be invoked without any other goal being specified" + @$(ECHO) "" +endif + $(NUTTX_SRC): @$(ECHO) "" @$(ECHO) "NuttX sources missing - clone https://github.com/PX4/NuttX.git and try again." From 70f272bd22e9ccdb9dbc1c15dd76fce4449ea0ab Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 12 Aug 2013 13:44:11 +0200 Subject: [PATCH 353/486] Disabled SDIO DMA, enabled CCM memory --- nuttx-configs/px4fmu-v2/nsh/defconfig | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 6e8e239bc1..da40d18b0b 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -95,7 +95,7 @@ CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y CONFIG_ARMV7M_STACKCHECK=y CONFIG_SERIAL_TERMIOS=y CONFIG_SERIAL_DISABLE_REORDERING=y -CONFIG_SDIO_DMA=y +CONFIG_SDIO_DMA=n # CONFIG_SDIO_DMAPRIO is not set # CONFIG_SDIO_WIDTH_D1_ONLY is not set @@ -257,7 +257,7 @@ CONFIG_STM32_JTAG_SW_ENABLE=y CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y # CONFIG_STM32_FORCEPOWER is not set # CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set -CONFIG_STM32_CCMEXCLUDE=y +CONFIG_STM32_CCMEXCLUDE=n CONFIG_STM32_DMACAPABLE=y # CONFIG_STM32_TIM1_PWM is not set # CONFIG_STM32_TIM3_PWM is not set From e1037e20bea85834d7080a08b5a99d3dec5c0d41 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 12 Aug 2013 13:57:33 +0200 Subject: [PATCH 354/486] Fixed inconsistend defconfig - switching to menuconfig ASAP --- nuttx-configs/px4fmu-v2/nsh/defconfig | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index da40d18b0b..886fdcb969 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -95,9 +95,6 @@ CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y CONFIG_ARMV7M_STACKCHECK=y CONFIG_SERIAL_TERMIOS=y CONFIG_SERIAL_DISABLE_REORDERING=y -CONFIG_SDIO_DMA=n -# CONFIG_SDIO_DMAPRIO is not set -# CONFIG_SDIO_WIDTH_D1_ONLY is not set # # STM32 Configuration Options @@ -339,7 +336,9 @@ CONFIG_MTD=y # # STM32 SDIO-based MMC/SD driver # -CONFIG_SDIO_DMA=y +CONFIG_SDIO_DMA=n +# CONFIG_SDIO_DMAPRIO is not set +# CONFIG_SDIO_WIDTH_D1_ONLY is not set CONFIG_MMCSD_MULTIBLOCK_DISABLE=y CONFIG_MMCSD_MMCSUPPORT=n CONFIG_MMCSD_HAVECARDDETECT=n From 92fc6a05c3c5fb4b498a300b0d3190d7c2b14926 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 12 Aug 2013 14:07:42 +0200 Subject: [PATCH 355/486] Putting SDIO back to DMA and disabling CCM again --- nuttx-configs/px4fmu-v2/nsh/defconfig | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 886fdcb969..9454d116a7 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -254,7 +254,7 @@ CONFIG_STM32_JTAG_SW_ENABLE=y CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y # CONFIG_STM32_FORCEPOWER is not set # CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set -CONFIG_STM32_CCMEXCLUDE=n +CONFIG_STM32_CCMEXCLUDE=y CONFIG_STM32_DMACAPABLE=y # CONFIG_STM32_TIM1_PWM is not set # CONFIG_STM32_TIM3_PWM is not set @@ -336,7 +336,7 @@ CONFIG_MTD=y # # STM32 SDIO-based MMC/SD driver # -CONFIG_SDIO_DMA=n +CONFIG_SDIO_DMA=y # CONFIG_SDIO_DMAPRIO is not set # CONFIG_SDIO_WIDTH_D1_ONLY is not set CONFIG_MMCSD_MULTIBLOCK_DISABLE=y From d7730a3444b1c4277bca24988402839a98a52fdc Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 14 Aug 2013 10:59:22 +0200 Subject: [PATCH 356/486] commander, mavlink: fixed base_mode and custom_mode in mavlink --- src/modules/commander/commander.cpp | 14 +++----- src/modules/mavlink/mavlink.c | 52 ++++++++++++++--------------- src/modules/mavlink/orb_listener.c | 22 ++++++------ src/modules/mavlink/util.h | 2 +- 4 files changed, 43 insertions(+), 47 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7ede3e1e64..82b5754054 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -84,6 +84,7 @@ #include #include +#include "px4_custom_mode.h" #include "commander_helper.h" #include "state_machine_helper.h" #include "calibration_routines.h" @@ -138,13 +139,6 @@ enum MAV_MODE_FLAG { MAV_MODE_FLAG_ENUM_END = 129, /* | */ }; -enum PX4_CUSTOM_MODE { - PX4_CUSTOM_MODE_MANUAL = 1, - PX4_CUSTOM_MODE_SEATBELT, - PX4_CUSTOM_MODE_EASY, - PX4_CUSTOM_MODE_AUTO, -}; - /* Mavlink file descriptors */ static int mavlink_fd; @@ -1321,8 +1315,10 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gp } else if (armed->ready_to_arm) { /* ready to arm, blink at 2.5Hz */ - if (leds_counter % 8 == 0) { - led_toggle(LED_AMBER); + if (leds_counter & 8) { + led_on(LED_AMBER); + } else { + led_off(LED_AMBER); } } else { diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 9132d1b491..6d9ca1120e 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -64,6 +64,7 @@ #include #include #include +#include #include "waypoints.h" #include "orb_topics.h" @@ -181,10 +182,11 @@ set_hil_on_off(bool hil_enabled) } void -get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) +get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) { /* reset MAVLink mode bitfield */ - *mavlink_mode = 0; + *mavlink_base_mode = 0; + *mavlink_custom_mode = 0; /** * Set mode flags @@ -192,36 +194,31 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) /* HIL */ if (v_status.hil_state == HIL_STATE_ON) { - *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; + *mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED; } - /* autonomous mode */ - if (v_status.main_state == MAIN_STATE_AUTO) { - *mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED; - } - - /* set arming state */ + /* arming state */ if (v_status.arming_state == ARMING_STATE_ARMED || v_status.arming_state == ARMING_STATE_ARMED_ERROR) { - *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; - - } else { - *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; + *mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } - if (v_status.main_state == MAIN_STATE_MANUAL - || v_status.main_state == MAIN_STATE_SEATBELT - || v_status.main_state == MAIN_STATE_EASY) { - - *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + /* main state */ + *mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; + if (v_status.main_state == MAIN_STATE_MANUAL) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); + *mavlink_custom_mode = PX4_CUSTOM_MODE_MANUAL; + } else if (v_status.main_state == MAIN_STATE_SEATBELT) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; + *mavlink_custom_mode = PX4_CUSTOM_MODE_SEATBELT; + } else if (v_status.main_state == MAIN_STATE_EASY) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + *mavlink_custom_mode = PX4_CUSTOM_MODE_EASY; + } else if (v_status.main_state == MAIN_STATE_AUTO) { + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + *mavlink_custom_mode = PX4_CUSTOM_MODE_AUTO; } - if (v_status.navigation_state == MAIN_STATE_EASY) { - - *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; - } - - /** * Set mavlink state **/ @@ -645,11 +642,12 @@ int mavlink_thread_main(int argc, char *argv[]) /* translate the current system state to mavlink state and mode */ uint8_t mavlink_state = 0; - uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); + uint8_t mavlink_base_mode = 0; + uint32_t mavlink_custom_mode = 0; + get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); /* send heartbeat */ - mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state); + mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, mavlink_custom_mode, mavlink_state); /* switch HIL mode if required */ if (v_status.hil_state == HIL_STATE_ON) diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index d088d421ed..2a260861d3 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -279,15 +279,16 @@ l_vehicle_status(const struct listener *l) /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; - uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); + uint8_t mavlink_base_mode = 0; + uint32_t mavlink_custom_mode = 0; + get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); /* send heartbeat */ mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, - mavlink_mode, - v_status.navigation_state, + mavlink_base_mode, + mavlink_custom_mode, mavlink_state); } @@ -473,8 +474,9 @@ l_actuator_outputs(const struct listener *l) /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; - uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); + uint8_t mavlink_base_mode = 0; + uint32_t mavlink_custom_mode = 0; + get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); /* HIL message as per MAVLink spec */ @@ -491,7 +493,7 @@ l_actuator_outputs(const struct listener *l) -1, -1, -1, - mavlink_mode, + mavlink_base_mode, 0); } else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) { @@ -505,7 +507,7 @@ l_actuator_outputs(const struct listener *l) ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, -1, -1, - mavlink_mode, + mavlink_base_mode, 0); } else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) { @@ -519,7 +521,7 @@ l_actuator_outputs(const struct listener *l) ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f, - mavlink_mode, + mavlink_base_mode, 0); } else { @@ -533,7 +535,7 @@ l_actuator_outputs(const struct listener *l) (act_outputs.output[5] - 1500.0f) / 500.0f, (act_outputs.output[6] - 1500.0f) / 500.0f, (act_outputs.output[7] - 1500.0f) / 500.0f, - mavlink_mode, + mavlink_base_mode, 0); } } diff --git a/src/modules/mavlink/util.h b/src/modules/mavlink/util.h index a4ff06a883..5e5ee8261c 100644 --- a/src/modules/mavlink/util.h +++ b/src/modules/mavlink/util.h @@ -51,4 +51,4 @@ extern mavlink_wpm_storage *wpm; /** * Translate the custom state into standard mavlink modes and state. */ -extern void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode); +extern void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); From d0a9d250f74f9f1386760af3d324480d173a1b43 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 Aug 2013 14:57:30 +0200 Subject: [PATCH 357/486] Enforced consistency between configs --- nuttx-configs/px4fmu-v2/nsh/defconfig | 1 + 1 file changed, 1 insertion(+) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 9454d116a7..85bf6dd2fc 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -640,6 +640,7 @@ CONFIG_USBDEV=y # CONFIG_USBDEV_SELFPOWERED is not set CONFIG_USBDEV_BUSPOWERED=y CONFIG_USBDEV_MAXPOWER=500 +# CONFIG_USBDEV_REMOTEWAKEUP is not set # CONFIG_USBDEV_DMA is not set # CONFIG_USBDEV_TRACE is not set From 21a919d973c13d7d2259b47116480ade819d7b8c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 12 Aug 2013 14:49:32 +1000 Subject: [PATCH 358/486] rgbled: added LED_MODE_RGB to allow for constant RGB values this allows apps to fully control the 3 LED elements --- src/drivers/rgbled/rgbled.cpp | 48 ++++++++++++++++++++++++++++++----- src/include/device/rgbled.h | 14 ++++++++++ 2 files changed, 56 insertions(+), 6 deletions(-) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 5c4fa4bb6f..4275375082 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -80,7 +80,8 @@ enum ledModes { LED_MODE_TEST, LED_MODE_SYSTEMSTATE, - LED_MODE_OFF + LED_MODE_OFF, + LED_MODE_RGB }; class RGBLED : public device::I2C @@ -119,6 +120,9 @@ private: int led_colors[8]; int led_blink; + // RGB values for MODE_RGB + struct RGBLEDSet rgb; + int mode; int running; @@ -178,6 +182,7 @@ RGBLED::setMode(enum ledModes new_mode) switch (new_mode) { case LED_MODE_SYSTEMSTATE: case LED_MODE_TEST: + case LED_MODE_RGB: mode = new_mode; if (!running) { running = true; @@ -185,6 +190,7 @@ RGBLED::setMode(enum ledModes new_mode) work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1); } break; + case LED_MODE_OFF: default: @@ -237,6 +243,12 @@ RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg) int ret = ENOTTY; switch (cmd) { + case RGBLED_SET: { + /* set the specified RGB values */ + memcpy(&rgb, (struct RGBLEDSet *)arg, sizeof(rgb)); + setMode(LED_MODE_RGB); + return OK; + } default: break; @@ -290,6 +302,11 @@ RGBLED::led() break; + case LED_MODE_RGB: + set_rgb(rgb.red, rgb.green, rgb.blue); + running = false; + return; + case LED_MODE_OFF: default: return; @@ -308,10 +325,12 @@ RGBLED::led() led_thread_runcount++; - if(running) { /* re-queue ourselves to run again later */ work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, led_interval); + } else if (mode == LED_MODE_RGB) { + // no need to run again until the colour changes + set_on(true); } else { set_on(false); } @@ -412,7 +431,7 @@ void rgbled_usage(); void rgbled_usage() { - warnx("missing command: try 'start', 'systemstate', 'test', 'info', 'off'"); + warnx("missing command: try 'start', 'systemstate', 'test', 'info', 'off', 'rgb'"); warnx("options:"); warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED); errx(0, " -a addr (0x%x)", ADDR); @@ -461,9 +480,9 @@ rgbled_main(int argc, char *argv[]) /* need the driver past this point */ if (g_rgbled == nullptr) { - fprintf(stderr, "not started\n"); - rgbled_usage(); - exit(0); + fprintf(stderr, "not started\n"); + rgbled_usage(); + exit(0); } if (!strcmp(verb, "test")) { @@ -486,5 +505,22 @@ rgbled_main(int argc, char *argv[]) exit(0); } + if (!strcmp(verb, "rgb")) { + int fd = open(RGBLED_DEVICE_PATH, 0); + if (fd == -1) { + errx(1, "Unable to open " RGBLED_DEVICE_PATH); + } + if (argc < 4) { + errx(1, "Usage: rgbled rgb "); + } + struct RGBLEDSet v; + v.red = strtol(argv[1], NULL, 0); + v.green = strtol(argv[2], NULL, 0); + v.blue = strtol(argv[3], NULL, 0); + int ret = ioctl(fd, RGBLED_SET, (unsigned long)&v); + close(fd); + exit(ret); + } + rgbled_usage(); } diff --git a/src/include/device/rgbled.h b/src/include/device/rgbled.h index a18920892b..600a65d285 100644 --- a/src/include/device/rgbled.h +++ b/src/include/device/rgbled.h @@ -65,3 +65,17 @@ * The script is terminated by a zero command. */ #define RGBLED_SET_USER_SCRIPT _RGBLEDIOC(3) + +/** set constant RGB values */ +#define RGBLED_SET _RGBLEDIOC(4) + +/* + structure passed to RGBLED_SET ioctl() + Note that the driver scales the brightness to 0 to 255, regardless + of the hardware scaling + */ +struct RGBLEDSet { + uint8_t red; + uint8_t green; + uint8_t blue; +}; From 3b10f8431def73222823c1c2abe1bb7422d851dc Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 12 Aug 2013 14:59:58 +1000 Subject: [PATCH 359/486] rgbled: try expansion bus first, unless specific bus given this will allow "rgbled start" to detect which bus the LED is on, supporting boards with either external or internal LEDs --- src/drivers/rgbled/rgbled.cpp | 33 +++++++++++++++++++++++---------- 1 file changed, 23 insertions(+), 10 deletions(-) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 4275375082..44aa922e6e 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -166,7 +166,6 @@ RGBLED::init() ret = I2C::init(); if (ret != OK) { - warnx("I2C init failed"); return ret; } @@ -440,7 +439,7 @@ void rgbled_usage() { int rgbled_main(int argc, char *argv[]) { - int i2cdevice = PX4_I2C_BUS_LED; + int i2cdevice = -1; int rgbledadr = ADDR; /* 7bit */ int ch; @@ -464,15 +463,29 @@ rgbled_main(int argc, char *argv[]) if (g_rgbled != nullptr) errx(1, "already started"); - g_rgbled = new RGBLED(i2cdevice, rgbledadr); + if (i2cdevice == -1) { + // try the external bus first + i2cdevice = PX4_I2C_BUS_EXPANSION; + g_rgbled = new RGBLED(PX4_I2C_BUS_EXPANSION, rgbledadr); + if (g_rgbled != nullptr && OK != g_rgbled->init()) { + delete g_rgbled; + g_rgbled = nullptr; + } + if (g_rgbled == nullptr) { + // fall back to default bus + i2cdevice = PX4_I2C_BUS_LED; + } + } + if (g_rgbled == nullptr) { + g_rgbled = new RGBLED(i2cdevice, rgbledadr); + if (g_rgbled == nullptr) + errx(1, "new failed"); - if (g_rgbled == nullptr) - errx(1, "new failed"); - - if (OK != g_rgbled->init()) { - delete g_rgbled; - g_rgbled = nullptr; - errx(1, "init failed"); + if (OK != g_rgbled->init()) { + delete g_rgbled; + g_rgbled = nullptr; + errx(1, "init failed"); + } } exit(0); From 29d78367846ebf7834ecd87b2cf528573c3fcdd8 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 14 Aug 2013 10:53:47 +0200 Subject: [PATCH 360/486] RGBled fixes: options and off after rgb working now --- src/drivers/rgbled/rgbled.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 44aa922e6e..236f138a74 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -195,8 +195,8 @@ RGBLED::setMode(enum ledModes new_mode) default: if (running) { running = false; - set_on(false); } + set_on(false); mode = LED_MODE_OFF; break; } @@ -443,7 +443,8 @@ rgbled_main(int argc, char *argv[]) int rgbledadr = ADDR; /* 7bit */ int ch; - while ((ch = getopt(argc, argv, "a:b:")) != EOF) { + /* jump over start/off/etc and look at options first */ + while ((ch = getopt(argc-1, &argv[1], "a:b:")) != EOF) { switch (ch) { case 'a': rgbledadr = strtol(optarg, NULL, 0); @@ -455,9 +456,8 @@ rgbled_main(int argc, char *argv[]) rgbled_usage(); } } - argc -= optind; - argv += optind; - const char *verb = argv[0]; + + const char *verb = argv[1]; if (!strcmp(verb, "start")) { if (g_rgbled != nullptr) @@ -523,13 +523,13 @@ rgbled_main(int argc, char *argv[]) if (fd == -1) { errx(1, "Unable to open " RGBLED_DEVICE_PATH); } - if (argc < 4) { + if (argc < 5) { errx(1, "Usage: rgbled rgb "); } struct RGBLEDSet v; - v.red = strtol(argv[1], NULL, 0); - v.green = strtol(argv[2], NULL, 0); - v.blue = strtol(argv[3], NULL, 0); + v.red = strtol(argv[2], NULL, 0); + v.green = strtol(argv[3], NULL, 0); + v.blue = strtol(argv[4], NULL, 0); int ret = ioctl(fd, RGBLED_SET, (unsigned long)&v); close(fd); exit(ret); From 9505654f9103c8965891991514ea690b3e6aea25 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 14 Aug 2013 17:57:01 +0200 Subject: [PATCH 361/486] commander/px4_custom_mode.h added --- src/modules/commander/px4_custom_mode.h | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) create mode 100644 src/modules/commander/px4_custom_mode.h diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h new file mode 100644 index 0000000000..4d4859a5c5 --- /dev/null +++ b/src/modules/commander/px4_custom_mode.h @@ -0,0 +1,18 @@ +/* + * px4_custom_mode.h + * + * Created on: 09.08.2013 + * Author: ton + */ + +#ifndef PX4_CUSTOM_MODE_H_ +#define PX4_CUSTOM_MODE_H_ + +enum PX4_CUSTOM_MODE { + PX4_CUSTOM_MODE_MANUAL = 1, + PX4_CUSTOM_MODE_SEATBELT, + PX4_CUSTOM_MODE_EASY, + PX4_CUSTOM_MODE_AUTO, +}; + +#endif /* PX4_CUSTOM_MODE_H_ */ From 53def47216d5bbacdfdb76428c024ba3feaea64e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 8 Aug 2013 17:23:51 +0200 Subject: [PATCH 362/486] Fixed gyro scale calibration (it was storing scale even though the scale calibration was skipped --- src/modules/commander/gyro_calibration.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 9e6909db07..60b747ee0f 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -177,8 +177,12 @@ void do_gyro_calibration(int mavlink_fd) /* abort this loop if not rotated more than 180 degrees within 5 seconds */ if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f) - && (hrt_absolute_time() - start_time > 5 * 1e6)) - break; + && (hrt_absolute_time() - start_time > 5 * 1e6)) { + mavlink_log_info(mavlink_fd, "gyro scale calibration skipped"); + mavlink_log_info(mavlink_fd, "gyro calibration done"); + tune_positive(); + return; + } /* wait blocking for new data */ struct pollfd fds[1]; From 27d0885453711a3d3ab6abf3cf227afc837e14bd Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 14 Aug 2013 22:38:14 +0200 Subject: [PATCH 363/486] gyro_calibration: confusing typo in mavlink message fixed --- src/modules/commander/gyro_calibration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 60b747ee0f..f1afb0107e 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -154,7 +154,7 @@ void do_gyro_calibration(int mavlink_fd) /*** --- SCALING --- ***/ - mavlink_log_info(mavlink_fd, "offset calibration finished. Rotate for scale 130x"); + mavlink_log_info(mavlink_fd, "offset calibration finished. Rotate for scale 30x"); mavlink_log_info(mavlink_fd, "or do not rotate and wait for 5 seconds to skip."); warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip."); From 50cf1c01b701fced6437dfe574fd09cd312b9f15 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 14 Aug 2013 22:29:36 -0700 Subject: [PATCH 364/486] Compile fix. --- src/drivers/stm32/drv_hrt.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index 83a1a1abbb..e79d7e10a3 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -59,7 +59,7 @@ #include #include -#include +#include #include #include "chip.h" From 0bbc4b7012c72fda61dca01a897552e9483a4f5f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 Aug 2013 08:42:08 +0200 Subject: [PATCH 365/486] Build fixes --- makefiles/config_px4fmu-v1_default.mk | 2 +- src/drivers/px4io/px4io.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 2f70d001d8..33ffba6bfd 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -76,7 +76,7 @@ MODULES += examples/flow_position_estimator # # Vehicle Control # -MODULES += modules/segway +#MODULES += modules/segway # XXX needs state machine update MODULES += modules/fixedwing_backside MODULES += modules/fixedwing_att_control MODULES += modules/fixedwing_pos_control diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 2953639bf2..65e8fa4b6b 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -870,7 +870,7 @@ PX4IO::io_set_arming_state() uint16_t set = 0; uint16_t clear = 0; - _system_armed = vstatus.flag_system_armed; + _system_armed = armed.armed; if (armed.armed && !armed.lockdown) { set |= PX4IO_P_SETUP_ARMING_FMU_ARMED; From cc9f1e81adaa71c5f86f56df45cf14f8bc8e7abc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 Aug 2013 09:52:22 +0200 Subject: [PATCH 366/486] Rejecting arming if safety switch is not in safe position, added reboot command --- src/modules/commander/commander.cpp | 49 +++++++++++++------ .../commander/state_machine_helper.cpp | 21 ++++++-- src/modules/commander/state_machine_helper.h | 6 ++- 3 files changed, 58 insertions(+), 18 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 82b5754054..d4c2c4c844 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -265,7 +265,7 @@ void usage(const char *reason) exit(1); } -void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) +void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) { /* result of the command */ uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; @@ -282,7 +282,7 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode transition_result_t arming_res = TRANSITION_NOT_CHANGED; if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - arming_res = arming_state_transition(status, ARMING_STATE_ARMED, armed); + arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed); if (arming_res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_fd, "[cmd] ARMED by command"); @@ -291,7 +291,7 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode } else { if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) { arming_state_t new_arming_state = (status->arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - arming_res = arming_state_transition(status, new_arming_state, armed); + arming_res = arming_state_transition(status, safety, new_arming_state, armed); if (arming_res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command"); @@ -356,6 +356,23 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode break; case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: + if (is_safe(status, safety, armed)) { + + if (((int)(cmd->param1)) == 1) { + /* reboot */ + up_systemreset(); + } else if (((int)(cmd->param1)) == 3) { + /* reboot to bootloader */ + + // XXX implement + result = VEHICLE_CMD_RESULT_UNSUPPORTED; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } break; case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { @@ -388,7 +405,7 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode /* check if we have new task and no other task is scheduled */ if (low_prio_task == LOW_PRIO_TASK_NONE && new_low_prio_task != LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (TRANSITION_DENIED != arming_state_transition(status, ARMING_STATE_INIT, armed)) { + if (TRANSITION_DENIED != arming_state_transition(status, safety, ARMING_STATE_INIT, armed)) { result = VEHICLE_CMD_RESULT_ACCEPTED; low_prio_task = new_low_prio_task; @@ -407,10 +424,10 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode low_prio_task_t new_low_prio_task = LOW_PRIO_TASK_NONE; if (((int)(cmd->param1)) == 0) { - low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; + new_low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; } else if (((int)(cmd->param1)) == 1) { - low_prio_task = LOW_PRIO_TASK_PARAM_SAVE; + new_low_prio_task = LOW_PRIO_TASK_PARAM_SAVE; } /* check if we have new task and no other task is scheduled */ @@ -605,6 +622,8 @@ int commander_thread_main(int argc, char *argv[]) int safety_sub = orb_subscribe(ORB_ID(safety)); struct safety_s safety; memset(&safety, 0, sizeof(safety)); + safety.safety_switch_available = false; + safety.safety_off = false; /* Subscribe to manual control data */ int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); @@ -746,7 +765,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - handle_command(&status, &control_mode, &cmd, &armed); + handle_command(&status, &safety, &control_mode, &cmd, &armed); } /* update safety topic */ @@ -871,7 +890,7 @@ int commander_thread_main(int argc, char *argv[]) critical_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY: CRITICAL BATTERY"); status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; - arming_state_transition(&status, ARMING_STATE_ARMED_ERROR, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); status_changed = true; } @@ -887,7 +906,7 @@ 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) { // XXX check for sensors - arming_state_transition(&status, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); } else { // XXX: Add emergency stuff if sensors are lost @@ -1026,7 +1045,7 @@ 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); - res = arming_state_transition(&status, new_arming_state, &armed); + res = arming_state_transition(&status, &safety, new_arming_state, &armed); stick_off_counter = 0; } else { @@ -1045,7 +1064,7 @@ int commander_thread_main(int argc, char *argv[]) status.main_state == MAIN_STATE_MANUAL) { if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - res = arming_state_transition(&status, ARMING_STATE_ARMED, &armed); + res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); stick_on_counter = 0; } else { @@ -1168,10 +1187,10 @@ int commander_thread_main(int argc, char *argv[]) // XXX check if this is correct /* arm / disarm on request */ if (sp_offboard.armed && !armed.armed) { - arming_state_transition(&status, ARMING_STATE_ARMED, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); } else if (!sp_offboard.armed && armed.armed) { - arming_state_transition(&status, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); } } else { @@ -1243,7 +1262,7 @@ 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_off)) { + if (!arm_tune_played && armed.armed) { /* play tune when armed */ if (tune_arm() == OK) arm_tune_played = true; @@ -1540,6 +1559,8 @@ void *commander_low_prio_loop(void *arg) /* Set thread name */ prctl(PR_SET_NAME, "commander_low_prio", getpid()); + low_prio_task = LOW_PRIO_TASK_NONE; + while (!thread_should_exit) { switch (low_prio_task) { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 06cd060c5d..163aceed2e 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -67,7 +67,7 @@ static bool main_state_changed = true; static bool navigation_state_changed = true; transition_result_t -arming_state_transition(struct vehicle_status_s *status, arming_state_t new_arming_state, struct actuator_armed_s *armed) +arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, arming_state_t new_arming_state, struct actuator_armed_s *armed) { transition_result_t ret = TRANSITION_DENIED; @@ -108,8 +108,10 @@ arming_state_transition(struct vehicle_status_s *status, arming_state_t new_armi case ARMING_STATE_ARMED: /* allow arming from STANDBY and IN-AIR-RESTORE */ - if (status->arming_state == ARMING_STATE_STANDBY - || status->arming_state == ARMING_STATE_IN_AIR_RESTORE) { + if ((status->arming_state == ARMING_STATE_STANDBY + || status->arming_state == ARMING_STATE_IN_AIR_RESTORE) + && (!safety->safety_switch_available || safety->safety_off)) /* only allow arming if safety is off */ + { ret = TRANSITION_CHANGED; armed->armed = true; armed->ready_to_arm = false; @@ -172,6 +174,19 @@ arming_state_transition(struct vehicle_status_s *status, arming_state_t new_armi return ret; } +bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed) +{ + // System is safe if: + // 1) Not armed + // 2) Armed, but in software lockdown (HIL) + // 3) Safety switch is present AND engaged -> actuators locked + if (!armed->armed || (armed->armed && armed->lockdown) || (safety->safety_switch_available && !safety->safety_off)) { + return true; + } else { + return false; + } +} + bool check_arming_state_changed() { diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index c8c77e5d8a..a38c2497eb 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -47,6 +47,7 @@ #include #include #include +#include #include typedef enum { @@ -56,7 +57,10 @@ typedef enum { } transition_result_t; -transition_result_t arming_state_transition(struct vehicle_status_s *current_state, arming_state_t new_arming_state, struct actuator_armed_s *armed); +transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety, + arming_state_t new_arming_state, struct actuator_armed_s *armed); + +bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed); bool check_arming_state_changed(); From e5af29be1706b1d20d6bafe8c481213c76cc0d34 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 14 Aug 2013 12:38:25 +0200 Subject: [PATCH 367/486] Fixed param save and load --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 82b5754054..5b3654bcde 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -407,10 +407,10 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode low_prio_task_t new_low_prio_task = LOW_PRIO_TASK_NONE; if (((int)(cmd->param1)) == 0) { - low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; + new_low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; } else if (((int)(cmd->param1)) == 1) { - low_prio_task = LOW_PRIO_TASK_PARAM_SAVE; + new_low_prio_task = LOW_PRIO_TASK_PARAM_SAVE; } /* check if we have new task and no other task is scheduled */ From 39ae01dd07d53e3509826ae3737fc6a509adec34 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 14 Aug 2013 13:29:42 +0200 Subject: [PATCH 368/486] Fix the low priority loop, calibration routines work again --- src/modules/commander/commander.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5b3654bcde..c6e209f0f3 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1552,7 +1552,7 @@ void *commander_low_prio_loop(void *arg) mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); tune_error(); } - + low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_PARAM_SAVE: @@ -1564,37 +1564,43 @@ void *commander_low_prio_loop(void *arg) mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); tune_error(); } - + low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_GYRO_CALIBRATION: do_gyro_calibration(mavlink_fd); + low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_MAG_CALIBRATION: do_mag_calibration(mavlink_fd); + low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_ALTITUDE_CALIBRATION: // do_baro_calibration(mavlink_fd); + low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_RC_CALIBRATION: // do_rc_calibration(mavlink_fd); + low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_ACCEL_CALIBRATION: do_accel_calibration(mavlink_fd); + low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_AIRSPEED_CALIBRATION: do_airspeed_calibration(mavlink_fd); + low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_NONE: @@ -1604,8 +1610,6 @@ void *commander_low_prio_loop(void *arg) break; } - low_prio_task = LOW_PRIO_TASK_NONE; - } return 0; From f51320d1afac836085ee4d9dbdaeda7af18bcbda Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 15 Aug 2013 17:27:29 +0200 Subject: [PATCH 369/486] Lov voltage warning should work again --- src/modules/commander/commander.cpp | 23 ++++++++++------------- 1 file changed, 10 insertions(+), 13 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 41e22d21d5..71f33d81be 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -550,6 +550,7 @@ int commander_thread_main(int argc, char *argv[]) /* set battery warning flag */ status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; + status.condition_battery_voltage_valid = false; // XXX for now just set sensors as initialized status.condition_system_sensors_initialized = true; @@ -606,8 +607,8 @@ int commander_thread_main(int argc, char *argv[]) enum VEHICLE_BATTERY_WARNING battery_warning_previous = VEHICLE_BATTERY_WARNING_NONE; bool armed_previous = false; - bool low_battery_voltage_actions_done; - bool critical_battery_voltage_actions_done; + bool low_battery_voltage_actions_done = false; + bool critical_battery_voltage_actions_done = false; uint64_t last_idle_time = 0; @@ -810,19 +811,15 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(battery_status), battery_sub, &battery); - status.battery_voltage = battery.voltage_v; - status.condition_battery_voltage_valid = true; - } - /* - * Only update battery voltage estimate if system has - * been running for two and a half seconds. - */ - if (t - start_time > 2500000 && status.condition_battery_voltage_valid) { - status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage); + warnx("bat v: %2.2f", battery.voltage_v); - } else { - status.battery_voltage = 0.0f; + /* only consider battery voltage if system has been running 2s and battery voltage is not 0 */ + if ((t - start_time) > 2000000 && battery.voltage_v > 0.001f) { + status.battery_voltage = battery.voltage_v; + status.condition_battery_voltage_valid = true; + status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage); + } } /* update subsystem */ From 98f403002f72e7fb3e38398de9d87746f7918347 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 15 Aug 2013 17:27:29 +0200 Subject: [PATCH 370/486] Lov voltage warning should work again --- src/modules/commander/commander.cpp | 23 ++++++++++------------- 1 file changed, 10 insertions(+), 13 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5de99040cc..1ae88d17ab 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -550,6 +550,7 @@ int commander_thread_main(int argc, char *argv[]) /* set battery warning flag */ status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; + status.condition_battery_voltage_valid = false; // XXX for now just set sensors as initialized status.condition_system_sensors_initialized = true; @@ -606,8 +607,8 @@ int commander_thread_main(int argc, char *argv[]) enum VEHICLE_BATTERY_WARNING battery_warning_previous = VEHICLE_BATTERY_WARNING_NONE; bool armed_previous = false; - bool low_battery_voltage_actions_done; - bool critical_battery_voltage_actions_done; + bool low_battery_voltage_actions_done = false; + bool critical_battery_voltage_actions_done = false; uint64_t last_idle_time = 0; @@ -810,19 +811,15 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(battery_status), battery_sub, &battery); - status.battery_voltage = battery.voltage_v; - status.condition_battery_voltage_valid = true; - } - /* - * Only update battery voltage estimate if system has - * been running for two and a half seconds. - */ - if (t - start_time > 2500000 && status.condition_battery_voltage_valid) { - status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage); + warnx("bat v: %2.2f", battery.voltage_v); - } else { - status.battery_voltage = 0.0f; + /* only consider battery voltage if system has been running 2s and battery voltage is not 0 */ + if ((t - start_time) > 2000000 && battery.voltage_v > 0.001f) { + status.battery_voltage = battery.voltage_v; + status.condition_battery_voltage_valid = true; + status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage); + } } /* update subsystem */ From 0c4e3dce0ef82ea3a7dd2b7bed8b23108f34205d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 15 Aug 2013 17:34:49 +0200 Subject: [PATCH 371/486] Added LED_TOGGLE for normal LEDs --- src/drivers/boards/px4fmu-v1/px4fmu_led.c | 19 +++++++++++++++++++ src/drivers/boards/px4fmu-v2/px4fmu2_led.c | 12 ++++++++++++ src/drivers/drv_led.h | 1 + src/drivers/led/led.cpp | 6 ++++++ 4 files changed, 38 insertions(+) diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_led.c b/src/drivers/boards/px4fmu-v1/px4fmu_led.c index aa83ec130d..ea91f34ad6 100644 --- a/src/drivers/boards/px4fmu-v1/px4fmu_led.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_led.c @@ -57,6 +57,7 @@ __BEGIN_DECLS extern void led_init(); extern void led_on(int led); extern void led_off(int led); +extern void led_toggle(int led); __END_DECLS __EXPORT void led_init() @@ -94,3 +95,21 @@ __EXPORT void led_off(int led) stm32_gpiowrite(GPIO_LED2, true); } } + +__EXPORT void led_toggle(int led) +{ + if (led == 0) + { + if (stm32_gpioread(GPIO_LED1)) + stm32_gpiowrite(GPIO_LED1, false); + else + stm32_gpiowrite(GPIO_LED1, true); + } + if (led == 1) + { + if (stm32_gpioread(GPIO_LED2)) + stm32_gpiowrite(GPIO_LED2, false); + else + stm32_gpiowrite(GPIO_LED2, true); + } +} diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_led.c b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c index 5ded4294e6..a856ccb027 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_led.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c @@ -57,6 +57,7 @@ __BEGIN_DECLS extern void led_init(); extern void led_on(int led); extern void led_off(int led); +extern void led_toggle(int led); __END_DECLS __EXPORT void led_init() @@ -83,3 +84,14 @@ __EXPORT void led_off(int led) stm32_gpiowrite(GPIO_LED1, true); } } + +__EXPORT void led_toggle(int led) +{ + if (led == 1) + { + if (stm32_gpioread(GPIO_LED1)) + stm32_gpiowrite(GPIO_LED1, false); + else + stm32_gpiowrite(GPIO_LED1, true); + } +} diff --git a/src/drivers/drv_led.h b/src/drivers/drv_led.h index 97f2db107a..4ce04696e0 100644 --- a/src/drivers/drv_led.h +++ b/src/drivers/drv_led.h @@ -54,6 +54,7 @@ #define LED_ON _IOC(_LED_BASE, 0) #define LED_OFF _IOC(_LED_BASE, 1) +#define LED_TOGGLE _IOC(_LED_BASE, 2) __BEGIN_DECLS diff --git a/src/drivers/led/led.cpp b/src/drivers/led/led.cpp index fe307223f2..a37eaca533 100644 --- a/src/drivers/led/led.cpp +++ b/src/drivers/led/led.cpp @@ -52,6 +52,7 @@ __BEGIN_DECLS extern void led_init(); extern void led_on(int led); extern void led_off(int led); +extern void led_toggle(int led); __END_DECLS class LED : device::CDev @@ -98,6 +99,11 @@ LED::ioctl(struct file *filp, int cmd, unsigned long arg) led_off(arg); break; + case LED_TOGGLE: + led_toggle(arg); + break; + + default: result = CDev::ioctl(filp, cmd, arg); } From 901cef922cb286a247872bd2ea46ec13f779d61e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 15 Aug 2013 17:43:01 +0200 Subject: [PATCH 372/486] Some cleanup for the RGB LED driver, added ioctl to set color --- .../device/rgbled.h => drivers/drv_rgbled.h} | 24 ++- src/drivers/rgbled/rgbled.cpp | 171 ++++++++---------- 2 files changed, 102 insertions(+), 93 deletions(-) rename src/{include/device/rgbled.h => drivers/drv_rgbled.h} (87%) diff --git a/src/include/device/rgbled.h b/src/drivers/drv_rgbled.h similarity index 87% rename from src/include/device/rgbled.h rename to src/drivers/drv_rgbled.h index 600a65d285..f7cc5809a3 100644 --- a/src/include/device/rgbled.h +++ b/src/drivers/drv_rgbled.h @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file rgbled.h + * @file drv_rgbled.h * * RGB led device API */ @@ -69,6 +69,9 @@ /** set constant RGB values */ #define RGBLED_SET _RGBLEDIOC(4) +/** set color */ +#define RGBLED_SET_COLOR _RGBLEDIOC(5) + /* structure passed to RGBLED_SET ioctl() Note that the driver scales the brightness to 0 to 255, regardless @@ -79,3 +82,22 @@ struct RGBLEDSet { uint8_t green; uint8_t blue; }; + +typedef enum { + RGBLED_COLOR_OFF, + RGBLED_COLOR_RED, + RGBLED_COLOR_YELLOW, + RGBLED_COLOR_PURPLE, + RGBLED_COLOR_GREEN, + RGBLED_COLOR_BLUE, + RGBLED_COLOR_WHITE, + RGBLED_COLOR_AMBER, +} rgbled_color_t; + +typedef enum { + RGBLED_BLINK_ON, + RGBLED_BLINK_FAST, + RGBLED_BLINK_NORMAL, + RGBLED_BLINK_SLOW, + RGBLED_BLINK_OFF +} rgbled_blinkmode_t; diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 44aa922e6e..35fdc51580 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -61,10 +61,10 @@ #include -#include "device/rgbled.h" +#include -#define LED_ONTIME 120 -#define LED_OFFTIME 120 +#define RGBLED_ONTIME 120 +#define RGBLED_OFFTIME 120 #define ADDR PX4_I2C_OBDEV_LED /**< I2C adress of TCA62724FMG */ #define SUB_ADDR_START 0x01 /**< write everything (with auto-increment) */ @@ -78,10 +78,10 @@ enum ledModes { - LED_MODE_TEST, - LED_MODE_SYSTEMSTATE, - LED_MODE_OFF, - LED_MODE_RGB + RGBLED_MODE_TEST, + RGBLED_MODE_SYSTEMSTATE, + RGBLED_MODE_OFF, + RGBLED_MODE_RGB }; class RGBLED : public device::I2C @@ -98,35 +98,18 @@ public: virtual int ioctl(struct file *filp, int cmd, unsigned long arg); private: - - enum ledColors { - LED_COLOR_OFF, - LED_COLOR_RED, - LED_COLOR_YELLOW, - LED_COLOR_PURPLE, - LED_COLOR_GREEN, - LED_COLOR_BLUE, - LED_COLOR_WHITE, - LED_COLOR_AMBER, - }; - - enum ledBlinkModes { - LED_BLINK_ON, - LED_BLINK_OFF - }; - work_s _work; - int led_colors[8]; - int led_blink; + rgbled_color_t _led_colors[8]; + rgbled_blinkmode_t _led_blinkmode; // RGB values for MODE_RGB - struct RGBLEDSet rgb; + struct RGBLEDSet _rgb; - int mode; - int running; + int _mode; + int _running; - void setLEDColor(int ledcolor); + void setLEDColor(rgbled_color_t ledcolor); static void led_trampoline(void *arg); void led(); @@ -147,10 +130,10 @@ extern "C" __EXPORT int rgbled_main(int argc, char *argv[]); RGBLED::RGBLED(int bus, int rgbled) : I2C("rgbled", RGBLED_DEVICE_PATH, bus, rgbled, 100000), - led_colors({0,0,0,0,0,0,0,0}), - led_blink(LED_BLINK_OFF), - mode(LED_MODE_OFF), - running(false) + _led_colors({RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF}), + _led_blinkmode(RGBLED_BLINK_OFF), + _mode(RGBLED_MODE_OFF), + _running(false) { memset(&_work, 0, sizeof(_work)); } @@ -179,25 +162,25 @@ int RGBLED::setMode(enum ledModes new_mode) { switch (new_mode) { - case LED_MODE_SYSTEMSTATE: - case LED_MODE_TEST: - case LED_MODE_RGB: - mode = new_mode; - if (!running) { - running = true; + case RGBLED_MODE_SYSTEMSTATE: + case RGBLED_MODE_TEST: + case RGBLED_MODE_RGB: + _mode = new_mode; + if (!_running) { + _running = true; set_on(true); work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1); } break; - case LED_MODE_OFF: + case RGBLED_MODE_OFF: default: - if (running) { - running = false; + if (_running) { + _running = false; set_on(false); } - mode = LED_MODE_OFF; + _mode = RGBLED_MODE_OFF; break; } @@ -244,10 +227,14 @@ RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg) switch (cmd) { case RGBLED_SET: { /* set the specified RGB values */ - memcpy(&rgb, (struct RGBLEDSet *)arg, sizeof(rgb)); - setMode(LED_MODE_RGB); + memcpy(&_rgb, (struct RGBLEDSet *)arg, sizeof(_rgb)); + setMode(RGBLED_MODE_RGB); return OK; } + case RGBLED_SET_COLOR: { + /* set the specified color name */ + setLEDColor((rgbled_color_t)arg); + } default: break; @@ -271,42 +258,42 @@ void RGBLED::led() { static int led_thread_runcount=0; - static int led_interval = 1000; + static int _led_interval = 1000; - switch (mode) { - case LED_MODE_TEST: + switch (_mode) { + case RGBLED_MODE_TEST: /* Demo LED pattern for now */ - led_colors[0] = LED_COLOR_YELLOW; - led_colors[1] = LED_COLOR_AMBER; - led_colors[2] = LED_COLOR_RED; - led_colors[3] = LED_COLOR_PURPLE; - led_colors[4] = LED_COLOR_BLUE; - led_colors[5] = LED_COLOR_GREEN; - led_colors[6] = LED_COLOR_WHITE; - led_colors[7] = LED_COLOR_OFF; - led_blink = LED_BLINK_ON; + _led_colors[0] = RGBLED_COLOR_YELLOW; + _led_colors[1] = RGBLED_COLOR_AMBER; + _led_colors[2] = RGBLED_COLOR_RED; + _led_colors[3] = RGBLED_COLOR_PURPLE; + _led_colors[4] = RGBLED_COLOR_BLUE; + _led_colors[5] = RGBLED_COLOR_GREEN; + _led_colors[6] = RGBLED_COLOR_WHITE; + _led_colors[7] = RGBLED_COLOR_OFF; + _led_blinkmode = RGBLED_BLINK_ON; break; - case LED_MODE_SYSTEMSTATE: + case RGBLED_MODE_SYSTEMSTATE: /* XXX TODO set pattern */ - led_colors[0] = LED_COLOR_OFF; - led_colors[1] = LED_COLOR_OFF; - led_colors[2] = LED_COLOR_OFF; - led_colors[3] = LED_COLOR_OFF; - led_colors[4] = LED_COLOR_OFF; - led_colors[5] = LED_COLOR_OFF; - led_colors[6] = LED_COLOR_OFF; - led_colors[7] = LED_COLOR_OFF; - led_blink = LED_BLINK_OFF; + _led_colors[0] = RGBLED_COLOR_OFF; + _led_colors[1] = RGBLED_COLOR_OFF; + _led_colors[2] = RGBLED_COLOR_OFF; + _led_colors[3] = RGBLED_COLOR_OFF; + _led_colors[4] = RGBLED_COLOR_OFF; + _led_colors[5] = RGBLED_COLOR_OFF; + _led_colors[6] = RGBLED_COLOR_OFF; + _led_colors[7] = RGBLED_COLOR_OFF; + _led_blinkmode = RGBLED_BLINK_OFF; break; - case LED_MODE_RGB: - set_rgb(rgb.red, rgb.green, rgb.blue); - running = false; + case RGBLED_MODE_RGB: + set_rgb(_rgb.red, _rgb.green, _rgb.blue); + _running = false; return; - case LED_MODE_OFF: + case RGBLED_MODE_OFF: default: return; break; @@ -314,20 +301,20 @@ RGBLED::led() if (led_thread_runcount & 1) { - if (led_blink == LED_BLINK_ON) - setLEDColor(LED_COLOR_OFF); - led_interval = LED_OFFTIME; + if (_led_blinkmode == RGBLED_BLINK_ON) + setLEDColor(RGBLED_COLOR_OFF); + _led_interval = RGBLED_OFFTIME; } else { - setLEDColor(led_colors[(led_thread_runcount/2) % 8]); - led_interval = LED_ONTIME; + setLEDColor(_led_colors[(led_thread_runcount/2) % 8]); + _led_interval = RGBLED_ONTIME; } led_thread_runcount++; - if(running) { + if(_running) { /* re-queue ourselves to run again later */ - work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, led_interval); - } else if (mode == LED_MODE_RGB) { + work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, _led_interval); + } else if (_mode == RGBLED_MODE_RGB) { // no need to run again until the colour changes set_on(true); } else { @@ -335,30 +322,30 @@ RGBLED::led() } } -void RGBLED::setLEDColor(int ledcolor) { +void RGBLED::setLEDColor(rgbled_color_t ledcolor) { switch (ledcolor) { - case LED_COLOR_OFF: // off + case RGBLED_COLOR_OFF: // off set_rgb(0,0,0); break; - case LED_COLOR_RED: // red + case RGBLED_COLOR_RED: // red set_rgb(255,0,0); break; - case LED_COLOR_YELLOW: // yellow + case RGBLED_COLOR_YELLOW: // yellow set_rgb(255,70,0); break; - case LED_COLOR_PURPLE: // purple + case RGBLED_COLOR_PURPLE: // purple set_rgb(255,0,255); break; - case LED_COLOR_GREEN: // green + case RGBLED_COLOR_GREEN: // green set_rgb(0,255,0); break; - case LED_COLOR_BLUE: // blue + case RGBLED_COLOR_BLUE: // blue set_rgb(0,0,255); break; - case LED_COLOR_WHITE: // white + case RGBLED_COLOR_WHITE: // white set_rgb(255,255,255); break; - case LED_COLOR_AMBER: // amber + case RGBLED_COLOR_AMBER: // amber set_rgb(255,20,0); break; } @@ -499,12 +486,12 @@ rgbled_main(int argc, char *argv[]) } if (!strcmp(verb, "test")) { - g_rgbled->setMode(LED_MODE_TEST); + g_rgbled->setMode(RGBLED_MODE_TEST); exit(0); } if (!strcmp(verb, "systemstate")) { - g_rgbled->setMode(LED_MODE_SYSTEMSTATE); + g_rgbled->setMode(RGBLED_MODE_SYSTEMSTATE); exit(0); } @@ -514,7 +501,7 @@ rgbled_main(int argc, char *argv[]) } if (!strcmp(verb, "off")) { - g_rgbled->setMode(LED_MODE_OFF); + g_rgbled->setMode(RGBLED_MODE_OFF); exit(0); } From d75c3c4e7369510db1d91721c2793c23dcd873fa Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 15 Aug 2013 17:48:28 +0200 Subject: [PATCH 373/486] Try to open RGBLEDs in commander (WIP) --- src/modules/commander/commander.cpp | 9 ++--- src/modules/commander/commander_helper.cpp | 45 +++++++++++++++++----- 2 files changed, 38 insertions(+), 16 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 71f33d81be..4e6ecd1e4b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1331,11 +1331,11 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gp } else if (armed->ready_to_arm) { /* ready to arm, blink at 2.5Hz */ - if (leds_counter & 8) { - led_on(LED_AMBER); + if (leds_counter % 8 == 0) { + led_toggle(LED_AMBER); } else { - led_off(LED_AMBER); + led_toggle(LED_AMBER); } } else { @@ -1358,9 +1358,6 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gp } leds_counter++; - - if (leds_counter >= 16) - leds_counter = 0; } void diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 681a11568f..d9b255f4f8 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -53,6 +53,8 @@ #include #include #include +#include + #include "commander_helper.h" @@ -136,9 +138,11 @@ void tune_stop() } static int leds; +static int rgbleds; int led_init() { + /* first open normal LEDs */ leds = open(LED_DEVICE_PATH, 0); if (leds < 0) { @@ -146,10 +150,29 @@ int led_init() return ERROR; } - if (ioctl(leds, LED_ON, LED_BLUE) || ioctl(leds, LED_ON, LED_AMBER)) { - warnx("LED: ioctl fail\n"); + /* the blue LED is only available on FMUv1 but not FMUv2 */ +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + + if (ioctl(leds, LED_ON, LED_BLUE)) { + warnx("Blue LED: ioctl fail\n"); return ERROR; } +#endif + + if (ioctl(leds, LED_ON, LED_AMBER)) { + warnx("Amber LED: ioctl fail\n"); + return ERROR; + } + + /* then try RGB LEDs, this can fail on FMUv1*/ + rgbleds = open(RGBLED_DEVICE_PATH, 0); + if (rgbleds == -1) { +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + errx(1, "Unable to open " RGBLED_DEVICE_PATH); +#else + warnx("No RGB LED found"); +#endif + } return 0; } @@ -157,18 +180,15 @@ int led_init() void led_deinit() { close(leds); + + if (rgbleds != -1) { + close(rgbleds); + } } int led_toggle(int led) { - static int last_blue = LED_ON; - static int last_amber = LED_ON; - - if (led == LED_BLUE) last_blue = (last_blue == LED_ON) ? LED_OFF : LED_ON; - - if (led == LED_AMBER) last_amber = (last_amber == LED_ON) ? LED_OFF : LED_ON; - - return ioctl(leds, ((led == LED_BLUE) ? last_blue : last_amber), led); + return ioctl(leds, LED_TOGGLE, led); } int led_on(int led) @@ -181,6 +201,11 @@ int led_off(int led) return ioctl(leds, LED_OFF, led); } +int rgbled_set_color(rgbled_color_t color) { + + return ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)&color); +} + float battery_remaining_estimate_voltage(float voltage) { float ret = 0; From 3f9f1d60e03f501209deb6c7532c37dfb786f343 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 16 Aug 2013 09:23:39 +0200 Subject: [PATCH 374/486] Added audio and text warning if arming is refused due to mode switch --- src/modules/commander/commander.cpp | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 1ae88d17ab..6181dafaba 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -204,6 +204,7 @@ void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicl transition_result_t check_main_state_machine(struct vehicle_status_s *current_status); void print_reject_mode(const char *msg); +void print_reject_arm(const char *msg); transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode); @@ -1082,6 +1083,16 @@ int commander_thread_main(int argc, char *argv[]) } else { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); } + } else if (res == TRANSITION_DENIED) { + /* DENIED here indicates safety switch not pressed */ + + if (!(!safety.safety_switch_available || safety.safety_off)) { + print_reject_arm("NOT ARMING: Press safety switch first."); + + } else { + warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + } } /* fill current_status according to mode switches */ @@ -1490,6 +1501,20 @@ print_reject_mode(const char *msg) } } +void +print_reject_arm(const char *msg) +{ + hrt_abstime t = hrt_absolute_time(); + + if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { + last_print_mode_reject_time = t; + char s[80]; + sprintf(s, "[cmd] %s", msg); + mavlink_log_critical(mavlink_fd, s); + tune_negative(); + } +} + transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode) { From ec9de4ad84be8e62b762567c58ec3bb948684b43 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 16 Aug 2013 09:27:05 +0200 Subject: [PATCH 375/486] Critical voltage now leads to a proper arming state transition --- src/modules/commander/commander.cpp | 10 +++++++--- src/modules/commander/state_machine_helper.cpp | 2 ++ 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 4e6ecd1e4b..926be91b9f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -812,8 +812,6 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(battery_status), battery_sub, &battery); - warnx("bat v: %2.2f", battery.voltage_v); - /* only consider battery voltage if system has been running 2s and battery voltage is not 0 */ if ((t - start_time) > 2000000 && battery.voltage_v > 0.001f) { status.battery_voltage = battery.voltage_v; @@ -887,7 +885,13 @@ int commander_thread_main(int argc, char *argv[]) critical_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY: CRITICAL BATTERY"); status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; - arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); + + if (armed.armed) { + // XXX not sure what should happen when voltage is low in flight + //arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); + } else { + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); + } status_changed = true; } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 163aceed2e..ef3890b713 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -168,6 +168,8 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * if (ret == TRANSITION_CHANGED) { status->arming_state = new_arming_state; arming_state_changed = true; + } else { + warnx("arming transition rejected"); } } From 02c23c785e5aa5d85f737a7735bc62355f945066 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 16 Aug 2013 10:17:57 +0200 Subject: [PATCH 376/486] System status load is now from 0 to 1 instead of non-intuitive 0 to 1000 --- src/modules/commander/commander.cpp | 2 +- src/modules/mavlink/mavlink.c | 2 +- src/modules/uORB/topics/vehicle_status.h | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 926be91b9f..fc7670ee52 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -862,7 +862,7 @@ int commander_thread_main(int argc, char *argv[]) uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; if (last_idle_time > 0) - status.load = 1000 - (interval_runtime / 1000); //system load is time spent in non-idle + status.load = 1.0f - ((float)interval_runtime / 1e6f); //system load is time spent in non-idle last_idle_time = system_load.tasks[0].total_runtime; } diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 6d9ca1120e..3d34346704 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -660,7 +660,7 @@ int mavlink_thread_main(int argc, char *argv[]) v_status.onboard_control_sensors_present, v_status.onboard_control_sensors_enabled, v_status.onboard_control_sensors_health, - v_status.load, + v_status.load * 1000.0f, v_status.battery_voltage * 1000.0f, v_status.battery_current * 1000.0f, v_status.battery_remaining, diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index e7feaa98eb..4d49316c3d 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -224,7 +224,7 @@ struct vehicle_status_s uint32_t onboard_control_sensors_enabled; uint32_t onboard_control_sensors_health; - float load; + float load; /**< processor load from 0 to 1 */ float battery_voltage; float battery_current; float battery_remaining; From 2c6570cec803775feef1c1214cbe9236f05adde0 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 16 Aug 2013 10:20:04 +0200 Subject: [PATCH 377/486] Forgot load change in mavlink_onboard --- src/modules/mavlink_onboard/mavlink.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c index c5dbd00dd9..e713449823 100644 --- a/src/modules/mavlink_onboard/mavlink.c +++ b/src/modules/mavlink_onboard/mavlink.c @@ -448,7 +448,7 @@ int mavlink_thread_main(int argc, char *argv[]) v_status.onboard_control_sensors_present, v_status.onboard_control_sensors_enabled, v_status.onboard_control_sensors_health, - v_status.load, + v_status.load * 1000.0f, v_status.battery_voltage * 1000.0f, v_status.battery_current * 1000.0f, v_status.battery_remaining, From 0fe612e843d6d0e7167c5ec4d33958c02efbbab6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 16 Aug 2013 13:04:57 +0200 Subject: [PATCH 378/486] Simplified the RGBLED driver --- src/drivers/drv_rgbled.h | 27 ++-- src/drivers/rgbled/rgbled.cpp | 252 ++++++++++++++++------------------ 2 files changed, 136 insertions(+), 143 deletions(-) diff --git a/src/drivers/drv_rgbled.h b/src/drivers/drv_rgbled.h index f7cc5809a3..66741e5497 100644 --- a/src/drivers/drv_rgbled.h +++ b/src/drivers/drv_rgbled.h @@ -67,22 +67,26 @@ #define RGBLED_SET_USER_SCRIPT _RGBLEDIOC(3) /** set constant RGB values */ -#define RGBLED_SET _RGBLEDIOC(4) +#define RGBLED_SET_RGB _RGBLEDIOC(4) /** set color */ #define RGBLED_SET_COLOR _RGBLEDIOC(5) +/** set blink pattern and speed */ +#define RGBLED_SET_MODE _RGBLEDIOC(6) + /* - structure passed to RGBLED_SET ioctl() + structure passed to RGBLED_SET_RGB ioctl() Note that the driver scales the brightness to 0 to 255, regardless of the hardware scaling */ -struct RGBLEDSet { +typedef struct { uint8_t red; uint8_t green; uint8_t blue; -}; +} rgbled_rgbset_t; +/* enum passed to RGBLED_SET_COLOR ioctl()*/ typedef enum { RGBLED_COLOR_OFF, RGBLED_COLOR_RED, @@ -91,13 +95,14 @@ typedef enum { RGBLED_COLOR_GREEN, RGBLED_COLOR_BLUE, RGBLED_COLOR_WHITE, - RGBLED_COLOR_AMBER, + RGBLED_COLOR_AMBER } rgbled_color_t; +/* enum passed to RGBLED_SET_MODE ioctl()*/ typedef enum { - RGBLED_BLINK_ON, - RGBLED_BLINK_FAST, - RGBLED_BLINK_NORMAL, - RGBLED_BLINK_SLOW, - RGBLED_BLINK_OFF -} rgbled_blinkmode_t; + RGBLED_MODE_OFF, + RGBLED_MODE_ON, + RGBLED_MODE_BLINK_SLOW, + RGBLED_MODE_BLINK_NORMAL, + RGBLED_MODE_BLINK_FAST +} rgbled_mode_t; diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 35fdc51580..96b994888f 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -77,13 +77,6 @@ #define SETTING_ENABLE 0x02 /**< on */ -enum ledModes { - RGBLED_MODE_TEST, - RGBLED_MODE_SYSTEMSTATE, - RGBLED_MODE_OFF, - RGBLED_MODE_RGB -}; - class RGBLED : public device::I2C { public: @@ -94,29 +87,29 @@ public: virtual int init(); virtual int probe(); virtual int info(); - virtual int setMode(enum ledModes mode); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); private: work_s _work; - rgbled_color_t _led_colors[8]; - rgbled_blinkmode_t _led_blinkmode; + rgbled_color_t _colors[8]; + rgbled_mode_t _mode; - // RGB values for MODE_RGB - struct RGBLEDSet _rgb; + bool _should_run; + bool _running; + int _led_interval; + int _counter; - int _mode; - int _running; + void set_color(rgbled_color_t ledcolor); + void set_mode(rgbled_mode_t mode); - void setLEDColor(rgbled_color_t ledcolor); static void led_trampoline(void *arg); void led(); - int set(bool on, uint8_t r, uint8_t g, uint8_t b); - int set_on(bool on); - int set_rgb(uint8_t r, uint8_t g, uint8_t b); - int get(bool &on, bool ¬_powersave, uint8_t &r, uint8_t &g, uint8_t &b); + int set(bool on, uint8_t r, uint8_t g, uint8_t b); + int set_on(bool on); + int set_rgb(uint8_t r, uint8_t g, uint8_t b); + int get(bool &on, bool ¬_powersave, uint8_t &r, uint8_t &g, uint8_t &b); }; /* for now, we only support one RGBLED */ @@ -130,10 +123,11 @@ extern "C" __EXPORT int rgbled_main(int argc, char *argv[]); RGBLED::RGBLED(int bus, int rgbled) : I2C("rgbled", RGBLED_DEVICE_PATH, bus, rgbled, 100000), - _led_colors({RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF}), - _led_blinkmode(RGBLED_BLINK_OFF), + _colors({RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF}), _mode(RGBLED_MODE_OFF), - _running(false) + _running(false), + _led_interval(0), + _counter(0) { memset(&_work, 0, sizeof(_work)); } @@ -158,35 +152,6 @@ RGBLED::init() return OK; } -int -RGBLED::setMode(enum ledModes new_mode) -{ - switch (new_mode) { - case RGBLED_MODE_SYSTEMSTATE: - case RGBLED_MODE_TEST: - case RGBLED_MODE_RGB: - _mode = new_mode; - if (!_running) { - _running = true; - set_on(true); - work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1); - } - break; - - case RGBLED_MODE_OFF: - - default: - if (_running) { - _running = false; - set_on(false); - } - _mode = RGBLED_MODE_OFF; - break; - } - - return OK; -} - int RGBLED::probe() { @@ -225,16 +190,24 @@ RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg) int ret = ENOTTY; switch (cmd) { - case RGBLED_SET: { + case RGBLED_SET_RGB: /* set the specified RGB values */ - memcpy(&_rgb, (struct RGBLEDSet *)arg, sizeof(_rgb)); - setMode(RGBLED_MODE_RGB); + rgbled_rgbset_t rgbset; + memcpy(&rgbset, (rgbled_rgbset_t*)arg, sizeof(rgbset)); + set_rgb(rgbset.red, rgbset.green, rgbset.blue); + set_mode(RGBLED_MODE_ON); return OK; - } - case RGBLED_SET_COLOR: { + + case RGBLED_SET_COLOR: /* set the specified color name */ - setLEDColor((rgbled_color_t)arg); - } + set_color((rgbled_color_t)arg); + return OK; + + case RGBLED_SET_MODE: + /* set the specified blink pattern/speed */ + set_mode((rgbled_mode_t)arg); + return OK; + default: break; @@ -257,77 +230,32 @@ RGBLED::led_trampoline(void *arg) void RGBLED::led() { - static int led_thread_runcount=0; - static int _led_interval = 1000; - switch (_mode) { - case RGBLED_MODE_TEST: - /* Demo LED pattern for now */ - _led_colors[0] = RGBLED_COLOR_YELLOW; - _led_colors[1] = RGBLED_COLOR_AMBER; - _led_colors[2] = RGBLED_COLOR_RED; - _led_colors[3] = RGBLED_COLOR_PURPLE; - _led_colors[4] = RGBLED_COLOR_BLUE; - _led_colors[5] = RGBLED_COLOR_GREEN; - _led_colors[6] = RGBLED_COLOR_WHITE; - _led_colors[7] = RGBLED_COLOR_OFF; - _led_blinkmode = RGBLED_BLINK_ON; + case RGBLED_MODE_BLINK_SLOW: + case RGBLED_MODE_BLINK_NORMAL: + case RGBLED_MODE_BLINK_FAST: + if(_counter % 2 == 0) + set_on(true); + else + set_on(false); break; - - case RGBLED_MODE_SYSTEMSTATE: - /* XXX TODO set pattern */ - _led_colors[0] = RGBLED_COLOR_OFF; - _led_colors[1] = RGBLED_COLOR_OFF; - _led_colors[2] = RGBLED_COLOR_OFF; - _led_colors[3] = RGBLED_COLOR_OFF; - _led_colors[4] = RGBLED_COLOR_OFF; - _led_colors[5] = RGBLED_COLOR_OFF; - _led_colors[6] = RGBLED_COLOR_OFF; - _led_colors[7] = RGBLED_COLOR_OFF; - _led_blinkmode = RGBLED_BLINK_OFF; - - break; - - case RGBLED_MODE_RGB: - set_rgb(_rgb.red, _rgb.green, _rgb.blue); - _running = false; - return; - - case RGBLED_MODE_OFF: default: - return; break; } + _counter++; - if (led_thread_runcount & 1) { - if (_led_blinkmode == RGBLED_BLINK_ON) - setLEDColor(RGBLED_COLOR_OFF); - _led_interval = RGBLED_OFFTIME; - } else { - setLEDColor(_led_colors[(led_thread_runcount/2) % 8]); - _led_interval = RGBLED_ONTIME; - } - - led_thread_runcount++; - - if(_running) { - /* re-queue ourselves to run again later */ - work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, _led_interval); - } else if (_mode == RGBLED_MODE_RGB) { - // no need to run again until the colour changes - set_on(true); - } else { - set_on(false); - } + /* re-queue ourselves to run again later */ + work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, _led_interval); } -void RGBLED::setLEDColor(rgbled_color_t ledcolor) { - switch (ledcolor) { - case RGBLED_COLOR_OFF: // off +void +RGBLED::set_color(rgbled_color_t color) { + switch (color) { + case RGBLED_COLOR_OFF: // off set_rgb(0,0,0); break; - case RGBLED_COLOR_RED: // red + case RGBLED_COLOR_RED: // red set_rgb(255,0,0); break; case RGBLED_COLOR_YELLOW: // yellow @@ -339,7 +267,7 @@ void RGBLED::setLEDColor(rgbled_color_t ledcolor) { case RGBLED_COLOR_GREEN: // green set_rgb(0,255,0); break; - case RGBLED_COLOR_BLUE: // blue + case RGBLED_COLOR_BLUE: // blue set_rgb(0,0,255); break; case RGBLED_COLOR_WHITE: // white @@ -348,6 +276,52 @@ void RGBLED::setLEDColor(rgbled_color_t ledcolor) { case RGBLED_COLOR_AMBER: // amber set_rgb(255,20,0); break; + default: + warnx("color unknown"); + break; + } +} + +void +RGBLED::set_mode(rgbled_mode_t mode) +{ + _mode = mode; + + switch (mode) { + case RGBLED_MODE_OFF: + _should_run = false; + set_on(false); + break; + case RGBLED_MODE_ON: + _should_run = false; + set_on(true); + break; + case RGBLED_MODE_BLINK_SLOW: + _should_run = true; + _led_interval = 2000; + break; + case RGBLED_MODE_BLINK_NORMAL: + _should_run = true; + _led_interval = 1000; + break; + case RGBLED_MODE_BLINK_FAST: + _should_run = true; + _led_interval = 500; + break; + default: + warnx("mode unknown"); + break; + } + + /* if it should run now, start the workq */ + if (_should_run && !_running) { + _running = true; + work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1); + } + /* if it should stop, then cancel the workq */ + if (!_should_run && _running) { + _running = false; + work_cancel(LPWORK, &_work); } } @@ -417,7 +391,7 @@ void rgbled_usage(); void rgbled_usage() { - warnx("missing command: try 'start', 'systemstate', 'test', 'info', 'off', 'rgb'"); + warnx("missing command: try 'start', 'test', 'info', 'off', 'rgb'"); warnx("options:"); warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED); errx(0, " -a addr (0x%x)", ADDR); @@ -446,6 +420,9 @@ rgbled_main(int argc, char *argv[]) argv += optind; const char *verb = argv[0]; + int fd; + int ret; + if (!strcmp(verb, "start")) { if (g_rgbled != nullptr) errx(1, "already started"); @@ -480,19 +457,25 @@ rgbled_main(int argc, char *argv[]) /* need the driver past this point */ if (g_rgbled == nullptr) { - fprintf(stderr, "not started\n"); + warnx("not started"); rgbled_usage(); exit(0); } if (!strcmp(verb, "test")) { - g_rgbled->setMode(RGBLED_MODE_TEST); - exit(0); - } + fd = open(RGBLED_DEVICE_PATH, 0); + if (fd == -1) { + errx(1, "Unable to open " RGBLED_DEVICE_PATH); + } + ret = ioctl(fd, RGBLED_SET_COLOR, (unsigned long)RGBLED_COLOR_WHITE); - if (!strcmp(verb, "systemstate")) { - g_rgbled->setMode(RGBLED_MODE_SYSTEMSTATE); - exit(0); + if(ret != OK) { + close(fd); + exit(ret); + } + ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_BLINK_NORMAL); + close(fd); + exit(ret); } if (!strcmp(verb, "info")) { @@ -501,23 +484,28 @@ rgbled_main(int argc, char *argv[]) } if (!strcmp(verb, "off")) { - g_rgbled->setMode(RGBLED_MODE_OFF); - exit(0); + fd = open(RGBLED_DEVICE_PATH, 0); + if (fd == -1) { + errx(1, "Unable to open " RGBLED_DEVICE_PATH); + } + ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_OFF); + close(fd); + exit(ret); } if (!strcmp(verb, "rgb")) { - int fd = open(RGBLED_DEVICE_PATH, 0); + fd = open(RGBLED_DEVICE_PATH, 0); if (fd == -1) { errx(1, "Unable to open " RGBLED_DEVICE_PATH); } if (argc < 4) { errx(1, "Usage: rgbled rgb "); } - struct RGBLEDSet v; + rgbled_rgbset_t v; v.red = strtol(argv[1], NULL, 0); v.green = strtol(argv[2], NULL, 0); v.blue = strtol(argv[3], NULL, 0); - int ret = ioctl(fd, RGBLED_SET, (unsigned long)&v); + ret = ioctl(fd, RGBLED_SET_RGB, (unsigned long)&v); close(fd); exit(ret); } From 63af0a80ee19a73a94a3b46bbddffe1b80610a3c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 16 Aug 2013 15:08:10 +0200 Subject: [PATCH 379/486] multirotor_att_control: run rate controller only if vehicle_control_mode flag set, codestyle fixed --- .../multirotor_att_control_main.c | 43 ++++++++++--------- 1 file changed, 23 insertions(+), 20 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 7014d22c4a..65b19c8e9c 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -242,7 +242,7 @@ mc_thread_main(int argc, char *argv[]) /* control attitude, update attitude setpoint depending on mode */ /* initialize to current yaw if switching to manual or att control */ if (control_mode.flag_control_attitude_enabled != flag_control_attitude_enabled || - control_mode.flag_control_manual_enabled != flag_control_manual_enabled) { + control_mode.flag_control_manual_enabled != flag_control_manual_enabled) { att_sp.yaw_body = att.yaw; } @@ -305,6 +305,7 @@ mc_thread_main(int argc, char *argv[]) att_sp.yaw_body = att.yaw; reset_yaw_sp = false; } + control_yaw_position = true; } @@ -312,6 +313,7 @@ mc_thread_main(int argc, char *argv[]) /* don't update attitude setpoint in position control mode */ att_sp.roll_body = manual.roll; att_sp.pitch_body = manual.pitch; + if (!control_mode.flag_control_position_enabled) { /* don't set throttle in altitude hold modes */ att_sp.thrust = manual.throttle; @@ -355,24 +357,25 @@ mc_thread_main(int argc, char *argv[]) /* measure in what intervals the controller runs */ perf_count(mc_interval_perf); - float gyro[3]; + if (control_mode.flag_control_rates_enabled) { + /* get current rate setpoint */ + bool rates_sp_valid = false; + orb_check(rates_sp_sub, &rates_sp_valid); - /* get current rate setpoint */ - bool rates_sp_valid = false; - orb_check(rates_sp_sub, &rates_sp_valid); + if (rates_sp_valid) { + orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp); + } - if (rates_sp_valid) { - orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp); + /* apply controller */ + float gyro[3]; + gyro[0] = att.rollspeed; + gyro[1] = att.pitchspeed; + gyro[2] = att.yawspeed; + + multirotor_control_rates(&rates_sp, gyro, &actuators); + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); } - /* apply controller */ - gyro[0] = att.rollspeed; - gyro[1] = att.pitchspeed; - gyro[2] = att.yawspeed; - - multirotor_control_rates(&rates_sp, gyro, &actuators); - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - /* update state */ flag_control_attitude_enabled = control_mode.flag_control_attitude_enabled; flag_control_manual_enabled = control_mode.flag_control_manual_enabled; @@ -449,11 +452,11 @@ int multirotor_att_control_main(int argc, char *argv[]) thread_should_exit = false; mc_task = task_spawn_cmd("multirotor_att_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 15, - 2048, - mc_thread_main, - NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 15, + 2048, + mc_thread_main, + NULL); exit(0); } From 05e4c086cecf5fa13cac80d0d9724f1b6bac431c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 16 Aug 2013 16:24:44 +0200 Subject: [PATCH 380/486] Added orientation support and detection to the L3GD20/H driver to support the different variants in use --- src/drivers/l3gd20/l3gd20.cpp | 55 ++++++++++++++++++++++++++++++++--- 1 file changed, 51 insertions(+), 4 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 42a0c264c4..05739f04ff 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -71,6 +71,12 @@ #endif static const int ERROR = -1; +/* Orientation on board */ +#define SENSOR_BOARD_ROTATION_000_DEG 0 +#define SENSOR_BOARD_ROTATION_090_DEG 1 +#define SENSOR_BOARD_ROTATION_180_DEG 2 +#define SENSOR_BOARD_ROTATION_270_DEG 3 + /* SPI protocol address bits */ #define DIR_READ (1<<7) #define DIR_WRITE (0<<7) @@ -186,6 +192,7 @@ private: unsigned _current_rate; unsigned _current_range; + unsigned _orientation; perf_counter_t _sample_perf; @@ -283,6 +290,7 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : _gyro_topic(-1), _current_rate(0), _current_range(0), + _orientation(SENSOR_BOARD_ROTATION_270_DEG), _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")), _gyro_filter_x(250, 30), _gyro_filter_y(250, 30), @@ -363,8 +371,23 @@ L3GD20::probe() (void)read_reg(ADDR_WHO_AM_I); /* verify that the device is attached and functioning, accept L3GD20 and L3GD20H */ - if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM || read_reg(ADDR_WHO_AM_I) == WHO_I_AM_H) + if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) { + + #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + _orientation = SENSOR_BOARD_ROTATION_270_DEG; + #elif CONFIG_ARCH_BOARD_PX4FMU_V2 + _orientation = SENSOR_BOARD_ROTATION_270_DEG; + #else + #error This driver needs a board selection, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2 + #endif return OK; + } + + + if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_H) { + _orientation = SENSOR_BOARD_ROTATION_180_DEG; + return OK; + } return -EIO; } @@ -717,9 +740,33 @@ L3GD20::measure() */ report->timestamp = hrt_absolute_time(); - /* swap x and y and negate y */ - report->x_raw = raw_report.y; - report->y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x); + switch (_orientation) { + + case SENSOR_BOARD_ROTATION_000_DEG: + /* keep axes in place */ + report->x_raw = raw_report.x; + report->y_raw = raw_report.y; + break; + + case SENSOR_BOARD_ROTATION_090_DEG: + /* swap x and y */ + report->x_raw = raw_report.y; + report->y_raw = raw_report.x; + break; + + case SENSOR_BOARD_ROTATION_180_DEG: + /* swap x and y and negate both */ + report->x_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x); + report->y_raw = ((raw_report.y == -32768) ? 32767 : -raw_report.y); + break; + + case SENSOR_BOARD_ROTATION_270_DEG: + /* swap x and y and negate y */ + report->x_raw = raw_report.y; + report->y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x); + break; + } + report->z_raw = raw_report.z; report->x = ((report->x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; From 1d7b8bb565a5450d30a6adc72b0130c5d03ba3be Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 16 Aug 2013 18:03:04 +0200 Subject: [PATCH 381/486] Somehow alarm 14 didn't always work --- src/drivers/stm32/tone_alarm/tone_alarm.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index f314b5876c..b06920a765 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -456,9 +456,7 @@ const char * const ToneAlarm::_default_tunes[] = { "O2E2P64", "MNT75L1O2G", //arming warning "MBNT100a8", //battery warning slow - "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8" - "a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8" - "a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8", //battery warning fast // XXX why is there a break before a repetition + "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8" //battery warning fast // XXX why is there a break before a repetition }; const unsigned ToneAlarm::_default_ntunes = sizeof(_default_tunes) / sizeof(_default_tunes[0]); From af3e0d459a018fe37d647d3089b4ea681d9244f4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 16 Aug 2013 18:04:24 +0200 Subject: [PATCH 382/486] Add pattern ioctl for RGBLED --- src/drivers/drv_rgbled.h | 15 ++++++++-- src/drivers/rgbled/rgbled.cpp | 52 +++++++++++++++++++++++++++-------- 2 files changed, 54 insertions(+), 13 deletions(-) diff --git a/src/drivers/drv_rgbled.h b/src/drivers/drv_rgbled.h index 66741e5497..0f48f6f790 100644 --- a/src/drivers/drv_rgbled.h +++ b/src/drivers/drv_rgbled.h @@ -72,9 +72,12 @@ /** set color */ #define RGBLED_SET_COLOR _RGBLEDIOC(5) -/** set blink pattern and speed */ +/** set blink speed */ #define RGBLED_SET_MODE _RGBLEDIOC(6) +/** set pattern */ +#define RGBLED_SET_PATTERN _RGBLEDIOC(7) + /* structure passed to RGBLED_SET_RGB ioctl() Note that the driver scales the brightness to 0 to 255, regardless @@ -104,5 +107,13 @@ typedef enum { RGBLED_MODE_ON, RGBLED_MODE_BLINK_SLOW, RGBLED_MODE_BLINK_NORMAL, - RGBLED_MODE_BLINK_FAST + RGBLED_MODE_BLINK_FAST, + RGBLED_MODE_PATTERN } rgbled_mode_t; + +#define RGBLED_PATTERN_LENGTH 20 + +typedef struct { + rgbled_color_t color[RGBLED_PATTERN_LENGTH]; + unsigned duration[RGBLED_PATTERN_LENGTH]; +} rgbled_pattern_t; diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 96b994888f..858be80587 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -92,8 +92,9 @@ public: private: work_s _work; - rgbled_color_t _colors[8]; + rgbled_color_t _color; rgbled_mode_t _mode; + rgbled_pattern_t _pattern; bool _should_run; bool _running; @@ -102,6 +103,7 @@ private: void set_color(rgbled_color_t ledcolor); void set_mode(rgbled_mode_t mode); + void set_pattern(rgbled_pattern_t *pattern); static void led_trampoline(void *arg); void led(); @@ -123,13 +125,14 @@ extern "C" __EXPORT int rgbled_main(int argc, char *argv[]); RGBLED::RGBLED(int bus, int rgbled) : I2C("rgbled", RGBLED_DEVICE_PATH, bus, rgbled, 100000), - _colors({RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF}), + _color(RGBLED_COLOR_OFF), _mode(RGBLED_MODE_OFF), _running(false), _led_interval(0), _counter(0) { memset(&_work, 0, sizeof(_work)); + memset(&_pattern, 0, sizeof(_pattern)); } RGBLED::~RGBLED() @@ -204,10 +207,14 @@ RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg) return OK; case RGBLED_SET_MODE: - /* set the specified blink pattern/speed */ + /* set the specified blink speed */ set_mode((rgbled_mode_t)arg); return OK; + case RGBLED_SET_PATTERN: + /* set a special pattern */ + set_pattern((rgbled_pattern_t*)arg); + return OK; default: break; @@ -239,6 +246,14 @@ RGBLED::led() else set_on(false); break; + case RGBLED_MODE_PATTERN: + /* don't run out of the pattern array and stop if the next frame is 0 */ + if (_counter >= RGBLED_PATTERN_LENGTH || _pattern.duration[_counter] <= 0) + _counter = 0; + + set_color(_pattern.color[_counter]); + _led_interval = _pattern.duration[_counter]; + break; default: break; } @@ -251,6 +266,9 @@ RGBLED::led() void RGBLED::set_color(rgbled_color_t color) { + + _color = color; + switch (color) { case RGBLED_COLOR_OFF: // off set_rgb(0,0,0); @@ -302,11 +320,16 @@ RGBLED::set_mode(rgbled_mode_t mode) break; case RGBLED_MODE_BLINK_NORMAL: _should_run = true; - _led_interval = 1000; + _led_interval = 500; break; case RGBLED_MODE_BLINK_FAST: _should_run = true; - _led_interval = 500; + _led_interval = 100; + break; + case RGBLED_MODE_PATTERN: + _should_run = true; + set_on(true); + _counter = 0; break; default: warnx("mode unknown"); @@ -325,6 +348,14 @@ RGBLED::set_mode(rgbled_mode_t mode) } } +void +RGBLED::set_pattern(rgbled_pattern_t *pattern) +{ + memcpy(&_pattern, pattern, sizeof(rgbled_pattern_t)); + + set_mode(RGBLED_MODE_PATTERN); +} + int RGBLED::set(bool on, uint8_t r, uint8_t g, uint8_t b) { @@ -467,13 +498,12 @@ rgbled_main(int argc, char *argv[]) if (fd == -1) { errx(1, "Unable to open " RGBLED_DEVICE_PATH); } - ret = ioctl(fd, RGBLED_SET_COLOR, (unsigned long)RGBLED_COLOR_WHITE); - if(ret != OK) { - close(fd); - exit(ret); - } - ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_BLINK_NORMAL); + rgbled_pattern_t pattern = { {RGBLED_COLOR_RED, RGBLED_COLOR_GREEN, RGBLED_COLOR_BLUE, RGBLED_COLOR_OFF}, + {200, 200, 200, 400 } }; + + ret = ioctl(fd, RGBLED_SET_PATTERN, (unsigned long)&pattern); + close(fd); exit(ret); } From 451adf2aa0d9795f69f5675b00ff3fb245312eb0 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 16 Aug 2013 18:05:59 +0200 Subject: [PATCH 383/486] Added part of RGBLED stuff to commander --- src/modules/commander/commander.cpp | 118 ++++++++++++++++----- src/modules/commander/commander_helper.cpp | 27 +++-- src/modules/commander/commander_helper.h | 9 +- 3 files changed, 118 insertions(+), 36 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index fc7670ee52..2e5d080b9f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -197,7 +197,7 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode */ int commander_thread_main(int argc, char *argv[]); -void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gps_position_s *gps_position); +void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed); void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status); @@ -698,6 +698,8 @@ int commander_thread_main(int argc, char *argv[]) while (!thread_should_exit) { hrt_abstime t = hrt_absolute_time(); + status_changed = false; + /* update parameters */ orb_check(param_changed_sub, &updated); @@ -855,8 +857,6 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; } - toggle_status_leds(&status, &armed, &gps_position); - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { /* compute system load */ uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; @@ -867,14 +867,17 @@ int commander_thread_main(int argc, char *argv[]) last_idle_time = system_load.tasks[0].total_runtime; } + warnx("bat remaining: %2.2f", status.battery_remaining); + /* if battery voltage is getting lower, warn using buzzer, etc. */ - if (status.condition_battery_voltage_valid && status.battery_remaining < 0.15f && !low_battery_voltage_actions_done) { + if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { low_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] WARNING: LOW BATTERY"); status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING; status_changed = true; + battery_tune_played = false; } low_voltage_counter++; @@ -885,12 +888,14 @@ int commander_thread_main(int argc, char *argv[]) critical_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY: CRITICAL BATTERY"); status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; + battery_tune_played = false; if (armed.armed) { // XXX not sure what should happen when voltage is low in flight //arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); } else { - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); + // XXX should we still allow to arm with critical battery? + //arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); } status_changed = true; } @@ -1259,7 +1264,6 @@ int commander_thread_main(int argc, char *argv[]) status.counter++; status.timestamp = t; orb_publish(ORB_ID(vehicle_status), status_pub, &status); - status_changed = false; } /* play arming and battery warning tunes */ @@ -1273,7 +1277,7 @@ int commander_thread_main(int argc, char *argv[]) if (tune_low_bat() == OK) battery_tune_played = true; - } else if (status.battery_remaining == VEHICLE_BATTERY_WARNING_ALERT) { + } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { /* play tune on battery critical */ if (tune_critical_bat() == OK) battery_tune_played = true; @@ -1294,6 +1298,9 @@ int commander_thread_main(int argc, char *argv[]) fflush(stdout); counter++; + + toggle_status_leds(&status, &armed, arming_state_changed || status_changed); + usleep(COMMANDER_MONITORING_INTERVAL); } @@ -1325,40 +1332,93 @@ int commander_thread_main(int argc, char *argv[]) } void -toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gps_position_s *gps_position) +toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed) { - if (leds_counter % 2 == 0) { - /* run at 10Hz, full cycle is 16 ticks = 10/16Hz */ +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + /* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */ + if (armed->armed) { + /* armed, solid */ + led_on(LED_BLUE); + + } else if (armed->ready_to_arm) { + /* ready to arm, blink at 1Hz */ + if (leds_counter % 20 == 0) + led_toggle(LED_BLUE); + } else { + /* not ready to arm, blink at 10Hz */ + if (leds_counter % 2 == 0) + led_toggle(LED_BLUE); + } +#endif + + if (changed) { + + warnx("changed"); + + int i; + rgbled_pattern_t pattern; + memset(&pattern, 0, sizeof(pattern)); + if (armed->armed) { /* armed, solid */ - led_on(LED_AMBER); + if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { + pattern.color[0] = RGBLED_COLOR_AMBER; + } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { + pattern.color[0] = RGBLED_COLOR_RED; + } else { + pattern.color[0] = RGBLED_COLOR_GREEN; + } + pattern.duration[0] = 1000; } else if (armed->ready_to_arm) { - /* ready to arm, blink at 2.5Hz */ - if (leds_counter % 8 == 0) { - led_toggle(LED_AMBER); + for (i=0; i<3; i++) { + if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { + pattern.color[i*2] = RGBLED_COLOR_AMBER; + } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { + pattern.color[i*2] = RGBLED_COLOR_RED; + } else { + pattern.color[i*2] = RGBLED_COLOR_GREEN; + } + pattern.duration[i*2] = 200; + pattern.color[i*2+1] = RGBLED_COLOR_OFF; + pattern.duration[i*2+1] = 800; + } + if (status->condition_global_position_valid) { + pattern.color[i*2] = RGBLED_COLOR_BLUE; + pattern.duration[i*2] = 1000; + pattern.color[i*2+1] = RGBLED_COLOR_OFF; + pattern.duration[i*2+1] = 800; } else { - led_toggle(LED_AMBER); + for (i=3; i<6; i++) { + pattern.color[i*2] = RGBLED_COLOR_BLUE; + pattern.duration[i*2] = 100; + pattern.color[i*2+1] = RGBLED_COLOR_OFF; + pattern.duration[i*2+1] = 100; + } + pattern.color[6*2] = RGBLED_COLOR_OFF; + pattern.duration[6*2] = 700; } } else { + for (i=0; i<3; i++) { + pattern.color[i*2] = RGBLED_COLOR_RED; + pattern.duration[i*2] = 200; + pattern.color[i*2+1] = RGBLED_COLOR_OFF; + pattern.duration[i*2+1] = 200; + } /* not ready to arm, blink at 10Hz */ + } + + rgbled_set_pattern(&pattern); + } + + /* give system warnings on error LED, XXX maybe add memory usage warning too */ + if (status->load > 0.95f) { + if (leds_counter % 2 == 0) led_toggle(LED_AMBER); - } - - if (status->condition_global_position_valid) { - /* position estimated, solid */ - led_on(LED_BLUE); - - } else if (leds_counter == 0) { - /* waiting for position estimate, short blink at 1.25Hz */ - led_on(LED_BLUE); - - } else { - /* no position estimator available, off */ - led_off(LED_BLUE); - } + } else { + led_off(LED_AMBER); } leds_counter++; diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index d9b255f4f8..5df5d8d0c3 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -122,15 +122,17 @@ int tune_arm() return ioctl(buzzer, TONE_SET_ALARM, 12); } +int tune_low_bat() +{ + return ioctl(buzzer, TONE_SET_ALARM, 13); +} + int tune_critical_bat() { return ioctl(buzzer, TONE_SET_ALARM, 14); } -int tune_low_bat() -{ - return ioctl(buzzer, TONE_SET_ALARM, 13); -} + void tune_stop() { @@ -201,9 +203,22 @@ int led_off(int led) return ioctl(leds, LED_OFF, led); } -int rgbled_set_color(rgbled_color_t color) { +void rgbled_set_color(rgbled_color_t color) { - return ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)&color); + if (rgbleds != -1) + ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color); +} + +void rgbled_set_mode(rgbled_mode_t mode) { + + if (rgbleds != -1) + ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode); +} + +void rgbled_set_pattern(rgbled_pattern_t *pattern) { + + if (rgbleds != -1) + ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern); } float battery_remaining_estimate_voltage(float voltage) diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index c621b98232..027d0535f0 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -45,6 +45,7 @@ #include #include #include +#include bool is_multirotor(const struct vehicle_status_s *current_status); @@ -58,8 +59,8 @@ void tune_positive(void); void tune_neutral(void); void tune_negative(void); int tune_arm(void); -int tune_critical_bat(void); int tune_low_bat(void); +int tune_critical_bat(void); void tune_stop(void); int led_init(void); @@ -68,6 +69,12 @@ int led_toggle(int led); int led_on(int led); int led_off(int led); +void rgbled_set_color(rgbled_color_t color); + +void rgbled_set_mode(rgbled_mode_t mode); + +void rgbled_set_pattern(rgbled_pattern_t *pattern); + /** * Provides a coarse estimate of remaining battery power. * From 4882bc0f1c74e9ddebbeaa73bc3e753ac43bdf9c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 16 Aug 2013 21:24:19 +0200 Subject: [PATCH 384/486] position_estimator_inav: fixed global_pos topic publishing --- .../position_estimator_inav/position_estimator_inav_main.c | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index d4b2f0e43c..0530c2deac 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -116,6 +116,7 @@ int position_estimator_inav_main(int argc, char *argv[]) } verbose_mode = false; + if (argc > 1) if (!strcmp(argv[2], "-v")) verbose_mode = true; @@ -375,6 +376,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (att.R_valid) { /* correct accel bias, now only for Z */ sensor.accelerometer_m_s2[2] -= accel_bias[2]; + /* transform acceleration vector from body frame to NED frame */ for (int i = 0; i < 3; i++) { accel_NED[i] = 0.0f; @@ -551,9 +553,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); if (params.use_gps) { - global_pos.valid = local_pos.valid; double est_lat, est_lon; map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); + + global_pos.valid = local_pos.valid; + global_pos.timestamp = t; + global_pos.time_gps_usec = gps.time_gps_usec; global_pos.lat = (int32_t)(est_lat * 1e7); global_pos.lon = (int32_t)(est_lon * 1e7); global_pos.alt = local_pos.home_alt - local_pos.z; From c543f89ec17048c1b5264623a885a9247a05304c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 16 Aug 2013 23:36:49 +0200 Subject: [PATCH 385/486] commander, multirotor_pos_control, multirotor_att_control: bugfixes --- src/modules/commander/commander.cpp | 132 ++++++++++-------- .../commander/state_machine_helper.cpp | 89 ++++++------ .../multirotor_att_control_main.c | 8 +- .../multirotor_pos_control.c | 22 +-- .../uORB/topics/vehicle_control_mode.h | 3 +- .../uORB/topics/vehicle_local_position.h | 1 + 6 files changed, 134 insertions(+), 121 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 6181dafaba..872939d6dc 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -122,7 +122,7 @@ extern struct system_load_s system_load; #define GPS_QUALITY_GOOD_HYSTERIS_TIME_MS 5000 #define GPS_QUALITY_GOOD_COUNTER_LIMIT (GPS_QUALITY_GOOD_HYSTERIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) -#define LOCAL_POSITION_TIMEOUT 1000000 /**< consider the local position estimate invalid after 1s */ +#define POSITION_TIMEOUT 1000000 /**< consider the local position estimate invalid after 1s */ #define PRINT_INTERVAL 5000000 #define PRINT_MODE_REJECT_INTERVAL 2000000 @@ -353,27 +353,42 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe break; } - case VEHICLE_CMD_COMPONENT_ARM_DISARM: + case VEHICLE_CMD_NAV_TAKEOFF: { + transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); + if (nav_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command"); + } + + if (nav_res != TRANSITION_DENIED) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } break; + } case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: - if (is_safe(status, safety, armed)) { + if (is_safe(status, safety, armed)) { - if (((int)(cmd->param1)) == 1) { - /* reboot */ - up_systemreset(); - } else if (((int)(cmd->param1)) == 3) { - /* reboot to bootloader */ + if (((int)(cmd->param1)) == 1) { + /* reboot */ + up_systemreset(); + + } else if (((int)(cmd->param1)) == 3) { + /* reboot to bootloader */ + + // XXX implement + result = VEHICLE_CMD_RESULT_UNSUPPORTED; - // XXX implement - result = VEHICLE_CMD_RESULT_UNSUPPORTED; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } else { result = VEHICLE_CMD_RESULT_DENIED; } + + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + break; case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { @@ -519,6 +534,7 @@ int commander_thread_main(int argc, char *argv[]) orb_advert_t status_pub; /* make sure we are in preflight state */ memset(&status, 0, sizeof(status)); + status.condition_landed = true; // initialize to safe value /* armed topic */ struct actuator_armed_s armed; @@ -760,6 +776,14 @@ int commander_thread_main(int argc, char *argv[]) last_diff_pres_time = diff_pres.timestamp; } + /* Check for valid airspeed/differential pressure measurements */ + if (t - last_diff_pres_time < 2000000 && t > 2000000) { + status.condition_airspeed_valid = true; + + } else { + status.condition_airspeed_valid = false; + } + orb_check(cmd_sub, &updated); if (updated) { @@ -785,6 +809,20 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); } + /* set the condition to valid if there has recently been a global position estimate */ + if (t - global_position.timestamp < POSITION_TIMEOUT && t > POSITION_TIMEOUT && global_position.valid) { + if (!status.condition_global_position_valid) { + status.condition_global_position_valid = true; + status_changed = true; + } + + } else { + if (status.condition_global_position_valid) { + status.condition_global_position_valid = false; + status_changed = true; + } + } + /* update local position estimate */ orb_check(local_position_sub, &updated); @@ -794,7 +832,7 @@ int commander_thread_main(int argc, char *argv[]) } /* set the condition to valid if there has recently been a local position estimate */ - if (t - local_position.timestamp < LOCAL_POSITION_TIMEOUT) { + if (t - local_position.timestamp < POSITION_TIMEOUT && t > POSITION_TIMEOUT && local_position.valid) { if (!status.condition_local_position_valid) { status.condition_local_position_valid = true; status_changed = true; @@ -920,36 +958,6 @@ int commander_thread_main(int argc, char *argv[]) * set of position measurements is available. */ - /* store current state to reason later about a state change */ - // bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; - bool global_pos_valid = status.condition_global_position_valid; - bool local_pos_valid = status.condition_local_position_valid; - bool airspeed_valid = status.condition_airspeed_valid; - - - /* check for global or local position updates, set a timeout of 2s */ - if (t - global_position.timestamp < 2000000 && t > 2000000 && global_position.valid) { - status.condition_global_position_valid = true; - - } else { - status.condition_global_position_valid = false; - } - - if (t - local_position.timestamp < 2000000 && t > 2000000 && local_position.valid) { - status.condition_local_position_valid = true; - - } else { - status.condition_local_position_valid = false; - } - - /* Check for valid airspeed/differential pressure measurements */ - if (t - last_diff_pres_time < 2000000 && t > 2000000) { - status.condition_airspeed_valid = true; - - } else { - status.condition_airspeed_valid = false; - } - orb_check(gps_sub, &updated); if (updated) { @@ -1083,6 +1091,7 @@ int commander_thread_main(int argc, char *argv[]) } else { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); } + } else if (res == TRANSITION_DENIED) { /* DENIED here indicates safety switch not pressed */ @@ -1536,8 +1545,10 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v break; case MAIN_STATE_AUTO: - if (current_status->navigation_state != NAVIGATION_STATE_AUTO_TAKEOFF) { + if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { /* don't act while taking off */ + res = TRANSITION_NOT_CHANGED; + } else { if (current_status->condition_landed) { /* if landed: transitions only to AUTO_READY are allowed */ res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); @@ -1545,20 +1556,27 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v break; } else { - /* if not landed: act depending on switches */ - if (current_status->return_switch == RETURN_SWITCH_RETURN) { - /* RTL */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); - - } else { - if (current_status->mission_switch == MISSION_SWITCH_MISSION) { - /* MISSION */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); + /* if not landed: */ + if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) { + /* act depending on switches when manual control enabled */ + if (current_status->return_switch == RETURN_SWITCH_RETURN) { + /* RTL */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); } else { - /* LOITER */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + if (current_status->mission_switch == MISSION_SWITCH_MISSION) { + /* MISSION */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); + + } else { + /* LOITER */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + } } + + } else { + /* always switch to loiter in air when no RC control */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); } } } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 163aceed2e..f313adce47 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -109,9 +109,8 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * /* allow arming from STANDBY and IN-AIR-RESTORE */ if ((status->arming_state == ARMING_STATE_STANDBY - || status->arming_state == ARMING_STATE_IN_AIR_RESTORE) - && (!safety->safety_switch_available || safety->safety_off)) /* only allow arming if safety is off */ - { + || status->arming_state == ARMING_STATE_IN_AIR_RESTORE) + && (!safety->safety_switch_available || safety->safety_off)) { /* only allow arming if safety is off */ ret = TRANSITION_CHANGED; armed->armed = true; armed->ready_to_arm = false; @@ -182,6 +181,7 @@ bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s // 3) Safety switch is present AND engaged -> actuators locked if (!armed->armed || (armed->armed && armed->lockdown) || (safety->safety_switch_available && !safety->safety_off)) { return true; + } else { return false; } @@ -284,6 +284,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_velocity_enabled = false; control_mode->flag_control_position_enabled = false; control_mode->flag_control_altitude_enabled = false; + control_mode->flag_control_climb_rate_enabled = false; control_mode->flag_control_manual_enabled = false; break; @@ -294,6 +295,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_velocity_enabled = false; control_mode->flag_control_position_enabled = false; control_mode->flag_control_altitude_enabled = false; + control_mode->flag_control_climb_rate_enabled = false; control_mode->flag_control_manual_enabled = true; break; @@ -304,6 +306,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_velocity_enabled = false; control_mode->flag_control_position_enabled = false; control_mode->flag_control_altitude_enabled = false; + control_mode->flag_control_climb_rate_enabled = false; control_mode->flag_control_manual_enabled = true; break; @@ -314,6 +317,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_velocity_enabled = false; control_mode->flag_control_position_enabled = false; control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = true; break; @@ -324,80 +328,68 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = true; break; case NAVIGATION_STATE_AUTO_READY: ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_rates_enabled = false; + control_mode->flag_control_attitude_enabled = false; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = false; + control_mode->flag_control_climb_rate_enabled = false; control_mode->flag_control_manual_enabled = false; break; case NAVIGATION_STATE_AUTO_TAKEOFF: /* only transitions from AUTO_READY */ - if (current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { + if (current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) { ret = TRANSITION_CHANGED; control_mode->flag_control_rates_enabled = true; control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = false; } break; case NAVIGATION_STATE_AUTO_LOITER: - - /* deny transitions from landed states */ - if (current_status->navigation_state != NAVIGATION_STATE_STANDBY && - current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; + control_mode->flag_control_manual_enabled = false; break; case NAVIGATION_STATE_AUTO_MISSION: - - /* deny transitions from landed states */ - if (current_status->navigation_state != NAVIGATION_STATE_STANDBY && - current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; + control_mode->flag_control_manual_enabled = false; break; case NAVIGATION_STATE_AUTO_RTL: - - /* deny transitions from landed states */ - if (current_status->navigation_state != NAVIGATION_STATE_STANDBY && - current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; + control_mode->flag_control_manual_enabled = false; break; case NAVIGATION_STATE_AUTO_LAND: @@ -411,6 +403,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = false; } diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 65b19c8e9c..1aa24a4fc8 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -250,12 +250,12 @@ mc_thread_main(int argc, char *argv[]) /* if the RC signal is lost, try to stay level and go slowly back down to ground */ if (control_mode.failsave_highlevel) { - if (!control_mode.flag_control_position_enabled) { + if (!control_mode.flag_control_velocity_enabled) { /* Don't reset attitude setpoint in position control mode, it's handled by position controller. */ att_sp.roll_body = 0.0f; att_sp.pitch_body = 0.0f; - if (!control_mode.flag_control_altitude_enabled) { + if (!control_mode.flag_control_climb_rate_enabled) { /* Don't touch throttle in modes with altitude hold, it's handled by position controller. * * Only go to failsafe throttle if last known throttle was @@ -309,12 +309,12 @@ mc_thread_main(int argc, char *argv[]) control_yaw_position = true; } - if (!control_mode.flag_control_position_enabled) { + if (!control_mode.flag_control_velocity_enabled) { /* don't update attitude setpoint in position control mode */ att_sp.roll_body = manual.roll; att_sp.pitch_body = manual.pitch; - if (!control_mode.flag_control_position_enabled) { + if (!control_mode.flag_control_climb_rate_enabled) { /* don't set throttle in altitude hold modes */ att_sp.thrust = manual.throttle; } diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 0120fa61c7..1cb4702409 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -221,10 +221,10 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) hrt_abstime home_alt_t = 0; uint64_t local_home_timestamp = 0; - static PID_t xy_pos_pids[2]; - static PID_t xy_vel_pids[2]; - static PID_t z_pos_pid; - static thrust_pid_t z_vel_pid; + PID_t xy_pos_pids[2]; + PID_t xy_vel_pids[2]; + PID_t z_pos_pid; + thrust_pid_t z_vel_pid; thread_running = true; @@ -238,7 +238,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); } - pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f); + pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max, PID_MODE_DERIVATIV_SET, 0.02f); thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); int paramcheck_counter = 0; @@ -247,9 +247,9 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) while (!thread_should_exit) { orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); - /* check parameters at 1 Hz*/ - if (--paramcheck_counter <= 0) { - paramcheck_counter = 50; + /* check parameters at 1 Hz */ + if (++paramcheck_counter >= 50) { + paramcheck_counter = 0; bool param_updated; orb_check(param_sub, ¶m_updated); @@ -441,14 +441,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp); // TODO subcrive to velocity setpoint if altitude/position control disabled - if (control_mode.flag_control_velocity_enabled) { + if (control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) { /* run velocity controllers, calculate thrust vector with attitude-thrust compensation */ float thrust_sp[3] = { 0.0f, 0.0f, 0.0f }; - if (control_mode.flag_control_altitude_enabled) { + if (control_mode.flag_control_climb_rate_enabled) { thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]); att_sp.thrust = -thrust_sp[2]; } - if (control_mode.flag_control_position_enabled) { + if (control_mode.flag_control_velocity_enabled) { /* calculate thrust set point in NED frame */ thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt); thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt); diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index d83eb45d9a..fe169c6e6b 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -75,9 +75,10 @@ struct vehicle_control_mode_s //bool flag_auto_enabled; bool flag_control_rates_enabled; /**< true if rates are stabilized */ bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ - bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ + bool flag_control_velocity_enabled; /**< true if horisontal speed (implies direction) is controlled */ bool flag_control_position_enabled; /**< true if position is controlled */ bool flag_control_altitude_enabled; /**< true if altitude is controlled */ + bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ bool failsave_highlevel; /**< Set to true if high-level failsafe mode is enabled */ diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index 76eddeacd2..9d3b4468c6 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -75,6 +75,7 @@ struct vehicle_local_position_s float home_alt; /**< Altitude in meters LOGME */ float home_hdg; /**< Compass heading in radians -PI..+PI. */ + bool landed; /**< true if vehicle is landed */ }; /** From 7aeaf10ae832af01bb3a1b511df0bae34a42bf89 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 16 Aug 2013 16:24:44 +0200 Subject: [PATCH 386/486] Added orientation support and detection to the L3GD20/H driver to support the different variants in use --- src/drivers/l3gd20/l3gd20.cpp | 55 ++++++++++++++++++++++++++++++++--- 1 file changed, 51 insertions(+), 4 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 42a0c264c4..05739f04ff 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -71,6 +71,12 @@ #endif static const int ERROR = -1; +/* Orientation on board */ +#define SENSOR_BOARD_ROTATION_000_DEG 0 +#define SENSOR_BOARD_ROTATION_090_DEG 1 +#define SENSOR_BOARD_ROTATION_180_DEG 2 +#define SENSOR_BOARD_ROTATION_270_DEG 3 + /* SPI protocol address bits */ #define DIR_READ (1<<7) #define DIR_WRITE (0<<7) @@ -186,6 +192,7 @@ private: unsigned _current_rate; unsigned _current_range; + unsigned _orientation; perf_counter_t _sample_perf; @@ -283,6 +290,7 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : _gyro_topic(-1), _current_rate(0), _current_range(0), + _orientation(SENSOR_BOARD_ROTATION_270_DEG), _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")), _gyro_filter_x(250, 30), _gyro_filter_y(250, 30), @@ -363,8 +371,23 @@ L3GD20::probe() (void)read_reg(ADDR_WHO_AM_I); /* verify that the device is attached and functioning, accept L3GD20 and L3GD20H */ - if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM || read_reg(ADDR_WHO_AM_I) == WHO_I_AM_H) + if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) { + + #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + _orientation = SENSOR_BOARD_ROTATION_270_DEG; + #elif CONFIG_ARCH_BOARD_PX4FMU_V2 + _orientation = SENSOR_BOARD_ROTATION_270_DEG; + #else + #error This driver needs a board selection, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2 + #endif return OK; + } + + + if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_H) { + _orientation = SENSOR_BOARD_ROTATION_180_DEG; + return OK; + } return -EIO; } @@ -717,9 +740,33 @@ L3GD20::measure() */ report->timestamp = hrt_absolute_time(); - /* swap x and y and negate y */ - report->x_raw = raw_report.y; - report->y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x); + switch (_orientation) { + + case SENSOR_BOARD_ROTATION_000_DEG: + /* keep axes in place */ + report->x_raw = raw_report.x; + report->y_raw = raw_report.y; + break; + + case SENSOR_BOARD_ROTATION_090_DEG: + /* swap x and y */ + report->x_raw = raw_report.y; + report->y_raw = raw_report.x; + break; + + case SENSOR_BOARD_ROTATION_180_DEG: + /* swap x and y and negate both */ + report->x_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x); + report->y_raw = ((raw_report.y == -32768) ? 32767 : -raw_report.y); + break; + + case SENSOR_BOARD_ROTATION_270_DEG: + /* swap x and y and negate y */ + report->x_raw = raw_report.y; + report->y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x); + break; + } + report->z_raw = raw_report.z; report->x = ((report->x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; From 2be5240b9f70803417c9648133490409ba40ba55 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 17 Aug 2013 12:37:41 +0200 Subject: [PATCH 387/486] commander, multirotor_att_control, position_estimator_inav: position valid flag fixed, other fixes and cleaunup --- src/modules/commander/commander.cpp | 124 ++++++++---------- .../multirotor_att_control_main.c | 20 ++- .../position_estimator_inav_main.c | 15 +-- 3 files changed, 74 insertions(+), 85 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 872939d6dc..d40f6675b5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -117,12 +117,9 @@ extern struct system_load_s system_load; #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) -#define GPS_FIX_TYPE_2D 2 -#define GPS_FIX_TYPE_3D 3 -#define GPS_QUALITY_GOOD_HYSTERIS_TIME_MS 5000 -#define GPS_QUALITY_GOOD_COUNTER_LIMIT (GPS_QUALITY_GOOD_HYSTERIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) - -#define POSITION_TIMEOUT 1000000 /**< consider the local position estimate invalid after 1s */ +#define POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */ +#define RC_TIMEOUT 100000 +#define DIFFPRESS_TIMEOUT 2000000 #define PRINT_INTERVAL 5000000 #define PRINT_MODE_REJECT_INTERVAL 2000000 @@ -197,6 +194,8 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode */ int commander_thread_main(int argc, char *argv[]); +void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed); + void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gps_position_s *gps_position); void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status); @@ -354,19 +353,21 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } case VEHICLE_CMD_NAV_TAKEOFF: { - transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); - if (nav_res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command"); - } + transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); - if (nav_res != TRANSITION_DENIED) { - result = VEHICLE_CMD_RESULT_ACCEPTED; + if (nav_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command"); + } - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + if (nav_res != TRANSITION_DENIED) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + + break; } - break; - } case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: if (is_safe(status, safety, armed)) { @@ -713,8 +714,6 @@ int commander_thread_main(int argc, char *argv[]) start_time = hrt_absolute_time(); while (!thread_should_exit) { - hrt_abstime t = hrt_absolute_time(); - /* update parameters */ orb_check(param_changed_sub, &updated); @@ -773,16 +772,9 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - last_diff_pres_time = diff_pres.timestamp; } - /* Check for valid airspeed/differential pressure measurements */ - if (t - last_diff_pres_time < 2000000 && t > 2000000) { - status.condition_airspeed_valid = true; - - } else { - status.condition_airspeed_valid = false; - } + check_valid(diff_pres.timestamp, DIFFPRESS_TIMEOUT, true, &(status.condition_airspeed_valid), &status_changed); orb_check(cmd_sub, &updated); @@ -809,19 +801,8 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); } - /* set the condition to valid if there has recently been a global position estimate */ - if (t - global_position.timestamp < POSITION_TIMEOUT && t > POSITION_TIMEOUT && global_position.valid) { - if (!status.condition_global_position_valid) { - status.condition_global_position_valid = true; - status_changed = true; - } - - } else { - if (status.condition_global_position_valid) { - status.condition_global_position_valid = false; - status_changed = true; - } - } + /* update condition_global_position_valid */ + check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.valid, &(status.condition_global_position_valid), &status_changed); /* update local position estimate */ orb_check(local_position_sub, &updated); @@ -831,19 +812,8 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); } - /* set the condition to valid if there has recently been a local position estimate */ - if (t - local_position.timestamp < POSITION_TIMEOUT && t > POSITION_TIMEOUT && local_position.valid) { - if (!status.condition_local_position_valid) { - status.condition_local_position_valid = true; - status_changed = true; - } - - } else { - if (status.condition_local_position_valid) { - status.condition_local_position_valid = false; - status_changed = true; - } - } + /* update condition_local_position_valid */ + check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.valid, &(status.condition_local_position_valid), &status_changed); /* update battery status */ orb_check(battery_sub, &updated); @@ -854,7 +824,7 @@ int commander_thread_main(int argc, char *argv[]) warnx("bat v: %2.2f", battery.voltage_v); /* only consider battery voltage if system has been running 2s and battery voltage is not 0 */ - if ((t - start_time) > 2000000 && battery.voltage_v > 0.001f) { + if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 0.001f) { status.battery_voltage = battery.voltage_v; status.condition_battery_voltage_valid = true; status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage); @@ -981,10 +951,9 @@ int commander_thread_main(int argc, char *argv[]) * position to the current position. */ - if (!home_position_set && gps_position.fix_type == GPS_FIX_TYPE_3D && + if (!home_position_set && gps_position.fix_type >= 3 && (hdop_m < hdop_threshold_m) && (vdop_m < vdop_threshold_m) && // XXX note that vdop is 0 for mtk - (t - gps_position.timestamp_position < 2000000) - && !armed.armed) { + (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed) { /* copy position data to uORB home message, store it locally as well */ // TODO use global position estimate home.lat = gps_position.lat; @@ -1019,7 +988,7 @@ int commander_thread_main(int argc, char *argv[]) /* ignore RC signals if in offboard control mode */ if (!status.offboard_control_signal_found_once && sp_man.timestamp != 0) { /* start RC input check */ - if ((t - sp_man.timestamp) < 100000) { + if (hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; @@ -1123,7 +1092,7 @@ int commander_thread_main(int argc, char *argv[]) } else { /* print error message for first RC glitch and then every 5s */ - if (!status.rc_signal_cutting_off || (t - last_print_control_time) > PRINT_INTERVAL) { + if (!status.rc_signal_cutting_off || (hrt_absolute_time() - last_print_control_time) > PRINT_INTERVAL) { // TODO remove debug code if (!status.rc_signal_cutting_off) { warnx("Reason: not rc_signal_cutting_off\n"); @@ -1136,11 +1105,11 @@ int commander_thread_main(int argc, char *argv[]) status.rc_signal_cutting_off = true; mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: NO RC CONTROL"); - last_print_control_time = t; + last_print_control_time = hrt_absolute_time(); } /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - status.rc_signal_lost_interval = t - sp_man.timestamp; + status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp; /* if the RC signal is gone for a full second, consider it lost */ if (status.rc_signal_lost_interval > 1000000) { @@ -1157,7 +1126,7 @@ int commander_thread_main(int argc, char *argv[]) // TODO check this /* state machine update for offboard control */ if (!status.rc_signal_found_once && sp_offboard.timestamp != 0) { - if ((t - sp_offboard.timestamp) < 5000000) { // TODO 5s is too long ? + if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) { // TODO 5s is too long ? // /* decide about attitude control flag, enable in att/pos/vel */ // bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE || @@ -1213,23 +1182,23 @@ int commander_thread_main(int argc, char *argv[]) } else { /* print error message for first offboard signal glitch and then every 5s */ - if (!status.offboard_control_signal_weak || ((t - last_print_control_time) > PRINT_INTERVAL)) { + if (!status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_control_time) > PRINT_INTERVAL)) { status.offboard_control_signal_weak = true; mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: NO OFFBOARD CONTROL"); - last_print_control_time = t; + last_print_control_time = hrt_absolute_time(); } /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - status.offboard_control_signal_lost_interval = t - sp_offboard.timestamp; + status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp; /* if the signal is gone for 0.1 seconds, consider it lost */ if (status.offboard_control_signal_lost_interval > 100000) { status.offboard_control_signal_lost = true; - status.failsave_lowlevel_start_time = t; + status.failsave_lowlevel_start_time = hrt_absolute_time(); tune_positive(); /* kill motors after timeout */ - if (t - status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) { + if (hrt_absolute_time() > status.failsave_lowlevel_start_time + failsafe_lowlevel_timeout_ms * 1000) { status.failsave_lowlevel = true; status_changed = true; } @@ -1256,9 +1225,11 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; } + hrt_abstime t1 = hrt_absolute_time(); + /* publish arming state */ if (arming_state_changed) { - armed.timestamp = t; + armed.timestamp = t1; orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); } @@ -1266,14 +1237,14 @@ int commander_thread_main(int argc, char *argv[]) if (navigation_state_changed) { /* publish new navigation state */ control_mode.counter++; - control_mode.timestamp = t; + control_mode.timestamp = t1; orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); } /* publish vehicle status at least with 1 Hz */ if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) { status.counter++; - status.timestamp = t; + status.timestamp = t1; orb_publish(ORB_ID(vehicle_status), status_pub, &status); status_changed = false; } @@ -1340,6 +1311,18 @@ int commander_thread_main(int argc, char *argv[]) return 0; } +void +check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed) +{ + hrt_abstime t = hrt_absolute_time(); + bool valid_new = (t < timestamp + timeout && t > timeout && valid_in); + + if (*valid_out != valid_new) { + *valid_out = valid_new; + *changed = true; + } +} + void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gps_position_s *gps_position) { @@ -1367,7 +1350,7 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gp /* position estimated, solid */ led_on(LED_BLUE); - } else if (leds_counter == 0) { + } else if (leds_counter == 6) { /* waiting for position estimate, short blink at 1.25Hz */ led_on(LED_BLUE); @@ -1548,6 +1531,7 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { /* don't act while taking off */ res = TRANSITION_NOT_CHANGED; + } else { if (current_status->condition_landed) { /* if landed: transitions only to AUTO_READY are allowed */ diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 1aa24a4fc8..5cad667f61 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -367,14 +367,20 @@ mc_thread_main(int argc, char *argv[]) } /* apply controller */ - float gyro[3]; - gyro[0] = att.rollspeed; - gyro[1] = att.pitchspeed; - gyro[2] = att.yawspeed; - - multirotor_control_rates(&rates_sp, gyro, &actuators); - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + float rates[3]; + rates[0] = att.rollspeed; + rates[1] = att.pitchspeed; + rates[2] = att.yawspeed; + multirotor_control_rates(&rates_sp, rates, &actuators); + } else { + /* rates controller disabled, set actuators to zero for safety */ + actuators.control[0] = 0.0f; + actuators.control[1] = 0.0f; + actuators.control[2] = 0.0f; + actuators.control[3] = 0.0f; } + actuators.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); /* update state */ flag_control_attitude_enabled = control_mode.flag_control_attitude_enabled; diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 0530c2deac..0e7e0ef5d7 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -75,6 +75,7 @@ static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int position_estimator_inav_task; /**< Handle of deamon task / thread */ static bool verbose_mode = false; +static hrt_abstime gps_timeout = 1000000; // GPS timeout = 1s __EXPORT int position_estimator_inav_main(int argc, char *argv[]); @@ -490,7 +491,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(accel_corr[2], dt, z_est, 2, params.w_alt_acc); /* dont't try to estimate position when no any position source available */ - bool can_estimate_pos = params.use_gps && gps.fix_type >= 3; + bool can_estimate_pos = params.use_gps && gps.fix_type >= 3 && t - gps.timestamp_position < gps_timeout; if (can_estimate_pos) { /* inertial filter prediction for position */ @@ -501,14 +502,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(accel_corr[0], dt, x_est, 2, params.w_pos_acc); inertial_filter_correct(accel_corr[1], dt, y_est, 2, params.w_pos_acc); - if (params.use_gps && gps.fix_type >= 3) { - inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p); - inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p); + inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p); + inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p); - if (gps.vel_ned_valid) { - inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v); - inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v); - } + if (gps.vel_ned_valid && t - gps.timestamp_velocity < gps_timeout) { + inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v); + inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v); } } From b71c0c1f491fc91561393c1ff1c1646b251fd96e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 17 Aug 2013 15:46:13 +0200 Subject: [PATCH 388/486] Added support for FMUv1 for RGB led and dim led support --- makefiles/config_px4fmu-v1_default.mk | 1 + src/drivers/boards/px4fmu-v1/board_config.h | 2 ++ src/drivers/drv_rgbled.h | 9 ++++++++- 3 files changed, 11 insertions(+), 1 deletion(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index d7d9a1ecbd..8b4ea18bf5 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -30,6 +30,7 @@ MODULES += drivers/hil MODULES += drivers/hott/hott_telemetry MODULES += drivers/hott/hott_sensors MODULES += drivers/blinkm +MODULES += drivers/rgbled MODULES += drivers/mkblctrl MODULES += drivers/md25 MODULES += drivers/airspeed diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h index 9d7c81f859..27621211a1 100644 --- a/src/drivers/boards/px4fmu-v1/board_config.h +++ b/src/drivers/boards/px4fmu-v1/board_config.h @@ -103,6 +103,7 @@ __BEGIN_DECLS #define PX4_I2C_BUS_ESC 1 #define PX4_I2C_BUS_ONBOARD 2 #define PX4_I2C_BUS_EXPANSION 3 +#define PX4_I2C_BUS_LED 3 /* * Devices on the onboard bus. @@ -112,6 +113,7 @@ __BEGIN_DECLS #define PX4_I2C_OBDEV_HMC5883 0x1e #define PX4_I2C_OBDEV_MS5611 0x76 #define PX4_I2C_OBDEV_EEPROM NOTDEFINED +#define PX4_I2C_OBDEV_LED 0x55 #define PX4_I2C_OBDEV_PX4IO_BL 0x18 #define PX4_I2C_OBDEV_PX4IO 0x1a diff --git a/src/drivers/drv_rgbled.h b/src/drivers/drv_rgbled.h index 0f48f6f790..3c8bdec5d3 100644 --- a/src/drivers/drv_rgbled.h +++ b/src/drivers/drv_rgbled.h @@ -98,7 +98,14 @@ typedef enum { RGBLED_COLOR_GREEN, RGBLED_COLOR_BLUE, RGBLED_COLOR_WHITE, - RGBLED_COLOR_AMBER + RGBLED_COLOR_AMBER, + RGBLED_COLOR_DIM_RED, + RGBLED_COLOR_DIM_YELLOW, + RGBLED_COLOR_DIM_PURPLE, + RGBLED_COLOR_DIM_GREEN, + RGBLED_COLOR_DIM_BLUE, + RGBLED_COLOR_DIM_WHITE, + RGBLED_COLOR_DIM_AMBER } rgbled_color_t; /* enum passed to RGBLED_SET_MODE ioctl()*/ From 64145252d4133b6df36229f548d02a692c3ec6fb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 17 Aug 2013 15:46:34 +0200 Subject: [PATCH 389/486] Added dim RGB implementation --- src/drivers/rgbled/rgbled.cpp | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 858be80587..f2543a33cc 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -294,6 +294,27 @@ RGBLED::set_color(rgbled_color_t color) { case RGBLED_COLOR_AMBER: // amber set_rgb(255,20,0); break; + case RGBLED_COLOR_DIM_RED: // red + set_rgb(90,0,0); + break; + case RGBLED_COLOR_DIM_YELLOW: // yellow + set_rgb(80,30,0); + break; + case RGBLED_COLOR_DIM_PURPLE: // purple + set_rgb(45,0,45); + break; + case RGBLED_COLOR_DIM_GREEN: // green + set_rgb(0,90,0); + break; + case RGBLED_COLOR_DIM_BLUE: // blue + set_rgb(0,0,90); + break; + case RGBLED_COLOR_DIM_WHITE: // white + set_rgb(30,30,30); + break; + case RGBLED_COLOR_DIM_AMBER: // amber + set_rgb(80,20,0); + break; default: warnx("color unknown"); break; From 6c45d9cb5cc4e9efb99aff84a1fe10f084a998c9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 17 Aug 2013 15:47:13 +0200 Subject: [PATCH 390/486] Fixed in-air timout, bumped protocol version --- src/drivers/px4io/px4io.cpp | 2 +- src/modules/px4iofirmware/protocol.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 3aecd9b691..045b3ebcbb 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -508,7 +508,7 @@ PX4IO::init() usleep(10000); /* abort after 5s */ - if ((hrt_absolute_time() - try_start_time)/1000 > 50000) { + if ((hrt_absolute_time() - try_start_time)/1000 > 3000) { log("failed to recover from in-air restart (1), aborting IO driver init."); return 1; } diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 02df760685..97809d9c29 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -74,7 +74,7 @@ #define REG_TO_FLOAT(_reg) ((float)REG_TO_SIGNED(_reg) / 10000.0f) #define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)((_float) * 10000.0f)) -#define PX4IO_PROTOCOL_VERSION 2 +#define PX4IO_PROTOCOL_VERSION 3 /* static configuration page */ #define PX4IO_PAGE_CONFIG 0 From 6ff3f51f88c2335f225a2a391de5ee353487a69b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 17 Aug 2013 15:47:42 +0200 Subject: [PATCH 391/486] Added dim RGB led support, not operational yet --- src/modules/commander/commander.cpp | 35 ++++++++++++++++++++--------- 1 file changed, 25 insertions(+), 10 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2e5d080b9f..30906034ea 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -52,6 +52,7 @@ #include #include #include +#include #include #include #include @@ -153,6 +154,8 @@ static unsigned int failsafe_lowlevel_timeout_ms; static unsigned int leds_counter; /* To remember when last notification was sent */ static uint64_t last_print_mode_reject_time = 0; +/* if connected via USB */ +static bool on_usb_power = false; /* tasks waiting for low prio thread */ @@ -205,6 +208,8 @@ transition_result_t check_main_state_machine(struct vehicle_status_s *current_st void print_reject_mode(const char *msg); +void print_status(); + transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode); /** @@ -244,6 +249,7 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { warnx("\tcommander is running\n"); + print_status(); } else { warnx("\tcommander not started\n"); @@ -265,6 +271,10 @@ void usage(const char *reason) exit(1); } +void print_status() { + warnx("usb powered: %s", (on_usb_power) ? "yes" : "no"); +} + void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) { /* result of the command */ @@ -865,9 +875,14 @@ int commander_thread_main(int argc, char *argv[]) status.load = 1.0f - ((float)interval_runtime / 1e6f); //system load is time spent in non-idle last_idle_time = system_load.tasks[0].total_runtime; + + /* check if board is connected via USB */ + struct stat statbuf; + //on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0); } - warnx("bat remaining: %2.2f", status.battery_remaining); + // XXX remove later + //warnx("bat remaining: %2.2f", status.battery_remaining); /* if battery voltage is getting lower, warn using buzzer, etc. */ if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) { @@ -1362,22 +1377,22 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang if (armed->armed) { /* armed, solid */ if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { - pattern.color[0] = RGBLED_COLOR_AMBER; + pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER; } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { - pattern.color[0] = RGBLED_COLOR_RED; + pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; } else { - pattern.color[0] = RGBLED_COLOR_GREEN; + pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN :RGBLED_COLOR_GREEN; } pattern.duration[0] = 1000; } else if (armed->ready_to_arm) { for (i=0; i<3; i++) { if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { - pattern.color[i*2] = RGBLED_COLOR_AMBER; + pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER; } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { - pattern.color[i*2] = RGBLED_COLOR_RED; + pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; } else { - pattern.color[i*2] = RGBLED_COLOR_GREEN; + pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN : RGBLED_COLOR_GREEN; } pattern.duration[i*2] = 200; @@ -1385,13 +1400,13 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang pattern.duration[i*2+1] = 800; } if (status->condition_global_position_valid) { - pattern.color[i*2] = RGBLED_COLOR_BLUE; + pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE; pattern.duration[i*2] = 1000; pattern.color[i*2+1] = RGBLED_COLOR_OFF; pattern.duration[i*2+1] = 800; } else { for (i=3; i<6; i++) { - pattern.color[i*2] = RGBLED_COLOR_BLUE; + pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE; pattern.duration[i*2] = 100; pattern.color[i*2+1] = RGBLED_COLOR_OFF; pattern.duration[i*2+1] = 100; @@ -1402,7 +1417,7 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang } else { for (i=0; i<3; i++) { - pattern.color[i*2] = RGBLED_COLOR_RED; + pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; pattern.duration[i*2] = 200; pattern.color[i*2+1] = RGBLED_COLOR_OFF; pattern.duration[i*2+1] = 200; From e9b6cfd671c72b75f2bf79bf1031ea8697f430b2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 17 Aug 2013 15:48:13 +0200 Subject: [PATCH 392/486] Fixed startup order of F330 script --- ROMFS/px4fmu_common/init.d/10_io_f330 | 18 +++++------------- ROMFS/px4fmu_common/init.d/rcS | 13 +++++++++++++ 2 files changed, 18 insertions(+), 13 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_io_f330 index b3fb027573..0634d650e0 100644 --- a/ROMFS/px4fmu_common/init.d/10_io_f330 +++ b/ROMFS/px4fmu_common/init.d/10_io_f330 @@ -44,6 +44,11 @@ param set MAV_TYPE 2 # mavlink start usleep 5000 + +# +# Start the commander (depends on orb, mavlink) +# +commander start # # Start PX4IO interface (depends on orb, commander) @@ -77,11 +82,6 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix # Start the sensors (depends on orb, px4io) # sh /etc/init.d/rc.sensors - -# -# Start the commander (depends on orb, mavlink) -# -commander start # # Start GPS interface (depends on orb) @@ -102,15 +102,7 @@ if [ $BOARD == fmuv1 ] then px4io limit 200 sdlog2 start -r 50 -a -b 16 - if blinkm start - then - blinkm systemstate - fi else px4io limit 400 sdlog2 start -r 200 -a -b 16 - if rgbled start - then - #rgbled systemstate - fi fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index f0ee1a0c6e..60cc13611b 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -81,6 +81,19 @@ else fi fi +# +# Start system state indicator +# +if rgbled start +then + echo "Using external RGB Led" +else + if blinkm start + then + blinkm systemstate + fi +fi + # # Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) # From 74802f0692ad61ef3995b2f05c7af043ab9fcaf3 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 17 Aug 2013 18:01:51 +0200 Subject: [PATCH 393/486] Added possibility to set board orientation --- src/modules/sensors/sensor_params.c | 3 + src/modules/sensors/sensors.cpp | 148 ++++++++++++++++++++++++++-- 2 files changed, 141 insertions(+), 10 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 255dfed184..8d39929636 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -70,6 +70,9 @@ PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0); +PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); +PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0); + PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f); PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f); PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 42268b9715..7299b21bc7 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -50,6 +50,7 @@ #include #include #include +#include #include @@ -137,6 +138,77 @@ #define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg) +/** + * Enum for board and external compass rotations + * copied from https://github.com/diydrones/ardupilot/blob/master/libraries/AP_Math/rotations.h + */ +enum Rotation { + ROTATION_NONE = 0, + ROTATION_YAW_45 = 1, + ROTATION_YAW_90 = 2, + ROTATION_YAW_135 = 3, + ROTATION_YAW_180 = 4, + ROTATION_YAW_225 = 5, + ROTATION_YAW_270 = 6, + ROTATION_YAW_315 = 7, + ROTATION_ROLL_180 = 8, + ROTATION_ROLL_180_YAW_45 = 9, + ROTATION_ROLL_180_YAW_90 = 10, + ROTATION_ROLL_180_YAW_135 = 11, + ROTATION_PITCH_180 = 12, + ROTATION_ROLL_180_YAW_225 = 13, + ROTATION_ROLL_180_YAW_270 = 14, + ROTATION_ROLL_180_YAW_315 = 15, + ROTATION_ROLL_90 = 16, + ROTATION_ROLL_90_YAW_45 = 17, + ROTATION_ROLL_90_YAW_90 = 18, + ROTATION_ROLL_90_YAW_135 = 19, + ROTATION_ROLL_270 = 20, + ROTATION_ROLL_270_YAW_45 = 21, + ROTATION_ROLL_270_YAW_90 = 22, + ROTATION_ROLL_270_YAW_135 = 23, + ROTATION_PITCH_90 = 24, + ROTATION_PITCH_270 = 25, + ROTATION_MAX +}; + +typedef struct +{ + uint16_t roll; + uint16_t pitch; + uint16_t yaw; +} rot_lookup_t; + +const rot_lookup_t rot_lookup[] = +{ + { 0, 0, 0 }, + { 0, 0, 45 }, + { 0, 0, 90 }, + { 0, 0, 135 }, + { 0, 0, 180 }, + { 0, 0, 225 }, + { 0, 0, 270 }, + { 0, 0, 315 }, + {180, 0, 0 }, + {180, 0, 45 }, + {180, 0, 90 }, + {180, 0, 135 }, + { 0, 180, 0 }, + {180, 0, 225 }, + {180, 0, 270 }, + {180, 0, 315 }, + { 90, 0, 0 }, + { 90, 0, 45 }, + { 90, 0, 90 }, + { 90, 0, 135 }, + {270, 0, 0 }, + {270, 0, 45 }, + {270, 0, 90 }, + {270, 0, 135 }, + { 0, 90, 0 }, + { 0, 270, 0 } +}; + /** * Sensor app start / stop handling function * @@ -210,6 +282,9 @@ private: struct differential_pressure_s _diff_pres; struct airspeed_s _airspeed; + math::Matrix _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ + math::Matrix _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ + struct { float min[_rc_max_chan_count]; float trim[_rc_max_chan_count]; @@ -227,6 +302,9 @@ private: float accel_scale[3]; float diff_pres_offset_pa; + int board_rotation; + int external_mag_rotation; + int rc_type; int rc_map_roll; @@ -306,6 +384,9 @@ private: param_t battery_voltage_scaling; + param_t board_rotation; + param_t external_mag_rotation; + } _parameter_handles; /**< handles for interesting parameters */ @@ -314,6 +395,11 @@ private: */ int parameters_update(); + /** + * Get the rotation matrices + */ + void get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix); + /** * Do accel-related initialisation. */ @@ -450,7 +536,10 @@ Sensors::Sensors() : _diff_pres_pub(-1), /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")) + _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")), + + _board_rotation(3,3), + _external_mag_rotation(3,3) { /* basic r/c parameters */ @@ -540,6 +629,10 @@ Sensors::Sensors() : _parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING"); + /* rotations */ + _parameter_handles.board_rotation = param_find("SENS_BOARD_ROT"); + _parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT"); + /* fetch initial parameter values */ parameters_update(); } @@ -731,9 +824,33 @@ Sensors::parameters_update() warnx("Failed updating voltage scaling param"); } + param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation)); + param_get(_parameter_handles.external_mag_rotation, &(_parameters.external_mag_rotation)); + + get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation); + get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation); + return OK; } +void +Sensors::get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix) +{ + /* first set to zero */ + rot_matrix->Matrix::zero(3,3); + + float roll = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll; + float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch; + float yaw = M_DEG_TO_RAD_F * (float)rot_lookup[rot].yaw; + + math::EulerAngles euler(roll, pitch, yaw); + + math::Dcm R(euler); + + for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) + (*rot_matrix)(i,j) = R(i, j); +} + void Sensors::accel_init() { @@ -874,9 +991,12 @@ Sensors::accel_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); - raw.accelerometer_m_s2[0] = accel_report.x; - raw.accelerometer_m_s2[1] = accel_report.y; - raw.accelerometer_m_s2[2] = accel_report.z; + math::Vector3 vect = {accel_report.x, accel_report.y, accel_report.z}; + vect = _board_rotation*vect; + + raw.accelerometer_m_s2[0] = vect(0); + raw.accelerometer_m_s2[1] = vect(1); + raw.accelerometer_m_s2[2] = vect(2); raw.accelerometer_raw[0] = accel_report.x_raw; raw.accelerometer_raw[1] = accel_report.y_raw; @@ -897,9 +1017,12 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); - raw.gyro_rad_s[0] = gyro_report.x; - raw.gyro_rad_s[1] = gyro_report.y; - raw.gyro_rad_s[2] = gyro_report.z; + math::Vector3 vect = {gyro_report.x, gyro_report.y, gyro_report.z}; + vect = _board_rotation*vect; + + raw.gyro_rad_s[0] = vect(0); + raw.gyro_rad_s[1] = vect(1); + raw.gyro_rad_s[2] = vect(2); raw.gyro_raw[0] = gyro_report.x_raw; raw.gyro_raw[1] = gyro_report.y_raw; @@ -920,9 +1043,14 @@ Sensors::mag_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report); - raw.magnetometer_ga[0] = mag_report.x; - raw.magnetometer_ga[1] = mag_report.y; - raw.magnetometer_ga[2] = mag_report.z; + // XXX TODO add support for external mag orientation + + math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z}; + vect = _board_rotation*vect; + + raw.magnetometer_ga[0] = vect(0); + raw.magnetometer_ga[1] = vect(1); + raw.magnetometer_ga[2] = vect(2); raw.magnetometer_raw[0] = mag_report.x_raw; raw.magnetometer_raw[1] = mag_report.y_raw; From 36d474bfa31a44c49647cf8fc9d825cb1e919182 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 17 Aug 2013 18:39:46 +0200 Subject: [PATCH 394/486] WIP on getting low-priority non-command tasks out of the commander main loop --- src/modules/commander/commander.cpp | 312 ++++++++++++++-------------- 1 file changed, 161 insertions(+), 151 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 30906034ea..891dd893b8 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -280,6 +280,8 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* result of the command */ uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; + /* only handle high-priority commands here */ + /* request to set different system mode */ switch (cmd->command) { case VEHICLE_CMD_DO_SET_MODE: { @@ -363,95 +365,10 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } case VEHICLE_CMD_COMPONENT_ARM_DISARM: + // XXX implement later + result = VEHICLE_CMD_RESULT_DENIED; break; - case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: - if (is_safe(status, safety, armed)) { - - if (((int)(cmd->param1)) == 1) { - /* reboot */ - systemreset(false); - } else if (((int)(cmd->param1)) == 3) { - /* reboot to bootloader */ - - // XXX implement - result = VEHICLE_CMD_RESULT_UNSUPPORTED; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - break; - - case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { - low_prio_task_t new_low_prio_task = LOW_PRIO_TASK_NONE; - - if ((int)(cmd->param1) == 1) { - /* gyro calibration */ - new_low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION; - - } else if ((int)(cmd->param2) == 1) { - /* magnetometer calibration */ - new_low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION; - - } else if ((int)(cmd->param3) == 1) { - /* zero-altitude pressure calibration */ - //new_low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION; - } else if ((int)(cmd->param4) == 1) { - /* RC calibration */ - new_low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION; - - } else if ((int)(cmd->param5) == 1) { - /* accelerometer calibration */ - new_low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION; - - } else if ((int)(cmd->param6) == 1) { - /* airspeed calibration */ - new_low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION; - } - - /* check if we have new task and no other task is scheduled */ - if (low_prio_task == LOW_PRIO_TASK_NONE && new_low_prio_task != LOW_PRIO_TASK_NONE) { - /* try to go to INIT/PREFLIGHT arming state */ - if (TRANSITION_DENIED != arming_state_transition(status, safety, ARMING_STATE_INIT, armed)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - low_prio_task = new_low_prio_task; - - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - - break; - } - - case VEHICLE_CMD_PREFLIGHT_STORAGE: { - low_prio_task_t new_low_prio_task = LOW_PRIO_TASK_NONE; - - if (((int)(cmd->param1)) == 0) { - new_low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; - - } else if (((int)(cmd->param1)) == 1) { - new_low_prio_task = LOW_PRIO_TASK_PARAM_SAVE; - } - - /* check if we have new task and no other task is scheduled */ - if (low_prio_task == LOW_PRIO_TASK_NONE && new_low_prio_task != LOW_PRIO_TASK_NONE) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - low_prio_task = new_low_prio_task; - - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - - break; - } - default: break; } @@ -460,6 +377,8 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe if (result == VEHICLE_CMD_RESULT_ACCEPTED) { tune_positive(); + } else if (result == VEHICLE_CMD_RESULT_UNSUPPORTED) { + /* we do not care in the high prio loop about commands we don't know */ } else { tune_negative(); @@ -472,19 +391,24 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) { mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd->command); - } else if (result == VEHICLE_CMD_RESULT_UNSUPPORTED) { - mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd->command); } } /* send any requested ACKs */ - if (cmd->confirmation > 0) { + if (cmd->confirmation > 0 && result != VEHICLE_CMD_RESULT_UNSUPPORTED) { /* send acknowledge command */ // XXX TODO } } +static struct vehicle_status_s status; + +/* armed topic */ +static struct actuator_armed_s armed; + +static struct safety_s safety; + int commander_thread_main(int argc, char *argv[]) { /* not yet initialized */ @@ -524,13 +448,11 @@ int commander_thread_main(int argc, char *argv[]) } /* Main state machine */ - struct vehicle_status_s status; orb_advert_t status_pub; /* make sure we are in preflight state */ memset(&status, 0, sizeof(status)); /* armed topic */ - struct actuator_armed_s armed; orb_advert_t armed_pub; /* Initialize armed with all false */ memset(&armed, 0, sizeof(armed)); @@ -631,7 +553,6 @@ int commander_thread_main(int argc, char *argv[]) /* Subscribe to safety topic */ int safety_sub = orb_subscribe(ORB_ID(safety)); - struct safety_s safety; memset(&safety, 0, sizeof(safety)); safety.safety_switch_available = false; safety.safety_off = false; @@ -1633,80 +1554,169 @@ void *commander_low_prio_loop(void *arg) /* Set thread name */ prctl(PR_SET_NAME, "commander_low_prio", getpid()); - low_prio_task = LOW_PRIO_TASK_NONE; + /* Subscribe to command topic */ + int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); + struct vehicle_command_s cmd; + memset(&cmd, 0, sizeof(cmd)); + + /* wakeup source(s) */ + struct pollfd fds[1]; + + /* use the gyro to pace output - XXX BROKEN if we are using the L3GD20 */ + fds[0].fd = cmd_sub; + fds[0].events = POLLIN; while (!thread_should_exit) { - switch (low_prio_task) { - case LOW_PRIO_TASK_PARAM_LOAD: + /* wait for up to 100ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000); - if (0 == param_load_default()) { - mavlink_log_info(mavlink_fd, "[cmd] parameters loaded"); + /* timed out - periodic check for _task_should_exit, etc. */ + if (pret == 0) + continue; - } else { - mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); - tune_error(); + /* this is undesirable but not much we can do - might want to flag unhappy status */ + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + continue; + } + + /* if we reach here, we have a valid command */ + orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); + + /* ignore commands the high-prio loop handles */ + if (cmd.command == VEHICLE_CMD_DO_SET_MODE || + cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM) + continue; + + /* result of the command */ + uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; + + /* only handle low-priority commands here */ + switch (cmd.command) { + + case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: + if (is_safe(&status, &safety, &armed)) { + + if (((int)(cmd.param1)) == 1) { + /* reboot */ + systemreset(false); + } else if (((int)(cmd.param1)) == 3) { + /* reboot to bootloader */ + systemreset(true); + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + break; + + case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { + + /* try to go to INIT/PREFLIGHT arming state */ + + // XXX disable interrupts in arming_state_transition + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) { + result = VEHICLE_CMD_RESULT_DENIED; + break; + } + + if ((int)(cmd.param1) == 1) { + /* gyro calibration */ + do_gyro_calibration(mavlink_fd); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else if ((int)(cmd.param2) == 1) { + /* magnetometer calibration */ + do_mag_calibration(mavlink_fd); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else if ((int)(cmd.param3) == 1) { + /* zero-altitude pressure calibration */ + result = VEHICLE_CMD_RESULT_DENIED; + + } else if ((int)(cmd.param4) == 1) { + /* RC calibration */ + result = VEHICLE_CMD_RESULT_DENIED; + + } else if ((int)(cmd.param5) == 1) { + /* accelerometer calibration */ + do_accel_calibration(mavlink_fd); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else if ((int)(cmd.param6) == 1) { + /* airspeed calibration */ + do_airspeed_calibration(mavlink_fd); + result = VEHICLE_CMD_RESULT_ACCEPTED; + } + + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + + break; } - low_prio_task = LOW_PRIO_TASK_NONE; - break; + case VEHICLE_CMD_PREFLIGHT_STORAGE: { - case LOW_PRIO_TASK_PARAM_SAVE: + if (((int)(cmd.param1)) == 0) { + if (0 == param_load_default()) { + mavlink_log_info(mavlink_fd, "[cmd] parameters loaded"); + result = VEHICLE_CMD_RESULT_ACCEPTED; - if (0 == param_save_default()) { - mavlink_log_info(mavlink_fd, "[cmd] parameters saved"); + } else { + mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); + tune_error(); + result = VEHICLE_CMD_RESULT_FAILED; + } - } else { - mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); - tune_error(); + } else if (((int)(cmd.param1)) == 1) { + if (0 == param_save_default()) { + mavlink_log_info(mavlink_fd, "[cmd] parameters saved"); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); + tune_error(); + result = VEHICLE_CMD_RESULT_FAILED; + } + } + + break; } - low_prio_task = LOW_PRIO_TASK_NONE; - break; - - case LOW_PRIO_TASK_GYRO_CALIBRATION: - - do_gyro_calibration(mavlink_fd); - low_prio_task = LOW_PRIO_TASK_NONE; - break; - - case LOW_PRIO_TASK_MAG_CALIBRATION: - - do_mag_calibration(mavlink_fd); - low_prio_task = LOW_PRIO_TASK_NONE; - break; - - case LOW_PRIO_TASK_ALTITUDE_CALIBRATION: - - // do_baro_calibration(mavlink_fd); - low_prio_task = LOW_PRIO_TASK_NONE; - break; - - case LOW_PRIO_TASK_RC_CALIBRATION: - - // do_rc_calibration(mavlink_fd); - low_prio_task = LOW_PRIO_TASK_NONE; - break; - - case LOW_PRIO_TASK_ACCEL_CALIBRATION: - - do_accel_calibration(mavlink_fd); - low_prio_task = LOW_PRIO_TASK_NONE; - break; - - case LOW_PRIO_TASK_AIRSPEED_CALIBRATION: - - do_airspeed_calibration(mavlink_fd); - low_prio_task = LOW_PRIO_TASK_NONE; - break; - - case LOW_PRIO_TASK_NONE: default: - /* slow down to 10Hz */ - usleep(100000); break; } + /* supported command handling stop */ + if (result == VEHICLE_CMD_RESULT_ACCEPTED) { + tune_positive(); + + } else { + tune_negative(); + + if (result == VEHICLE_CMD_RESULT_DENIED) { + mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command); + + } else if (result == VEHICLE_CMD_RESULT_FAILED) { + mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command); + + } else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) { + mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd.command); + + } else if (result == VEHICLE_CMD_RESULT_UNSUPPORTED) { + mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command); + } + } + + /* send any requested ACKs */ + if (cmd.confirmation > 0 && cmd.command != VEHICLE_CMD_DO_SET_MODE + && cmd.command != VEHICLE_CMD_COMPONENT_ARM_DISARM) { + /* send acknowledge command */ + // XXX TODO + } + } return 0; From eda528157a04185cbb1342c152c4ac715f67771c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 17 Aug 2013 18:40:28 +0200 Subject: [PATCH 395/486] Make state updates atomic (just to be really, really sure) --- src/modules/commander/state_machine_helper.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index ef3890b713..5842a33b19 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -69,6 +69,11 @@ static bool navigation_state_changed = true; transition_result_t arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, arming_state_t new_arming_state, struct actuator_armed_s *armed) { + /* + * Perform an atomic state update + */ + irqstate_t flags = irqsave(); + transition_result_t ret = TRANSITION_DENIED; /* only check transition if the new state is actually different from the current one */ @@ -168,11 +173,15 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * if (ret == TRANSITION_CHANGED) { status->arming_state = new_arming_state; arming_state_changed = true; - } else { - warnx("arming transition rejected"); } } + /* end of atomic state update */ + irqrestore(flags); + + if (ret == TRANSITION_DENIED) + warnx("arming transition rejected"); + return ret; } From 628e806df5b05ea8a44d46f29606db03fee1fce9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 17 Aug 2013 18:48:14 +0200 Subject: [PATCH 396/486] Minor style changes --- src/modules/sensors/sensors.cpp | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 7299b21bc7..c47f6bb7d9 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -139,8 +139,8 @@ #define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg) /** - * Enum for board and external compass rotations - * copied from https://github.com/diydrones/ardupilot/blob/master/libraries/AP_Math/rotations.h + * Enum for board and external compass rotations. + * This enum maps from board attitude to airframe attitude. */ enum Rotation { ROTATION_NONE = 0, @@ -261,8 +261,8 @@ private: int _mag_sub; /**< raw mag data subscription */ int _rc_sub; /**< raw rc channels data subscription */ int _baro_sub; /**< raw baro data subscription */ - int _airspeed_sub; /**< airspeed subscription */ - int _diff_pres_sub; /**< raw differential pressure subscription */ + int _airspeed_sub; /**< airspeed subscription */ + int _diff_pres_sub; /**< raw differential pressure subscription */ int _vstatus_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ @@ -282,8 +282,8 @@ private: struct differential_pressure_s _diff_pres; struct airspeed_s _airspeed; - math::Matrix _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ - math::Matrix _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ + math::Matrix _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ + math::Matrix _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ struct { float min[_rc_max_chan_count]; @@ -291,7 +291,6 @@ private: float max[_rc_max_chan_count]; float rev[_rc_max_chan_count]; float dz[_rc_max_chan_count]; - // float ex[_rc_max_chan_count]; float scaling_factor[_rc_max_chan_count]; float gyro_offset[3]; @@ -343,7 +342,6 @@ private: param_t max[_rc_max_chan_count]; param_t rev[_rc_max_chan_count]; param_t dz[_rc_max_chan_count]; - // param_t ex[_rc_max_chan_count]; param_t rc_type; param_t rc_demix; @@ -874,7 +872,7 @@ Sensors::accel_init() /* set the driver to poll at 1000Hz */ ioctl(fd, SENSORIOCSPOLLRATE, 1000); - #else + #elif CONFIG_ARCH_BOARD_PX4FMU_V2 /* set the accel internal sampling rate up to at leat 800Hz */ ioctl(fd, ACCELIOCSSAMPLERATE, 800); @@ -882,6 +880,9 @@ Sensors::accel_init() /* set the driver to poll at 800Hz */ ioctl(fd, SENSORIOCSPOLLRATE, 800); + #else + #error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2 + #endif warnx("using system accel"); From 408eaf0ad1c8aa87c74f83281de3a7c25fc4b4e6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 18 Aug 2013 09:22:40 +0200 Subject: [PATCH 397/486] Add ioctl to find out if mag is external or onboard --- src/drivers/drv_mag.h | 3 +++ src/drivers/hmc5883/hmc5883.cpp | 24 +++++++++++++++++++----- src/drivers/lsm303d/lsm303d.cpp | 15 ++++++++++++--- 3 files changed, 34 insertions(+), 8 deletions(-) diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h index 9aab995a17..e95034e8e3 100644 --- a/src/drivers/drv_mag.h +++ b/src/drivers/drv_mag.h @@ -111,4 +111,7 @@ ORB_DECLARE(sensor_mag); /** perform self test and report status */ #define MAGIOCSELFTEST _MAGIOC(7) +/** determine if mag is external or onboard */ +#define MAGIOCGEXTERNAL _MAGIOC(8) + #endif /* _DRV_MAG_H */ diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 039b496f4b..9e9c067d53 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -167,6 +167,8 @@ private: bool _sensor_ok; /**< sensor was found and reports ok */ bool _calibrated; /**< the calibration is valid */ + int _bus; /**< the bus the device is connected to */ + /** * Test whether the device supported by the driver is present at a * specific address. @@ -326,7 +328,8 @@ HMC5883::HMC5883(int bus) : _comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")), _sensor_ok(false), - _calibrated(false) + _calibrated(false), + _bus(bus) { // enable debug() calls _debug_enabled = false; @@ -665,6 +668,12 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) case MAGIOCSELFTEST: return check_calibration(); + case MAGIOCGEXTERNAL: + if (_bus == PX4_I2C_BUS_EXPANSION) + return 1; + else + return 0; + default: /* give it to the superclass */ return I2C::ioctl(filp, cmd, arg); @@ -851,12 +860,12 @@ HMC5883::collect() _reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; } else { #endif - /* XXX axis assignment of external sensor is yet unknown */ - _reports[_next_report].x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; + /* the standard external mag seems to be rolled 180deg, therefore y and z inverted */ + _reports[_next_report].x = ((report.x * _range_scale) - _scale.x_offset) * _scale.x_scale; /* flip axes and negate value for y */ - _reports[_next_report].y = ((((report.x == -32768) ? 32767 : -report.x) * _range_scale) - _scale.y_offset) * _scale.y_scale; + _reports[_next_report].y = ((((report.y == -32768) ? 32767 : -report.y) * _range_scale) - _scale.y_offset) * _scale.y_scale; /* z remains z */ - _reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; + _reports[_next_report].z = ((((report.z == -32768) ? 32767 : -report.z) * _range_scale) - _scale.z_offset) * _scale.z_scale; #ifdef PX4_I2C_BUS_ONBOARD } #endif @@ -1293,6 +1302,11 @@ test() warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); warnx("time: %lld", report.timestamp); + /* check if mag is onboard or external */ + if ((ret = ioctl(fd, MAGIOCGEXTERNAL, 0)) < 0) + errx(1, "failed to get if mag is onboard or external"); + warnx("device active: %s", ret ? "external" : "onboard"); + /* set the queue depth to 10 */ if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) errx(1, "failed to set queue depth"); diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 117583fafe..3e6ce64b81 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -869,6 +869,10 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) // return self_test(); return -EINVAL; + case MAGIOCGEXTERNAL: + /* no external mag board yet */ + return 0; + default: /* give it to the superclass */ return SPI::ioctl(filp, cmd, arg); @@ -1422,7 +1426,7 @@ test() int fd_accel = -1; struct accel_report accel_report; ssize_t sz; - int filter_bandwidth; + int ret; /* get the driver */ fd_accel = open(ACCEL_DEVICE_PATH, O_RDONLY); @@ -1445,10 +1449,10 @@ test() warnx("accel z: \t%d\traw", (int)accel_report.z_raw); warnx("accel range: %8.4f m/s^2", (double)accel_report.range_m_s2); - if (ERROR == (filter_bandwidth = ioctl(fd_accel, ACCELIOCGLOWPASS, 0))) + if (ERROR == (ret = ioctl(fd_accel, ACCELIOCGLOWPASS, 0))) warnx("accel antialias filter bandwidth: fail"); else - warnx("accel antialias filter bandwidth: %d Hz", filter_bandwidth); + warnx("accel antialias filter bandwidth: %d Hz", ret); int fd_mag = -1; struct mag_report m_report; @@ -1459,6 +1463,11 @@ test() if (fd_mag < 0) err(1, "%s open failed", MAG_DEVICE_PATH); + /* check if mag is onboard or external */ + if ((ret = ioctl(fd_mag, MAGIOCGEXTERNAL, 0)) < 0) + errx(1, "failed to get if mag is onboard or external"); + warnx("device active: %s", ret ? "external" : "onboard"); + /* do a simple demand read */ sz = read(fd_mag, &m_report, sizeof(m_report)); From bc717f1715590a6915c223dde53fe6e1139f92d2 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 18 Aug 2013 09:33:59 +0200 Subject: [PATCH 398/486] Sensors should now take into account the orientation of an external mag --- src/modules/sensors/sensors.cpp | 20 ++++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index c47f6bb7d9..198da9f0ae 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -284,6 +284,7 @@ private: math::Matrix _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ math::Matrix _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ + bool _mag_is_external; /**< true if the active mag is on an external board */ struct { float min[_rc_max_chan_count]; @@ -537,7 +538,8 @@ Sensors::Sensors() : _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")), _board_rotation(3,3), - _external_mag_rotation(3,3) + _external_mag_rotation(3,3), + _mag_is_external(false) { /* basic r/c parameters */ @@ -948,6 +950,14 @@ Sensors::mag_init() /* set the driver to poll at 150Hz */ ioctl(fd, SENSORIOCSPOLLRATE, 150); + int ret = ioctl(fd, MAGIOCGEXTERNAL, 0); + if (ret < 0) + errx(1, "FATAL: unknown if magnetometer is external or onboard"); + else if (ret == 1) + _mag_is_external = true; + else + _mag_is_external = false; + close(fd); } @@ -1044,10 +1054,12 @@ Sensors::mag_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report); - // XXX TODO add support for external mag orientation - math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z}; - vect = _board_rotation*vect; + + if (_mag_is_external) + vect = _external_mag_rotation*vect; + else + vect = _board_rotation*vect; raw.magnetometer_ga[0] = vect(0); raw.magnetometer_ga[1] = vect(1); From e5f1a7c2c3a67648829ec0dff5bf290dddc25847 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 18 Aug 2013 12:42:24 +0200 Subject: [PATCH 399/486] Better transparency in IO mode display, fixes to commander arming, minimum publishing rate for arming --- src/drivers/px4io/px4io.cpp | 4 +- src/modules/commander/commander.cpp | 51 +++++++++++++++++-- .../commander/state_machine_helper.cpp | 2 +- 3 files changed, 49 insertions(+), 8 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 8318238e2e..0f90db858a 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1519,8 +1519,8 @@ PX4IO::print_status() uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING); printf("arming 0x%04x%s%s%s%s%s%s\n", arming, - ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : ""), - ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : " FMU_DISARMED"), + ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : " IO_ARM_DENIED"), ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""), ((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ? " FAILSAFE_CUSTOM" : ""), ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""), diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 30906034ea..12543800e3 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -273,6 +273,43 @@ void usage(const char *reason) void print_status() { warnx("usb powered: %s", (on_usb_power) ? "yes" : "no"); + + /* read all relevant states */ + int state_sub = orb_subscribe(ORB_ID(vehicle_status)); + struct vehicle_status_s state; + orb_copy(ORB_ID(vehicle_status), state_sub, &state); + + const char* armed_str; + + switch (state.arming_state) { + case ARMING_STATE_INIT: + armed_str = "INIT"; + break; + case ARMING_STATE_STANDBY: + armed_str = "STANDBY"; + break; + case ARMING_STATE_ARMED: + armed_str = "ARMED"; + break; + case ARMING_STATE_ARMED_ERROR: + armed_str = "ARMED_ERROR"; + break; + case ARMING_STATE_STANDBY_ERROR: + armed_str = "STANDBY_ERROR"; + break; + case ARMING_STATE_REBOOT: + armed_str = "REBOOT"; + break; + case ARMING_STATE_IN_AIR_RESTORE: + armed_str = "IN_AIR_RESTORE"; + break; + default: + armed_str = "ERR: UNKNOWN STATE"; + break; + } + + + warnx("arming: %s", armed_str); } void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) @@ -945,9 +982,9 @@ int commander_thread_main(int argc, char *argv[]) /* store current state to reason later about a state change */ // bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; - bool global_pos_valid = status.condition_global_position_valid; - bool local_pos_valid = status.condition_local_position_valid; - bool airspeed_valid = status.condition_airspeed_valid; + // bool global_pos_valid = status.condition_global_position_valid; + // bool local_pos_valid = status.condition_local_position_valid; + // bool airspeed_valid = status.condition_airspeed_valid; /* check for global or local position updates, set a timeout of 2s */ @@ -1274,11 +1311,15 @@ int commander_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); } - /* publish vehicle status at least with 1 Hz */ - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) { + /* publish states (armed, control mode, vehicle status) at least with 5 Hz */ + if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) { status.counter++; status.timestamp = t; orb_publish(ORB_ID(vehicle_status), status_pub, &status); + control_mode.timestamp = t; + orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); + armed.timestamp = t; + orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); } /* play arming and battery warning tunes */ diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 5842a33b19..1e31573d6b 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -119,7 +119,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * { ret = TRANSITION_CHANGED; armed->armed = true; - armed->ready_to_arm = false; + armed->ready_to_arm = true; } break; From 57c5240f0283e2f3e325287f46ec87bd790122d4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 18 Aug 2013 13:57:21 +0200 Subject: [PATCH 400/486] Make a distinctive sound when the IO start fails (e.g. due to version mismatch) --- ROMFS/px4fmu_common/init.d/10_io_f330 | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_io_f330 index 0634d650e0..48636292c2 100644 --- a/ROMFS/px4fmu_common/init.d/10_io_f330 +++ b/ROMFS/px4fmu_common/init.d/10_io_f330 @@ -53,8 +53,13 @@ commander start # # Start PX4IO interface (depends on orb, commander) # -px4io start -pwm -u 400 -m 0xff +if px4io start +then + pwm -u 400 -m 0xff +else + # SOS + tone_alarm 6 +fi # # Allow PX4IO to recover from midair restarts. From 80f38e3dea6b707314b407c7c511c19aa4eade39 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 18 Aug 2013 21:00:47 +0200 Subject: [PATCH 401/486] Put console and syslog on UART8, added support to nshterm for proper serial port config --- ROMFS/px4fmu_common/init.d/rcS | 9 ++++++++ makefiles/config_px4fmu-v1_default.mk | 1 + makefiles/config_px4fmu-v2_default.mk | 2 ++ nuttx-configs/px4fmu-v2/nsh/defconfig | 14 ++++++------ src/systemcmds/nshterm/nshterm.c | 31 ++++++++++++++++++++++++--- 5 files changed, 47 insertions(+), 10 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index f0ee1a0c6e..e06d90d1c9 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -58,6 +58,15 @@ fi if [ $MODE == autostart ] then +# +# Start terminal +# +if sercon +then + echo "USB connected" + nshterm /dev/ttyACM0 & +fi + # # Start the ORB (first app to start) # diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index db13cc1972..8f2ade8dc8 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -54,6 +54,7 @@ MODULES += systemcmds/reboot MODULES += systemcmds/top MODULES += systemcmds/tests MODULES += systemcmds/config +MODULES += systemcmds/nshterm # # General system control diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index cc182e6af5..101b497125 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -52,6 +52,8 @@ MODULES += systemcmds/pwm MODULES += systemcmds/reboot MODULES += systemcmds/top MODULES += systemcmds/tests +MODULES += systemcmds/config +MODULES += systemcmds/nshterm # # General system control diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 85bf6dd2fc..3e2a481087 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -420,7 +420,7 @@ CONFIG_TASK_NAME_SIZE=24 CONFIG_START_YEAR=1970 CONFIG_START_MONTH=1 CONFIG_START_DAY=1 -# CONFIG_DEV_CONSOLE is not set +CONFIG_DEV_CONSOLE=y # CONFIG_MUTEX_TYPES is not set CONFIG_PRIORITY_INHERITANCE=y CONFIG_SEM_PREALLOCHOLDERS=8 @@ -523,7 +523,7 @@ CONFIG_PIPES=y # CONFIG_POWER is not set # CONFIG_SENSORS is not set CONFIG_SERIAL=y -# CONFIG_DEV_LOWCONSOLE is not set +CONFIG_DEV_LOWCONSOLE=y CONFIG_SERIAL_REMOVABLE=y # CONFIG_16550_UART is not set CONFIG_ARCH_HAVE_UART4=y @@ -542,8 +542,8 @@ CONFIG_SERIAL_NPOLLWAITERS=2 # CONFIG_UART4_SERIAL_CONSOLE is not set # CONFIG_USART6_SERIAL_CONSOLE is not set # CONFIG_UART7_SERIAL_CONSOLE is not set -# CONFIG_UART8_SERIAL_CONSOLE is not set -CONFIG_NO_SERIAL_CONSOLE=y +CONFIG_UART8_SERIAL_CONSOLE=y +# CONFIG_NO_SERIAL_CONSOLE is not set # # USART1 Configuration @@ -650,7 +650,7 @@ CONFIG_USBDEV_MAXPOWER=500 # CONFIG_USBDEV_COMPOSITE is not set # CONFIG_PL2303 is not set CONFIG_CDCACM=y -CONFIG_CDCACM_CONSOLE=y +CONFIG_CDCACM_CONSOLE=n CONFIG_CDCACM_EP0MAXPACKET=64 CONFIG_CDCACM_EPINTIN=1 CONFIG_CDCACM_EPINTIN_FSSIZE=64 @@ -716,10 +716,10 @@ CONFIG_FS_BINFS=y # # System Logging # -# CONFIG_SYSLOG_ENABLE is not set +CONFIG_SYSLOG_ENABLE=y CONFIG_SYSLOG=y CONFIG_SYSLOG_CHAR=y -CONFIG_SYSLOG_DEVPATH="/dev/ttyS0" +CONFIG_SYSLOG_DEVPATH="/dev/ttyS6" # # Graphics Support diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index bde0e78411..2341068a22 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -40,6 +40,7 @@ */ #include +#include #include #include #include @@ -48,6 +49,7 @@ #include #include #include +#include __EXPORT int nshterm_main(int argc, char *argv[]); @@ -61,8 +63,8 @@ nshterm_main(int argc, char *argv[]) uint8_t retries = 0; int fd = -1; while (retries < 5) { - // the retries are to cope with the behaviour of /dev/ttyACM0 - // which may not be ready immediately. + /* the retries are to cope with the behaviour of /dev/ttyACM0 */ + /* which may not be ready immediately. */ fd = open(argv[1], O_RDWR); if (fd != -1) { break; @@ -74,7 +76,30 @@ nshterm_main(int argc, char *argv[]) perror(argv[1]); exit(1); } - // setup standard file descriptors + + /* set up the serial port with output processing */ + + /* Try to set baud rate */ + struct termios uart_config; + int termios_state; + + /* Back up the original uart configuration to restore it after exit */ + if ((termios_state = tcgetattr(fd, &uart_config)) < 0) { + warnx("ERROR get termios config %s: %d\n", argv[1], termios_state); + close(fd); + return -1; + } + + /* Set ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag |= (ONLCR | OPOST | OCRNL); + + if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) { + warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", argv[1]); + close(fd); + return -1; + } + + /* setup standard file descriptors */ close(0); close(1); close(2); From ffc2a8b7a358a2003e5ed548c41878b33e7aa726 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 18 Aug 2013 23:05:26 +0200 Subject: [PATCH 402/486] vehicle_local_position topic updated, position_estimator_inav and commander fixes, only altitude estimate required for SETBELT now. --- src/modules/commander/commander.cpp | 100 +++++---- .../commander/state_machine_helper.cpp | 27 +-- .../multirotor_pos_control.c | 20 +- .../position_estimator_inav_main.c | 195 +++++++++--------- .../position_estimator_inav_params.c | 3 - .../position_estimator_inav_params.h | 2 - src/modules/sdlog2/sdlog2.c | 7 +- src/modules/sdlog2/sdlog2_messages.h | 9 +- .../uORB/topics/vehicle_local_position.h | 39 ++-- src/modules/uORB/topics/vehicle_status.h | 4 +- 10 files changed, 194 insertions(+), 212 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d40f6675b5..ab7d2e6cf6 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -551,7 +551,7 @@ int commander_thread_main(int argc, char *argv[]) memset(&control_mode, 0, sizeof(control_mode)); status.main_state = MAIN_STATE_MANUAL; - status.navigation_state = NAVIGATION_STATE_STANDBY; + status.navigation_state = NAVIGATION_STATE_DIRECT; status.arming_state = ARMING_STATE_INIT; status.hil_state = HIL_STATE_OFF; @@ -812,8 +812,9 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); } - /* update condition_local_position_valid */ - check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.valid, &(status.condition_local_position_valid), &status_changed); + /* update condition_local_position_valid and condition_local_altitude_valid */ + check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(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); /* update battery status */ orb_check(battery_sub, &updated); @@ -1512,68 +1513,61 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v { transition_result_t res = TRANSITION_DENIED; - if (current_status->arming_state == ARMING_STATE_ARMED || current_status->arming_state == ARMING_STATE_ARMED_ERROR) { - /* ARMED */ - switch (current_status->main_state) { - case MAIN_STATE_MANUAL: - res = navigation_state_transition(current_status, current_status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode); - break; + switch (current_status->main_state) { + case MAIN_STATE_MANUAL: + res = navigation_state_transition(current_status, current_status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode); + break; - case MAIN_STATE_SEATBELT: - res = navigation_state_transition(current_status, NAVIGATION_STATE_ALTHOLD, control_mode); - break; + case MAIN_STATE_SEATBELT: + res = navigation_state_transition(current_status, NAVIGATION_STATE_ALTHOLD, control_mode); + break; - case MAIN_STATE_EASY: - res = navigation_state_transition(current_status, NAVIGATION_STATE_VECTOR, control_mode); - break; + case MAIN_STATE_EASY: + res = navigation_state_transition(current_status, NAVIGATION_STATE_VECTOR, control_mode); + break; - case MAIN_STATE_AUTO: - if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - /* don't act while taking off */ - res = TRANSITION_NOT_CHANGED; + case MAIN_STATE_AUTO: + if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + /* don't act while taking off */ + res = TRANSITION_NOT_CHANGED; + + } else { + if (current_status->condition_landed) { + /* if landed: transitions only to AUTO_READY are allowed */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); + // TRANSITION_DENIED is not possible here + break; } else { - if (current_status->condition_landed) { - /* if landed: transitions only to AUTO_READY are allowed */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); - // TRANSITION_DENIED is not possible here - break; - - } else { - /* if not landed: */ - if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) { - /* act depending on switches when manual control enabled */ - if (current_status->return_switch == RETURN_SWITCH_RETURN) { - /* RTL */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); - - } else { - if (current_status->mission_switch == MISSION_SWITCH_MISSION) { - /* MISSION */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); - - } else { - /* LOITER */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); - } - } + /* if not landed: */ + if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) { + /* act depending on switches when manual control enabled */ + if (current_status->return_switch == RETURN_SWITCH_RETURN) { + /* RTL */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); } else { - /* always switch to loiter in air when no RC control */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + if (current_status->mission_switch == MISSION_SWITCH_MISSION) { + /* MISSION */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); + + } else { + /* LOITER */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + } } + + } else { + /* always switch to loiter in air when no RC control */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); } } - - break; - - default: - break; } - } else { - /* DISARMED */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_STANDBY, control_mode); + break; + + default: + break; } return res; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index f313adce47..76c7eaf92f 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -217,9 +217,8 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m case MAIN_STATE_SEATBELT: - /* need position estimate */ - // TODO only need altitude estimate really - if (current_state->condition_local_position_valid) { + /* need altitude estimate */ + if (current_state->condition_local_altitude_valid) { ret = TRANSITION_CHANGED; } @@ -227,7 +226,7 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m case MAIN_STATE_EASY: - /* need position estimate */ + /* need local position estimate */ if (current_state->condition_local_position_valid) { ret = TRANSITION_CHANGED; } @@ -236,8 +235,8 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m case MAIN_STATE_AUTO: - /* need position estimate */ - if (current_state->condition_local_position_valid) { + /* need global position estimate */ + if (current_state->condition_global_position_valid) { ret = TRANSITION_CHANGED; } @@ -277,17 +276,6 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ } else { switch (new_navigation_state) { - case NAVIGATION_STATE_STANDBY: - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = false; - control_mode->flag_control_attitude_enabled = false; - control_mode->flag_control_velocity_enabled = false; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = false; - control_mode->flag_control_climb_rate_enabled = false; - control_mode->flag_control_manual_enabled = false; - break; - case NAVIGATION_STATE_DIRECT: ret = TRANSITION_CHANGED; control_mode->flag_control_rates_enabled = true; @@ -394,9 +382,8 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ case NAVIGATION_STATE_AUTO_LAND: - /* deny transitions from landed states */ - if (current_status->navigation_state != NAVIGATION_STATE_STANDBY && - current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { + /* deny transitions from landed state */ + if (current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { ret = TRANSITION_CHANGED; control_mode->flag_control_rates_enabled = true; control_mode->flag_control_attitude_enabled = true; diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 1cb4702409..80ce33cda5 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -219,7 +219,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) const float pos_ctl_dz = 0.05f; float home_alt = 0.0f; hrt_abstime home_alt_t = 0; - uint64_t local_home_timestamp = 0; + uint64_t local_ref_timestamp = 0; PID_t xy_pos_pids[2]; PID_t xy_vel_pids[2]; @@ -316,11 +316,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } else { /* non-manual mode, project global setpoints to local frame */ //orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); - if (local_pos.home_timestamp != local_home_timestamp) { - local_home_timestamp = local_pos.home_timestamp; + if (local_pos.ref_timestamp != local_ref_timestamp) { + local_ref_timestamp = local_pos.ref_timestamp; /* init local projection using local position home */ - double lat_home = local_pos.home_lat * 1e-7; - double lon_home = local_pos.home_lon * 1e-7; + double lat_home = local_pos.ref_lat * 1e-7; + double lon_home = local_pos.ref_lon * 1e-7; map_projection_init(lat_home, lon_home); warnx("local pos home: lat = %.10f, lon = %.10f", lat_home, lon_home); mavlink_log_info(mavlink_fd, "local pos home: %.7f, %.7f", lat_home, lon_home); @@ -338,7 +338,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) local_pos_sp.z = -global_pos_sp.altitude; } else { - local_pos_sp.z = local_pos.home_alt - global_pos_sp.altitude; + local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; } warnx("new setpoint: lat = %.10f, lon = %.10f, x = %.2f, y = %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); @@ -355,14 +355,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* manual control - move setpoints */ if (control_mode.flag_control_manual_enabled) { - if (local_pos.home_timestamp != home_alt_t) { + if (local_pos.ref_timestamp != home_alt_t) { if (home_alt_t != 0) { /* home alt changed, don't follow large ground level changes in manual flight */ - local_pos_sp.z += local_pos.home_alt - home_alt; + local_pos_sp.z += local_pos.ref_alt - home_alt; } - home_alt_t = local_pos.home_timestamp; - home_alt = local_pos.home_alt; + home_alt_t = local_pos.ref_timestamp; + home_alt = local_pos.ref_alt; } if (control_mode.flag_control_altitude_enabled) { diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 0e7e0ef5d7..81f938719c 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -76,6 +76,7 @@ static bool thread_running = false; /**< Deamon status flag */ static int position_estimator_inav_task; /**< Handle of deamon task / thread */ static bool verbose_mode = false; static hrt_abstime gps_timeout = 1000000; // GPS timeout = 1s +static hrt_abstime flow_timeout = 1000000; // optical flow timeout = 1s __EXPORT int position_estimator_inav_main(int argc, char *argv[]); @@ -169,10 +170,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) int baro_init_num = 200; float baro_alt0 = 0.0f; /* to determine while start up */ - double lat_current = 0.0f; //[°] --> 47.0 - double lon_current = 0.0f; //[°] --> 8.5 - double alt_current = 0.0f; //[m] above MSL - uint32_t accel_counter = 0; uint32_t baro_counter = 0; @@ -216,21 +213,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* first parameters update */ parameters_update(&pos_inav_param_handles, ¶ms); - struct pollfd fds_init[2] = { + struct pollfd fds_init[1] = { { .fd = sensor_combined_sub, .events = POLLIN }, - { .fd = vehicle_gps_position_sub, .events = POLLIN } }; - /* wait for initial sensors values: baro, GPS fix, only then can we initialize the projection */ - bool wait_gps = params.use_gps; + /* wait for initial baro value */ bool wait_baro = true; - hrt_abstime wait_gps_start = 0; - const hrt_abstime wait_gps_delay = 5000000; // wait for 5s after 3D fix thread_running = true; - while ((wait_gps || wait_baro) && !thread_should_exit) { - int ret = poll(fds_init, params.use_gps ? 2 : 1, 1000); + while (wait_baro && !thread_should_exit) { + int ret = poll(fds_init, 1, 1000); if (ret < 0) { /* poll error */ @@ -253,43 +246,21 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) baro_alt0 /= (float) baro_init_cnt; warnx("init baro: alt = %.3f", baro_alt0); mavlink_log_info(mavlink_fd, "[inav] init baro: alt = %.3f", baro_alt0); - local_pos.home_alt = baro_alt0; - local_pos.home_timestamp = hrt_absolute_time(); - } - } - } - - if (params.use_gps && (fds_init[1].revents & POLLIN)) { - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - - if (wait_gps && gps.fix_type >= 3) { - hrt_abstime t = hrt_absolute_time(); - - if (wait_gps_start == 0) { - wait_gps_start = t; - - } else if (t - wait_gps_start > wait_gps_delay) { - wait_gps = false; - /* get GPS position for first initialization */ - lat_current = gps.lat * 1e-7; - lon_current = gps.lon * 1e-7; - alt_current = gps.alt * 1e-3; - - local_pos.home_lat = lat_current * 1e7; - local_pos.home_lon = lon_current * 1e7; - local_pos.home_hdg = 0.0f; - local_pos.home_timestamp = t; - - /* initialize coordinates */ - map_projection_init(lat_current, lon_current); - warnx("init GPS: lat = %.10f, lon = %.10f", lat_current, lon_current); - mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat_current, lon_current); + local_pos.ref_alt = baro_alt0; + local_pos.ref_timestamp = hrt_absolute_time(); + local_pos.z_valid = true; + local_pos.v_z_valid = true; + local_pos.global_z = true; } } } } } + bool ref_xy_inited = false; + hrt_abstime ref_xy_init_start = 0; + const hrt_abstime ref_xy_init_delay = 5000000; // wait for 5s after 3D fix + hrt_abstime t_prev = 0; uint16_t accel_updates = 0; @@ -337,7 +308,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } while (!thread_should_exit) { - int ret = poll(fds, params.use_gps ? 6 : 5, 10); // wait maximal this 10 ms = 100 Hz minimum rate + int ret = poll(fds, 6, 10); // wait maximal this 10 ms = 100 Hz minimum rate hrt_abstime t = hrt_absolute_time(); if (ret < 0) { @@ -428,8 +399,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) baro_alt0 += sonar_corr; warnx("new home: alt = %.3f", baro_alt0); mavlink_log_info(mavlink_fd, "[inav] new home: alt = %.3f", baro_alt0); - local_pos.home_alt = baro_alt0; - local_pos.home_timestamp = hrt_absolute_time(); + local_pos.ref_alt = baro_alt0; + local_pos.ref_timestamp = hrt_absolute_time(); z_est[0] += sonar_corr; sonar_corr = 0.0f; sonar_corr_filtered = 0.0f; @@ -444,29 +415,57 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) flow_updates++; } - if (params.use_gps && (fds[5].revents & POLLIN)) { + if (fds[5].revents & POLLIN) { /* vehicle GPS position */ orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); if (gps.fix_type >= 3) { - /* project GPS lat lon to plane */ - float gps_proj[2]; - map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1])); - gps_corr[0][0] = gps_proj[0] - x_est[0]; - gps_corr[1][0] = gps_proj[1] - y_est[0]; + /* initialize reference position if needed */ + if (ref_xy_inited) { + /* project GPS lat lon to plane */ + float gps_proj[2]; + map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1])); + /* calculate correction for position */ + gps_corr[0][0] = gps_proj[0] - x_est[0]; + gps_corr[1][0] = gps_proj[1] - y_est[0]; - if (gps.vel_ned_valid) { - gps_corr[0][1] = gps.vel_n_m_s - x_est[1]; - gps_corr[1][1] = gps.vel_e_m_s - y_est[1]; + /* calculate correction for velocity */ + if (gps.vel_ned_valid) { + gps_corr[0][1] = gps.vel_n_m_s - x_est[1]; + gps_corr[1][1] = gps.vel_e_m_s - y_est[1]; + + } else { + gps_corr[0][1] = 0.0f; + gps_corr[1][1] = 0.0f; + } } else { - gps_corr[0][1] = 0.0f; - gps_corr[1][1] = 0.0f; + hrt_abstime t = hrt_absolute_time(); + + if (ref_xy_init_start == 0) { + ref_xy_init_start = t; + + } else if (t > ref_xy_init_start + ref_xy_init_delay) { + ref_xy_inited = true; + /* reference GPS position */ + double lat = gps.lat * 1e-7; + double lon = gps.lon * 1e-7; + + local_pos.ref_lat = gps.lat; + local_pos.ref_lon = gps.lon; + local_pos.ref_timestamp = t; + + /* initialize projection */ + map_projection_init(lat, lon); + warnx("init GPS: lat = %.10f, lon = %.10f", lat, lon); + mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat, lon); + } } gps_updates++; } else { + /* no GPS lock */ memset(gps_corr, 0, sizeof(gps_corr)); } } @@ -490,10 +489,14 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(sonar_corr, dt, z_est, 0, params.w_alt_sonar); inertial_filter_correct(accel_corr[2], dt, z_est, 2, params.w_alt_acc); - /* dont't try to estimate position when no any position source available */ - bool can_estimate_pos = params.use_gps && gps.fix_type >= 3 && t - gps.timestamp_position < gps_timeout; + bool gps_valid = ref_xy_inited && gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout; + bool flow_valid = false; // TODO implement opt flow - if (can_estimate_pos) { + /* try to estimate xy even if no absolute position source available, + * if using optical flow velocity will be correct in this case */ + bool can_estimate_xy = gps_valid || flow_valid; + + if (can_estimate_xy) { /* inertial filter prediction for position */ inertial_filter_predict(dt, x_est); inertial_filter_predict(dt, y_est); @@ -502,12 +505,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(accel_corr[0], dt, x_est, 2, params.w_pos_acc); inertial_filter_correct(accel_corr[1], dt, y_est, 2, params.w_pos_acc); - inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p); - inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p); - - if (gps.vel_ned_valid && t - gps.timestamp_velocity < gps_timeout) { - inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v); - inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v); + if (gps_valid) { + inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p); + inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p); + if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_timeout) { + inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v); + inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v); + } } } @@ -533,43 +537,48 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (t - pub_last > pub_interval) { pub_last = t; + /* publish local position */ local_pos.timestamp = t; - local_pos.valid = can_estimate_pos; + local_pos.xy_valid = can_estimate_xy && gps_valid; + local_pos.v_xy_valid = can_estimate_xy; + local_pos.global_xy = local_pos.xy_valid && gps_valid; // will make sense when local position sources (e.g. vicon) will be implemented local_pos.x = x_est[0]; local_pos.vx = x_est[1]; local_pos.y = y_est[0]; local_pos.vy = y_est[1]; local_pos.z = z_est[0]; local_pos.vz = z_est[1]; - local_pos.absolute_alt = local_pos.home_alt - local_pos.z; - local_pos.hdg = att.yaw; + local_pos.landed = false; // TODO - if ((isfinite(local_pos.x)) && (isfinite(local_pos.vx)) - && (isfinite(local_pos.y)) - && (isfinite(local_pos.vy)) - && (isfinite(local_pos.z)) - && (isfinite(local_pos.vz))) { - orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); + orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); - if (params.use_gps) { - double est_lat, est_lon; - map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); - - global_pos.valid = local_pos.valid; - global_pos.timestamp = t; - global_pos.time_gps_usec = gps.time_gps_usec; - global_pos.lat = (int32_t)(est_lat * 1e7); - global_pos.lon = (int32_t)(est_lon * 1e7); - global_pos.alt = local_pos.home_alt - local_pos.z; - global_pos.relative_alt = -local_pos.z; - global_pos.vx = local_pos.vx; - global_pos.vy = local_pos.vy; - global_pos.vz = local_pos.vz; - global_pos.hdg = local_pos.hdg; - - orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos); - } + /* publish global position */ + global_pos.valid = local_pos.global_xy; + if (local_pos.global_xy) { + double est_lat, est_lon; + map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); + global_pos.lat = (int32_t)(est_lat * 1e7); + global_pos.lon = (int32_t)(est_lon * 1e7); + global_pos.time_gps_usec = gps.time_gps_usec; } + /* set valid values even if position is not valid */ + if (local_pos.v_xy_valid) { + global_pos.vx = local_pos.vx; + global_pos.vy = local_pos.vy; + global_pos.hdg = atan2f(local_pos.vy, local_pos.vx); + } + if (local_pos.z_valid) { + global_pos.relative_alt = -local_pos.z; + } + if (local_pos.global_z) { + global_pos.alt = local_pos.ref_alt - local_pos.z; + } + if (local_pos.v_z_valid) { + global_pos.vz = local_pos.vz; + } + global_pos.timestamp = t; + + orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos); } } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 70248b3b72..801e207816 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -40,7 +40,6 @@ #include "position_estimator_inav_params.h" -PARAM_DEFINE_INT32(INAV_USE_GPS, 1); PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 1.0f); PARAM_DEFINE_FLOAT(INAV_W_ALT_ACC, 50.0f); PARAM_DEFINE_FLOAT(INAV_W_ALT_SONAR, 3.0f); @@ -55,7 +54,6 @@ PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); int parameters_init(struct position_estimator_inav_param_handles *h) { - h->use_gps = param_find("INAV_USE_GPS"); h->w_alt_baro = param_find("INAV_W_ALT_BARO"); h->w_alt_acc = param_find("INAV_W_ALT_ACC"); h->w_alt_sonar = param_find("INAV_W_ALT_SONAR"); @@ -73,7 +71,6 @@ int parameters_init(struct position_estimator_inav_param_handles *h) int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) { - param_get(h->use_gps, &(p->use_gps)); param_get(h->w_alt_baro, &(p->w_alt_baro)); param_get(h->w_alt_acc, &(p->w_alt_acc)); param_get(h->w_alt_sonar, &(p->w_alt_sonar)); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index 1e70a3c48a..ed6f61e04c 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -41,7 +41,6 @@ #include struct position_estimator_inav_params { - int use_gps; float w_alt_baro; float w_alt_acc; float w_alt_sonar; @@ -56,7 +55,6 @@ struct position_estimator_inav_params { }; struct position_estimator_inav_param_handles { - param_t use_gps; param_t w_alt_baro; param_t w_alt_acc; param_t w_alt_sonar; diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 30dc7df9e1..7f8648d95d 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1042,10 +1042,9 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_LPOS.vx = buf.local_pos.vx; log_msg.body.log_LPOS.vy = buf.local_pos.vy; log_msg.body.log_LPOS.vz = buf.local_pos.vz; - log_msg.body.log_LPOS.hdg = buf.local_pos.hdg; - log_msg.body.log_LPOS.home_lat = buf.local_pos.home_lat; - log_msg.body.log_LPOS.home_lon = buf.local_pos.home_lon; - log_msg.body.log_LPOS.home_alt = buf.local_pos.home_alt; + log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat; + log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon; + log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt; LOGBUFFER_WRITE_AND_COUNT(LPOS); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 38e2596b2a..dd98f65a9c 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -106,10 +106,9 @@ struct log_LPOS_s { float vx; float vy; float vz; - float hdg; - int32_t home_lat; - int32_t home_lon; - float home_alt; + int32_t ref_lat; + int32_t ref_lon; + float ref_alt; }; /* --- LPSP - LOCAL POSITION SETPOINT --- */ @@ -260,7 +259,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"), - LOG_FORMAT(LPOS, "fffffffLLf", "X,Y,Z,VX,VY,VZ,Heading,HomeLat,HomeLon,HomeAlt"), + LOG_FORMAT(LPOS, "fffffffLLf", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt"), LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index 9d3b4468c6..26e8f335b5 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -54,27 +54,26 @@ */ struct vehicle_local_position_s { - uint64_t timestamp; /**< time of this estimate, in microseconds since system start */ - bool valid; /**< true if position satisfies validity criteria of estimator */ - - float x; /**< X positin in meters in NED earth-fixed frame */ - float y; /**< X positin in meters in NED earth-fixed frame */ - float z; /**< Z positin in meters in NED earth-fixed frame (negative altitude) */ - float absolute_alt; /**< Altitude as defined by pressure / GPS, LOGME */ - float vx; /**< Ground X Speed (Latitude), m/s in NED LOGME */ - float vy; /**< Ground Y Speed (Longitude), m/s in NED LOGME */ - float vz; /**< Ground Z Speed (Altitude), m/s in NED LOGME */ - float hdg; /**< Compass heading in radians -PI..+PI. */ - - // TODO Add covariances here - + uint64_t timestamp; /**< Time of this estimate, in microseconds since system start */ + bool xy_valid; /**< true if x and y are valid */ + bool z_valid; /**< true if z is valid */ + bool v_xy_valid; /**< true if vy and vy are valid */ + bool v_z_valid; /**< true if vz is valid */ + /* Position in local NED frame */ + float x; /**< X position in meters in NED earth-fixed frame */ + float y; /**< X position in meters in NED earth-fixed frame */ + float z; /**< Z position in meters in NED earth-fixed frame (negative altitude) */ + /* Velocity in NED frame */ + float vx; /**< Ground X Speed (Latitude), m/s in NED */ + float vy; /**< Ground Y Speed (Longitude), m/s in NED */ + float vz; /**< Ground Z Speed (Altitude), m/s in NED */ /* Reference position in GPS / WGS84 frame */ - uint64_t home_timestamp;/**< Time when home position was set */ - int32_t home_lat; /**< Latitude in 1E7 degrees LOGME */ - int32_t home_lon; /**< Longitude in 1E7 degrees LOGME */ - float home_alt; /**< Altitude in meters LOGME */ - float home_hdg; /**< Compass heading in radians -PI..+PI. */ - + bool global_xy; /**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */ + bool global_z; /**< true if z is valid and has valid global reference (ref_alt) */ + uint64_t ref_timestamp; /**< Time when reference position was set */ + int32_t ref_lat; /**< Reference point latitude in 1E7 degrees */ + int32_t ref_lon; /**< Reference point longitude in 1E7 degrees */ + float ref_alt; /**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */ bool landed; /**< true if vehicle is landed */ }; diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 6690fb2be2..4317e07f25 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -68,8 +68,7 @@ typedef enum { /* navigation state machine */ typedef enum { - NAVIGATION_STATE_STANDBY = 0, // standby state, disarmed - NAVIGATION_STATE_DIRECT, // true manual control, no any stabilization + NAVIGATION_STATE_DIRECT = 0, // true manual control, no any stabilization NAVIGATION_STATE_STABILIZE, // attitude stabilization NAVIGATION_STATE_ALTHOLD, // attitude + altitude stabilization NAVIGATION_STATE_VECTOR, // attitude + altitude + position stabilization @@ -203,6 +202,7 @@ struct vehicle_status_s bool condition_launch_position_valid; /**< indicates a valid launch position */ bool condition_home_position_valid; /**< indicates a valid home position (a valid home position is not always a valid launch) */ bool condition_local_position_valid; + bool condition_local_altitude_valid; bool condition_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */ bool condition_landed; /**< true if vehicle is landed, always true if disarmed */ From 3370ceceaf706dda0856888b09c1086e8bf79c8d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 19 Aug 2013 08:43:16 +0200 Subject: [PATCH 403/486] vehicle_control_mode.flag_armed added, reset integrals in multirotor_att_control when disarmed --- src/modules/commander/commander.cpp | 3 +- .../multirotor_att_control_main.c | 33 ++++++++----------- .../multirotor_attitude_control.c | 9 ++--- .../multirotor_attitude_control.h | 2 +- .../multirotor_rate_control.c | 7 ++-- .../multirotor_rate_control.h | 2 +- .../uORB/topics/vehicle_control_mode.h | 2 ++ 7 files changed, 29 insertions(+), 29 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ab7d2e6cf6..7d74e0cfe2 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1235,8 +1235,9 @@ int commander_thread_main(int argc, char *argv[]) } /* publish control mode */ - if (navigation_state_changed) { + if (navigation_state_changed || arming_state_changed) { /* publish new navigation state */ + control_mode.flag_armed = armed.armed; // copy armed state to vehicle_control_mode topic control_mode.counter++; control_mode.timestamp = t1; orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 5cad667f61..c057ef3647 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -142,16 +142,13 @@ mc_thread_main(int argc, char *argv[]) perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "multirotor_att_control_err"); /* welcome user */ - printf("[multirotor_att_control] starting\n"); + warnx("starting"); /* store last control mode to detect mode switches */ bool flag_control_manual_enabled = false; bool flag_control_attitude_enabled = false; - /* store if yaw position or yaw speed has been changed */ bool control_yaw_position = true; - - /* store if we stopped a yaw movement */ bool reset_yaw_sp = true; /* prepare the handle for the failsafe throttle */ @@ -211,8 +208,7 @@ mc_thread_main(int argc, char *argv[]) /* get a local copy of the current sensor values */ orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); - - /** STEP 1: Define which input is the dominating control input */ + /* define which input is the dominating control input */ if (control_mode.flag_control_offboard_enabled) { /* offboard inputs */ if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) { @@ -220,7 +216,6 @@ mc_thread_main(int argc, char *argv[]) rates_sp.pitch = offboard_sp.p2; rates_sp.yaw = offboard_sp.p3; rates_sp.thrust = offboard_sp.p4; -// printf("thrust_rate=%8.4f\n",offboard_sp.p4); rates_sp.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); @@ -229,13 +224,11 @@ mc_thread_main(int argc, char *argv[]) att_sp.pitch_body = offboard_sp.p2; att_sp.yaw_body = offboard_sp.p3; att_sp.thrust = offboard_sp.p4; -// printf("thrust_att=%8.4f\n",offboard_sp.p4); att_sp.timestamp = hrt_absolute_time(); /* STEP 2: publish the result to the vehicle actuators */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); } - } else if (control_mode.flag_control_manual_enabled) { /* direct manual input */ if (control_mode.flag_control_attitude_enabled) { @@ -265,7 +258,7 @@ mc_thread_main(int argc, char *argv[]) * multicopter (throttle = 0) does not make it jump up in the air * if shutting down his remote. */ - if (isfinite(manual.throttle) && manual.throttle > 0.2f) { + if (isfinite(manual.throttle) && manual.throttle > 0.2f) { // TODO use landed status instead of throttle /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ param_get(failsafe_throttle_handle, &failsafe_throttle); att_sp.thrust = failsafe_throttle; @@ -305,7 +298,6 @@ mc_thread_main(int argc, char *argv[]) att_sp.yaw_body = att.yaw; reset_yaw_sp = false; } - control_yaw_position = true; } @@ -347,22 +339,25 @@ mc_thread_main(int argc, char *argv[]) } } - /** STEP 3: Identify the controller setup to run and set up the inputs correctly */ - if (control_mode.flag_control_attitude_enabled) { - multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position); + /* check if we should we reset integrals */ + bool reset_integral = !control_mode.flag_armed || att_sp.thrust < 0.1f; // TODO use landed status instead of throttle + /* run attitude controller if needed */ + if (control_mode.flag_control_attitude_enabled) { + multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position, reset_integral); orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); } /* measure in what intervals the controller runs */ perf_count(mc_interval_perf); + /* run rates controller if needed */ if (control_mode.flag_control_rates_enabled) { /* get current rate setpoint */ - bool rates_sp_valid = false; - orb_check(rates_sp_sub, &rates_sp_valid); + bool rates_sp_updated = false; + orb_check(rates_sp_sub, &rates_sp_updated); - if (rates_sp_valid) { + if (rates_sp_updated) { orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp); } @@ -371,7 +366,7 @@ mc_thread_main(int argc, char *argv[]) rates[0] = att.rollspeed; rates[1] = att.pitchspeed; rates[2] = att.yawspeed; - multirotor_control_rates(&rates_sp, rates, &actuators); + multirotor_control_rates(&rates_sp, rates, &actuators, reset_integral); } else { /* rates controller disabled, set actuators to zero for safety */ actuators.control[0] = 0.0f; @@ -391,7 +386,7 @@ mc_thread_main(int argc, char *argv[]) } /* end of poll return value check */ } - printf("[multirotor att control] stopping, disarming motors.\n"); + warnx("stopping, disarming motors"); /* kill all outputs */ for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index 8f19c6a4bc..12d16f7c73 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -166,7 +166,7 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc } void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position) + const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position, bool reset_integral) { static uint64_t last_run = 0; static uint64_t last_input = 0; @@ -210,13 +210,13 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); } - /* reset integral if on ground */ - if (att_sp->thrust < 0.1f) { + /* reset integrals if needed */ + if (reset_integral) { pid_reset_integral(&pitch_controller); pid_reset_integral(&roll_controller); + //TODO pid_reset_integral(&yaw_controller); } - /* calculate current control outputs */ /* control pitch (forward) output */ @@ -229,6 +229,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s if (control_yaw_position) { /* control yaw rate */ + // TODO use pid lib /* positive error: rotate to right, negative error, rotate to left (NED frame) */ // yaw_error = _wrap_pi(att_sp->yaw_body - att->yaw); diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.h b/src/modules/multirotor_att_control/multirotor_attitude_control.h index e78f45c474..431a435f7d 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.h +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.h @@ -60,6 +60,6 @@ #include void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position); + const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position, bool reset_integral); #endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */ diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index e58d357d58..0a336be47d 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -152,7 +152,7 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru } void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], struct actuator_controls_s *actuators) + const float rates[], struct actuator_controls_s *actuators, bool reset_integral) { static uint64_t last_run = 0; const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; @@ -193,10 +193,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f); } - /* reset integral if on ground */ - if (rate_sp->thrust < 0.01f) { + /* reset integrals if needed */ + if (reset_integral) { pid_reset_integral(&pitch_rate_controller); pid_reset_integral(&roll_rate_controller); + // TODO pid_reset_integral(&yaw_rate_controller); } /* control pitch (forward) output */ diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.h b/src/modules/multirotor_att_control/multirotor_rate_control.h index 362b5ed86d..ca7794c59b 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.h +++ b/src/modules/multirotor_att_control/multirotor_rate_control.h @@ -59,6 +59,6 @@ #include void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], struct actuator_controls_s *actuators); + const float rates[], struct actuator_controls_s *actuators, bool reset_integral); #endif /* MULTIROTOR_RATE_CONTROL_H_ */ diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index fe169c6e6b..67aeac3728 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -64,6 +64,8 @@ struct vehicle_control_mode_s uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ + bool flag_armed; + bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ // XXX needs yet to be set by state machine helper From f96bb824d4fc6f6d36ddf24e8879d3025af39d70 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 19 Aug 2013 09:31:33 +0200 Subject: [PATCH 404/486] multirotor_pos_control: reset integrals when disarmed --- .../multirotor_pos_control.c | 16 ++++++++++++---- src/modules/multirotor_pos_control/thrust_pid.c | 5 +++++ src/modules/multirotor_pos_control/thrust_pid.h | 1 + 3 files changed, 18 insertions(+), 4 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 80ce33cda5..b2f6b33e3c 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -301,7 +301,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (reset_sp_alt) { reset_sp_alt = false; local_pos_sp.z = local_pos.z; - z_vel_pid.integral = -manual.throttle; // thrust PID uses Z downside + thrust_pid_set_integral(&z_vel_pid, -manual.throttle); // thrust PID uses Z downside mavlink_log_info(mavlink_fd, "reset alt setpoint: z = %.2f, throttle = %.2f", local_pos_sp.z, manual.throttle); } @@ -309,8 +309,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) reset_sp_pos = false; local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; - xy_vel_pids[0].integral = 0.0f; - xy_vel_pids[1].integral = 0.0f; + pid_reset_integral(&xy_vel_pids[0]); + pid_reset_integral(&xy_vel_pids[1]); mavlink_log_info(mavlink_fd, "reset pos setpoint: x = %.2f, y = %.2f", local_pos_sp.x, local_pos_sp.y); } } else { @@ -439,17 +439,25 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* publish new velocity setpoint */ orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp); - // TODO subcrive to velocity setpoint if altitude/position control disabled + // TODO subscribe to velocity setpoint if altitude/position control disabled if (control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) { /* run velocity controllers, calculate thrust vector with attitude-thrust compensation */ float thrust_sp[3] = { 0.0f, 0.0f, 0.0f }; + bool reset_integral = !control_mode.flag_armed; if (control_mode.flag_control_climb_rate_enabled) { + if (reset_integral) { + thrust_pid_set_integral(&z_vel_pid, params.thr_min); + } thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]); att_sp.thrust = -thrust_sp[2]; } if (control_mode.flag_control_velocity_enabled) { /* calculate thrust set point in NED frame */ + if (reset_integral) { + pid_reset_integral(&xy_vel_pids[0]); + pid_reset_integral(&xy_vel_pids[1]); + } thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt); thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt); diff --git a/src/modules/multirotor_pos_control/thrust_pid.c b/src/modules/multirotor_pos_control/thrust_pid.c index 51dcd7df24..b985630aec 100644 --- a/src/modules/multirotor_pos_control/thrust_pid.c +++ b/src/modules/multirotor_pos_control/thrust_pid.c @@ -182,3 +182,8 @@ __EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, floa return pid->last_output; } + +__EXPORT void thrust_pid_set_integral(thrust_pid_t *pid, float i) +{ + pid->integral = i; +} diff --git a/src/modules/multirotor_pos_control/thrust_pid.h b/src/modules/multirotor_pos_control/thrust_pid.h index 551c032fe2..5e169c1ba5 100644 --- a/src/modules/multirotor_pos_control/thrust_pid.h +++ b/src/modules/multirotor_pos_control/thrust_pid.h @@ -69,6 +69,7 @@ typedef struct { __EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min); __EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max); __EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22); +__EXPORT void thrust_pid_set_integral(thrust_pid_t *pid, float i); __END_DECLS From 9610f7a0d7ba7abf7d88c4b3096285e3da68e04d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Aug 2013 09:53:00 +0200 Subject: [PATCH 405/486] Fixed merge compile errors --- src/modules/commander/commander.cpp | 103 ++++++++++++---------------- 1 file changed, 42 insertions(+), 61 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 0f145e1eb1..daab7e4360 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -418,23 +418,14 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: if (is_safe(status, safety, armed)) { -<<<<<<< HEAD - if (((int)(cmd->param1)) == 1) { - /* reboot */ - systemreset(false); - } else if (((int)(cmd->param1)) == 3) { - /* reboot to bootloader */ -======= if (((int)(cmd->param1)) == 1) { /* reboot */ - up_systemreset(); + systemreset(false); } else if (((int)(cmd->param1)) == 3) { /* reboot to bootloader */ - // XXX implement - result = VEHICLE_CMD_RESULT_UNSUPPORTED; ->>>>>>> f96bb824d4fc6f6d36ddf24e8879d3025af39d70 + systemreset(true); } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -768,13 +759,7 @@ int commander_thread_main(int argc, char *argv[]) start_time = hrt_absolute_time(); while (!thread_should_exit) { -<<<<<<< HEAD - hrt_abstime t = hrt_absolute_time(); - status_changed = false; - -======= ->>>>>>> f96bb824d4fc6f6d36ddf24e8879d3025af39d70 /* update parameters */ orb_check(param_changed_sub, &updated); @@ -882,17 +867,11 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(battery_status), battery_sub, &battery); -<<<<<<< HEAD - /* only consider battery voltage if system has been running 2s and battery voltage is not 0 */ - if ((t - start_time) > 2000000 && battery.voltage_v > 0.001f) { -======= - - warnx("bat v: %2.2f", battery.voltage_v); + // warnx("bat v: %2.2f", battery.voltage_v); /* only consider battery voltage if system has been running 2s and battery voltage is not 0 */ if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 0.001f) { ->>>>>>> f96bb824d4fc6f6d36ddf24e8879d3025af39d70 status.battery_voltage = battery.voltage_v; status.condition_battery_voltage_valid = true; status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage); @@ -1010,39 +989,39 @@ int commander_thread_main(int argc, char *argv[]) * set of position measurements is available. */ -<<<<<<< HEAD - /* store current state to reason later about a state change */ - // bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; - // bool global_pos_valid = status.condition_global_position_valid; - // bool local_pos_valid = status.condition_local_position_valid; - // bool airspeed_valid = status.condition_airspeed_valid; +// <<<<<<< HEAD +// /* store current state to reason later about a state change */ +// // bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; +// // bool global_pos_valid = status.condition_global_position_valid; +// // bool local_pos_valid = status.condition_local_position_valid; +// // bool airspeed_valid = status.condition_airspeed_valid; - /* check for global or local position updates, set a timeout of 2s */ - if (t - global_position.timestamp < 2000000 && t > 2000000 && global_position.valid) { - status.condition_global_position_valid = true; +// /* check for global or local position updates, set a timeout of 2s */ +// if (t - global_position.timestamp < 2000000 && t > 2000000 && global_position.valid) { +// status.condition_global_position_valid = true; - } else { - status.condition_global_position_valid = false; - } +// } else { +// status.condition_global_position_valid = false; +// } - if (t - local_position.timestamp < 2000000 && t > 2000000 && local_position.valid) { - status.condition_local_position_valid = true; +// if (t - local_position.timestamp < 2000000 && t > 2000000 && local_position.valid) { +// status.condition_local_position_valid = true; - } else { - status.condition_local_position_valid = false; - } +// } else { +// status.condition_local_position_valid = false; +// } - /* Check for valid airspeed/differential pressure measurements */ - if (t - last_diff_pres_time < 2000000 && t > 2000000) { - status.condition_airspeed_valid = true; +// /* Check for valid airspeed/differential pressure measurements */ +// if (t - last_diff_pres_time < 2000000 && t > 2000000) { +// status.condition_airspeed_valid = true; - } else { - status.condition_airspeed_valid = false; - } +// } else { +// status.condition_airspeed_valid = false; +// } -======= ->>>>>>> f96bb824d4fc6f6d36ddf24e8879d3025af39d70 +// ======= +// >>>>>>> f96bb824d4fc6f6d36ddf24e8879d3025af39d70 orb_check(gps_sub, &updated); if (updated) { @@ -1362,9 +1341,9 @@ int commander_thread_main(int argc, char *argv[]) status.counter++; status.timestamp = t1; orb_publish(ORB_ID(vehicle_status), status_pub, &status); - control_mode.timestamp = t; + control_mode.timestamp = t1; orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); - armed.timestamp = t; + armed.timestamp = t1; orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); } @@ -1433,6 +1412,18 @@ int commander_thread_main(int argc, char *argv[]) return 0; } +void +check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed) +{ + hrt_abstime t = hrt_absolute_time(); + bool valid_new = (t < timestamp + timeout && t > timeout && valid_in); + + if (*valid_out != valid_new) { + *valid_out = valid_new; + *changed = true; + } +} + void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed) { @@ -1512,18 +1503,8 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang /* not ready to arm, blink at 10Hz */ } -<<<<<<< HEAD rgbled_set_pattern(&pattern); } -======= - if (status->condition_global_position_valid) { - /* position estimated, solid */ - led_on(LED_BLUE); - - } else if (leds_counter == 6) { - /* waiting for position estimate, short blink at 1.25Hz */ - led_on(LED_BLUE); ->>>>>>> f96bb824d4fc6f6d36ddf24e8879d3025af39d70 /* give system warnings on error LED, XXX maybe add memory usage warning too */ if (status->load > 0.95f) { From 1ae5a6ac1d3da98e5612b77ff28afa86669c287f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Aug 2013 15:02:15 +0200 Subject: [PATCH 406/486] Minimized nshterm flags, fixed some pretty stupid non-standard coding in top, now behaving with two shell instances as expected --- src/systemcmds/nshterm/nshterm.c | 2 +- src/systemcmds/top/top.c | 18 ++++++++++-------- 2 files changed, 11 insertions(+), 9 deletions(-) diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index 2341068a22..41d108ffc5 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -91,7 +91,7 @@ nshterm_main(int argc, char *argv[]) } /* Set ONLCR flag (which appends a CR for every LF) */ - uart_config.c_oflag |= (ONLCR | OPOST | OCRNL); + uart_config.c_oflag |= (ONLCR | OPOST/* | OCRNL*/); if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) { warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", argv[1]); diff --git a/src/systemcmds/top/top.c b/src/systemcmds/top/top.c index 0d064a05eb..1ca3fc9281 100644 --- a/src/systemcmds/top/top.c +++ b/src/systemcmds/top/top.c @@ -107,9 +107,6 @@ top_main(void) float interval_time_ms_inv = 0.f; - /* Open console directly to grab CTRL-C signal */ - int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY); - /* clear screen */ printf("\033[2J"); @@ -256,17 +253,24 @@ top_main(void) interval_start_time = new_time; /* Sleep 200 ms waiting for user input five times ~ 1s */ - /* XXX use poll ... */ for (int k = 0; k < 5; k++) { char c; - if (read(console, &c, 1) == 1) { + struct pollfd fds; + int ret; + fds.fd = 0; /* stdin */ + fds.events = POLLIN; + ret = poll(&fds, 1, 0); + + if (ret > 0) { + + read(0, &c, 1); + switch (c) { case 0x03: // ctrl-c case 0x1b: // esc case 'c': case 'q': - close(console); return OK; /* not reached */ } @@ -278,7 +282,5 @@ top_main(void) new_time = hrt_absolute_time(); } - close(console); - return OK; } From a9743f04be59aee7bb96a5bb99ae8d8155eb19de Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Aug 2013 15:09:06 +0200 Subject: [PATCH 407/486] Fixed status changed flag --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index daab7e4360..98aab8788a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1317,6 +1317,8 @@ int commander_thread_main(int argc, char *argv[]) if (arming_state_changed || main_state_changed || navigation_state_changed) { mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state); status_changed = true; + } else { + status_changed = false; } hrt_abstime t1 = hrt_absolute_time(); @@ -1446,8 +1448,6 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang if (changed) { - warnx("changed"); - int i; rgbled_pattern_t pattern; memset(&pattern, 0, sizeof(pattern)); From cdd09333f946e746527c8e9af36e08dc3a29a975 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Aug 2013 15:02:15 +0200 Subject: [PATCH 408/486] Minimized nshterm flags, fixed some pretty stupid non-standard coding in top, now behaving with two shell instances as expected --- src/systemcmds/nshterm/nshterm.c | 2 +- src/systemcmds/top/top.c | 18 ++++++++++-------- 2 files changed, 11 insertions(+), 9 deletions(-) diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index 2341068a22..41d108ffc5 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -91,7 +91,7 @@ nshterm_main(int argc, char *argv[]) } /* Set ONLCR flag (which appends a CR for every LF) */ - uart_config.c_oflag |= (ONLCR | OPOST | OCRNL); + uart_config.c_oflag |= (ONLCR | OPOST/* | OCRNL*/); if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) { warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", argv[1]); diff --git a/src/systemcmds/top/top.c b/src/systemcmds/top/top.c index 0d064a05eb..1ca3fc9281 100644 --- a/src/systemcmds/top/top.c +++ b/src/systemcmds/top/top.c @@ -107,9 +107,6 @@ top_main(void) float interval_time_ms_inv = 0.f; - /* Open console directly to grab CTRL-C signal */ - int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY); - /* clear screen */ printf("\033[2J"); @@ -256,17 +253,24 @@ top_main(void) interval_start_time = new_time; /* Sleep 200 ms waiting for user input five times ~ 1s */ - /* XXX use poll ... */ for (int k = 0; k < 5; k++) { char c; - if (read(console, &c, 1) == 1) { + struct pollfd fds; + int ret; + fds.fd = 0; /* stdin */ + fds.events = POLLIN; + ret = poll(&fds, 1, 0); + + if (ret > 0) { + + read(0, &c, 1); + switch (c) { case 0x03: // ctrl-c case 0x1b: // esc case 'c': case 'q': - close(console); return OK; /* not reached */ } @@ -278,7 +282,5 @@ top_main(void) new_time = hrt_absolute_time(); } - close(console); - return OK; } From 871b4c19bc65bf923887e0bd32e1889db1c71aca Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Aug 2013 15:19:51 +0200 Subject: [PATCH 409/486] Added stop command to RGB led --- src/drivers/rgbled/rgbled.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 467379a776..05f079ede2 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -443,7 +443,7 @@ void rgbled_usage(); void rgbled_usage() { - warnx("missing command: try 'start', 'test', 'info', 'off', 'rgb'"); + warnx("missing command: try 'start', 'test', 'info', 'stop'/'off', 'rgb 30 40 50'"); warnx("options:"); warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED); errx(0, " -a addr (0x%x)", ADDR); @@ -534,7 +534,8 @@ rgbled_main(int argc, char *argv[]) exit(0); } - if (!strcmp(verb, "off")) { + if (!strcmp(verb, "stop") || !strcmp(verb, "off")) { + /* although technically it doesn't stop, this is the excepted syntax */ fd = open(RGBLED_DEVICE_PATH, 0); if (fd == -1) { errx(1, "Unable to open " RGBLED_DEVICE_PATH); From 12df5dd2699f163bc5551b2be611665fc58fb001 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 19 Aug 2013 16:32:56 +0200 Subject: [PATCH 410/486] Corrected orientation of external mag --- src/drivers/hmc5883/hmc5883.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 9e9c067d53..692f890bdd 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -860,12 +860,13 @@ HMC5883::collect() _reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; } else { #endif - /* the standard external mag seems to be rolled 180deg, therefore y and z inverted */ - _reports[_next_report].x = ((report.x * _range_scale) - _scale.x_offset) * _scale.x_scale; + /* the standard external mag by 3DR has x pointing to the right, y pointing backwards, and z down, + * therefore switch and invert x and y */ + _reports[_next_report].x = ((((report.y == -32768) ? 32767 : -report.y) * _range_scale) - _scale.x_offset) * _scale.x_scale; /* flip axes and negate value for y */ - _reports[_next_report].y = ((((report.y == -32768) ? 32767 : -report.y) * _range_scale) - _scale.y_offset) * _scale.y_scale; + _reports[_next_report].y = ((((report.x == -32768) ? 32767 : -report.x) * _range_scale) - _scale.y_offset) * _scale.y_scale; /* z remains z */ - _reports[_next_report].z = ((((report.z == -32768) ? 32767 : -report.z) * _range_scale) - _scale.z_offset) * _scale.z_scale; + _reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; #ifdef PX4_I2C_BUS_ONBOARD } #endif From 00c9b8f87f40ffca47ade881087a50b6162fd185 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 19 Aug 2013 16:34:12 +0200 Subject: [PATCH 411/486] Start the hmc5883 before the lsm303d so that an external mag is used --- ROMFS/px4fmu_common/init.d/rc.sensors | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 17591be5b3..762e80e1f0 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -18,10 +18,12 @@ fi ms5611 start adc start +# mag might be external +hmc5883 start + if mpu6000 start then - echo "using MPU6000 and HMC5883L" - hmc5883 start + echo "using MPU6000" set BOARD fmuv1 else echo "using L3GD20 and LSM303D" From a95e48c0b25c75548e923657247cdf268e654097 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 19 Aug 2013 16:51:22 +0200 Subject: [PATCH 412/486] Don't stop the startup script if no external HMC5883 is connected --- ROMFS/px4fmu_common/init.d/rc.sensors | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 762e80e1f0..61bb097281 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -19,7 +19,10 @@ ms5611 start adc start # mag might be external -hmc5883 start +if hmc5883 start +then + echo "using HMC5883" +fi if mpu6000 start then From f4b5a17a7b6385869933cd195afd674fa532e735 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Aug 2013 17:35:07 +0200 Subject: [PATCH 413/486] Improved sensor startup and error checking --- ROMFS/px4fmu_common/init.d/rc.sensors | 4 ++++ src/modules/sensors/sensors.cpp | 2 +- src/systemcmds/preflight_check/preflight_check.c | 2 +- 3 files changed, 6 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 17591be5b3..4cfd59d543 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -24,6 +24,10 @@ then hmc5883 start set BOARD fmuv1 else + if hmc5883 start + then + echo "Using external mag" + fi echo "using L3GD20 and LSM303D" l3gd20 start lsm303d start diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index a6204c9fab..6e57a79a83 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -953,7 +953,7 @@ Sensors::baro_init() if (fd < 0) { warn("%s", BARO_DEVICE_PATH); - warnx("No barometer found, ignoring"); + errx(1, "FATAL: No barometer found"); } /* set the driver to poll at 150Hz */ diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index d1dd85d479..e7d9ce85fb 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -263,7 +263,7 @@ system_eval: led_toggle(leds, LED_BLUE); /* display and sound error */ - for (int i = 0; i < 150; i++) + for (int i = 0; i < 50; i++) { led_toggle(leds, LED_BLUE); led_toggle(leds, LED_AMBER); From 7bd952c7acf7bc1c588a123d395930525be1349c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Aug 2013 17:38:03 +0200 Subject: [PATCH 414/486] Prevented double HMC start after merge --- ROMFS/px4fmu_common/init.d/rc.sensors | 4 ---- 1 file changed, 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 02b70e7da4..61bb097281 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -29,10 +29,6 @@ then echo "using MPU6000" set BOARD fmuv1 else - if hmc5883 start - then - echo "Using external mag" - fi echo "using L3GD20 and LSM303D" l3gd20 start lsm303d start From 449dc78ae69e888d986185f120aa8c6549ef5c2b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 19 Aug 2013 18:33:04 +0200 Subject: [PATCH 415/486] commander, multirotor_pos_control, sdlog2: bugfixes --- src/modules/commander/commander.cpp | 17 +- .../multirotor_pos_control.c | 249 ++++++++++++------ .../multirotor_pos_control_params.c | 7 +- .../multirotor_pos_control_params.h | 11 +- .../position_estimator_inav_main.c | 7 + src/modules/sdlog2/sdlog2_messages.h | 2 +- .../uORB/topics/vehicle_control_mode.h | 6 +- 7 files changed, 197 insertions(+), 102 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7d74e0cfe2..50acec7e0c 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -353,16 +353,21 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } case VEHICLE_CMD_NAV_TAKEOFF: { - transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); + if (armed->armed) { + transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); - if (nav_res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command"); - } + if (nav_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command"); + } - if (nav_res != TRANSITION_DENIED) { - result = VEHICLE_CMD_RESULT_ACCEPTED; + if (nav_res != TRANSITION_DENIED) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } } else { + /* reject TAKEOFF not armed */ result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index b2f6b33e3c..0d5a537eac 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -1,7 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2013 Anton Babushkin. All rights reserved. - * Author: Anton Babushkin + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -211,14 +211,23 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) orb_advert_t global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &global_vel_sp); orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); - bool reset_sp_alt = true; - bool reset_sp_pos = true; + bool global_pos_sp_reproject = false; + bool global_pos_sp_valid = false; + bool local_pos_sp_valid = false; + bool reset_sp_z = true; + bool reset_sp_xy = true; + bool reset_int_z = true; + bool reset_int_z_manual = false; + bool reset_int_xy = true; + bool was_armed = false; + bool reset_integral = true; + hrt_abstime t_prev = 0; /* integrate in NED frame to estimate wind but not attitude offset */ const float alt_ctl_dz = 0.2f; const float pos_ctl_dz = 0.05f; - float home_alt = 0.0f; - hrt_abstime home_alt_t = 0; + float ref_alt = 0.0f; + hrt_abstime ref_alt_t = 0; uint64_t local_ref_timestamp = 0; PID_t xy_pos_pids[2]; @@ -242,11 +251,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); int paramcheck_counter = 0; - bool global_pos_sp_updated = false; while (!thread_should_exit) { - orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); - /* check parameters at 1 Hz */ if (++paramcheck_counter >= 50) { paramcheck_counter = 0; @@ -260,11 +266,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f); /* use integral_limit_out = tilt_max / 2 */ float i_limit; + if (params.xy_vel_i == 0.0) { i_limit = params.tilt_max / params.xy_vel_i / 2.0; + } else { i_limit = 1.0f; // not used really } + pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max); } @@ -273,9 +282,20 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } } - /* only check global position setpoint updates but not read */ - if (!global_pos_sp_updated) { - orb_check(global_pos_sp_sub, &global_pos_sp_updated); + bool updated; + + orb_check(control_mode_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); + } + + orb_check(global_pos_sp_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp); + global_pos_sp_valid = true; + global_pos_sp_reproject = true; } hrt_abstime t = hrt_absolute_time(); @@ -288,6 +308,16 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) dt = 0.0f; } + if (control_mode.flag_armed && !was_armed) { + /* reset setpoints and integrals on arming */ + reset_sp_z = true; + reset_sp_xy = true; + reset_int_z = true; + reset_int_xy = true; + } + + was_armed = control_mode.flag_armed; + t_prev = t; if (control_mode.flag_control_altitude_enabled || control_mode.flag_control_velocity_enabled || control_mode.flag_control_position_enabled) { @@ -296,77 +326,33 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); - if (control_mode.flag_control_manual_enabled) { - /* manual mode, reset setpoints if needed */ - if (reset_sp_alt) { - reset_sp_alt = false; - local_pos_sp.z = local_pos.z; - thrust_pid_set_integral(&z_vel_pid, -manual.throttle); // thrust PID uses Z downside - mavlink_log_info(mavlink_fd, "reset alt setpoint: z = %.2f, throttle = %.2f", local_pos_sp.z, manual.throttle); - } - - if (control_mode.flag_control_position_enabled && reset_sp_pos) { - reset_sp_pos = false; - local_pos_sp.x = local_pos.x; - local_pos_sp.y = local_pos.y; - pid_reset_integral(&xy_vel_pids[0]); - pid_reset_integral(&xy_vel_pids[1]); - mavlink_log_info(mavlink_fd, "reset pos setpoint: x = %.2f, y = %.2f", local_pos_sp.x, local_pos_sp.y); - } - } else { - /* non-manual mode, project global setpoints to local frame */ - //orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); - if (local_pos.ref_timestamp != local_ref_timestamp) { - local_ref_timestamp = local_pos.ref_timestamp; - /* init local projection using local position home */ - double lat_home = local_pos.ref_lat * 1e-7; - double lon_home = local_pos.ref_lon * 1e-7; - map_projection_init(lat_home, lon_home); - warnx("local pos home: lat = %.10f, lon = %.10f", lat_home, lon_home); - mavlink_log_info(mavlink_fd, "local pos home: %.7f, %.7f", lat_home, lon_home); - } - - if (global_pos_sp_updated) { - global_pos_sp_updated = false; - orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp); - double sp_lat = global_pos_sp.lat * 1e-7; - double sp_lon = global_pos_sp.lon * 1e-7; - /* project global setpoint to local setpoint */ - map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y)); - - if (global_pos_sp.altitude_is_relative) { - local_pos_sp.z = -global_pos_sp.altitude; - - } else { - local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; - } - - warnx("new setpoint: lat = %.10f, lon = %.10f, x = %.2f, y = %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); - mavlink_log_info(mavlink_fd, "new setpoint: %.7f, %.7f, %.2f, %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); - /* publish local position setpoint as projection of global position setpoint */ - orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); - } - } - float z_sp_offs_max = params.z_vel_max / params.z_p * 2.0f; float xy_sp_offs_max = params.xy_vel_max / params.xy_p * 2.0f; - float sp_move_rate[3] = { 0.0f, 0.0f, 0.0f }; - /* manual control - move setpoints */ if (control_mode.flag_control_manual_enabled) { - if (local_pos.ref_timestamp != home_alt_t) { - if (home_alt_t != 0) { + /* manual control */ + /* check for reference point updates and correct setpoint */ + if (local_pos.ref_timestamp != ref_alt_t) { + if (ref_alt_t != 0) { /* home alt changed, don't follow large ground level changes in manual flight */ - local_pos_sp.z += local_pos.ref_alt - home_alt; + local_pos_sp.z += local_pos.ref_alt - ref_alt; } - home_alt_t = local_pos.ref_timestamp; - home_alt = local_pos.ref_alt; + ref_alt_t = local_pos.ref_timestamp; + ref_alt = local_pos.ref_alt; + // TODO also correct XY setpoint } + /* reset setpoints to current position if needed */ if (control_mode.flag_control_altitude_enabled) { - /* move altitude setpoint with manual controls */ + if (reset_sp_z) { + reset_sp_z = false; + local_pos_sp.z = local_pos.z; + mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", -local_pos_sp.z); + } + + /* move altitude setpoint with throttle stick */ float z_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz); if (z_sp_ctl != 0.0f) { @@ -383,7 +369,16 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } if (control_mode.flag_control_position_enabled) { - /* move position setpoint with manual controls */ + if (reset_sp_xy) { + reset_sp_xy = false; + local_pos_sp.x = local_pos.x; + local_pos_sp.y = local_pos.y; + pid_reset_integral(&xy_vel_pids[0]); + pid_reset_integral(&xy_vel_pids[1]); + mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y); + } + + /* move position setpoint with roll/pitch stick */ float pos_pitch_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz); float pos_roll_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz); @@ -410,12 +405,68 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* publish local position setpoint */ orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); + + /* local position setpoint is valid and can be used for loiter after position controlled mode */ + local_pos_sp_valid = control_mode.flag_control_position_enabled; + + /* force reprojection of global setpoint after manual mode */ + global_pos_sp_reproject = true; + + } else { + /* non-manual mode, use global setpoint */ + /* init local projection using local position ref */ + if (local_pos.ref_timestamp != local_ref_timestamp) { + global_pos_sp_reproject = true; + local_ref_timestamp = local_pos.ref_timestamp; + double lat_home = local_pos.ref_lat * 1e-7; + double lon_home = local_pos.ref_lon * 1e-7; + map_projection_init(lat_home, lon_home); + mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", lat_home, lon_home); + } + + if (global_pos_sp_reproject) { + /* update global setpoint projection */ + global_pos_sp_reproject = false; + + if (global_pos_sp_valid) { + /* global position setpoint valid, use it */ + double sp_lat = global_pos_sp.lat * 1e-7; + double sp_lon = global_pos_sp.lon * 1e-7; + /* project global setpoint to local setpoint */ + map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y)); + + if (global_pos_sp.altitude_is_relative) { + local_pos_sp.z = -global_pos_sp.altitude; + + } else { + local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; + } + + mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); + + } else { + if (!local_pos_sp_valid) { + /* local position setpoint is invalid, + * use current position as setpoint for loiter */ + local_pos_sp.x = local_pos.x; + local_pos_sp.y = local_pos.y; + local_pos_sp.z = local_pos.z; + } + + mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y); + } + + /* publish local position setpoint as projection of global position setpoint */ + orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); + } } /* run position & altitude controllers, calculate velocity setpoint */ if (control_mode.flag_control_altitude_enabled) { global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2]; + } else { + reset_sp_z = true; global_vel_sp.vz = 0.0f; } @@ -426,13 +477,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* limit horizontal speed */ float xy_vel_sp_norm = norm(global_vel_sp.vx, global_vel_sp.vy) / params.xy_vel_max; + if (xy_vel_sp_norm > 1.0f) { global_vel_sp.vx /= xy_vel_sp_norm; global_vel_sp.vy /= xy_vel_sp_norm; } } else { - reset_sp_pos = true; + reset_sp_xy = true; global_vel_sp.vx = 0.0f; global_vel_sp.vy = 0.0f; } @@ -444,20 +496,44 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) { /* run velocity controllers, calculate thrust vector with attitude-thrust compensation */ float thrust_sp[3] = { 0.0f, 0.0f, 0.0f }; - bool reset_integral = !control_mode.flag_armed; + if (control_mode.flag_control_climb_rate_enabled) { - if (reset_integral) { - thrust_pid_set_integral(&z_vel_pid, params.thr_min); + if (reset_int_z) { + reset_int_z = false; + float i = params.thr_min; + + if (reset_int_z_manual) { + i = manual.throttle; + + if (i < params.thr_min) { + i = params.thr_min; + + } else if (i > params.thr_max) { + i = params.thr_max; + } + } + + thrust_pid_set_integral(&z_vel_pid, -i); + mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", i); } + thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]); att_sp.thrust = -thrust_sp[2]; + + } else { + /* reset thrust integral when altitude control enabled */ + reset_int_z = true; } + if (control_mode.flag_control_velocity_enabled) { /* calculate thrust set point in NED frame */ - if (reset_integral) { + if (reset_int_xy) { + reset_int_xy = false; pid_reset_integral(&xy_vel_pids[0]); pid_reset_integral(&xy_vel_pids[1]); + mavlink_log_info(mavlink_fd, "[mpc] reset pos integral"); } + thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt); thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt); @@ -471,11 +547,15 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (tilt > params.tilt_max) { tilt = params.tilt_max; } + /* convert direction to body frame */ thrust_xy_dir -= att.yaw; /* calculate roll and pitch */ att_sp.roll_body = sinf(thrust_xy_dir) * tilt; att_sp.pitch_body = -cosf(thrust_xy_dir) * tilt / cosf(att_sp.roll_body); + + } else { + reset_int_xy = true; } att_sp.timestamp = hrt_absolute_time(); @@ -483,14 +563,21 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* publish new attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); } + } else { - reset_sp_alt = true; - reset_sp_pos = true; + /* position controller disabled, reset setpoints */ + reset_sp_z = true; + reset_sp_xy = true; + reset_int_z = true; + reset_int_xy = true; + global_pos_sp_reproject = true; } + /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ + reset_int_z_manual = control_mode.flag_armed && control_mode.flag_control_manual_enabled && !control_mode.flag_control_climb_rate_enabled; + /* run at approximately 50 Hz */ usleep(20000); - } warnx("stopped"); diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c index 1094fd23b7..0b09c9ea77 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -1,8 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Tobias Naegeli - * Lorenz Meier + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,7 +34,7 @@ /* * @file multirotor_pos_control_params.c - * + * * Parameters for multirotor_pos_control */ diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h index 14a3de2e59..3ec85a3641 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h @@ -1,8 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Tobias Naegeli - * Lorenz Meier + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,9 +33,9 @@ ****************************************************************************/ /* - * @file multirotor_position_control_params.h - * - * Parameters for position controller + * @file multirotor_pos_control_params.h + * + * Parameters for multirotor_pos_control */ #include diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 81f938719c..c0cfac880e 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -508,6 +508,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (gps_valid) { inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p); inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p); + if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_timeout) { inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v); inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v); @@ -554,6 +555,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* publish global position */ global_pos.valid = local_pos.global_xy; + if (local_pos.global_xy) { double est_lat, est_lon; map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); @@ -561,21 +563,26 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) global_pos.lon = (int32_t)(est_lon * 1e7); global_pos.time_gps_usec = gps.time_gps_usec; } + /* set valid values even if position is not valid */ if (local_pos.v_xy_valid) { global_pos.vx = local_pos.vx; global_pos.vy = local_pos.vy; global_pos.hdg = atan2f(local_pos.vy, local_pos.vx); } + if (local_pos.z_valid) { global_pos.relative_alt = -local_pos.z; } + if (local_pos.global_z) { global_pos.alt = local_pos.ref_alt - local_pos.z; } + if (local_pos.v_z_valid) { global_pos.vz = local_pos.vz; } + global_pos.timestamp = t; orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos); diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index dd98f65a9c..d99892fe2e 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -259,7 +259,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"), - LOG_FORMAT(LPOS, "fffffffLLf", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt"), + LOG_FORMAT(LPOS, "ffffffLLf", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt"), LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 67aeac3728..4f4db5dbcb 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -61,7 +61,7 @@ */ struct vehicle_control_mode_s { - uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ + uint16_t counter; /**< incremented by the writing thread every time new data is stored */ uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ bool flag_armed; @@ -73,11 +73,9 @@ struct vehicle_control_mode_s bool flag_control_manual_enabled; /**< true if manual input is mixed in */ bool flag_control_offboard_enabled; /**< true if offboard control input is on */ - // XXX shouldn't be necessairy - //bool flag_auto_enabled; bool flag_control_rates_enabled; /**< true if rates are stabilized */ bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ - bool flag_control_velocity_enabled; /**< true if horisontal speed (implies direction) is controlled */ + bool flag_control_velocity_enabled; /**< true if horizontal velocity (implies direction) is controlled */ bool flag_control_position_enabled; /**< true if position is controlled */ bool flag_control_altitude_enabled; /**< true if altitude is controlled */ bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ From de124619b6f64aafc10f1cdebef986a9e193b74b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 19 Aug 2013 20:26:00 +0200 Subject: [PATCH 416/486] RC mode switching diagram updated --- Documentation/rc_mode_switch.odg | Bin 14872 -> 22232 bytes Documentation/rc_mode_switch.pdf | Bin 16097 -> 28728 bytes 2 files changed, 0 insertions(+), 0 deletions(-) diff --git a/Documentation/rc_mode_switch.odg b/Documentation/rc_mode_switch.odg index a8a6f93f31f90c26e4e6d2ae4914fcefd53301aa..29d738c39c9598003b84fe5bd1565cfdff644ee0 100644 GIT binary patch delta 20835 zcmZU(byOV96ZgBgdvFcz9^BpCCAhmg0|^NPcMTHUgS)%CySuw{dGdSTd+s@RKYz@e z-Ky!@nVzn1S9h}OLKzsck}L$|ClCl01e%P+5RE~W24G=f|DA*WKcA1EL3U=YMvR_z zwh{62QEAMmKhIw=L+;?;K;T*(YsExOOEPIPt!3e-x%QnBb=A`Een>&|hZ5?!dKY|b z=e*la#lfoUP=C9$iqr|Saef9W$+;BWxjpzjbJKPO?}_Gl@D4(5rV@Jp0rX}NBq1`3 z=~G1AJefl+t@9{C@4C?YLx_TOR0dKh<0gEak8+>%g9X#^VL|=dECQ6396kXUX9W#AV+pXyFBXaIolJ&&BPa6o#+MUuGmwd zy7`Reh5RF0)Kfu$s-><`pHoD_`jcZ1U<3IOJs+f+14iC#Rr@+w8)ECSi%WK;#+fJr zSu0{xtD|35{+Ie1KF_*VC*g?Q7Yab}@K^i^nxuiwEH3ifLJhq6hOMr+Jb&B?6hVtraIU_ok@ zQ&}CHj>~ReH?-)x6BE!@6|-_z6fR@*@KY{I`nP92FhQQwZuei>Ex&N+!3$Gc-yeZ|BW*BI~@ z&D=xPeYmFcFcnW?Ybp89ukUFI;)6>Lq`X*N9BxGpUo1jw3g}ICb~IrdxDc3rS5+EQ z!#FGn#vQVOK3B|kI86C>y4h^^4s_Ohkh>*M%=qleI^j)pXwHBWoE|2W6;4{K_1^dQ z@2Q_#%@yVmX;?3@pu)Sw4rRqx2i^(!wC`LG^i!HM(D?|jq>FC<0P08nT|#hu+P_cWz!t$0KO?qcP7w~6$3WJ`hjn}$$^I3jS6F6i#Ck?`BjEo= z5fA>-L_vLFhuXg73Y7QLV&FoX*7fVmXpSn*HdRyXiet1>x6XY{$&p-w_AMGcw07=- zf~}0MNemgMlG#OR(vB7kwom$6{8Qe3(~TyLZ2rg2EXNe*%@eCWvdPh}Q;CLDi+pj> z-pqidrr8lckmyF?_j+;u91S}W6tvmZDTxEr`<<5>g_~DcSU``wvt~T1px{xLgy2=Q zc?PN1fP{=P$wvVG7g<-SbrVt45}UkCCi6(XaEsyu#d8B;MA>tU`W0lZ(bdN8kX1=`QWqd|$3fxmo(uDMHRVo8bM9*BXmZ~};O zNm{cYa3WT&&w%7=nO&rN*}6>p?hFAd?zs1 zNJRNet&xX|yOP9wh?S2_?UcLaj^zcL+f7#MNqj7xhh@-Z`LA5hY@WZ7?t%r9_gWWU z-kMEPWo7-Eu1=wD<*FCi&ckLmbEnfsLbnfduUGsIG=kloR_uI6$6S{P!V89bU9bK) z2&!AuY3j}UdKw2hI5s^p^%7bc4OL&nBtm@L>~1vSO#6ugFhy|9Jt)@e3%diD2eHU# z0)32T*dICXC7)v7CTAY-{~O>%R=gjpp+F!EivQmLzYb3b9LMHxCB6wN3VXU!HF+jS{xzcAvgY3L?XN(XJQE2&oeOgAA25+3x~w{&Vo&~aOj#IIxxh} zBO)m|b$>{p|FYfOpFp)TP3;cxehNwEM5?q7Z#!?$lK#T_KCG76()`vP6}a7i)mjB% z)0*wiS_>`+7_~zsE=T>XnyHU-+cAJLKk3tKYs|h>H`a&xe9PHbJ~X9irM(!(<#pw4 zq4;CC5#uTOW^}}i&TF*!jiR7ycg5XQ;J+_Yl!q77vmEyN7t(mZs-I}tF+3#0B*$+7 zA6{)rY0ocd{Cs>_FmOgE+GmL^P@E0-VN7|m8 z7OQ05yxk;o*78{}s^^N0#7ro=iidg4+p!a_)t=^Gvpvq64A_$wbyAhj{S)Cen?p_S z&uL@z$C_E=%tW7$s-x&xUeC8j>kPz8+RU65LyJlWkbE@R;u!Z)zGW#TSPVe>6f>fU zdnE#XjM9QNp@L%eA~~@Qv$Xap8R}YmTBmGyaouc`EP3}W!`RaYss~OWr^RoiYion`q|xi%;9hK zwN?JGV%L~=Y=sYha_nk4N^Hm$?<`ZOxH~N+WQCaOkCtFmWNl?%R^LU!lyEAl4@}8^ z@%w9(s-Veo>}KlRXEDI6d#p zWJ&yd<iJEr*{B?zMU)S`8>gOZ*xJBv|Wp ze~WcGxIS$x`7%rP1cl(r8 zU#zp+ReH5IR=C;3n?=?Q+?+lQ@QIlwhi%l*(rD1h3u-2jeju<{FAJ-SLpAH& zp=mjePpvMq8{D+RUwKW}8T_6`|J|)i{;@?upoW6`fHe-;ZZrQUZ+m_Wqsu~X;Qmq` zmAE$YvxT;o3~Fj(T7`^>^2_-!L0$^hL zetE2Tf>HZuVDRfLVRBZ}*f+h!vmTt?g5fVgE1OzBlWVl*obU{WYxKy3euR!qJRUZe zf`^L}!k0P`^s;05c#mlMh!oe|CvoUGX!J4p(M$)PX(NugHkLIG#9`Vcc+X}Frpflo zuU-4@>t`opPR6Nn#M5fZfVGsrwT!<#F&pS~dBUH0$^;G3H!CoPS{erLQKVZjLTYgF zQ}k7h)UQl5nCCgo-s#$J3$N)3p$W`I^eh+ss%-A^Ks!wgQog+kdlO0B}UvrT2Ei**9oatju`&t+3w zUDLVz%3L~u+PzNve(&)bH{SK)#Mc;tVs7;>QadNfiOFw%*$Ns6w=+nuv^;VQI4tJ; zAqy%|ufN3kr*__MfkN(P9$U-V)i!L- zKz9z3{Muz5-jS(&POsw{*XJEOz3XrRAN*;){_>Yc`Ok_!yRPxToyz)2V%e_b_i8(V zAQ_@Y_*yU?4NL_Nu(bJ~i-S!|CBHq#ZPr%NQeS?YezATp2E2l(sx^7zBXkB+IXP4) z6!$wuY$6x5PF;ECN*@h#)HiF023pR7BGluCUCH;{l1@#bdD8`%|2E4?7VdJnNk!dy z+y7=NP&;z^PGtrbwWi8s;MSo`Lci9d>mL)f565vO^F;?V#P8}2`qWu zH7fm{ldO*R{ljG!Zuw`p$Lj2JgemE+8<-mncc#rafH%Ig2j>y^s^&X@vi|iw`HXE# z(y^LQa&hH1(ASHWR$xq~<{iqpKJbZ(U1t1j&q`3aU(aU+MH;HqZR94oPnIqB7CXWMe(H^?~YE3YO$`ig-mU_J-mbw&V!+z{I{1S}2>_Bb1N8%OdnyIO{346aHDWOB6rd^1V^TB&>%u29Ls z5~5)bkEy2JXj<0AK7Tw$d<%wVL*zleQo$I$tIu^;E0k+&T*& z(Vgx3H2VE|?6ZiEfET9$dJumVP4nUuQu*{*_>p~|Ew5eF@1-?7R?&Kk>3j7Gwxr1{ za|;Q*W8M_AAzbbAx>kams`dgS)aFGfO}S_*uA(dJI;$X!B3vC}mgzhpxjSLW-hRn0ef}~j zinJM|RIDSHgt0&nJ2{>+?t4rj?f1?1?PtaI78q#IKe%9^Jq7xPx>+>Df2w4d*huv* zF^5yc9pBg23$Bj|zCcnV{2y$n8~EZ9VnQw*lV8o=a7nN}0I*2z5J}u`lzxWCPwlWH zS%gS%XZ#^mpDRI&P|IS-b%JCtGghmON3xO}JxvGc$^U$>qHGPwRuNa`Se$o1)n=iR zfIn2J4%5n$a7$1yHoIUdakHLfX8em$0TZioX;i~VEOA7lX?5HUTMrPXnP}R7f>J(g z@pIA^;r4RT16oo&6<+AAlh+pvEkmhIFb{A)c=SDdd?boqf}2SvLPbMkxv4_PC8duw zVvOc%e0yN~emI`lfdHNi^#MObG9e(WDIqp9eC>WG)@|*z6^SV(HQTEcolD^0+>>mbTeYE{O3*L^R64?U2e)oIVp;k{D%R72rpX6j)0{G{vYk* zAag=#pF|jGFE1qtZ?ofwZ-@-Kge!^O*nIZUO)mNuIfqZ+-JfYq6EWKW-AnwII1=wC z;piZFla@u4J4DE!xA>ZaJ9GLJn?(4w;De+I=4(xsd+xdgRnJxz3CS*0j9d z0cqs79@(RjBq}Ke^g!rEGO=q*`foZwey(pjd?ktQT73{M%yM5wNEob5*T0_N4dZFV zI+J&hhnh!wAwla3>68#6u3<@*;b%4&PZx~p>N&lj)8lSuPDIx*x2f40Wd=k(!0(Ek-8W@aHq^E4RM zajLGa8SbvG4&=r%{^p6qrur;S9*pe&-3`HHpetthr(jOkgkFEmY6|aOKq4OW&BNPb zH04xu;ZQR}5E=)q0Gz~64g2h^Z-x7{sA=Kk_MgawKN-N8$2JV6Zo3+mnPlbkhcD85>WVJiq!qVt};Znzcn}TOrFY*eQ!zQzdrM zgl2?!XfEd+Izo45xzPiItlFEo53Zsi08f{ARaL;UE;Z{AD!+ksH?en>>ux+Al`j zr;9HP3tpf<6tD~_s5(tS6H~+MHI)ccct;C6i1)T2If#E`I)7halsksSuMk8u#QVpS zZ{5tv7CQ8D17q<7x_(Tx#-X%$ zl|jl|F3l56`0&cm5yQ9C4Cc-3ffUis{g)sTx|KNeh#|x-r z2%31#?vg2mGhv^>Cy6`eMOH3v2Sr2ugC8QA-(3ziN#Gm~es}OM1+6KGSA1dgHVEo} z0D7cQCq23klhbnn2#HGbe@ZY8sB4cbNnR76epImYQGR;pQ2iek9jXu;ZsAPjg?-=I za}mF5`Z?RJ!IY1dppYQ^!v~tg=f*SoXLy~bQsRrwmavvE9%N7e#Q*Sy(RCGDf)Ih` zd2|zQ=CLMxPOfOG21IW##extaSw3gceo!p?r+6R2_P%q+c6j`(atFPQR<~xf4i-sF zGtvKf-op_cSAi6Q`9Go{M<5_>1BU)D+X_iv*k3ZffXx4~=YW&kzp&iZP7f1D%SAWU zVF|syf&Nq3P)8OP2Vw&JgAA|QTm_ z4nufbcQ6sq2Mwko%+YP4uso!pDwL32Sy<=(*r{{Uumuz9qSgiFv|X5_T@zumt4tmG|6$Rs&xAK-QA&9Z9WnC<6@kSss%W`*(0fF{WMZpC7KV|a&yQs3x%kzfaTQd@;>Ld-TQ8l!fP^FvU>Cm8tJn6T$5&v0Kk;pkRVos5pk&gb z@1Fw?OR)Jh(AKOFm{1EIH7G=L z`xN})Y`0(m!9XVJ)18Mf$?Nxe1jC*dvl{f{a}}W{BnNCE!;^|PM(|w0OG7Xc*eSZl z4y7#4?ILi)(soV6`g%3UOC>&aP}1PL7k&z}LqoRPaG$CRD|j34$XC{1uek~&R_W#f8oTdk_f!O{&{MfzAVw6xX#&}Z zqwHT>AR(i0(|4j&sU>P%3jNkKM}MQ4OYHg%g=ck0U9uSVI>k#{ZYSTJj8uY8PEhAo zFxcKDgUKPgpZ;J1Y<1b$Qsq%cP)ug5xlIR|0P(jX(IC>|ov`f|pZ+WVM>hvTKu*G$ zM3clYPNF}<_4+>Tu3&}J0It_}+p`5^9o2Fl!SfdPOWVPL5g`%tWYMUVj>2sj0f?a_ zipP#uOI}u&mfq3zG)m<|dj7og{YddE0}=t&z%HvrNGPn&;9Hz)kn8;1RX+yQ!Dy1v)%Q$4 zwglK6QaRlXY{AYbn+@67W|ZHHRjQi^sJ{{9%u!j{TUyHqw?l&n^m#kP^vTBKY98j3?}LIl z*w+U$zlCV->7VOjEHB#yj3FMF1HdFWFyDvvYY7Wo6_v}1)rd;~9u%gr%IN4Rz;0!# zK~8Oa*(!(|$Hs{IjZX7j+^iQ?AS=0M9z2O3pgHS4U^cpDk(j_!1FJub_m7(uDEn zxIB2_>L)?CBxc>OF9&A$w89f9dE6K{*P%t!br`sOh6(8O%zK;I*-InwV4y(;?9ipx zJ(tm(Ol<)fD=6?4tI?(_pxZ}x8$0T97ag7vgsrgq*eA)Fv|v9~XVR00901n|8DKv= zXfK7!+pzfpS7z`V5&Hs^Ck#gu3I5ws1z$D_(8jA{PiNMaikIGLfTccKkhC*Z6PCgp z!79@z;k~tUv;0}eMaL&l6_Nd@mIfyU`@2zZ=ErpJpUXd3q318KKqF7a=YZpYP2}J{ zC%lH8sl;JAVT? zk3&#H-Hk~Mi1b&8`r(KKQKsV3f=as~cr8TQ4=dR0ZBH8jKr>Nqx)inbuTIhibTWVZ zQkn>5#;f^;Hk}U(e~T`=d1#FhH)qSSD5kuI^wB;=0N?d-9m730Iue`t>jnt+=bbK7xi+ zMTY0rj_hL5-@*l z+a7|kd8^JB68PMu39rOXszQV|Xe&=BnSvUB$e~m&A!n-BzNvYw4~g#cent6(-{)t* zBA1hN6J1m#9F2ADk3%>|g5x52H=W9Mq4kwYs8T@ZbirycyZBa;ia{nxXE|Bm7bMx} z&OuQ!fzG?z1tM4jLpT&f0e*fDp#pbKV96=@`!vR11FEuPlard@^|U>m-UQ7-Xie%1 z+2g_~FOs!$T^{5b(^-OkcnPld&PqXllt%P_NevAApsaw1B%1CL1SUjMkK%z1Dt-G5 z)MI{}KsxINf(Lkhv1b4sybIZLejH(U+##Sqm)0wqpo#au8?ZAv5Ns|=0C>R4pzE<@ zLlG$J9pVswMGb^1wf-T^J`9!r8RDK%e;AHC@`8075l%qI^5F!4{nyZw!9O3e zSEdgi;(tD`^#6RY7r_5d3ZKpd4|qe#T@Pdc^_2Y2gz`U=6|nzIQs2=2`|tMPwqO9s zSENrLHis}^LhmqJ0xRJlj-7?%UD02hzwfqfkH5p6q;VNz`C*ibd$f(pn(3+%Ye_aNebwnS8-tM#*Gr`-M7k;=InK&$_hLUeR>E!4A?^-CB<^I+?LK;bpYKH|J6CbF~52;dj!%zihtiB~c z^@2D_DNB!B{fyZQ4pwbTvXU6{^j+Nh%k92Hm<+G4@}GB8r9~3^oCD3+<$#Gs6|%kH zn*H2r_1Rz_M#MdR_}iK9g_!O2@)b8zeu?$i2^a#gwEfe|XvC1FW{*A<(D$UD|Ciy# zaeK}C7ayzx4V$574~dbsRot(=f!%K{-Ckh7l*?vKUE^geZD{N#1~BsJ2?kg1CNnq? z#Tx?SwrA61NeeED^|WsT!GK2c)mGQ{2MHD|p2$XlH#f2EpNDbAUiu!Z)mV;igF z7}`FUymbVhYh>^*6_(rWE2dYI*E&8o-v+Wx<--E27jP8#v{y(QBtUX>M>u7pj8=+A zXIBpE@DI2^PW7H)sDqd0z4Nspe5`hd={3$Z?}wF)bnd4@b%$mR@Wuwy!(pK5Kr1Sp zS)&6plsUX`M9@j;WEjFW1(HH7Mtxlk9CiLwIpeF}IW9alIGy4^;%BWAWD9(G`j{Yl zj-L@Y`!-_6lr(d`YCz9@Op)nlE8H#l0uw-WWLq&{GBa zsWVACE`q4_(0F1=Kcm)GaF+9SBd5Dv7?O0U!FF_fyp}^Z z*R*?c#gAN*QIvueE3a0WlAj{Uc__lx%S`vArm(fKbmDm5SM-Dr?VG+aP?d%-JEDRNs z-{}*D(b386InXBHkM3}N*D??kojScgVk8{6^7F-eVu?})wt5$<4iZ<+d?{2G7QC7x z0aGL1@>XDyJaWVzE1R_#QS_LQK7hhM@?(CvxQSfn@8}1nSKDOl zT^UZC$;Chj+9MVM<*AmKY*A_5TBDT$pOq1h6{(bzJPiQqq?D4>0Ub2KHwr5Ks zA}buH5>SdX$ZIxF*7Q?Uq*?5X^VB-r;+zaMQ_~1x^k>dSE@tQ{HBnf7$88g{dpfectGcDcD z;ya&C!&}Pv)h!mn;O}p^7dxzjkLx$NLZsI*E&%sYn4n+1TcMSuOsPvSZ%W!B1he3# zyrxO(?X|39yf!H{qi`TOX-^{(@qv!f2ysB9TZdM=Ns*d<`A-V|L1H6{Mh&I-2-=;o zFJ0>~ELmWRc};o@ReA6~KIsXRvN5bxT394g_PVV;eFa6Cjhs~Np57C?yyHU1Ky!$tY-O!s*w3|ROCpw;XYy0%;6WO??@1yr~ zmjhCcnXlRP+d}1h&nnQZq$W{H%AMoK9`ncp$^8D&Dihu0uOtC)SCA{A0&#(w`uhvg z9!dTeQR3H}d6Ah{J1%rEhO2bzWV!*oKY=y}q%&KYNHaXLJ-;Sdx&wGXM2A}dYXdjBxuW-b^*9}Lq%I~AviAf8bOn^71(1bk zAFtY;VQ?|Ud&;}UG&7YGuLSadRD$NGa&K^c)n!_W2~=co^1+4`VmgorU_F2f6s44# z!HyOh$oFUo*w7^S2myf8@cgdg+qMD3$B6kbO!zYnn`#tMP`?O_#QPvwS(v~1$CmvA zO3G7#6l>DE!FM>2QB>@QC?WA1Tu6xGw2}V=>Q?=F5m(ib9Q@82M7oDBn|ffy7C(Yq z6ozHwC*UUj*RXu(@yw8ALn60B0JNiH$n~e|mjb>L&4(=U7y(E(7XDZ$qBj%oE9`R= za?412y1H>YUSE9Joz|Bl+cOOzE6#aEyW|?8J-Rs^#Cmx%d zwq;W&a>*WyMH-F3OlHY2)5dJ;tybkEXb}*ADu9tx^Kb1J>i}bkK338$P4hn|YkC)` z!+zoCx8Cm!lnst=y|l7gaf94I`$VwzC|b$P5#m-CnSr{I!iXC1b>cMGr0DIbRsDUH zlltCBaFr!SDTE9sD(bdj$yHM#PoBElnx^%O92oJB3mI=N>XDuZ|E)_iHs8!O&s^a1 zs*K851eO0n)ii24t6jE3RDqf!XXGire`Rb=JV^vJ&q6iP&+j*+s~j0|XWBhDUR|N{ zFxE6QP6n{mG#i6RlR9K@`xDd~B8NV`uEr zI(q{xM|QCEiVkQ3p$XCT#;2=@7+JjZjkocz_Bs`A+i%luf*0KKiJ!{fb_D2u_lDa< z@y)fkCIzPkc|fPWMHpM!)q%=!qM+?9n<0qE0)VlvN)AZZ5U-}dneEfs^-2&z6q*orc|(ud|K#*|$$+(L zI8I1{c~aWd{ptJ^2o2>cYv6k?_OmTS-(P=2+x@}V@^_Oo7w2S(jAYN_yf0=J*BIyl zK4}66txi!yGZxosV6L6z+|4HQbqiTDR&(EEgpH}yee!8f{zwAm<@%Z}S8tu?{I9cq z=P#wXO) z{dfDR5l?uJGYOr&-2OS+QRffh-J2v@N;*ML!^I)EE!MjOEajz^RQ2~FH$-^_ijR>v zG~lDn0?+gX5{|$<+5N298Lf?_B}|94 zn+wC0hr=#X*!W6=-3z~fh=3~8A3a8Jr9V>iTgrF}O5f0h#5x8 zBfe&UOz_h~eI!9Jx&k9oXJS?RJ-8nwMZ5muuzKv;T+wIgO1W>;0Z6!?gK&D)-%0Cag9MeIb4(=gjzbDI?@e~yjuY6MXZpQQ z`+nu-cm%9KQei(JwdB;ERf$iSl_c*X{7xbmrT^y9<&U{*Gt4vDPolopa}Y*D$taZs z+7?LBnU(Wi*7o&>UyB+`P~srJYSC!N5IrnlNKRQG*^~DMYKdZyj!g!&pr$=1=+FAh zXE}6=$;qqy*5x#80-+(Mqd#{KIwzJWpoN7Dy#N?6y=|m3zNX8|4*sfjmn+|2^;gn* zOduWkQG1UxiDOAZwgQv3so%CH6Oe?^%VSyBSL*e~=q+B{8s(0!@m~A_)_&!YfwUz+wlE4w*mH?4%j7=m8pV&|Ol|a6 z0p3=clNGx&8e1*=WnHn}6XpZwGFM-a2vdJwygnP?<=|I|P3m2Hne(a^D4Ftx(IpKA z$z$GzWj^l9P7A&)TX5SDAO09qS$C0y1#O`3TM9t-rl0e`q3J(uT@5Svih4i!mLh0o z&Vnsh3OxpWdw;HH-=O}49N1V+Z1S4a4v2ULtDR(^4+fu=d1xHD zIL9{P44LW}vFeaaPfHcUM$iyAy_+Ef)_@j%+bk!T#`o|Ts+jNlBsOSsjHeEvJOyGF z7D83gHZzKGpEX>+YT@C{gWZ@z0(3Y|x5mGi?Eg5?e-A_3CRxdf=}x`V4<{HaQ<0ZW zAPJ!p9fudWr%(df!>C$7=nUU?GZGA!mCnPFV^tE*S$9yVgzGOsT2@_Td3Ua#a}YmY znzo)yPzhe{h*}1O?pmgOML(MMyovuIYyM6d8t%@+7rA113)p3WR$%k%0QW^Y&?hvu zhR6NwAt#omFdZb^(vBkqVqwuO$O3l_$p|=}Ho+wNy5~GZ*=pSs7@&2q7-ch1jP#Zt+ckcE^h zjN)QM@k+d!wKVosj^`o@0M$`?oy43ekL<3>?dd_mU{D(p4c;Z1U0raEY%>gQ0_&Y5 z8Z_m#;|a{!SEVGb22-v7Z-Im?A*#9zg|fnd^w0cM^XC4r7dR(^NHfZ6@|XXLBLII` z@#~3~tYPw3hfUb7=+tT~uzA6Wg7Sm{rJpDY$o$Q(bO=Bwn7x>s*6iU^kEKG)xPEm? z4T_sjuhnTPCe!HS&Fa*A2KLJP3TpPNh`sEJp#g{R1&BnRl5QJ~EuAlG_K!1M!S1vC za~yX5FBTeND0VaJoQI%mwEDy?jDlh@3`%5nOL=un2A0bhxvOfQJG#E`c+wCunD{AW zYGRyaDLp_~3&AB!tum$tb7n6o7#V>%*%8jX>JQ}l<4ttA=XmfkQ>0D}-xoE@8S6&K z$#M;sPw;6PHkMBe1@K5=zmSFG%Ngj^{HQYp-F5=LK1$nX2D(S`>D^Li2+W*O9w@iA zJ*-*KjuY9b&DUKQjA_T}G`y}pb>Ld52DRHi1jPbKn2*qosB%(<5^;JIr6bLTo60G> zT|q(?2%x5RD5KqH=jup;qCy)i;C--Oea=zvWdn^+z;NX#Z*!T$*Sl?v9u34tp1ST> zxk`no_KCpvi0dK@c+5Opf7y0mfKc&mWWbg=6#6xGhB2+>J4Iz0ke_|h@dp7#fXKjh z^Aq4(H5c=JMo?wee+RrwY{@UD`vjU$Q!L~fg^+$DoS2c<@gzuY#{0!n-f(3=n6|U< z@o*@l2m{<;(4i(A>-V$ovO{j8K3d_#Okvnnv1HzlLL~GVjg-7v>*HE)7dipM8$%5n z8Vu@P&R5}R9bb!#D|9#)iw5w$^bPiy^R}}wIl`B2i%iAU}O|Sp7A;R+>YzM>G zH>yVzl~6|`aS>Jzyr_O180fu*o;{!LF%AEe&ZnNv$6Cdd<5Mri32soyOd^Dfa`cEXSv#W{Oetz1q9W zjL>bhS~~iwwbusC`+g6m+rM7SGpBThs%)riMvs|Zul7`Rb-C#@Th;a64|@T(u5ybb>ar$%=3vDy^)b~w9fkcne2}<; z=WnKR3VPl{ne0|}1RY##@ZbQ1Y!d4f9(7rwBw`xIFD_^1*Nn%ZU}z3}7B~r3T}Yf? zT&klK(IXMqXVTa6c=Vf<7zHvIzR7Lo)8c~xJL(AN?ABj^$ga}4&`&|DxwYFti;caP zCoaxrdscM=aVPKIsNH*#Geirqze*c?qV@I3aBB|U7Yf>zZcHw|Q4s+?KC?yqye>DD zmHv8>+Jc@_`Xjw9Hu3Lt&q#m$Dqhkw=R=LBpPL^zB|bV@Qw56KlN5wnt|K2z*so7o z{r!=;D6j^fH$CLfjs$O32zwkwoFa5_^+Y<&-m>8Lk-6%x`xFLKDBkaJV8k!4;N9cwI2ajZ!+`gSi6Pl2n0YLe7|Hy*R3h@re3=%9W+7)S5t7o!N=Y? z50>b+qR$2EbeWG9X7bW*KFhtU>qHfbQJu0!CG}?h`0V^C>5Imsc6SpEiQ{W8O9CA4 z7Ds1JYLo(p;}8huegy`trBUPCi3e>cj&%1Fe0fxmha025WAy5!D}2~0 zuV}_;oo&#p&G?guH#Qe(Dcye=IFn`ND|ZGRCrXph+Ryq|c7Er;QU?6@^IeE>hCa@- zvZR6U7m2J8KLJTZ^uz>?6w63G#sS{n*wHw%I`R^dML~6mThI}FMnhdOD|W=ER^3WL z_*~pyTN}xp;r|v-rsfGnGje8}rkVgg?U$p#*pwxW2*~x*grziGWCFn&KC@eFM<_-^ zBXd2qp%&kK>AwYZ5g`tIUhpQcZc;mQNxp}nj*AAM8Cr|Im$&Niz5vCEuteZqNVkI- zmonOrk_~lccb8pdsz-`AQlyjBYP2v&v38LwtMprBM06eIOU2&s$pci$J~Fn|j6~5l zX4g=d%p!2#j3_#W>R!wycVp*7>x$Zl?9jHy8q&=e8sQ+D1up$|6J3r7YV&bLbH-NX zlHF*4qIvi`tHcwlfzI!giW-U(A7Hd9%FEt}-9;(p+eAt2-c_7B*mfL*R=F`VoHF8q z(l>dl6pG;kDN9bPW$yfs<^M;xqQasaep1@`b5}A@ewX>t+l&7CVy&A+VWo)SF5Dt3 zq|NN6ZO$)>*oo1;3bbwC2&obB-qfhj2LVLzS_o_Ev!$c{(y*yij{4#;DkaNOuJwnEu?lZM z8$PRcUl!?0a2-O>(Qg%HW5i)g469>ZYwOo#MB}2omdkk;EO<+0fiSXL*!ix03B(nxFQI#~dmM7mRSLxEiMo!Irpzf; zbc~;2N0!tyw3M@FpT(x*>jp{TcIG%;dMr{!GZcqj?~12$CvW?Q@=*A)p4RK848OK& z7h5+g6)MIlcWuwhJaN(pciR%N;X0NWD!c0 zUb1KHm&0*jqDZIu9)tBA$xtsG2xFC$VAcy@k2Vh)41*-gXDEMv%*iZ1;jkNo|nDhR?+s)UzKyEzzm3$C3Qh7&BkP*|>wG0F*fWI@v3Gykd z)bFMgsyZ=pbN1{cWBc)zbiA&fV(G0B7x&vCuGqtfkwIYJ(4@og?;HdS!A{4bDM0Ba zN-4!HqOE+P zibyXYiRQqts>_i}TeI!SrT3luh73oMXNik#S>DlOn}jd`4ROSTT38N?m?jJ5qJ*Vs2P7`=)48{VwNO)jAPnlL;1> zAui8l>eP1*o=thXpVfY3HpCF~wb6NVH zcNvNuNd$0aZ+({5mlQ0f>>rZpc-2*{2tk!)bME>rtFVM*gN-8jwMwfp5} z0P3iW6RNQaToX|^n?}~eD@7s;*!#Qx;4jSjj!H*pJ}#K>S2ZZ4wk&rX0;Q+*_Qpq` zJ661|*gGuLjwB~1{*n8mslD>$v3Am~-B6WFOyIlc7ph{+xHF=CCBr1xB&Ow&7<5yc z(LnkO$DcL1h(FL6nf~B|-HfJv^AyfWBoBUj-roLrf!dWQq?d-G2q$6=IwnQWLp8pf ztd~h%27VSW2KEFn0B1+T~OUwc+jM5Wo2?ybLO)HstrX^J&? z0&*hgG0@lJ`}3FF;GZNKT7v8$CpaD8Lqn0oCnhX1L#ZMp`i?8osGF5-Fi_F=n{m1b zzvFj)!&Kxk;Rl>4jQFzQtN@%>9NKasc8j`I%Oku^=Lj7+`MJVbTe!d({V!d++pru( zkn;ae8CM<;W%tFIlzkc58Ee)-jCC-PZA7M!H54gi-?L6swn3QeZ^s@Xn)p#a@7w$P-G83vp6A?i?(!_(=YG!letV($;^}jsXvx`d4pV~au%}Bkj-1Xv z5o2pM<&tb#+brhdn&Nn-I>`1j6gU{}?4kO)7&zs6F~zh;hC;n+jl_rcV^E;j=^b!} zKrUTs;566X162913kNDWf?=Dc)xf$ruXxks$`6cHN1d_bZ8QL!tS=-!&yBT z-Jss&YO1g|c3Q_j`j>8i=iS~?CVia8iX@7Dias2@P#vwK3&wKN@D*%*@{P2dnBr+5 z;9JmhFl$ozDzz}~?-XJ)R1-2!fwK%4omMf%);yi6h_#Y?!&5}$F(y=n#My|?$>ov5 zk;89K|NXRX5gCEJ!}A`&LcZ?#)1w>LB3@JoUM8?gxLu}JaM9|Hvnm3k~g_RAABKx4;!5b~RN8k`uswh5J1YPMx2NFXQ_jjdggOCUKLX56- z{EuO1V5w}S!j)^~`;)k*T#dMVvH&e|@(Vt+%L?qVyr9FOwtzdlbOACoAwe&Qm5jV|LG5avV{D z{1w=zD(hN1g)IE2kuE>Q44c{ajaywGPFM&;%ods*z5wf|8J;D5-<*=~OQu?pH8ho% zQ+7Tm^kEX2=Lx<%tS!)9rr4Job4gPa=t2Oo?3_O2P?SYdi76Iqw5SwD3CJ)miU?LT zgGRjz-ExO+1s+ndICa8?Pt#CWE!@8E%8!vux;nk}y7zU)$NENd2m(C=smeBUj)Yp7 zcsEzUF+8Jq`qq%BGrvD}qd_LTfg0^2vTnI2)s1b6bi zcZYn5_&AaMi==Rah_r?UbjKYrXRk~j<_x^h z>alz?qgpI2vVj#0Oe)!U9@EGnZ4MM_3cH>v3wJsJ{Qb`O74P#aCg?QuSK@zSf45IW zF;3SL3a-%-oR%AE2-RZc`$7Mvg+Uh!LW$1!VbIHhlIJI!XcZy4-<7*U|COSJq<>4I z!JOz+Su%=0$z{-ILLM;fZw)9|?_3pvJhOe~{=AT#p18St+dp(WW1JbgJLWp_j`qX4 zp7YRBQYv|(V(ewDX3reOzNIAFUIg0Qmy?#RCz^@kkb?)9@xD zI9SN1G#_h4ANuq?Q$pXRau1(PkN2r;W|hmp#LQQ1tuEOweq=yUIU#Mr?>yY{dt-KL zH^vF{PM&Npixyj5^ALTtdY%4;Il{9~W8_ddwDZm#F=UEa3hUwVs6Q(ZLGI@d7YWX%=9mXxSYpNrnp{C zdS76E3*9PwAnjf+9B+|E)wcA+;ou1{CFZ1*{yr$8c`=Ft@Dr8Sb3iPJL7E(2ccL*FM5OQ3FZiWmvqvk64sP zN=$T5twj?n@eua*cAtc9e-jtt#SN2}B! z^ZH3WBEz<_>Z1n?L?^=bDy-~f_h#zmYRzK;mWk7Kk!Fpm2|0K;d z0^-*#t>M=X+D&N8*^$Z>a1{YDm*Z)gWJPXe#;dd$+ouABl#d$E&0#Fc6^vnB%gvt_ zUd%6{;3_u9%G=+kfk2-mZ@CQ)O3e00L(ID&ZTx&By*b>@Kan;6wUnHFerWb{n^1>~ zJvqrj*jvkFuYwD~yRVpk*6n@lxf97YC{+st;oSj_@t9nCBx0!FmO+aZqbvB$Lo@ju zf-C&O=rdGyq0AeD+w((O4%$uy{1Q*4v?#CL=)*E4TlNw3L=6)0>)ke`&6Oe_8!R1@ zvteWUVV_ME8a%==l)N3P^f6Kl641<%W}&J&)361>Ub@dJQ6y^IWDFt5t^zf7#95nRe9t%)0XAEjDuGGktmPwaS5z0O~P{nMo!D7n1FZrxLy5h~}-Tikm_ER%~K9 zN3cTEuqQwMif8|LB1yaimeoM)lD*F$wa$#3>slBag0=c_^{$Vlu4LnW_~8(#&de(l zb6;7vRAsB$Ql-RilFl9+WhtpA1TAdXeorBpIPRRS0HySK#o$73nni7VpCd^-5?*uE zI$i_Ek0V*G+@KR-xHrIZb9nYq{LkcdVWzx8xu&|qRD=B;Mt{#MPuFm?Z7Wr*GGK(c+2-k=?hud`8=(l&3 z-J?dW!v)Ji%vqSeuzL5`nT$wdRg_D}i<1xizOf)*s6gvRd>IS88!LS_L`FkTAMOPy zp4)@hxkm0{CnomSo_>z$W`*+Yw#*ZdU7}8p&tI^6O3s*#?TH=P#Zb*l8(bm2{e1TQ z-l7CaoDc?UW@qde0bfX|QfP_oNRy;tOTU2UV^^IFSLYa@y;tQh@R-*sqItSDE?uVD zoRSBb6W1$_8>@XxjNl?oX?$<}gBLpEu_rV}h_qg(W9~c7ZBpYQ(G@J`q1C3X`<#!h z@mHh}h^z>8af%CoIEk*t;zgCbrtE6_;f46O;}1qZ^|Bt%q%EIZILi#XfW2Fsf6Abz zAo;)iI_GewB2rR|@HZM7l@bM>^Q-;MyoP3#7Bo4-NPklGP@K2^b#wxpBBqD#?zWEB zzcABnEkXbt0Ug1)tv}P#$p6*SlmwkB&CGdb{+~=W0s^LgDJ3$ar`T9%&aIxOwxJ1S zSO73}G)P9|clQyebsu$fhR|iSzYP69vT}8_xDY$KTZZf(**A6cp^U&U$A9`_`?Es8 z4L>!;N%ea$f4kLzvH(osrzRQbEG=BG{$IDwuE)aBz|(E7HQE2VJd$Mrw7}6!P^sTt xIMdpv@{4PCEeZKsYrt;NQX4i-TV*wdiCD> zzTf}-?S}(VP25QU4AJ^v4V`0asqATYnPDgU2}@&8H9|D&OwSKMu_ z>}`Z7ES;Sl`B_-p+}xPmIG7!rELhn1`1n|U+@<)dDKpc5(K)&}+5Vt1Gi3qUf*!gK zW?^GyWBEn<>PSq=^k|5EzF9zV_XLv?mmR!)|m z-+yThwzIPRUt4^jWioZJbA;$=Vhj4SQJ}q*-Tx})f%ZpF{}f>1WM&3Yn1#+YOrTNc({NKIh`;Xq5IRV}NPYyO#mIoRNvY*oV|Cp%k6vDq|<-_m^ z3;q~HQzy`mzA!-)1PQaVvavHkemGdw+1dHIIQTjF1X=!u?kBGW$R6YbbarqOR2(tXT`BOKL znUyo-kz_J+foKSMRE3q**;r-RxfHqh)cz9uUnKv|Y3gBW3jzysasCJSU)O&UJ33j} zJO9ge5T)3d*jd%tc=*}5f7|X4qMuy<@(SRGM+WR{WlHwL$POkC=7CI67kg(R3SL$U z7Gc4^W&r=sG5huHUlM*?{^`095fOj*LWukOS3!l;z#!)bQ-B}b*SxL)_!~F2_kKm6 z`_-6Ih_TGLK~m?V45H>&)Guiry4&%&s#jxIF|NKd5cE|WBQmUvD0%pGC5$vRH@BZ} zcJCHwW$3p8@RB~giEpVbL&OebQ~LcSt}gx+XSj1aLwFgr#hy>70V7$o6 zS5wiSXVxJvkP3#yl?s1}A+?3A190XjYZPk6k1Z>2#Zt(SpVS}7Od4j!jTrneLA_$g zt@RYU`qTXIm|vi>vWzr*hIFHlMam@5Ik-(tMKh;igQIsLa`LezN(AcY?3s(}sW*r2 zMN;ZO-bZudb%cR+y2}x*P?LLgd?8nd35Jr_vxZYu^J|tboTW!bw%H~>c!C?m=^f&E z4r`E7j&^6>l|-W^EPYRE);5~7q1Oqoya)gu;@WA>weD|3R*pR?aTJBY)lNxqA}CBr zk}J`#bf~$0D)MSB_+4aR`+xy$FNvwvh-nwfnT9ai%@bn>5}TI`?fmmx1f*~T3I^_Q zZiy(kheSBTDpmc71=^?)2`#4iG5S*e8k20X9LMi1Ue70JUz;NM4B$gamQ}BkQ~5hM zNxK=`zza)A;%7HVW@H<8at>bFEoQpKBAkbMC54~JoOltb@yfh_=kZRvLwjxO8wT%Cnp^=c)>cLskGtod}QZ=fq!9I2;Jw1aM z97Vju1Sg()V7GmkcR6RhWgDqotvZ(j9x2kradUCxl6*<3(B)~BnYp|cNG8k&*JftO z1OD7wt$JU=MO&))L`2aPMxeA8ah8Y8qw-uFTJVEs4)_#LBiIdPE%n9eCcZF_HyM47 ze@WLU@*I}R(>V?vIIUXAw4i~0u7I=+sMdkt2z1yVr#XLiPM=xfpM~0}L<> zmW1j<7hxreisrN3GFdbf#a{9_XQ$dUb`Whodg~RxLfqJlT+OQ4N;uGUe{Qu*zxG)i z8qLr&6ouE)oB6(LY=5y6eKsAnt;p;COLM7}q;lm_474tDJz!1D^?7=N&3XFR^iGQ~2UV$xZ~dF=^QEy}mlSdz zKY)iPhm{NYs>-Lk*m6~B#6G8|eqUoHH}G&Y7$?LL_;G{V&iubQD7SjV zW3gGmMUY|g74E9uyT8TGj@rBjzpmpf-Ufe?3)lJ%rm#*x!XtoJ+UvHCYa5BSpWZ!b zd*LmccwqCh4ccMwJ|5kw37k3cxAb=#`!UW#86{0tbvMwdny+S#?PZ=2sP3!ryO|IC|i4<@#gq@_`*V@;zmd$sU>T-iKT#t{#sG8`+HA@FT%Gp+V& z6tf5LSmx_r^ELD*`7kKYi(bdm#VE;ytLan4Xxu)AUL9fdd5+IMvPP z{v}uYIa4s%K%T4;vsWKgSocHPGc=u|D7MHYVFwKwXtz+MDm(Rxowi9kG!$)f)_fu? zA1l6MVwc-w$Jz$bWJh`w^DvgF?3(CbJA%F6hGwC&&rQTG@*GsOU6JafM3N><^!JD; zm8+3z`mAfQh#+Us_`*dGZuguNo1!15Q75bKYSuJMt`J>YjnAMy(V|C>-)<<1k^?P# z%_#S@@fnFqnb2J8Vs;LWT6Z)#m+tAc0nafy!@E}KoEJxZD)d|=8n(z;R`fd1PeN^d zvmA}@eFB{`(LvBYP6(ukvYm3n@OPd;!#p{(4YP@7%_g~Kn?MEC>t2JgZI*YDR7CBK=&_VU>BoTV0R zZ`HfNhUD~|%RYNH-au-I>whBo_Dgj4ERyt*WOml$CLi98w42?#Q8n<<@}AbBboz$C zNpDo@T0rphm$2^0+XU@h);2mh2iifZ*07x~&mF|Q#6G7CHHDcFYS!5&kMmr%g*%Mz za7{INGS8K!eM+?bI-svvXd|-Yv!UOpGpo3>VZZ>^G5=!j!ZK5LBu}W&7OLZoIs0yL z{2TQ_Plf|+v<~j#kooB7=%$XC3VQ}{u-e?is;p9N@WUWgU!J~&G|YfQ!U$`3jhw3( zhnLoEqpez=W0QMk|44onN6Y8p7vgF(-6V+u>-KoolbAQ(t=encgoeF_b_8eoM|=`c zBFeKHTUx@uaerst*{Le<9-gMvOu9ecS{bN#^P&9wTXjHD>icLnpCR#P9g*;CT@Ax5 z_i3<87-LDzH?YK&N6rQKn+2y;r~uK2CjySv5ziuAq)naNd#UnI!dhP5V!e>a6~gH& zeOex*)^I*9;(c{7?U9S(MZM#Imwp9Io?aec-|9wpwTGjbU8!~5+}}aguRq?Vnj`RJ zDj&uod?u^O6Vo7oRM`?UBON_Wu^GZ@s2eu&_^ScKz!w#B)Diq7g^ZV1H{ZiLl~SI3 zw&pW`bUItSDz3;pr42R6!K~JU1fIZoL4BO8*Jer3EG2*qpUc9g_|nh}!K03785ug~ zg0s*#zx}>>kX5lDBF^K9S4;LqzKR}Iq1j>e?AiqpPLttvIFs{>arOR5u7%rZ4nyv> zn<4zdR<#@>aT+%b+FU@Iv!*YQr6$y?oMTvrs_nYo`0b5v(M1Y_x4;WiLAs6u+q22m z>I!7T^xm^nCG%U24cUTi!K#&Ki$(3PcVD?h9ie~DRy{dj*)|*XIm2h-7)1||bqTEA z<23KhmRZt|sPFGW)1^H$BiQ(t0iDSt%Rdlxgtm>Y%1t{%ON3{cq(a8o6&0+-CP z*&8G0HYG3W>}i46<3}h9@H;9~dhc;e4Y})Jd`EPF%QafjvL2XTnu%_h1v=QrgFuay zuQu3%k~+Om?gl!dZH!vomN-;*H!n202~8(0qZAb-&Kw9IHI%0{hSbUAyblWqb#d`w z+PFV+C>0`FW0SivqPUyX7(S{1C0{OlFE2}!Kh!ekw-9Fc&JFq`Bp~x_x_-c=-i{Ei zefA>1J#k~JIXjM8#a6(lmxOTe$f4ph4l>jDvsn;*=x9qwVqwAI8}Wv20(}9H;-!U1 zu2JaCn%7xa^GyZI`G62aqdTUeD~)tlsYPBoJUa01rgG6y#*y+ zP^#&0bcay>oS`q$ErtTn5nt_2a6``>RqZr?+FcA_tP%o-wpb~62~CQ4`)y_OJ=zMd zd%aOvv}d&m6Toxbaw*K4O}y79AaaiW#ML6&YGdZ>sH*ShmxSgH<&rK&AxD=DLVL4N zH(BRQ+pD1L#*cFjy7OypuEcr1H#Q0qTQyJeKYY}Uk6(3Gl1RRIK> zF`r}WKSh3zAPOFWJKIch4e-)38wB&MdKY+&lKQH%Y=n9ao~McRCzZLF=s8P2%BlG< z9eUr+0l-gVDz8EDTqNL(962!K3>GQ==yr9NXmK&z`a<2t1s>kCX7aKZ_B@TPYs&UT z?}C;EFRQR#YQ9zmiyP`qEovf)=4B|YqQ9T;r_9J=5yn72b5)hT4hy#=Y|@BqI-^C% z1GMOSuclY^A~u-mnE$0XStY5q>=U)G5wiaCgiBy^+6*H+bQR@y#_ycQ9$o5hAip6} zdyMwESeFf^c}w=Z5{AGygtPvMtID<%Qdy*XKM( zpClaseRNw~_n>-t_##$C^p95f$@`~ z1dJLB2{Lao{*t#XK^aH4AmAnpD+&Yx&`CmfrA%Vgd7J{@ZHF=|dI>Ifn8XrHkmp^{ zwmf`2YQc>La-DtZWnJJVkp*8^e!vglLp^ErsF>vp@(VS8wC`N-WrV~s25JcU2Mhs2 z#cix!ucqgS&1GTCnaq~`S>^GEu3p;V`!W8&4xC@9t91-c4;=lQkU(3E!xbbbv!7rA zm_1KUlo`l-gY(`x)ELCtE1iChF^y#A3o`%>fg$$*2|44SiYBh(q$oc@WPM5L%9@aLpaX&x`;sRBV;X%re|hdx5~qq+^LWsxr?e4F8j{tAXWlC+M;F9||lXaes`l23_MNJ+XOOL}n7J>FM_(GHVqf zlkNwK2J?#+@;Yt~-ZHb~dT@SbG;mj3p*Sy^~ zRto+uhd93KTB(LN0-{D%? zKp#D;bW$Q0OX69wBeZk$BGE2-tiyM1p!7s@=RR^ZC;8Vx>bJB+MU(aAeX?^OjoUEm zc7^bZ7!tq9GbFV~trk*zLPS3#nOyh5s#1~lq@31Sw~%j-6R9D+>AVkW_SIfY7ciVZ zHY#Kgx?aAK*1cfaX~6bSeZkqb8gj@kkA9kX2fKY2^&+Y)s&hdT#f`|{c-$72z$)3J zRGJmuO!>T>^Y&WP{yR3uy-^gS;c?*Ud#!wGF(*XI^?LOnm<0l`4R}k&(EodJ)9TBT zZ%5UUJgOtWR=mV%g7^hsEO|1fw|3Dbh4Zf19T$&Xi{p)kt%{u!R8DVKQP{>>fIl5esi4`28A2!QPf!{M zG(2ab>g#UgLJj`h4}1d!AcWuwR*a~Wd>YH5G?r#W+wE#Q{=Jym_YfO7444v4zkcj- zKvF1hNi($neMm9CsG2Qe4J-}C!1^J&1n`6B;~XrhJ{Y!vrW_xQC{I_bvizDi-Rtp_ zH>2we10q_;JT5LPa??R+=A7@Gx~g}sP3WzvO`qj0l4y1aT|Em(YKouU%*OyS7~vF| zE%^`OqVM(&rq_sv=qI?D9x=nBa%ADbwBc`lh-|_Gvqq5n+i~S`B1Mt`x!a!#DW)DG zeoRm5o>oOBx__ilBQPIJD+=`!rkMa3ItsW=$vz=g6&qCWnF{ckghx&=@_1k2TgoAJ zyzFdx@69lp(wFHlLI>DC@CDI*+ivt7PPYPnjz1+7dz04^U@Zjfr~Ly{n4&vHIbw~6 z9*Vq$jO{V*F+pEU-vglTE9y}BmgC}&)jNxml$)H$Vi$*6y`2hjw7$FbFKGDg1ZDp00 zKwUD!H zb=!bHK0`y=5Ln7uLM{;KB=i87L$IC$ecVYVl*#@Vj$X73!Uyz>0g|A8V3D^XC_|!d z2C+lIP6b|?!`(@qm^OA-P`nf04;KPgSW&NVyLvX7vM>pG%(Hgj#Y`MI+PhEYO&E-u z`c%!9Fs5`do6ybx2pm5+8aA<$4HS0>T|Wg=z~mj#*OkS+95=qWFev)z6{-f;fLWj;^pUYTgZ@l zcwG|gyB2ahfrlP@J@fQ6}YQhrmXY~4Ve%bLV_kMBdH1iKtZa2 zIz(v5ZxQ2q2><}qR#s9>-92?TO{IF`DMm0~!pKVFWA8DpEs9#}RCQ+^t^b_5AA0kgwK|L?tU{jr4!dB9_x~jE6-QMpNrzX+pf!VRT0BGFaI`a{T=g3cOjq{(uNklZ7dVnwZo*BnE-mx zyjrVw!6a!L)OCX{1KRYHmOirdAxjU=d)uvP&aDnk3t%Frcm0^Hjwd6sXrueG@)pZ{ zIeE&PdRay>eI~mC<%np#sepD(r*EZt7du4ZzfG821}oHusc33{Hmo(TCKtnrfbKiPj3yc1N_l*l%flsqwcrR?nc36E z79B`3uhya~yCukjA34RDf0iL2AFH-~T=mqCZG+G@#~Fp$%WZ`E$?Z5sJ!ksJqqr?x za)~G$9qdl)TGv-fl9hv;G!pkN{hLjmUxp^v*x1TMKM!jncEX-he`dE6?jqx>M{l23qb%g@}FiOU#7=Bh$CyR#DMo1#jzdGNmA^f2Vkc~0iaDN zjc<|hW+-;T_~rQn^0NlLG65?JP?`6ik(IREmY_PvMFGZEN{<0oBAw6&km|!^-9QA~ zk{ZLhL#nihfMJnJ7{Q;@ci0lb38Mgo{&N;{r~@|d0X9EncaBH?kV5uLN+7J?9eO1a zgf$rxTJ#uw2>LHJa3f&seoLl;r)N1_vL12ivcUIdzhmEMp#SdXV)wcQPmlgIi7f-( z)BSqpUbx9O9(?h0)*GH$WoxMgoHM8|p@`*2|U~Di%z0zsi={O<6a8C5QE1|cd`*igV zf7ANni7D_ThsDDvGQ&c?Uj-Wo?h6QTol=@V1j-^K26uRK&l&Y`e^OB;G8Mh zv(9-_H-E!+5?R1BibB*v?A`OpEAN5V8BAgCDg{4RR6?bM^#sO={0x_L$imQEQ3;0NU>y--x)@QhIw{)7 z;LVIl-2q|D*+OiVfMQWVATKrKZ{*|7@zy&FmV8Yig8xim8NzLIXcT_VU7=4lw}jqQP{w%0-Z#(J6Vp6RvV@l@b!HhHWSB3 zd_WtSBgW%MC;%%oEPxpfB~Ob_4RGcqmhc_E8e#(gp&y_#FR2J+b?7{R@hh4cQsU6w zOF(6ap?{@K2_Z*!x$H+IwrdWyLE=$pmB{me9ZCfxZ45z_FK=}i>9{Gr+Rw)?>13|? zp`h80bs#}y;=Q*_`x@{Ve`*Ov5wVl8KrjC8n)AdBq&o|Z*vVYEj#jG_Tu%YuJ@jMs z>%rIRF^e_E_%zod3Zs&DD)crOwzBkbXNy=1hY?;MO$Z_NH%(kWfD%CQE=z$JseTe= z+mImX?o+K#_&@@n9NAN#r?W=1Ui7U_upP?@x9G(Fq{|m1$w-$qE!eJ zg!~6`da0?WAYTC0SC2WJ^0#1hlg7S<*<5i*>}%9#)y)fFILG&iqm) zO`2QtAz7|uqzGt7J(l!!Tx(6;h4=4HLo_->?+KqzjK<|d_gux}w zUD?r7qlBuALV{%->eEFb%!E3llR2?#=I{qFf>YtEr@NP(5um5!&)u+eLs?HgI01;D zd1FNG?p&6AUlvk%ops%we+8}R^ukPGbgH=j@D~Eiu;UZ6A+!8B)@?n+j^bo~j}Uu4 zTdt|VJ5w68wVoER2@v*}ckKWveGoIzi*`N17L%l!B|_OKF^^0OPZM!Xu3}kQf&s4s z2;0E#g5qp={ohQOYlz`Zy*>bdS3cIYTU03QvFu96&(V@8{%^Qfqq4}lfK&t*Ji?V4lfO{9jo z^mjLm&K=g8?ih)BK~dUYWS@#BDGeVfaOCET6rBp>0n&Z1 zKHlG5-+{Q+GDthTea}rC@j*OTj%$n|mDSQL5E8rq&0p>*o`vU)qr?KPlw zJ84G^0#DD{FN3V-M``Mi>gVa!lSmEP43%3f-i+-qtZtFmFiz-&%*=|;!ewjjCCV?5 zQJL(ZBUF|9QD|% z5Ecyl^w-;b#FtA327sLv%<{9_(1WX+#%2zz8_{&fp>-NbS1X$7Z{xd<;et21clt|F zGEY}9y{y%&BhOcxYEICs0C_pHf=rM6WLT#HK_#M8`1-htB~dc0+Jy0dxcw%#Xd@SN zKksem%>AZ{qm}cLy_$PuwhAEwM($ntrxZj}PEl{ExccR$xznJ$KTTKr!Q0_I)fIUR zqlbnvO&2?@!QEQ!I-@NkHl@NRtE%R&il3!!DQj8_y%Xk_j|`wrmd;}KBPC@gbq<{; zkJ5Xpg3R62O)TU6p5DH~oT9c`{Ir@*9&_zYJWqVeI`@%>KZ+V&XU(e6xrlE2X=l3Y zz+Fl?H(ceanK*}X*_%5$N}!h-mt0s7fRwW`q!}=NK3DSyomwW_^LA)l{kZ^UvyM`Q z7zQGYe4r@4HX&p<$2k#2J)*Jv!jzFj39Q+OVn0d7xk^``sl(I^Ddi5sjD2$Sl;HRJ zs0NnTQ}awtviEQobfM9^w5_Y@ua~M`P1WJ@!^A#@Lf#)7DMxmjEKy;~XEwKBJT8sm z|1<{IGDq5<&7}}wDn%I#LinIZ4xuyeFU~5TEW|}YLR!wXj-)G-kQG7oY#mUQqsO7p ziRlWforjN)n%LbNv7d5teDTaUm(V8Qm%2CTnptrKw}-dF4^fD$l#*n*xN*S0r&B)M z`Bg|E6$uA>b1RF#adzU7Vc$$~u671gi)SnXrI|DD$EG^C115GUm1pkPO3LCi^>O z9^zTBvj-&U?nkDYo?Y}TH`bj`N4N5)o_PdamNOa~qgJU3sZTJ;nVb0;c(szUwog?u z3s3odzX>ngw-`A|sItT{d93xeq|CxUr5KTTvA*mW%%Qp9eI8RaErdzEcG-KlkMI9c8@ySv`c}s7_;`#8&>jnRVs+XDzyjs?e}Q8a{lxQfH~@zVozi+rA4EWbr9f zCGA)c&bh@)an5Vt#tQNDus@kzv^f_-JiTAIAHR(6%P-HX6e<=Mi|98etQE=tFBBKk zB=wX}pb0u`C0n4h2_P*Z(T7zoEz7!x>P)eZ)R2z@Hd_2dR}Gt9+PFzio0UKbe52wm zcw3}3MX7}oexrg?p)ZEaY$h)wD74v4FGj?!`mX9QCMy?>6H|o~5zYmd`4u{E>t_0! zc^)MeqEy#c6aJK&V^>qyXw|0s{YdQ_^a@@tySSPq$=`6te`p<`tU(VJJyOOHi$~-T zvYYpkV4;ay6MqzYe8{8KA2Yy%q4GV>B5%60$;zo%HjigNuHJF-11*L5)xKO>)NvaI zaj9QtttRLFTM=7ghxK7-hx&sdUde=^5VqBlD$WC80sGr_Rph&{OvJ>Z+wlxbr9J5V zy=VBeu~)MOj^1w39}Fs+!Km!npsoWF7Y=iH;cFtmy+tc67g~r}ldxG}|&^hFBPqMo3Yq^RE5Ly(R zB4i>NG>y{C%^7BT?@ry$FSc;$krqYVA)?_Z?3-C>Y;dFLxF7}n zo1*J{p9odSMS0U9x$!q}FQ8=Bq>wb5R?O=hL0@C*G-NUK$0*t*JP(s)>FBeqz61`t z-s0f3-h6z9h_$u@Gd=9HLNGFHJPIB745^`KtuBdhn8@V`gEP{je(aEov_8 zp!hah@NsT85~>tDw)oT=Xy?o2jVx+%a^tD}Xx<>|=bK*ugWhzquBEz^``EP@av$N1 zIm;SaHS1%&m55}^TqrM=dt$8V>)4N2rLJ-Li+v;T8#04XXU62zJT0XduD&7^9Jej- zIi!vVpU@~5@nQlo`8GnJ8I-!ccynS+LqeWJIUB2rZB-QlkI*)e&nH~J30kh`JSXXv zRsZSk=W)aau0<|HK)nnsXEX2zjhy(`&4^$(?4lJ-^4%RMyAh=6`p z>?(zU9!Kxvm={+CtH+$zO!%lDKP-=XNtw*@jV@LV6y58S(|Bo&Y|lVMRWDg8&&`@o zczXyz2rbUXN{OYYrCj>V{qR`pT?M0;SaxEimHQty&XJs#eten2m)GOs-7Av{KU zNg`+GI;|AyYv3k6q+Qe8_eC6yR8iKvwpzWy?Pd>iv;XFH$eOKDdFF(3;zJB2oo1jOsYoz-5vs(eb zQM*6DvtFb4Euk9wOq`GK_uf72b*&sTt|@+X7?mhbLF(-VtMheI#iz- zGLJf$f!^F>aYx;SFHd(g2Iv@*wiR9&`mPqmQk}n9EoyrVnFZaOEh z`0Rs9Txm!Cvu!vgy0fKN1p5~kbeP+Eqn;J@`jYfa!n#P!IoBE}?D}%MZ?E?Rlvw4^ zir~TLw!Wj5rS=`3f^Xcel)93tJBUMqmvRGY9*6QH7WNgZ>}5R(j+@c`@WLU{hto*N zr7IgeHsW38z!qEAWdH04Dsk_iL!jxB^kLDn&ua?`fh>I**ncg0{%-k#16jT(N~w!6 z$tp>+*g+17n1jGS@-@rUy6k(%P}|NKDE9oz3XrIGf#dC&*8u5MY$Q}@H7;KftRlPo zBDI`TbJOcK3!%?Dqa&KMq5bZ*(+s{qoXoSdt}IYSNc2_Iu3Jhk9j6f0o5W0da8kL| zpp)d52D@ywQw){W7L4(Ri>n`%@7dtHtz;%x+i82X`3q0eEau}EtuE)&GyG5a>lNl; znuG*4Ba5w2$JlWKxO7#l9@FkzFeb6M9FZ40a`LlPNr%9wwwAhwBrYkK&{se!imJbq zd5$niVl{I*-56r(YE&4h7{E-9&O1lWri4b8e?_0*7TreRlxnA-msJaoLVc>TNxZU{ zavS}m&!@Q_ zR-3=eud`kM$@QN{wtjP{K&lT1B_7t5zqo!4;h!nE52q!57gR`M?w@BT{*&|1jhu%R z-QVRCWGVYc!tO7|f7<8y&G-QlHvH4B&won!%>@8FgjByvCf$EW5dRfm{kz3~GXo&| zuYZ>LzY+Fl!1ce%!J+?erTz@S{#QAQeO61>qL28W3>76A=?5J7W_*J{TuwM-u}Z8279#P3c(NQTU$O+S5kh>hmuk2+*pcWZ)fC zhPc3ZWeQjQOVTjMxgEr}_}@Cbi_9CYK%lx%8DyQ3=o(e~g%#6***$pQ%;{&@9`?%6 z&-pU-VgbG12e&IXC4P99Lq9#=j(|@?E`qc&5Z`n#h#P5?0spSAlf9Fhu;`z!BQNY> zK6N{*?4J2R0*TYw(9?V2H0`QKFLGL5_u+!bqRhzP92Snm$94n0`5Woc{i4n7Mpt5t zTW^VxCw2y9rk-ADavl+T_ORj^_`xs+JR_YX9IguOzK03)maoylR2;TQ*6CyL&e7#Z zqv}~2>%D|>ay1`!z9I9mm|*pciiPHeyTjR(Zg(i5CQ zF1EE&sNX)m5)oZ8Jca}es2QbyrjV8P@I^0$$&Cp&8aZ%fSvmQ^nGR*rNNoGFh%5tm z!)@wFj*dh6FH>W%T%&G3Cg@Sd{Z9fXi+hQ3qK+t>hvRUM)Y8ai;RnkY@B;Y;7(644 z|HYf*Rbz|J+KF*VpLz<^{V!CnKOD*i^2F^Ju%uZF^CF3I*XH--OrH~6L{U-C>Dj*I zaFY;e)m@?9^mtNT6gcQ4Q!_E6US}m^w$!hTPpGf4+;4>*hvEdf zZfS!}7%pkQqkPZ&0!Uu{hI&Ae?y{_Ni+zGjWr8`vsqq^fB*> zjiBkl`5%@4C7z5|%VAZ=cC%~V!MP?6>T1NlCUQeV*C@?CkM9`P`ex48vyZNZ#6W^2 z8*wbN57rsh)`F^A1Zp5Kbu_jPkkcXA%1N`L!W$PethBj1gw+~M0qNSdfi4i(`x>4J z02%&8+ip>`Q3+J-#w3WUPg^k2rUc}`rg>-K5cY^LIL9qwll~`I4sACeq>)7OPPN5% zz%gg^Xd}De1u|SKANv&Pl+KDA-8(?zdM+ z)VAjX8zn21MJjjK3HGXywW*TADbI)6D+wbQ?GCU&)`_V6H9fcBZxP`pfdW{oky^*EX327CS&f z#uBI;Jiv6;kIhkfHhE)2!bxAm4+x8eKF1Ucn0Xw^bKVtJC$o)pHZCZzcQ^c%Kn1tm z;9jgyckZ&|$a;gwtf&zyRF2@%s^fufP5hKq0uN|JLs!!pc3;Qkz)|U_%#L|p0GtwS z77$3Ind4ix-I&&wt##~<1M?or|mGn67g+L{4Kv!3WBHhat-Oea|^bG zUNCz}*2Mc;@^LmvbbP}7h!g^PlFjV4J@;8_BhIU%B34GZ? z9P9cdx}=|`9)iLCM?!su&L*Bkn*F|Y1nCOt{79WgzYig?+<^?^KaKfw9eKD0n<0$3 zH%8qMScEjpi^jTc_7w#@5YB8lV0Jf0_8?9IB`W~in2c1IbkuCXN_lX9NI0gAh;y8>W;y&vfmeOI_Lr6sa+Lu!s*B4eY>NTi&F`ZPOke7^d zNoODu$t9FBD2pa;ofHbrq^HrYu;b0e>1pil(6LBZIBt^0rcFp<-<|tXpU@nNB|&C{V18^N7~57?FH_TbZQ746~CHYGVcUM(a;SJhLoY z4^2?1Az}l&Hfl3E#HM~-uiw2RcU+m*&w58l6HbMuwcjcJ{IgSL+^1c0@W=rb`^Xd^ zdc)N_5XXaviNJ`O=*!~9``a-A^!PpInr%A|vb{chy6+n6NjLwZx2@;HtOqeJH< zb(k zA5kMzNv_M1cOlJ&!?|PI`qN-7WRS}x&t%fRVK5}eNL!xq@s@ZL4$BnDm0@&Gb;=Oek zpxsi1?$NZV*BO@t_@f}z8rErk5;@fO$8qJX=;XoQ^%~1)IDL3TEH?ZFRhH;l*J!HD z26o1qm{oH!_(_z*@7LRT$)XI2IvZs$$FO;LHwnY>@C|mWO&NN)McVa6u#_;It zRvO;zQmOY%rQ|*7|6aCY7B(H ztJDx{5bMhTWgb)(K)wSoB&TxJwGW7j04g0MmNgOljf$T&vc6(0gLNz&$ke>Av27pchmFF zHbgbzs)jt>DPyaDzRxZEK>XO|-Zj!qrnt5RKClSBLkW>1cU73EB)Ay(5o~5mABg~< zrw)QM6ab$E+3i-_$={vqwxZK$ixCP3|9h6i@ggmYU%G=!>yuafP;Z1e=DO}he&+|X zRPUR++wU8@ME)nI2lpHDGH|yW#>Ce6f1<-b>p!#s^B>6gkHO5y1o+SG|8lbYH;brv z*qbn@%Ntsn7&*f*D7zRs|HmO_=V$}NAS7hxu0_bg#7M}-@?RHLW+r+@LLC?eaYs8B z`+p>(%%I|EVC!UW;AmoN2x{wHK{{PX;OK~}YfZ0unR zQul@0sdZns;hi)X1ibMx6#qnsp5WP{0PfxCK4oJ6*QFX)NTtDpZ@8#Fr{^<9c2Zq5Sy{HvINVMHa3Ahbu zrvm?-Z>!h0O3RX;?`NIt;p{B8;wZgyf}UErAKs~+84M-P{(5dQlJ+d2;I~7ZDAtAH zu-A7Fiy@0$fQw1dAWTaCu);K&o)|_VyuHI;q&iFWExKrf5@|E+01XnR2beDFgB52# zOujAIMMwP%oCY%^;H9`)oGwj!or@lCZ>-Itv!FP65AQdJ&s&?V29>Yg&(By23Nv>!244utU)G>Z193~fXTujpcUl&v6u(M>eR<+AB2i6{k8w1dh*`oJ^j)ybv_cvLolJvLcN=Agk!OoIa<&L3yT+- zA@lX+ywqgwUVAMkd1f}O7RS5yoHXJeEZ5XGD~(sRmj&<7eoOLEN1$<*o@zvrY@tH( z{C(PR*QyC5|1`P$P@hFF4UdwPr_}#Dv9)BC;R5Pi#j2$IVu?)Nf4sGn`l|6#X#RED zr?3?g?n26B6)uZ%Zo_?_k8K2{=n18X zupo~vz(_7wN1RVGFUo#wHVO`4TTZgLa_ToK;0sf{3~6@P`6)rSi%$FtU%4}kdD7*K zK9zw~p;mB29{UwwMo+hse1*YdILVXQdf8ndkFB>gaa(bEln!qDq#LW@1);nAp@pP! zmyksjkVwx66(rO!V+`15Aui`4l?5X^FBe=iw=5G71*{SLUTFBzc_PjHJqTPH&p73L zCL;{65;4{NlWOxBO1!$T8E1s_pXK1~XLg$=f%o{%Vrix@&wJNFA^I{@yKw6A5Cbl) zG@uOAPuxzS?vuv%sTAD9(x32-m8s7P84I0)!Hb!xAAhItlHjZZH+tRR&UREd;#xu z!Z`y>B1QMey-)UbN%7THs)DnKSM2EY^-WZq@NV8-qoQQwCr&<;E+c>`qAq|6kHp== zPC}qYJw5JHe7~5fIt4aI27_I)M3&Ym_vZO`$0g0bNW6;sY0vGrJ(mM{cc zb4XMaJk}+J8CP)_b#a+Rl3~tMlVw}8vHpT46H|+E%_vOnf@TA2s8O`0)4%A%ET&fA zeQRqRIAq&BeU(?}&n?_&#Y{#TAq9a)9CVf_6bGn4vq+xP)h~clHEvXH!Iy$4ACAT7 zfb)m?=JbcogaX1d5Da`Xh$%b3wAma8EaOaC_l>}X%2wz+!~-9bYGLF4cA~6oXDfH} zEz?O&kJW4s z{!{;|?hY%a<}3#Vn7XJF{V=_f1plV9v@f^q4J$%UzvNUV2ut=W&-8 z>DZU$%b8gniwX*BNa6A${{Y?5uaJ+qbyZa4PO7?aWO@l*){|{uODPxGc`7Fm7mV5iSnVKl3%cR$!YwM!n(MK(v*9oh>t9p6$HeBufS4L#^N_+2KEQbm&<>M*%=_IV4P{v|?0 zdSn@A3z5=N3-~aENc?4ZBg74JcYk7HUfe(y2 z?Yf>t;W_t6;&NKuk);fsKHa-@LK8vU`BNl63R4)kUeI`{@b5~ObyLy_8S7F0JwA2T7kv$-*F7mh@85A+P| zM~)qd%M)G_Bt!v`K<)*Dxsa;SZoT2KoN=tpT0M~ig-4uw-KX=}I;Ni)bk2Px(_To? zE_v|qf)}zZ%^+q!_*0QwR}z$wN-EvMMKV8LOhX0uW{gip>?p?c^nkI!h`56USsi#x zgiRp&jWoBFY^tUuuC?ZJW|Dymn44NJRJsU_T6x6rx6-TjIf7us*`2T5lDpKOXaZfn z)Vs`|nB0&b(7vcl;{Ssh`j4jhkB(zv0DbA~Qsdnwq0fRs+3DGTk=FfRnwoq$!{Z;9qE2cJm@iBE} z2ix%V^Wneo>y>XzPd}MTXEr^7@qQOpKZ*P~+{d0i@b@pR!qp=*Lp9e+P8?xm_lk}HfCg7~!v zgnGuh=PsCJEZS_YnyKT)p>quvyK;gECY-KeK*CLtkclzPKuH^chKQ*i@a^|Pfh*!G z0ldd}cv3hPs<6*2k-NyWo%NcvT}t+bjO|v6EEHR4X3y*ifhWYUuY9{i?+oOUuSddw z9egc%F)u%7N?O3F-~^fhK>&R27w{EMuM&U{gv{$7BeWqs&w5IDudlZc+XSpL+YKzQ z6I(?I87-i!0NVkrm!KYfT!8pyrAXERRxME7M#9ML;I!W|Gkhwa`De$M;4JriTd zd#NMOlH$qY2>vX?j1GKr&fnP`)b4UK>Ixyou2TuC*p-8Q=k&zF8%6qrT_83WO4o-` zBi|OWWr3Own&*R%_l@`wtobtwaZTncvrlXc+O;WFAMG2=Hvp_1Kyjc^6xS5@(Z)tP zhy&SQio^=qa}%`9-^bXMYkr9N^6LlHPjxCPS7OzlSkW2ro$4LDWk3eL3+4x^H~L3J zEWzUt|DN#$=QE z{UdLKUy=K!c)Gx9hpd1TSN1yZNUSgU2WLT3w)^!4)2c~_MtI`j$T8x_Z`-q2;**WJ z+oPT1TgTn9hBP?1Q~S{zjM}+s!mUpJI&iniUAtWSTpL}(z3_S?cTI%8;qpz4zSe*8 z__F#D{9yg01JvAQUqw`gkM_yywLS?f!kqJ2L^G(DC0ivvirJ4mcW3kpd`{QCt?+Yq z*&GYoeNIi4Ww}tQ5}971mM11fvwZHjy+0pJ-z(Ck^)gH*}c3S0)P zzunxC_HSvEMstDAT5@O(8uK@SluYQyn!f5f5Cd>A`(=Z$3G8Rc`Y@OUg*M@Hxdr8J z`vj-!Mi{`Uo^h7QW(c>;`DhQ9;N}v8U3xvZW7LnN8cX7txpUH-jVtE10!LBPv%s=@mTPd zNn6OaL9Xe6puI3MVn+%S0h7IbIP}K|h4-(d4cv?&R>5z+VI}1F(n0kR=oBgWAPsVLJa-p`Z8my5OV+oYExcz1B&c-s` z^H|k4hlTNx4EO@IB=8FJbF~FMi~bdiC#`2J=NilhpDE-`+OL5rrVYhwE65jOtQGhQ z)=@E-+2Z1&QMNp0=hpsq*l6XfTwa(w@%B zYAx}H3nNXVqI!etOSzi-l%7GPz?YcwM_rEvw`*iBx0tN%_Ma`nR+pyQwY*fVR$Df% zn_RBWpDpW*mA^s+x3uFp%DpadU7k3E4g=1h+hR4f*<0bYp=#L9*wXiy7}^ zVx`FKlnw3|37&+q`c@PfE%aXUg!=drSpt9NNB&|plB{L3_Dk3dmd)9+uXEXK>GoQ@ z&h|3rxwl?R5WBWc=YX(S`Q?@0;0LNZ!N{il4b_klN5z7Ko423^=#w^*07~;&tgs_m z=rLiYcWcoHm9wJ-;*aG_jK5TUFsn;v-C#UChgRR+z;J(^y`6t%gs4QPmHyx<#@$O2 zRQ7+dY+^;apra|G$BTi;8bo0zO}ab27zw=%Y-9alt%e7LHkKhf?J zeKu@eoj4ol7tiQo)Jh7f)_`ptb+&ZDAa~qs>H6&Vs^GSd>Jx!>f-eo<=%jx6d9in* zlgrLyLjws4@tf0NB&%9$`QaM4l1z_-{x{HQAy#8!bbPisdDRrp>x~>PzLc^}-6B|0 zbP>G&Pr~o=$p>r*WqZ0$#cNyz9tg0lztu*>BWcx7S~wjjcn>5Y_-mg2T@t#R>s%dt z(;Qpjf$R|6qt=z++H@m>`ij4t=dYo*nOs`KTV-;YKY%#&M^dmFUw+Am{nb;-++lTJ81?*Ex;?w_L4_QE%Tl6JB zWNn4VzHot}DdenYChG0wt)#!9c~&%)O11rbbf?cB`k2?2N_pRVJUV`bvjz%>)1hESvXLhcIfDd#ooSkbjF-@ZT=@a=2JGS}7yIOQjNOj<)Jwjf zso&SucpBO@>$-&ud?)tct0-3wmX>t1u50{84)j|7O(tiD=IUO_wgE8`RQW^_DCMA2 z^+QmA*I&W*d#y=r?a2V)1W;BnOp|{P)Ii1u0R5&GkLjI&9yblCc*JEd0u)76s8BQ? z#auEWViFV_-g$P_K(WUiijyEgsvu%9wGm^F`}$k+-oGc;_Au7#ACrYIGR$Z@ocUiu z6YN3O>@c3igzgs)^pE%Iy4`G~d$cF2j;ieouGrvM#(Szc9VK6k&~J+v!58bV;eqKU z_K`Qhkix>@48h?RXI8CS-KO1)ylTw%6$K6A_I&_^Ip)Ti07a8jQ9sFy*gyoOia$Qs z%Xjo47VCvwKlFMs{E1@CzSv4P;~nx;5sL7l_jyX{7=ew<4R}LnvxluCfdNnHW<4J` zcvlFRHb_yb0+X`vC^?hJaZ?bI_{>+xd*A%+M7(3nKSL1vx9wlz5YFFC2=lQllWWk1gL8XBW)PQl0X2@5G$(m+|zZKFrt#xH#9> zI^WM%1ISxrT8J!(HHJ2=qClp>1i?35W6n{>oFkA)6T(ca{9p6xuG8q+CwnyX{PG1Zm4jJM+yzl z#r7=vKdexcwdFN(awYVfnLTJ|k>JztVXG$W&=a57Hi^$x0>x}{IA$j>Z#-7@Hf(NK zy(1leTh^$x!U)wgLRZNXp*JbVlPEM98p_=gI@|CT5Z)ZF86};OJEv8tf3*_+(rDC_ z2FDW6$TZCjk(CLPF|tLkTsB>#%0AD=liTN!z!z8LF^|R$dec=jOc{lXc_tEzyxYdyFt9=2&J?-Ss z2=%VROxu<81M!rfH8s$;epnV~LwuY!RG`Y3vq}5pi67b!4n?RQ zM&V*@Qv-TIAhg=g++s`q%k*X>+ttqBIiZ@R1I~3Y{gwQ?r9lef8V9HuR0*W4#l{3r zQzP`kvYd1P|0Yf<>ne=H9*p}MjNqw3Db^2=6B>N7-{?AQ1`9d+{q#4obaT+>XAASY zxiRn!#S9)(hQYj1lia}Cecso)@xt2XvzZ2*loQB*(e#S52Sr`CAoV=*a9FClCwQZ# zm$-`*|F8TRx<=V>;~1(>eX;US<}>@njx|i}o$B_;A20ITF4N}m0#Xa0$pZ2}=f&1m zX3{%xEbN=axZ)Q~=6Ghe88|sg4gT*M#1yWHs>*^5&%w|c;kZNSL4We(rUhh?*==C_ zqJH8|S}05kEz%3{QuvnIuCV`}HruAgNE!z&5h0z8KDX`w=R0=T784+Uet0H;T?42}+3M8b3~+$SA7U5A z8vdgzIU#D#8DqbeMUxqM9EmJG;U{g{`=fHhi`W(Ub5g^|neP~9WOVy*r)Z3@?XVk> zXBdc`B3@Y&1+!M_0VJ-nYo;s#v4oWXCy^9Bj@tB^y;RCQnTE17)rtDOAZ z(X=4Mql!^zi2F;PL10#ec@Ly)7`2s0OIlLU1IQs9W!5qVX3ZbYSoSJ!K^<;Xzu9^G zIE^~(d+*Dr!nlzZI_0-yUh5wV{?!}EI$G$3btq6-=_09ycB1yvr?W45Am&7Ua(^t; zPgA$tiqWqiT-p{#I2cIPwQdHd!`;c9Q3DH`L-xLgbB+x`swaVcW+F(+Be9zd%O$?d zT)JN32mA<8ME&{z0{Ym!(MhYzSeV=mGI%6$c8?lql}*Kjl3!b2<8?sLSq9%#UApg%;7bi@X5C0+kv&08d?t+yha1vFmt}`pr zU1LjDxGGZD>}gq%&KP`d-##e&Dj`RY1vh5z#dBmLjO+as8GYAi_f7t-a}jBj+x~v_ zHPCaRE4{NSzotzSdU zl7K^@lfcQvL+vK>?Odapk?CR44^OWZ6hX$-zR41o3Q=G|$PX{F+LaicD4>fSRswA9 zJ%THo4`RiVdg%D(A$*Azujo-IeT>;i%>0m|k)ocuj^2^!eL<_HS=*#nT|1+b+#0@N z+1}r=?3?K8bKUIGs^PZkW^7!{^6r0G#*7J1IWL4OS_arXoP-cnL}`Qn3w#m44kihh zHNuNa&JBG4ixnugRLU`hS7>P9!5y}jhZ~Z{D^gJmE$^T^lk^QZ_V3cpTqZUSWoiu2 zhYgUV-0#}NEkg;e+V%IBwD6V&g7oAu2P(1d|JxP=&jY@4pxT8O*@okmQ3L##{J0F* zzmq~m+aI_dO4VV*N!O4FH7%Y+gP72l<2Wy>lC3k+yDHg4ZddD_IO<-yHCTjF>fn&qI z-F4S6esm-a8*-!7_`X{4%s^R+A7D*VWs1<+i_WR2kpO;{@Uv=wkIo*l*@NS+bECsG=>%aZ5Fy|3;>4id*e z=F}@Sx!GG}a*d@de=bYzL~yusO`%^N+E@yjWTbO7(Q>}chY_ZEFIo6D7pXGehf4r9 znSH&aF?VGOQ5R1ujlA!E{$4JHjv0fDCsT4=6=!OH0KZ&^%&LEs{Ii<(LthOmSM2hK z?X$tRAf(-;kKP4EJ~xW&>b*!@{0Rh)_K73mOpx#8`8ZtM0g$J*dPx$Np zLx6wg59g{P`TGzpNJUGpVy7jV{PS)9?ON~ zR{UEqE@JP%rYzk>YBbfQ2pzq`}YAIgb>dXzcc z8(2GH9GC*NX)0iD*9%*>IUKnAu9;3cUFkT#fzF-hgy;uPA3;OojF|D7ab0GBR$8@o z#jNv`)AZfU!!(RGcGtWY#{8EC?#WaGC6=P_05Uv*LYI^UJ;W}maH&Wz;*nAKh^RVP z@OhQ>@je(AlS2uw)Hw08lj3JSxRbIqn4_r@HK30^Z7hG?lB@z0?|4Dc6-!=mxJk7* zW@0j~Fu)%m2z!gpbI8hbTwe3~Ej+ppN$|ru<9&k2(RLlF<%i-I+7*#{klTalcNj}B za z6!SDKmda(y&G2;QHsQ7|DP@p!!Lb`_q5_4sPHB+DMr0HM5bmTcR5V{oF>QjqTK|pZ zN)-5RCx?~gLA+=#1OHXX)D)9mAjlkf&@YWlrh%?9cNej2+G=H+x+ob-{L0jUvg`$Z z$ZTHDiPyw(V(f1b3MM1~)6OWyCz#U6)W)FRGIfGOU;^qL(ms(xoD+HNx& z?sN(K6UD2){2cz(%O_EcJ^H^++icOxY~&LM>pRgk-^^!m#{se?gD6E5Hs3j9>`b5Sk>6TIN33qm<>`D zwt9kkvU#8xopGnozUQ-mR=%X5ElWdGG&q=&>S{nkLjX!oW$Ba&r4uDLmik#N*wHDxJ3T7hS6Br=iy8=xac< z@TffIL*d81U*9A{Y)lrj^~lx6{wx%v6(Wnyx4Fmn174)-3gUhVCl4$|j1?7+S8ox# z4wvbAhoH(sWS-&(kPb_iac>pe!RuZgIp4;`qw1JK1{;t&zcKDKDt6|R$)Z*-dnjlh zhQM*ueXwJ^rr`m`!SKG}BqTAzEiT1G0&zsd7*&NBYka=uW`!m2iRxaN(#xFY4B;xg zmGlxV@iEjuitRo-+V9^SAg*E6nXfXU$|dQRp;Ak>U9(M^w7cL=i{yz~P()2)f0D1{ zl&6~j4Hi@xK3N4=cUP_Jx>GfwCRp5wkHc~+f9g|c`HCHSE&Vl?AX5%Z?L$m%1Hg|F zl+Br40$j$(ojncqg33NG^)j8cJQcZnvoF|Rl-cvfrAkvXv-QG5h?1Kk>}E_n(S!XSM3zHL9B)T=fo5H zZ%aO=NH^kN^5~Z@{A%_?epS0}ST`7Bg*+UQ^x%*tR;3Txw?=@cF;gYVOZX4NVNtZ- z-oNiSzW4Vv9Br)gH*vKJZT{-c_cZ(BIH{4J*@Du$KyTZS7J z(yKw)Ejlf=Hd(TTU=51A5*DULRthV_>1@$8WSQoz!nUvu+qUw^c*Y-ZhLkeXBO=H~qVuC731W~W4X=gfM#Pe4`xbL^PDp{tyHZ2W_Go2#^ArMx z%iZd?qD43r8<2n|%Y9hWDs6e{j8`6v)O2{<&%7e--%jap{+hVHtK#+=OeQKj z4`)7jk8%G8hrLOm(J+= z^(%b(3Q91fsXID+nItl_fpB=jLgiW>J4l?Cd9*VH8JadU=(OvdsUXl0P?w@YQyCQh zK2vZlsgR?D7KNw5Gzh1_l}ND?@P#I>jZn(*f6e~<@}}@hRUHO5iy_)Gf9H_lm8^ClzNRY}W$ndnu7R8>&1}+M0?01E=^t9J> zQRdukVLHav&guViP+weG8Ts>2Z)C>&db;*fR<#xN>xHVy5V2?789M8fIQC-eJ-2OE zyR?m&KE4#Dc-^pCz4U7WOtCn%QPxOMu2*`h@|SCZm@`D~XCoYwKC zB(Xu^*{u$fFEfWMv^fvXGGDo=?d)^+U8_R^Na+l@PN94py?dCJKq&N^8X1 zkvoLz`gTUyxpPxsbRK%BP*T@E{+)Be|8#NLn)k7`m@C<4Rb^%Ak)v$N6hBnlnX;lv zYS-XYG1MH^u=50&3cq<@8k6Y(#`SpM-`6~234 zLnu3FGr0Za=&*fQY?x8e1|dTTcxPZ)65!q%{Mo^chwE7q4}FdG`xfrHz*0CmU(ZeOuTI=G zJg2#vpjbtoRA=D2dxB&D_7z83N}}L;E@b?@1U_HPt*iZ&B<6D7cwNF7b9xl;su;3V z$Cb=6k$ePvkC{~zv@`nwWJ}*Kl#sF9%$4Ys>*t`o)ZR!?5V7>xf}Yeua?Rg(`k&1t_2i zT*M^a4&!OOJuBVroAK`Wc8r$qA}iMeGMv!6`3;KcO#YAEny>D+!_vuP_|C7rZSJw0 zW~A6}MQBt!%_kLs8!@XTOlxnI+L?Ch*Iov`WfDGxn^6^OcbZjLF6!O^^6h z+DWu`zzJYhf5+S>z$w72$*u;VmR_>LdBL;j{jYmns<3CXeRJ`&6U2TK1et_9?`D9C+$@2EL(9ATAvKJahpZ`d1{pglEOG$~rf1^0S=0iy~Jsk2(Y{ zn@+n2S~0R%0zDQo1^oN3w#dJ+g&9ctTJ#InO9)2K91EBvL6{_~m?WfU(;OHcn2>36 zWG18m5a86*<#oKRxm$&`QOnuo7p?tUCvOX1M5cDK7$r0h!EZ3J7nT~oHj33iI2D=P zL$qa@;Y|;5pKjrM-$9?XpH6W2qh4@V8$A|tx>xMBg|#_;j#=+wJ!8M$#5rKK^!%=# zcD~P>)eHoTkaL{~$bB&WC9Y1^#X75LfMydzY@z`cMZUMia~%RFeQ}@ z@xhfN=S<@qEeahY{r6=L1m8@bs$)!#-^MFm80ZQMk90M=COS)d%ukAYty3Y9g}66m z+}1~rN*Znp)&L+guacUkN`6;GP<--K5OOERJvH&zh2#uKeAv?8i6xE@s7| zMy9$YBZ)|EQM-?%=Bi%BhFS!&(rpSzIfm+(8sNC3T5%#{ZY?90r79#S7K&1`ZkHqy zBS9?0P^f6&FE%99py&sQ%r-br22VC8d@`0rTus}Zs;M$1k72$lQ`%^N_gZUkBo&si zrOLdD*}bG>W@w*8ME^PJ!?>%h|A@6S!1?}ma+!UCZF%YuFR>Ih+p7D9=9&CZz(6oU zuxcax7W~xOT6M8!?D1I~8;%W#ss4l3ic_XG;W1tLit9E5FzI5Vc2kv8p7A^~J?hvL z*O5o-9n(Ow>pP*#R`_gDgGxvbIyF&$nod5Uc3N&vX>>WI@0dd#vcgVWz5r)6qAlE!1z+>v9 zqR(S*d)y$CfHe{KTJ$hDwIjbseqCA;8R5dmGKY+?D^RoJ{6;--dO)UP{pvHGbsx7Q z{5EbkNAU&WGcntR&VN9`&mrekv`OsltXFpKjznKzmoJZ6EU(Du&j;L>`f(;K3jADQ zy?g!MFlbU#QdJn0H1850=`FeOhYE@$+=M9+*ck{F@a6nVtR|a^jvFUuD{uO8X@=#L z`|1)bJUg7yYbF#*86}wTClB=@&|iwqA76qC)h1C%PK~yuLGLhR`wtqhiR`S@vGX6+ zffjf3_{T1#7>qP5({KN(uwD8XF6Ywg1y06MOR#t)LhyP3Py;Qyw&@13U=m8brDHz! z5cpdJ^~ftB%tiVVAbh?|pUKFpL9{*^hrPhJAjuQUrAA6#N#I!g0TTK4hWf|i0*X(A zwEVk()$5X7yV>_@w~L_HdF2pEIMO!T_X*YsH+#L}_(_^4Ct=qm^g3<%b76UOdl(}q zrG+)RgK&S62B+0wVbukEZ*^FW?p`*NyP64k!nwTth~)2VcH_fB>Ee8$*&C(fefk-* zgj|J3+Nos6iJ_&@^ECbRg!--1q7YGNKAD|=Y_{{X{}bgQV6FLl7qB2AIaHt{TJhUR zkazkIbnQI?s|mElyb)95t9 zj6ogz-t|y;Q>PnoHJn_!6i25JqRH`M+6vXfn!PPJgJpy?9JOz0ABk=gC3@!NQhcP7 zG!duyI9MiyJavLm3k-`oHaTKz^!`Xh35#NmK9&W+1?ElH6yJ2aie(Z8=SsMm)YU!k zU7kd-j8ZNPnfwRTbB<c}Vvk7u35ls!2-Bt}gHD>P(bt#rX$6b6t!r`T ze1%#L-ObDE21iv}uZ+s9jvx$!5sN4 zpmaZwHRRDwh4L+Xkk|$Zl1MhW)2g%?xi7pOx@1GucwQeNGfvot$#9fTG7^mo8NAi} zQNGw4NJVI$z)?@{SQRO+&A^a^z;X5`m+hCVRNfPbtkp{Fu0J6qt}sfW5<8nWRYJpY z`hRX*vm+gq_R@G{2ITDKb--IPckdhM!0B@bQTAu?ybD4Y{Pz21`Y|qd&~7}h!w1}6 z&2M-q@M;s#y4`wQlW2K9@HKjGK8y%098vtpLwJP{`PRn2ljP!f1Fv2+-krSKM9!^L--HyK={LW>U)#@No3I$cWB zdlF88VJ+FDEr198m%g{$WDpho!bUQHk(F%~7YY**63E6vLoan#`Srn|IDM3E^0kU7 z7 zazDXl;*-L*7+7e0me_Rj=cwc6Ex5y|%kt{8g5PEpv9Qrt<6N%V_B$VRqwzRzL-zzB z(#l&;#u>AT(pk(kj+12zpsVq`A4`uvz=@wvm}9G8vy4(>BZP`|hZOwBsg6p4ivSC88{2#r2by!s0_b-i-1|WjMARtIM!vHgMHxkkv z(p^JIx1>l)mxOd9NJ^Jacn`e3-|rj0=id9=KkhuwnX~sgYp=EUsx$M&C-Z2ugUECm^YhrcGNc; z{3~NSm^ArJ&necUIgwF@$8{zmVee?x=>7bwe4fzGHcfoA*q0Z}q*e{58HiWm$r9F! z{-_BpP7 z`0<1B=@_6n#T}`Gj~*+im`mU7cfgtgr>V%P=*y;Ag0RnN8K7c2J4UPM&r5tXwMcLe zf%}AogHnf zJeSkiS?49MUS+9}f)rclFd6x|o}RvFmvs~y9bJGzjL&{l3l7AQxAqGDaFwgUM)Ml|KC!mz z#=$A6`9-%&ozeR-%bC4t>^57edX3Hlioey zIGr|VIff|qRcJYzd8wQ%Z{O^vi7HRim+t*Ar)EtlN%+&#)NrzA)Mq2&UTk?{ z56ws!evBc>UZ*-p3`lI52nA1CwX+)|WHYOXD_;rHk32VRYAjI`tfa*Muz)k*=S3XV-I5!70*AYJeE>O zcyveUy?wcTWHkBqm*JA<9a0AjL$O~-x$_D(rxWtkXW(e$CAr>B3h(`{_c6h`PT-uK zfRjORCU?O4dsdY_o#GIv(;WXkWIwXikS9dWQ&%l1Z{;x~29t`;)LOWbaPO9wunEBs zw-O2^&lST3N&7K!y(Jgo{V!bCd-$2{@~^PVneSaiOdT&+x=#Hhjhz3HJfvVGU*Zw} zlyy_w%6CvcmZtA)zEPD$)=zo#*trv=|EbIFzOY2pzI!&~Jz}l0nr~I$nc~f+yl$`! zCu3d>oY7=<W=-*)=+t6L!&NX^{ix&E%6IYtk486gRhJ`ga!tpilAUE? z7n0Q(PROQT0)G3i>lZk0?P}xtFr2rARGsRb%d+tM!TmIAuAu`a;l;0WgrVKgE-1Lm zoVAj4AF@;Y!)fPKr@%E1HpU76BBYhey4hVP=@SqNpWeBc#jpKfKj;SC^!)(gy3do< z4#hZVK4iLdatpf-T<(jrnOP{dNxt{+B00R*+x&WZk#1B`zCf8%VOB@UOE-&aPbSic zN)cR%sfK;OJ6d0*wBU#i?<<&4crNMmJ#Z5G#R63T@5tnGcg+>sKDz2HOWiJ6W=p!(#VsR z_=Uf%27B7B#3a(|bDZ#~&|GMD)KkJYqqNNT2@RjoMP9DPy;FF8c|gRZ@=QT&Sb>J| zo*@~A(~Mz-`Z~M7TS)XndoAo|d0Tg$nbHvvTA)>W8_9D%ID74IIM?0MaSzLmb#t!f zGOe>FvwJ~>$$q1^`RmW^8mF_Rro0Y|Iu|Pv7Ke>P^~&(vvYpo(NR*1D%y=G?;9^v~ zt&fQ1{w16Bpr+1JgluV#I9WFAz|-G>f?fkn=tdjx@npY|M<~XSa!K(u1vG)Cl=D&2#LCK%sMM^^Gmm_?8r_P#U+3r9DiC z!*WJdse(Fd?B>>0 zU%>^t!R~!}3X?>oyxwBQ5fTe!jv~a&57*f<@gI>tK6^TJ?B@rg9fc=TO#C~}GQz34KFiVEk)69ZiopNGa)5wy4vpS#cZ22Y|}5>EPOw0Ygf6nSeNkz2bR zOu@fvT#g6hfA4*}5_f+D8&BJADbi2U!*xe#ti-Xw)$lm7kGD2c_nvhSc5MP)c)6B2 zYuPjwM)QpJ2K=hv+NIp3Kt^TRv!Zcwop)>XDryWHWj^TDM3>2Nco4H#&-Ma)xEbfn zc#?HoE&RRclWJU=M;)odD1m<82&e8JCzvZYmk(Bib3s)a!?0J1M_wm#u$+z!$JS22 zAV2CZ$2@#76pY~J?v7~_@DXIY ziTC=0kSbMrA)8MJg?E(%b-&2HOZrV&b@otH+`=5LRcZtM{*V31Nt~KqW@8#qI9s{- zWR70@m#JMlaa-)9J_GZ*0|~waHaj*L_4y{}qLh zuWbt*{S)y%ABOTy+!z5)J|Bj@L%}^l+Se6WxJqCVy%AL>qR-Jr^w@g>gA8zbvjM_W ztWs&MJdHHVwuH{-KQT;#YZcF4)W}yWx{+|_)O#jJJLDCmYJc8Is~RrqPZ=&-D_6rA zdhOPcL6n-K_E}maEB8*;kUWKFM6Q@5*+={&j`XZh6QkE2ElXIf=mmO)cUrOXsJ&BF z*WP&WVtnyJusE>zOBjDjSZL2zpjQ1ZuTYil={Wi*m`H&z(b7RdBTB(fP=$7&IEFGI z>obBD?Nz`Jo`)@L_mQB4J|EfI_m_9pzHOx;FVAvVmr0LiE&$I18;FjaY1b4OT#z^| zJI|enc7n{hUVP&X249?Y))s@0N4!q4cqUzD(qkT*b;K;OvqY-%&3Y5A3MTaI5t+aC zVawUiO&^#$jugUv&U5Y}_+Wv3VzXQ#g0^29^oEyDe}utOYe7Fh$Qzwv#?#(Wj39jZ zzIB2nyx<#U2jUA|_}p9#V|6#z9*bi0NS!_K@G%QdG7(}XKPu0E3Uj{fXWcd+Ju|W-Q~P2y>^(&vWc9KX zcNS{5ACu75S8I@(^7D$^Q`{2eXSNY`Ks*K*^jhwnO0V6B%95g-ra)g@e@7RpZC8L~ ze9T)J4kzB6sHclQh#zYM85-LI8FRc2hqQ-vFZS4F8q#tzUQO%H`k?nF)8i0{U<*in zVSe>(K2g?rE&U$%JO!TJz#>_!)62WVo%W*>#~gjjUex>0%{LcVeL;=$yI&_=W=}53 zzt-=&FaLVTNx1;`ec%%KRUxM&;T!ewi*Ew+bv+FSbL;WRx{Jcz-oqWr%#9r<%Vbra zp%!Wzj8BPpoKKs-eIHeG{amz{BK`|SbYJ#8wZ*vln)D;iT~X%-->2y2?P2>-&f#1M z%HNmo?-vzZIh}I}PHv5z$6b9t=()CXH$J}jII$xgf2FEN-*-W`!d}KpbdfeMvwan5Zy2d`xh{-0yetB9HAE@&)Y|tBz0cP=D7P*? z*{OO}zdWM2SF~vLi(rS1<)AI2=&8p#?XDnlM=#vV&fF>1#NHuNS%0C$ptE_mfr$`l z*H3lp@*;S}2?!DhT|#@Z0vjVpr(t z+cS)1b=P>bD_ep`T}+u0?n3UEk{mfXMn@r7a$ET*E+hC5XiJfSccx~*BtaCgCO3F_Ne%w|#g&`~bz$emOKHl$>=S)5dQ zcdOR^Wu1YU=msnCj(`)?l>e>@BTv@Or9&OJ%hiXayR|jZTM$h!x^&w|o*|cY2d9eb zyZP%f#Qbs#;#E(@uTD{Oz!Fs-?2LBkTVBVJ$%ws?$@=1(_b4S(><8gi#t3o0P}Q@r_GZgT$w|HbD)*4GXhDu)f=&>Z;gUg^QuiPAmw7pQj(PQPBrXgOfV@MD zyD6!#9x59*-WyrBeECcXE$marr7&Fvl1V4qyuj-vD%ID;v~-7NkA6(~1n9Lod{wOS zOc2f|k{(hVklxC@e(+YD;w_on^qm(^O6v7nN4Y}n%t*57u-d&C-=G`eeU3~TXaD{- zG$vz`RA`w*9{8r1|3dmc=lJ`FAuJXcj<}t1IA5Nus(4Tts6(kSI9~liV|aBuMZM}E z`XX{TGfG91j!ng&$|`0e*$5gS@MiDZhJsFU_eytuX|YwAk&+4fEFIp1y4gt0%rYw< zBZc|IExJ~mBt3M8j#&4mN+JT?>e$>C3D0L5Je7-^3yyAw{f)9Vv$Kv1RSTqZ9#$%s z*m9nWb772iIhW=dn^(AG+N4FO;vTPE=}xFUD7plMzqIC8sh~NQQ1`h78kDOmK(FFj zV{*Q-$r(IeAhUYBm`vSU$x!KUqxA>v?K9;Ph|m zo~%vueUjXHRejhAkm$17ndjo$!;+~@exXvfK1f*Dyfaijv>8^g+vmEW$f6B2Fq zV!{)t`q1BE`p|rrE<5DyskV;IG}pfvQFn#aW*y*XD_tIEtroeKuj8Nmy6pZ+R`>R5 zu}`D=cyGe%v$H{{9UZ4t%gD}L;9eysXTA1*Z%KuWjc^0E>)Mfa{h_2OYY*~!Mbtc6 z?B&$Wo(KysQXhRc9k=ivd=~Uhxq>E<6;C};|8bFtoI9~jpH?}abf~Shsq&?28d7o6 zeC5dZ&$F`)+1T1*Ksb+L9ogjM(!p8+f$yLO$_&lK*0GcZqi z)8yO=DEu|6lli-+{Fjfbl2hn}{46`^EW&vrij==esMPhRtIbx0 zm5^0(>w8EyjFq&p4{5GdPuq1}pQl-lRQYickKd8nMuCV z`Cjm0({88ZD>YqqF@_fG;)P|G2L~O0nI~q-3iAiPMK)vNw1&2%+6Ld{Coa+k%1>On z{3qoh0WU>)C7QU(**|!%ThV#Mc?%bti($p|^XF&lJdR#amM`|MT-cE-j^+|C+|1a~ z(bd7i*vG<_g8)ECZ0L45LdN32=xQ)Jw%9{5iKb`ebJ5<^3N%|XP0;U*y~Bx^QuVeSlm(q{VjMl9C7Tj#@wM~hD2$O%Nn&>HMR7$i6m?~NR=Pxyk^n%3%7 zlc(TW3;_;Xl#3kXhwQZ|7RD30W1z19l?VFHH0Y>!iZ6h-5%tIUgEndb%;N&>hEKb7 zLPxZ_bVz+3K~%E!$|@n_=*2^CS$$^mrEC))=cl+3Z|NS%Fpc*myTvr|k3I+femA

QC6>|Wn_4MPLX-K+O60Y>`l-+#R$cQL z@m@q3&uMAAGpkYLIMpz_LPtY<^I)DPvq(q1`K^3km{Ayg-g{;7&pdOge_HTHSxOeh zzsVX77wEo^1aB_u9#S81;3DTBy)_lV z{$YtdZN-nB*8a`i?dmH{U&Y5e2Rpxh8p0wm#J0^xL;IU;VPToFqI0_*@0lhyu}1aw ztK+OkaFhu|G2-!LQNK#mJEITB6qDhMp1gr!q8d)1u&Fk+jr6GDUpA0+@>V4bV1~Y7 zEYi`_L>r6LEyRB8us(wv@{-Zm-SlPTcv7pZ;OBBILctk=<1Cm%cHF!*$5P%zNCX1X z4^lwaQ1&;SFvszqvX?D1OpI)K9f5&8p)#eaiMUmsm(rAeNJlVhT%XDTyqjNWXp zU*mN&+%;RDIL{S&be8{~MK~sNr8?b;pp#~B(Rg#LGTKI+GQxIbF2l!6v^tt6>7Ygyt)Z<|9{DiT&1OBMEZ!%K@8yZz`Tn z78uG54iv60uITfYFGG#@8ut1dX+lZ|2h-X@TBX`rVh9ezuU3Uu?W@(Tm%i#mcBwm!HOXH4ITQ`D7ZF-bd<# z8H2}!^&AE^Hd7rbEv^f%*zkx{e3kgYwZ=JmK&QNll?BFVWvp&m&Tv_U${epuylri@ z60~xhC8GD%MbPCTS8aO9r_5esFHVa#@0bz#$*K4RCEQELq{ETk)QOx*fBJZ32IzCJ z#&~UNC$Y{evp9UKxZzqIo99pk3LK}rpBrk-KKiZ%^Z41 zxzCC3_vq+$2RUTawHljijnv>+Hl~J*RvPALAUVdTnyVO;!zvi9EVBhu8p5RXAJ^KU zZ=+cD^GvE(n5j!HZlG@?DwP@hy$)Saqk8Fy7}RI(pfL^l7^{|;C%x|1FlJ#)!FM|` zt6T7J<20KUGnyMM5BeUFzB-K^F)n??n%J9@`gnG5wmfm6wBYgVApZD$636&??of*4N(8cdwDrTo+zUjVei^?qv{*H00hSZ3%lvex&nq9p!Y)}!$4laG_oX1!`zJI(*2wo z&mT?d`5xUrOC(RuOs-5iNH3zG2HDW!IN$x$8NR|_gFYxN_RLJwObuk=iMTWP!l8Ys z*2R)Opv2{5x+ADgGA6pD+ka_!n!};`WN=cS-Nv-^3G2%VbCuGG)FAz%#^4A8cbihg zO0<2yTt&=NGT>iN5b=#~RRUq1_x0O}{RXZyFgARJ*q9XcY`@o450oYau#xG8SrZ#S zk$$qWOO}^m?kn+N$!tkTcmL5~WUb)aD_;?Yn31Tzz5T4+&Po z$M`1iuHXBl7hdmA74dNQ>>dw!IDU9i?RyE(2wyy=3C$iSN;iBhS|o{J_mZ%FfPDxU}r6@@stkI+ewG z*iHkl=3cZNyURX?-&gjz_5#l!5%Bq5t7oW#1Y$?wY7~J; zb-4XgIjDmUVkd$dzGxa4)p(ttB7wJ02d8ofBJP@CQN=x(LoHf=-hM0u#(KXSJ{KPQ z;St0=>M}mMC^A;%jO2`vUfTPwb^fE_O-tEq5^4 z(Y{~{YN33_7}OU#0L`F?Qw4~9Pi`xI$l;7vM@MN~jp^3ZB|6{(g~s~!u8HaJ`4|r9 zK0-p9kk&Jb`Z(~^v5C4j9X^L~uQz?n0`;BY06iQ64*&OD?zk{(G?87Za^i9yrL$rk-8)ykQp(&dM~pc&N(pGgK~pUu=~^3|nXKBL?`LoBTj-TU4Zwcw8i-ZZrq%fx8D+N{Y@u0IKuRk1ONB+iQ{pQX z3VcCQc?XsvqLAP|^y+DDSy6wH_%Qa@FfED~@o3yq3^wj@5cDGg-9f?rNU2JkX8-ul zHbepem28~;UuH^;lXh!m;3 zxu@8N%+}LZxd}OH8#MVj?j61vD{Pi()-j}VkL2h#?B8XVv9cQR_r$~-z(>sR>>sz6 z55sk_;)m+)M7kvfehSRFbE-9&wOMU*maGx6x>U6!?yM2ko%`fN^L`FA9W9D~IdQOv zRw89Vk;;!Y=^Sqe{<2hpM1+n_FnZ5P9}#cwE++{`s5Brr{)CxHXZ+3FZ7@hoslGV6HDoE!i9dGp|6-av% zqyA|KA2BLb7n%V0-t&7R;5+(fkD>F=IJxyj_4oUYaO{YGWxQPUKnpqcc(0~4w5HXT zA5-F_Lj5|&OX2iU)tI2G86%OIf>c$mN^Yr=rf8koqG5X0fvLcIR_HSGv;o5C!CRjw zY{^U!cIBajIE}1um66`Mg-eBVKMl@04fz8jw1jpY`Hy)Yd76_%9Pis@GAWbT5!wlV z2!6-Sqisq{WmT`~aG1=}kAG3|AS~fb=f~pX5@+VUe#28Fo&qiqYJ6uO_5L)zccEli ztlo{0Y7}i>XbL?t0P+F&IUmT6f zKdKd29&vZs6&d*x4Ze#T;h(C+M(< zl_f&Rz)sKB#2R4*Y=8oErFAU~zyka?FM57k6J2vD1pqVHT-V4B!~!$`j$24#Mi_uc z%*YI70YM=U2#5&+foX!p5xV9kdi<6~<^~`L7MR~o&%hD^Vu!F|0hq*pbQqybOjuw+ zU28D|6C-29%|PI)Ji@?23B(4BxlsUY$6^2l-9paE-Cz^{i7d?Y7YO3t(84TG=KlfE z_^+6AnvuPhd60X4CtQB9+vz9?EtZ*gUPzI=2)@p#K0&_2_CiiCT#(55;=CuVU>JR^ z%+2tWPq2Qnuquu8plTe>$(t+qXmYLZiW;W1eN}=T2g}`#w^8e?V;>QW$)21!;yNFK zJvncE);%{g;T)oD5BzRh*U-B^HhwdHBXlzh|ra3Y)Yxxjgiaz_KCAk;l}j|mdfQK2WnMPn_i%Z}>UVNQEUVJ*W7HDC z&`$&1o;Wq*aM?Gx9S$E>?G7APmCATv!=*g1J)}H>jZDtf*^JNCznGkJo|>I=0&ULD zp*acv>tiymGwh+cK<2Bb(>PbS^#y4jfjs`#csrJBR#Y+`m~iGZNyaP5LwIBZy3F;r zL%1a5N^-ta+jM84j?UVA(4JX@8OR=NM)-_CLno@ZHGx zEeDW=1weq`Ed0Oe3%X5jIV`Y(m7=A|%`Qfu+l={p4*dn5TZ9S3bYtbeGyI8c2|$bA z$Okdqi~ywm1}F!}3bp_g`3(_hv4d_vFXe!Y0}DGLMCB2{7D@n%u&6v>)?0W*<@|4^yCc?>R@1hCa{8m69O!5p=)Fy_{T~7kCTAr-x41fdsEGBbl*Uu%L4qs z8vu9?OpobyCUORLR`#}f26iB(Kj6lt4D?NOZ@|-UATHTh03WflYW`n=ri=duO$Q4C z%Li20fo{MrZ|44IA^K-%{{NoOtw;VcAEv(mj|y7Z16F|oRS}4Ppg7-J=O+F^0ERrU zjKA9T_UdoL{xJ>$Rxm-B8*qsTi--V~nHebSg22HJ94x>A#NSQ1XJ!K24Fn!S{SV`- zsHy>?0Rw}8A4mT5%WYhMr2z;3*8GUa0Xb$+Q5=u4`E_u`G+k*w`;PrH#Y~}zCi%tGWG~_6U&>>zP0J!Jox9{0zhgA z0{fK#ck$o8V*&gH2b=|zHwYXKm<`A~eS1BF|6KpmuK>?2AK+USz~?a7@8ibjH~#w-)InK7M6eVL0I8Xz_owpV}-N)Yb+}}@B!%Wd{8(H_OCg!vcrJf_?tW^D;p~i zBLAep|HTIbT=Gvoz;yrA0R@uuU-Dq=fQA2|1I`Tpr>&rHmVem=3TOS7U4Yxtzvl;M z18#8t(Sfkl1wOsm-jr&2leY$d%fWJ1RzL~3t)W<8aZ5uh5cC#080ZLr)InUV%q(nh z2$Y$X4I<0}6NK}N2*TL;Sy%)hB7$rXI2#Yv|C!|`2ZgNk1dR>!%U zbMACccXd_WvaZuYBKt*{hJh9cOVXX&-O^p(odL@Tpa)p%nZa^#(McLw89SH&m_RM^ zbi$?<4u*Dg!WOy?hF=Wztqlx$cwp@v>&JG{j1X8&TTYwdITjc_m=WCb;%l zLv*e5S?_c_DBUdrDE;;rz1M>21SFIxN~)nf67x5%F_KW^sfQ-Cc4yPQJnv5jQ+X0q zoQGE1b`{98Hnm^wFXnkuUoQ1zo=y|ITQR+Co7(6V-nXSf2;NM=>H@4^pD(5fiZNdw zFEh+&KGiioiIW9y%+7a_w+fTc7PPlK6&}@bWSFa=rIdXs@}?srIWjNq>8US`w4f-^ zKJ+6fM(1r`kgV%M)<{WW}D2Pym zUwJ!DSHEDUbXFvMcgZX5{nXY(lE(9T)ezCw_A8>V)zgLmf#J#z(c1g;Nzb}8=$D#J* zr9tMThirzb_Lk?xLwT(3ztSt4@z>bPBI%=lnKu{VWEeux4sDEx^8ajSa7H%h{VTTN z;b|J<`(S*i@Mo(};*wP)jvvi#O!%t@RT?D<0(ZV}uzL~hfHm;J%nFcvOre|d{iz5O zKjSQyn*7mW`r_L-4o>s&E6mnh7M2iKe)Nn^Em#I59Ltl#)AN0Fvv(9jc@>D6!T?9HX zABm;#T;o;cpfnbP!IpF;!g9>#D&q846yPs#w3V|Te{?;%(M@SCfDm;wNSf6BXr=pp zE63yndV#ombv^Q3W7IEEOj0qwZU1orrnM-4K-C7}p^3Rb&}(DAIeGT0N^sZA6s~QH zQu2o~M3A22Nfpga1;kpuv~AGW0gJAuik5S;a^m4YlDTL}Lr8(XZb=K81BqmMEZ8q@ z$B6oD(!)Be&N{=JClS_wOAW}yPocb||1pJAdKncb&$U?Y;$D#FELBJXD*jy^D{AIV zID!=ATIIHRp6p?s!aXzVJ+W!0bJ{qLd;0eQ87HLQYJt_iJy&Gii@q6Wb2WuG6uryf z6`o?W8J>f|lGGyelbi8k%zQSe{NO<|E|0`T(K}YCPkIPE3$;Y@NW1zt4X$0J0X$P= zc#VsOlvu|h@j()=ddwc85*{?p%bk-bNQL4lOqZ`-zwApl_Hm9`@pJohSNJA#>Gc{P zZZEPk7)_r%?Bff=G^eP^pH5AgwV$bU9oBmzXq%^m}s^z9Y37qZDZ=&LaWK(2kCH9^Hxc_!ttgOXyYj|K)E@#y z5vdEmg>X{&1k;A;Kz>0s6Z84Dr*=!mp!Yo6!_O5WkE~h2RE6`|N^1F=GJlu)S_~iz z-+{^a%8Etl0_oU-ONZFEyGB;A zs~dtHxoAR9<_~FXt9JZ1Xem!)j^Fydx{=!PfVL_|Qn%V_&>~0D;_-YCeY^dok5_dqLqY!V^22|1ZU^)uG zn%T&4q@AeaCviL>N-Z8olPZLt>yk%+G-%~#KT9b08xmlpK?5m$ zW8q?E-uUt*dZusgEp~{u%rTs;+QoL>V6qnPNywKR9j$oU3Fix`YwYk#P3@mVIgljy zQQh!145Ag?C=73@qh8b`kma=Gh_xi&^olKG9Z)%W2qeyxdrWa+8w?SeEhUc9x;mjoQnws2`C=xBiDt~ys(vn z#Ms~OS}SnO(ta!uI7aIoV^#dlut}9CLWVE3F>f6K9clSAUmVIa!!{_PzjA5Purwk zvoWhy;-q1+lVQB#opScloIAPkHmzpFi!xbMQX5+!(zsbn+8o!{9vD4&K#Stkg$$gGTAU zUDM9Sc)a-W`#m1S#@uF5e@6XEehP+Kpm3Z=rFTCFy(6`YKUU78c1UBty_pfTS=81+ z!NrjkSW(15y*dnq`s!M}k{=uAd6i_Z=+4@Ity)l1{o&A(Of+5L2bB4^Uk5t_AzLtV zZzFtcVF6dQ>7CtB)`nhFqJRyOZ%Q4q?7Qe}Wfr`2-6EC`&AP32Ohu#Xb zRV?(gXp|KD#~|)BtnP2A0jEA$dQJ=9H|m;zxEayg8RX_J(#zqkkg<@tRuNrrd+)mu zMPb0K`&ae~vE(P94Bw}bc_Oivt(7?)LdVU=3*{)l_mKE|gpg$DZqNv3@_(x_sYOL@#i873c}&|Q%to&)dT7nN4>je(LS zmaYKP;80-~l)Q9{mqnF{E2=L)uPccU*{Lg0PSz#!mH`59bR;;whtijhF;Xro*~I3NdR_wclbc4CY44=1V&-E+dMSwD zw0=X1j#s<;#?{!)=G0Sf>-BkjYI8t?ieQo!_UW*L-$)m65`ySY->c_h97oo0Q(rt= z7}MP$f8zd;3Pi;vEg0vjW+rEAZz7S2+W4TK_GRsytx*Wzq80e?%YUlY$FEVQ1I zEY9K8R|M?S&mv|Kv}Rork$)a@d1@JZDlkyM1SWi%dTkQ!h> zLO5^~@3cDRUCTM>uKtdC1M1RxTu9hx*O`AY-+T3W-fkZaLi@aD{p6rsYRyeXA`#jzN+HtR^KYgA9{UvH8I zJ8@z$tc^oh3%=^j!TT_K8geBcCoyMLvtvc67ohtyLUuvT!KijNxW9zJWjz_zN(pl; zGoym;178m}ltua4`3@(K`nlS2ROxVHq2qUB{wb$6A5WnS;tJl6@?w>#yK3tKpLp<+~)u-xz z&{83?4O-l@Yd<)(jmfCU(2=9Bjf5;6uq`;P0VX9~x_mZm$0FqaB#ukgE;U*Cq#Pjs z@D(ep>r?cAd!s{e^mPLxaqO`(3Gsk{*qKn9?tK}2o?f#iY3wpNoTfRAnm3hx{hsS* zEKm~d3PKgfpy1Bh+d~ktHUz+;)2Rwt8oXM<(lrK;nPO|qfe<6{A+z11o1 z!3F~QbJ)AtWE&L{g72r)=z|5Yp|ErXeO200E^#3c(UvshH5f~D6Nv>!xhbTQgYt-{ z4<98rG#jMYZmD|D*_KDlY0=Fzk9!7qnKL=L1$v{_zoHag^-sUPQ;QpOf;F@<`1@)E zsP?vi0sFRY@m2vcGP1sP|7m3YujLJv{};;}WonugTP%p4;}vK2K7;D92ERJi!I&N) zugZ=gA7rlvpSy_EbRHSKy$9{d)sj*clIk(n5oL(vb44#@$6OG*3#MNKY+YE>4HFgE z#s#&ihuU8~9$rc|+6(9x+Sk0f!)|(QxPN6sUKnP=uShWKfwey$Uk$4i8@;wYH=Ll3 z5e67y@f<*FmrTBVyk2Q-dQESEdU{ z7h7EzesH~jBpj~?Q&(cKHy2tsn2Ky%UpyNr6VP&hrCC0eURy?(*4IhITbQfxy`EK{{_t6G_n0>d;iv7qPOEf7PXz53lNVfD%Kq;5*SenDZ%q`ZLQw=qP*MsvM} zC&eUg(QpqU@903I-#1*x;dBZXvFXTu7%}8obzHG(>fK3?l4=rH$5`O7@*H}QmGm<( z1-S8g{n7AhoL2Co&qqjpfr23;OUIrxT7)E|e2Hb`@4;U+lern_3HRH;?1( z$5~Q!lZOu@LMpQy6x1xAz=Nf5KLnGsnz(xG!_jqQr>z-T&dRHtnVPuHq!V>JCK^58 zr7h2hsVHZk9}M=F_GU4N9O{|I)_7>Cq8dx|=zZ6Ri@}$`R^ub8nVn})!!==O2@MFatS1XyBteNB$XUvpW|I);>9hVRstUJh3%d1`OsnT8v&FgEAOhZSh3fp#7UnI zvG%#B>Gz+Yij9nU&_d5hUnllU?)Liddh3c)Cwy>q3nT=0ea*X>8emJldT1w$F>VoS)|~&JfC>rCB2M6p|;D0STf03iZC< zl&h#vE%=Ky6kJd~YrcyV%G}oz<|OP8jx(#|RoeI{C7{9Cj%3=Gv$U-r?Nc(3&y|hU z{55>ZGLRyYzQH^qtS-l9`&q}o2KRhKy>YoZ5a`x7%kuD~2@QeriLb=?%Oeey6RUPVv$c;64Sfakea34d1XE z4q6Hh)Bo7z(H625y(2c*I{nAugr8$Ga)tLVJ%;%K3ms_G^3YjOPOjUN zm{-6se?6JvFbpSaL?LY07~9yh_OeJg!d4zRb(3Gh zfzN{ZbmEFrOIj8|MA?@f5z2<%=f7kvg2X3RfBy7>sF}8NchLrOyLxBqW{mE7lOUKfJUFK(vMm-oRtY^2;{H7 zVslL<>25|AOL}zlf&*Q$A}1Flukhb7_$oi?vz5pTw?@eSB=lM=({G(7TI~*|Jb- zC}?KEh+iJ;zojCS|K74H*r7Rce%jw1IV$|Z&}>aA&!)u+)%&+ytm-{_5?wh0!3yp6 zHEI0H7k41@F2b>?(ksNJc*F_D05PPWgEnEmpIybZh{z7tM)A;mgloOKp}4ikPRsfZ zt2gsN&2geC3l{GbF~6nbFn-w3vW?hE0RL`74x`lffd;{PiCWJ04RX*ury9>q#=#4) z75iuVzfvVfLV7D=_T`1f3?aG*Sa26s{DV=)O1A_i#aWeX_*+LIlmpRo+C0Twjx8IhauQIV=+#Y^hh4|VvI(3Cd46?o z1%lomTOGWaqlt;D#cJ~z!hphK6v)lLz6Spa-L>X9^*?f!w`AlkPhn(WV) zlAYZ=50fjOV zVcEt^%Cl8G`|g=@ox3X6-t;j&;geSovLl_qa#r533Qgm-s{@Kalb%0r%-5W#_)}|d zAavHur^t*c*w9<3`ZHNC%po-OCH)4^G>08E@(llr^Bug=sEE^6eG-99JXtHVh~>FP zwAOD~Pw!)U(W7qm^O8G^02M?%RtHD>;e@guvua0k+iIv5T}}EWzRU8w<1q92u^~tc z9*HfVZxS$T)1q9!iYaB(saN+4uRM2~;iPIyUS4S2#nhLT#&a`w5GzUT*Z%89Ue#xe z2Yh$TbnYYFo1?lqN%ZO*%MS=zq&8P5oxGGX;ACR#FBPVp{n{xL z>Q@9N7>mL6nJXrr)k8)LF$lePeEVz)byt@k*lP~koVQ9BlYbWv>11ZNSmd`m^j(>< zO43l!Cd(%#q{O7fu`_Uwa`I!Jib+*}g{T=UoHuoq?q&pjm&+&L%Ks+mE+j!u1eq(U zq@0f_YF0Kd1TWf2sumi^*pdh=Di${sLs7Fp-2u7?30I{U@QTtlN>33aTp>y5@uvpl zM-dXs@q8r=60zAzMV>%X6xEe`k<7DJG$oay!OU7e=YvzC$}Yo<^lPZUflB5RS+1Cm zlh~2dR3`mBf8a9%P9hV_@p#^YrAks5MKqp{2CMj!JKaXvsLmpq za)wf|+=;?`$WZ!N-r{p{j+CDF?%?XE2t@5qitY)bx-Xvs3^>3n-4FPzPoaKA6UszW zZi|)hiu)#s>_`?70gNNH%nyI`7vW){4wtG#_N-PQONu-kUDTzD`sSR&Y7+_!DK>^4 z3gX*=QcOo{YgtskR;kvg&wl2gB(ee5H#qREY45+`j3V-5h(JYH+6{M(>Z+NT^(u4+ z6+s`G8!n|n5++?h_0D~;S+7vM_}I)HubcGiwGjI~^*cns(K#|E3*LE7uWaP}2u$xV zk@^u&;1fHRZju=hGuaO_T!dVsQQ1+OOH!D*dMB$w6{D6}8u@p~YagTLsi6+lg+B?S z8cb`i;1|uCp-9+u6_QM?gqS{qE@c@;%gY1W-3dLoxTKXzAk=~8?|K&t`wCw(3(!xmAki*wCdY?MDv?US_? zh;o5E2tS7^KNN;-+*L-P(psP>c<*uSL&gC}HMr}&D1wwi9%1_OftFNXb+)<&X9YDCn!s1Jc7N8fgWIhzdmKR#$b~)D zF*v+jdZksxPbV4g9J6wWEU+mPh%~y0A)`Ku%3RB?E4kjv@d}vFs0$kw?(BZXEaLJC z{2_OVLOF)E37%I3evbmlEZNe>OFa56BI0tckn8%l2n1t$`1Pe%*%}`fg)=W{DQF28 z$~EXy4hH-};L7tvv6dl0r};yjm$QZ*g6Q2Awv`1YR#O(kmG66grfV4RxG1<0k|Oj^ zIQ4Pg76jmT!fnj06%yHS?7F5p2;!@N;PGO~?}JU?tEGeE2i=N7&m>^lfn)Q?JkX@U zX_(0sK6M#1`g-r-1n_OdY)4F;E6%Xgis&KYv^KV=WluhCBXJC%nDh|#OTdrH!;cci z#$;@Dv$x@DYn6}cpMUK13@&eb08JTYI^gbiDap#aXdPbzmbc++kj2RN6L3F;#LW|l znaAzJtX4_?=uRHNj0x)mE${0vnMcj~h~$(A|hWcxdo7(hwX8Mwz)&@FD0veI&LkKjNU`rb)ttu``s(T|!`krZn6H z14hp?|Bv+qsX4TKNlq%iym}7rhQSNTL%ALZAzt&oR^^Eo`vv!vAXw^znVmMrK zyA`_f2|G;VoDmnb(?*>cm?s)R#nW@I!*PFw1Md~^bnIEi}GK>wL3`souMHCdl; z?|{^BM(8eT)3G$qM^K~L43+|F5BNTK`|jolw3u*HsZFnawz;%j%SQ@ z83;sVr7>SCm+IWg;*6p-I8t;<;8UJ_lLR~IF*3Xk5^Lr$ZM0}HO{2i$MQT-ZyRwNg zy0^|crsU>E9*CWn40Z}4xKJ&+m|`<%e>2!Q};e07rxnlc`+7@;PN>AOltOE#F-w*UfSC`l)-Dkd2KW* zXo~-^1>TH*e;#Qhr1M!hHhAbM-4+{zF^L*GWWS1I-!rL(*Ov~FeY7Jx3<6~5(dfBVC9Tu8g7>2{(gPOQkI)9LY2oAY=$ z!4}XWKxP-sk#!U8)!uby(tawB^~L4|0|QN+7U~I}97gJD$#P(T&`Pp;3=94cLj9+3 zl|P404H#HWCM=B3hb6RpI5+x`I69uS^GUpgvwN}ykL#Uh8J_#AX48{AdD*V zL`K6+b9{;ym=*ill&!I{d^bGT20q%X zew>b8L|5o_d0w!V`aDs(Fr^m7?t0xHl-7*0Y1AnVS+zt*e7LNSONv$^1t!t0P8HbfBZPtqUkl?8QY|Brg5nzxtiPC zd$~Ik+97r{q|$P>nfJ4IKgm#b9)58~w307>(s1=7bMnLnvLT6L>ev@wxQAgXK$U%; zK6FTp*Gr`>L&1`hS}@5qS$q#Ca!7%miBcg*GJn*3?n0x+MFjqc57?Dji1BZJJYyMf zs=VOwTCcq*w0oJ#dLD0iB2b*}&_CL+--|;^fOn9UT57ObsfVhp@-+&}#DX?A5K8O? zj+(P4XO(orhm{%BU3MXvt!^rf`*vi|)^VS%-$uPb7S1zUt0aY!3>M*2Oc0 zXzlnwSqq~wFif)Oj$80pcpVw%Q42j0)tT(xacH@Ijs9}ZkuYq*5}JchyU#%6*KIbb z1{4Lis<0*cASrs+Z6z*FGo&&Oq#O3J$zO!J z+Yo41*BUmT$6ucAU(#mM+h`(qttv{alPe_WKOR}6mT4%y2q@;i+8l-6PP6j39v)e5 zax$hNCc~OkuUXBQIdTl?&8CEj?(rRc2$+kWnq)8|2ZN^&;#T6GRg$ZG+3~zxx;ubQ z{2su)dy7xN)}xr#$j-tR*hkpYmMHf7p)9qZqNNXJ;?-8$H%lV0cCS&zuE zhH?ox*rLWWEQo@Dc4lei#4&Sb-(%+}`K)e`_vHJ1vnc-7Na?DUle{?8npOFWOtOC0!el+NIsTGa+mRJd@+aJ(B&53ft=7 z>&Ib6Bb%gMa5X1{oOW%csMDn@sF?|H9GoxE*>5sPv}e<`*W*H#8_1_shlo&QOVuHk zX1&^r;3B`JAfVLlm8SW5pdxkWWH{-!l;LXMuPNnue2M+~nC?u+6v9FvxsmpxUi+>! z_TEGqKi%K)tGCVCXbGHeTAaNWNs|op7kI%ydX5f=q!Er}vGz|W)Uv6-UUgNzyvJ1K zp-SgR{S^Ggn{_k@)E)2GqQ{6WCY83HJ!$TWZT^Z#T3O>v6}-WZ>+d@CTYV<&BFW4K z=HPWH`#EYW&ov7(MZwX!d47+ewdAZP5Vlw`>9>;Y4)9ETXUgoC-!Wef=MmwmZW$md z3;&LG@ykEOMH5@8nW$xe%gA;k?IfvhT$5&Ktx4VXrR()_AAS6j`g;U6Z$U3s;r?oD zG=Jq3&HfpGM-GI3HS;dqvY^wkZrA(57~BQlTq9@~avB}=IJhA)=!di88UOdsC$Aod zJqKu^8{NDJsd`9AKO1=4Ea9-QXq9y$1X*-^8w+Gfxg*prq`*gQLUmZ=W_7vE85yX< zFq!$~FpOr_l@qW7xQHQiC1V`Ijgk?OaCscQQ7(zcQBLwiY^OTB)0VYc*jiJR9X=c1 zUHdgtwv0SZ2yBSd=9O*Z4f>&WC`FpnXX31r_I+-j9b2}tSX=Z?C-v~g11a1(AK9=f z!UIc=Sl6wd=qz7^zH86*DSXO!6CIrD=zeEb=xD>*Jh6LiW@4P%b~A-e=jeWBB@`w* z6lUjP1vsoTR_JGsm746hAc_SqjMs1Cf+mnxv4d2gd%glbt%YH}Wx}ztOQUCM_7!TC zQCRKv`u#n_J)B`TTMHrhrnpT1hT_fwQH$iS#t+a6NY3k05Vp7L1pBCgkXEt6rK3!9 zVQeV~VayB%Xeh}5Nzo!W85`K9-<2I2Xi(+%!r+1rT0q00xx8hIM zr5eueD`Nu)bQ$hD2?sSN!DC!5OEdD`V*HS+40(IdmreTD5*#hAILQ&b3M-Q>dS3HV z_&X{%eughYQ+HnDm>o=0GoI;!rA>#^R9RIY&BHf($0F>-RxTO zh>{Jt#9IX0orq&t89GhLn?=(kosr+7SuXr9LA!5oHq}uAy6@S2chay@?mGutLJgV2 zc)9O_FMKYv+nZ`hSsHr?On%0SZoB`n^fAE zGFl7NS_e&4&WF+@w%bo(4@6yH!vqCRQglfiHKjc&UWdoqjip@3CT1T57FPRZZ(gIZA*URc6gkPD7pvVE0Tdmlz(-C zgTf^l*=|3orQ}K^KJ7p za_1U~;mYuWUvCEa;aUCGklIe!mLmR+{HM(VC|ZQn{{A@FkK;o&7wK2YOGYaO={M+8 zecnjinu!gj+pO}vW20azfFFVV?Jk@zK0ePmUZTD8&y~sDsX_%mzebKY_V99`&n=|r*VU4&;h5J8%z<+y~gA0hJWH|@ZK8pq*(7|Z1B#GP0X3_&b&w^hRMr1`Wo>W znfe-SaIGX|GFMKoMP;&AZo8uak$9XL_zwv)UbfD>DGl_cd|ALWBQvE;7_}s84F+DB z9X3W`u%(Ao#U?&7$_e=9PXZ}P_?ZU8nMqAPk;o0B)k49{w8A8VbGnlAUz!kUre&*O;BHuLZ4W)9mi;?ih7` zrc)?A+E;DjqKtQTaCCAI(zC>)sy^h%q3UUjLSMO_%;O&ZtjW}zl0&7Qt*IB!ra=;a zT}xw)a%pG(L+vt0b7ewfWOz&=$)A;IZH`8`M^z=4WD)8cR~r#J^iz=KCa?4Lbuhw-BIbQz0xjyVbRZKZv-?$i z6uBL_?0mhJ&jW+V;E4Do!%3+>#b@3p{%IxkbMag@WgYBlh>}$T2k+EZ$yw5HyA%%f znju1y{^4OAY0Y8_IIff*wuexyulPx)eTk(C@IMs@&35=wi8Cw;+v^?evn}hz>-@Ic z7ippFj}?v4m7=s@t8Hg$W_gz0r>Xt6$o?(4d3lP(9DzB2l4~TuixL@{>Ahfk@g^v@9D(UZIjb54dyx@u6?Ac1EqPZDui(mAQ=1y@5QH|yY;neaIg9?#(=6y|dYL#w&_;8p3px*Qt1 z_<32!v#d0&m9G!S@KodCYTPuI8OLyD;wNAx;ynKB0UDpraLa7xsgY0(7o$Ax>o?@S zYfd?}pUs?~_2ryP$^0~`LQhot_9bXYX~3voFh*q1`v1pX*4rl4+g=t6D>Lw)dsz(h z|Juw_bhR<06SlT;_+n_UZ)a-bU=6xo1o|tjYiUR)!2kA8$=DcLDd<|+!@k`-GPVZ* zK@|{)fPl5jR~lv@JAj6fl^(#rNDl<&op>BJm#Ell+#J=#c|XxxPoN2twimh~@obNy1_Uo0X2!q1OH^*V$GDe`@#R%ByZ`kDioyAHou?2 z+Lo<+V}3zvL9>dLIIe+I>7pBAhdD*YsK-)$P~E5d5)t;m4E<0d$@Y8RWu`q+WNy-L zzy6F*9gW)rkxMaqP#7tDRv1Y_*9BncGQbRtFZ$RmNV_L-#+RM4i8s*j0bhKQ?@ZQ& zhwG!A3Y+e1OudJ1F*2N z0hsBTK;zj#B|U%@gbT>}H~ft3e;fPv5|$Cj3}9jcjiCqOXJQ1fgGy#*1^|!|G?twK zM2{7~!~)`lg#`cvm4Dh8nE;x9^OApgF35@g_zIn(wUU+T+xJkaez zVbCu1n|S{n0QfKWdXp#sB$77}I$1kweFZ~@uXLari*$;HE)JUi=9T{%^+zGHy2gfd zvbuI}&d&OVfKJ}f-rCVl-_Rbw@J2$OPRh{0RQD~4ys^y&1bsC-vnHL0owcJ4h&ct2 zazRnY<_(3uE1jSMD0VoR>Kn?72+)ZE96*Za;h_@*F$khy5BT#Le+d$Q`H25N*m*nS zDE}9B82)nTQPA2EBojtZzycX2fZ=b4AKwfU!0<;<|0dOcoJIf%6w}6=~)CpFFWW3f?klZG1Al1GcoY|iM0-2--4w)ovNBTs07IuK(F}^t^DCh zo=zI1$-h(nE5mQByiFbdUkNk*gYe&U{Qp2$*3QuB@1Oq{!hfOrPsM?zp8qr+*ne3M zh|zzV&zn<7m>SrB1^oGLz#k^w8viczzYt>h%bAWplmvA_^Om(SNIb?M8~vLq=-wtj zetid1YpcIW67WZqR*n`HfImOiED)q|7ErkQ^S){PP5b}QIH=Aj1kyYU z2-hD{AfCT~ZVHos;b5d^U;-gwWMBpzN@1d+XC$MiC!_d_EL`jijbP~kjP$S|+xhzg zU;za-W`GgkZ*2@fW+sp^zC8e|zqf(vpt$!RZ9t|sGk$yi3C92mTmJ(N6!ZQa4)igg zSorU4Af6dP3VwV3i4Oz^iiQ8)#=r!m|F5x3K*s-u1I6tB91CQ4i-Z40g8>Kx#rgkg zV*}aMf55RZ{`XimQ2hT-ICl2`lmP=XBWND^4>)F~zw+#0rwhu0?B3i^!PLzVq#`ViaO$7XtFLgC-GHAcHVJiy%J((-#3j1_n0% zFZ4hj*#9%go6CH$))zD})Hk Date: Tue, 20 Aug 2013 09:36:26 +0200 Subject: [PATCH 417/486] Fixed NSH terminal init --- ROMFS/px4fmu_common/init.d/rcS | 9 ++++----- src/systemcmds/nshterm/nshterm.c | 2 +- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index bb78b6a657..7f04095194 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -61,11 +61,7 @@ then # # Start terminal # -if sercon -then - echo "USB connected" - nshterm /dev/ttyACM0 & -fi +sercon # # Start the ORB (first app to start) @@ -164,5 +160,8 @@ then sh /etc/init.d/31_io_phantom fi +# Try to get an USB console +nshterm /dev/ttyACM0 & + # End of autostart fi diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index 41d108ffc5..458bb2259e 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -62,7 +62,7 @@ nshterm_main(int argc, char *argv[]) } uint8_t retries = 0; int fd = -1; - while (retries < 5) { + while (retries < 50) { /* the retries are to cope with the behaviour of /dev/ttyACM0 */ /* which may not be ready immediately. */ fd = open(argv[1], O_RDWR); From 6dcad01d03251093da2ee4ee0026672b224aae72 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 20 Aug 2013 10:35:23 +0200 Subject: [PATCH 418/486] Fixed accel self test --- src/drivers/mpu6000/mpu6000.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 9d498bbd6a..bfc74c73e9 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -689,6 +689,8 @@ MPU6000::accel_self_test() return 1; if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f) return 1; + + return 0; } int From db950f74893a108302a167729a91765269981e7b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 20 Aug 2013 12:17:15 +0200 Subject: [PATCH 419/486] position_estimator_inav: "landed" detector implemented, bugfixes --- src/modules/commander/commander.cpp | 11 ++ .../multirotor_pos_control.c | 4 + .../multirotor_pos_control_params.c | 4 +- .../position_estimator_inav_main.c | 128 ++++++++++++------ .../position_estimator_inav_params.c | 11 +- .../position_estimator_inav_params.h | 6 + 6 files changed, 118 insertions(+), 46 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 50acec7e0c..04e6dd2cb2 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -820,6 +820,17 @@ int commander_thread_main(int argc, char *argv[]) /* update condition_local_position_valid and condition_local_altitude_valid */ check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(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) { + status.condition_landed = local_position.landed; + status_changed = true; + if (status.condition_landed) { + mavlink_log_info(mavlink_fd, "[cmd] LANDED"); + } else { + mavlink_log_info(mavlink_fd, "[cmd] IN AIR"); + } + } + } /* update battery status */ orb_check(battery_sub, &updated); diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 0d5a537eac..a6ebeeacb1 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -459,6 +459,10 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* publish local position setpoint as projection of global position setpoint */ orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); } + + /* reset setpoints after non-manual modes */ + reset_sp_xy = true; + reset_sp_z = true; } /* run position & altitude controllers, calculate velocity setpoint */ diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c index 0b09c9ea77..9c1ef2edb3 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -41,8 +41,8 @@ #include "multirotor_pos_control_params.h" /* controller parameters */ -PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.3f); -PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.7f); +PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.2f); +PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.8f); PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f); PARAM_DEFINE_FLOAT(MPC_Z_D, 0.0f); PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.1f); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index c0cfac880e..3466841c4c 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -52,10 +52,11 @@ #include #include #include -#include #include +#include +#include +#include #include -#include #include #include #include @@ -169,14 +170,18 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) int baro_init_cnt = 0; int baro_init_num = 200; float baro_alt0 = 0.0f; /* to determine while start up */ + float alt_avg = 0.0f; + bool landed = true; + hrt_abstime landed_time = 0; uint32_t accel_counter = 0; uint32_t baro_counter = 0; /* declare and safely initialize all structs */ - struct vehicle_status_s vehicle_status; - memset(&vehicle_status, 0, sizeof(vehicle_status)); - /* make sure that baroINITdone = false */ + struct actuator_controls_effective_s actuator; + memset(&actuator, 0, sizeof(actuator)); + struct actuator_armed_s armed; + memset(&armed, 0, sizeof(armed)); struct sensor_combined_s sensor; memset(&sensor, 0, sizeof(sensor)); struct vehicle_gps_position_s gps; @@ -192,7 +197,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* subscribe */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); - int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); + int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow)); @@ -294,9 +300,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) hrt_abstime sonar_time = 0; /* main loop */ - struct pollfd fds[6] = { + struct pollfd fds[7] = { { .fd = parameter_update_sub, .events = POLLIN }, - { .fd = vehicle_status_sub, .events = POLLIN }, + { .fd = actuator_sub, .events = POLLIN }, + { .fd = armed_sub, .events = POLLIN }, { .fd = vehicle_attitude_sub, .events = POLLIN }, { .fd = sensor_combined_sub, .events = POLLIN }, { .fd = optical_flow_sub, .events = POLLIN }, @@ -308,7 +315,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } while (!thread_should_exit) { - int ret = poll(fds, 6, 10); // wait maximal this 10 ms = 100 Hz minimum rate + int ret = poll(fds, 7, 10); // wait maximal this 10 ms = 100 Hz minimum rate hrt_abstime t = hrt_absolute_time(); if (ret < 0) { @@ -328,20 +335,24 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) parameters_update(&pos_inav_param_handles, ¶ms); } - /* vehicle status */ + /* actuator */ if (fds[1].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, - &vehicle_status); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, actuator_sub, &actuator); + } + + /* armed */ + if (fds[2].revents & POLLIN) { + orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); } /* vehicle attitude */ - if (fds[2].revents & POLLIN) { + if (fds[3].revents & POLLIN) { orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); attitude_updates++; } /* sensor combined */ - if (fds[3].revents & POLLIN) { + if (fds[4].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); if (sensor.accelerometer_counter > accel_counter) { @@ -378,7 +389,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } /* optical flow */ - if (fds[4].revents & POLLIN) { + if (fds[5].revents & POLLIN) { orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && (flow.ground_distance_m != sonar_prev || t - sonar_time < 150000)) { @@ -415,33 +426,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) flow_updates++; } - if (fds[5].revents & POLLIN) { - /* vehicle GPS position */ + /* vehicle GPS position */ + if (fds[6].revents & POLLIN) { orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - if (gps.fix_type >= 3) { + if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout) { /* initialize reference position if needed */ - if (ref_xy_inited) { - /* project GPS lat lon to plane */ - float gps_proj[2]; - map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1])); - /* calculate correction for position */ - gps_corr[0][0] = gps_proj[0] - x_est[0]; - gps_corr[1][0] = gps_proj[1] - y_est[0]; - - /* calculate correction for velocity */ - if (gps.vel_ned_valid) { - gps_corr[0][1] = gps.vel_n_m_s - x_est[1]; - gps_corr[1][1] = gps.vel_e_m_s - y_est[1]; - - } else { - gps_corr[0][1] = 0.0f; - gps_corr[1][1] = 0.0f; - } - - } else { - hrt_abstime t = hrt_absolute_time(); - + if (!ref_xy_inited) { if (ref_xy_init_start == 0) { ref_xy_init_start = t; @@ -462,12 +453,32 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } - gps_updates++; + if (ref_xy_inited) { + /* project GPS lat lon to plane */ + float gps_proj[2]; + map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1])); + /* calculate correction for position */ + gps_corr[0][0] = gps_proj[0] - x_est[0]; + gps_corr[1][0] = gps_proj[1] - y_est[0]; + + /* calculate correction for velocity */ + if (gps.vel_ned_valid) { + gps_corr[0][1] = gps.vel_n_m_s - x_est[1]; + gps_corr[1][1] = gps.vel_e_m_s - y_est[1]; + + } else { + gps_corr[0][1] = 0.0f; + gps_corr[1][1] = 0.0f; + } + } } else { /* no GPS lock */ memset(gps_corr, 0, sizeof(gps_corr)); + ref_xy_init_start = 0; } + + gps_updates++; } } @@ -516,9 +527,40 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } + /* detect land */ + alt_avg += (z_est[0] - alt_avg) * dt / params.land_t; + float alt_disp = z_est[0] - alt_avg; + alt_disp = alt_disp * alt_disp; + float land_disp2 = params.land_disp * params.land_disp; + /* get actual thrust output */ + float thrust = armed.armed ? actuator.control_effective[3] : 0.0f; + + if (landed) { + if (alt_disp > land_disp2 && thrust > params.land_thr) { + landed = false; + landed_time = 0; + } + + } else { + if (alt_disp < land_disp2 && thrust < params.land_thr) { + if (landed_time == 0) { + landed_time = t; // land detected first time + + } else { + if (t > landed_time + params.land_t * 1000000.0f) { + landed = true; + landed_time = 0; + } + } + + } else { + landed_time = 0; + } + } + if (verbose_mode) { /* print updates rate */ - if (t - updates_counter_start > updates_counter_len) { + if (t > updates_counter_start + updates_counter_len) { float updates_dt = (t - updates_counter_start) * 0.000001f; warnx( "updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s", @@ -536,7 +578,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } - if (t - pub_last > pub_interval) { + if (t > pub_last + pub_interval) { pub_last = t; /* publish local position */ local_pos.timestamp = t; @@ -549,7 +591,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.vy = y_est[1]; local_pos.z = z_est[0]; local_pos.vz = z_est[1]; - local_pos.landed = false; // TODO + local_pos.landed = landed; orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 801e207816..4f9ddd009d 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -40,7 +40,7 @@ #include "position_estimator_inav_params.h" -PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 1.0f); +PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 0.5f); PARAM_DEFINE_FLOAT(INAV_W_ALT_ACC, 50.0f); PARAM_DEFINE_FLOAT(INAV_W_ALT_SONAR, 3.0f); PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_P, 1.0f); @@ -51,6 +51,9 @@ PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.0f); PARAM_DEFINE_FLOAT(INAV_FLOW_K, 1.0f); PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.02f); PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); +PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f); +PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f); +PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.3f); int parameters_init(struct position_estimator_inav_param_handles *h) { @@ -65,6 +68,9 @@ int parameters_init(struct position_estimator_inav_param_handles *h) h->flow_k = param_find("INAV_FLOW_K"); h->sonar_filt = param_find("INAV_SONAR_FILT"); h->sonar_err = param_find("INAV_SONAR_ERR"); + h->land_t = param_find("INAV_LAND_T"); + h->land_disp = param_find("INAV_LAND_DISP"); + h->land_thr = param_find("INAV_LAND_THR"); return OK; } @@ -82,6 +88,9 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->flow_k, &(p->flow_k)); param_get(h->sonar_filt, &(p->sonar_filt)); param_get(h->sonar_err, &(p->sonar_err)); + param_get(h->land_t, &(p->land_t)); + param_get(h->land_disp, &(p->land_disp)); + param_get(h->land_thr, &(p->land_thr)); return OK; } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index ed6f61e04c..61570aea7a 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -52,6 +52,9 @@ struct position_estimator_inav_params { float flow_k; float sonar_filt; float sonar_err; + float land_t; + float land_disp; + float land_thr; }; struct position_estimator_inav_param_handles { @@ -66,6 +69,9 @@ struct position_estimator_inav_param_handles { param_t flow_k; param_t sonar_filt; param_t sonar_err; + param_t land_t; + param_t land_disp; + param_t land_thr; }; /** From d2d59aa39278de9461ff72061cbd8a89d7e81f4b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 20 Aug 2013 13:04:57 +0200 Subject: [PATCH 420/486] Handle the config command line arguments a bit more intuitive --- src/systemcmds/config/config.c | 144 +++++++++++++-------------------- 1 file changed, 58 insertions(+), 86 deletions(-) diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index 5a02fd6206..766598ddd6 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -2,6 +2,7 @@ * * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. * Author: Lorenz Meier + * Author: Julian Oes * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,6 +36,7 @@ /** * @file config.c * @author Lorenz Meier + * @author Julian Oes * * config tool. */ @@ -69,27 +71,15 @@ config_main(int argc, char *argv[]) { if (argc >= 2) { if (!strcmp(argv[1], "gyro")) { - if (argc >= 3) { - do_gyro(argc - 2, argv + 2); - } else { - errx(1, "not enough parameters."); - } + do_gyro(argc - 2, argv + 2); } if (!strcmp(argv[1], "accel")) { - if (argc >= 3) { - do_accel(argc - 2, argv + 2); - } else { - errx(1, "not enough parameters."); - } + do_accel(argc - 2, argv + 2); } if (!strcmp(argv[1], "mag")) { - if (argc >= 3) { - do_mag(argc - 2, argv + 2); - } else { - errx(1, "not enough parameters."); - } + do_mag(argc - 2, argv + 2); } } @@ -109,44 +99,36 @@ do_gyro(int argc, char *argv[]) } else { - if (argc >= 2) { + if (argc == 2 && !strcmp(argv[0], "sampling")) { - char* end; - int i = strtol(argv[1],&end,10); + /* set the gyro internal sampling rate up to at least i Hz */ + ioctl(fd, GYROIOCSSAMPLERATE, strtoul(argv[1], NULL, 0)); - if (!strcmp(argv[0], "sampling")) { + } else if (argc == 2 && !strcmp(argv[0], "rate")) { - /* set the accel internal sampling rate up to at leat i Hz */ - ioctl(fd, GYROIOCSSAMPLERATE, i); + /* set the driver to poll at i Hz */ + ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0)); - } else if (!strcmp(argv[0], "rate")) { + } else if (argc == 2 && !strcmp(argv[0], "range")) { - /* set the driver to poll at i Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, i); - } else if (!strcmp(argv[0], "range")) { + /* set the range to i dps */ + ioctl(fd, GYROIOCSRANGE, strtoul(argv[1], NULL, 0)); - /* set the range to i dps */ - ioctl(fd, GYROIOCSRANGE, i); - } + } else if(argc == 1 && !strcmp(argv[0], "check")) { + int ret = ioctl(fd, GYROIOCSELFTEST, 0); - } else if (argc > 0) { - - if(!strcmp(argv[0], "check")) { - int ret = ioctl(fd, GYROIOCSELFTEST, 0); - - if (ret) { - warnx("gyro self test FAILED! Check calibration:"); - struct gyro_scale scale; - ret = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&scale); - warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset); - warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale); - } else { - warnx("gyro calibration and self test OK"); - } + if (ret) { + warnx("gyro self test FAILED! Check calibration:"); + struct gyro_scale scale; + ret = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&scale); + warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset); + warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale); + } else { + warnx("gyro calibration and self test OK"); } } else { - warnx("no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2000' to set measurement range to 2000 dps\n\t"); + errx(1, "wrong or no arguments given. Try: \n\n\t'check' for the self test\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2000' to set measurement range to 2000 dps\n\t"); } int srate = ioctl(fd, GYROIOCGSAMPLERATE, 0); @@ -174,29 +156,26 @@ do_mag(int argc, char *argv[]) } else { - if (argc > 0) { + if(argc == 1 && !strcmp(argv[0], "check")) { + int ret = ioctl(fd, MAGIOCSELFTEST, 0); - if (!strcmp(argv[0], "check")) { - int ret = ioctl(fd, MAGIOCSELFTEST, 0); - - if (ret) { - warnx("mag self test FAILED! Check calibration."); - struct mag_scale scale; - ret = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&scale); - warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset); - warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale); - } else { - warnx("mag calibration and self test OK"); - } + if (ret) { + warnx("mag self test FAILED! Check calibration:"); + struct mag_scale scale; + ret = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&scale); + warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset); + warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale); + } else { + warnx("mag calibration and self test OK"); } } else { - warnx("no arguments given. Try: \n\n\t'check' or 'info'\n\t"); + errx(1, "wrong or no arguments given. Try: \n\n\t'check' for the self test\n\t"); } - int srate = -1;//ioctl(fd, MAGIOCGSAMPLERATE, 0); + int srate = -1; //ioctl(fd, MAGIOCGSAMPLERATE, 0); int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0); - int range = -1;//ioctl(fd, MAGIOCGRANGE, 0); + int range = -1; //ioctl(fd, MAGIOCGRANGE, 0); warnx("mag: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d gauss", srate, prate, range); @@ -219,43 +198,36 @@ do_accel(int argc, char *argv[]) } else { - if (argc >= 2) { + if (argc == 2 && !strcmp(argv[0], "sampling")) { - char* end; - int i = strtol(argv[1],&end,10); + /* set the accel internal sampling rate up to at least i Hz */ + ioctl(fd, ACCELIOCSSAMPLERATE, strtoul(argv[1], NULL, 0)); - if (!strcmp(argv[0], "sampling")) { + } else if (argc == 2 && !strcmp(argv[0], "rate")) { - /* set the accel internal sampling rate up to at leat i Hz */ - ioctl(fd, ACCELIOCSSAMPLERATE, i); + /* set the driver to poll at i Hz */ + ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0)); - } else if (!strcmp(argv[0], "rate")) { + } else if (argc == 2 && !strcmp(argv[0], "range")) { - /* set the driver to poll at i Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, i); - } else if (!strcmp(argv[0], "range")) { + /* set the range to i m/s^2 */ + ioctl(fd, ACCELIOCSRANGE, strtoul(argv[1], NULL, 0)); - /* set the range to i dps */ - ioctl(fd, ACCELIOCSRANGE, i); - } - } else if (argc > 0) { + } else if(argc == 1 && !strcmp(argv[0], "check")) { + int ret = ioctl(fd, ACCELIOCSELFTEST, 0); - if (!strcmp(argv[0], "check")) { - int ret = ioctl(fd, ACCELIOCSELFTEST, 0); - - if (ret) { - warnx("accel self test FAILED! Check calibration."); - struct accel_scale scale; - ret = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&scale); - warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset); - warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale); - } else { - warnx("accel calibration and self test OK"); - } + if (ret) { + warnx("accel self test FAILED! Check calibration:"); + struct accel_scale scale; + ret = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&scale); + warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset); + warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale); + } else { + warnx("accel calibration and self test OK"); } } else { - warnx("no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2' to set measurement range to 2 G\n\t"); + errx(1,"no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2' to set measurement range to 4 G\n\t"); } int srate = ioctl(fd, ACCELIOCGSAMPLERATE, 0); From 307c9e52c775de2ce09ff4abf0bc1fb5db6dd41e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 20 Aug 2013 19:50:58 +0200 Subject: [PATCH 421/486] Sorry, finally got the axes of the external mag right --- src/drivers/hmc5883/hmc5883.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 692f890bdd..44304fc225 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -861,10 +861,10 @@ HMC5883::collect() } else { #endif /* the standard external mag by 3DR has x pointing to the right, y pointing backwards, and z down, - * therefore switch and invert x and y */ + * therefore switch x and y and invert y */ _reports[_next_report].x = ((((report.y == -32768) ? 32767 : -report.y) * _range_scale) - _scale.x_offset) * _scale.x_scale; /* flip axes and negate value for y */ - _reports[_next_report].y = ((((report.x == -32768) ? 32767 : -report.x) * _range_scale) - _scale.y_offset) * _scale.y_scale; + _reports[_next_report].y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; /* z remains z */ _reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; #ifdef PX4_I2C_BUS_ONBOARD From f5c92314f16fde650ee6f2f4fa20b7c2680a4b00 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 20 Aug 2013 20:02:06 +0200 Subject: [PATCH 422/486] Improved LSM303D driver, plus some fixes to the HMC5883 --- src/drivers/drv_mag.h | 25 +- src/drivers/hmc5883/hmc5883.cpp | 5 + src/drivers/lsm303d/lsm303d.cpp | 480 ++++++++++++++++++-------------- src/modules/sensors/sensors.cpp | 25 +- src/systemcmds/config/config.c | 71 ++++- 5 files changed, 378 insertions(+), 228 deletions(-) diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h index e95034e8e3..3de5625fd7 100644 --- a/src/drivers/drv_mag.h +++ b/src/drivers/drv_mag.h @@ -90,28 +90,37 @@ ORB_DECLARE(sensor_mag); /** set the mag internal sample rate to at least (arg) Hz */ #define MAGIOCSSAMPLERATE _MAGIOC(0) +/** return the mag internal sample rate in Hz */ +#define MAGIOCGSAMPLERATE _MAGIOC(1) + /** set the mag internal lowpass filter to no lower than (arg) Hz */ -#define MAGIOCSLOWPASS _MAGIOC(1) +#define MAGIOCSLOWPASS _MAGIOC(2) + +/** return the mag internal lowpass filter in Hz */ +#define MAGIOCGLOWPASS _MAGIOC(3) /** set the mag scaling constants to the structure pointed to by (arg) */ -#define MAGIOCSSCALE _MAGIOC(2) +#define MAGIOCSSCALE _MAGIOC(4) /** copy the mag scaling constants to the structure pointed to by (arg) */ -#define MAGIOCGSCALE _MAGIOC(3) +#define MAGIOCGSCALE _MAGIOC(5) /** set the measurement range to handle (at least) arg Gauss */ -#define MAGIOCSRANGE _MAGIOC(4) +#define MAGIOCSRANGE _MAGIOC(6) + +/** return the current mag measurement range in Gauss */ +#define MAGIOCGRANGE _MAGIOC(7) /** perform self-calibration, update scale factors to canonical units */ -#define MAGIOCCALIBRATE _MAGIOC(5) +#define MAGIOCCALIBRATE _MAGIOC(8) /** excite strap */ -#define MAGIOCEXSTRAP _MAGIOC(6) +#define MAGIOCEXSTRAP _MAGIOC(9) /** perform self test and report status */ -#define MAGIOCSELFTEST _MAGIOC(7) +#define MAGIOCSELFTEST _MAGIOC(10) /** determine if mag is external or onboard */ -#define MAGIOCGEXTERNAL _MAGIOC(8) +#define MAGIOCGEXTERNAL _MAGIOC(11) #endif /* _DRV_MAG_H */ diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 44304fc225..1afc16a9a6 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -637,13 +637,18 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; case MAGIOCSSAMPLERATE: + case MAGIOCGSAMPLERATE: /* not supported, always 1 sample per poll */ return -EINVAL; case MAGIOCSRANGE: return set_range(arg); + case MAGIOCGRANGE: + return _range_ga; + case MAGIOCSLOWPASS: + case MAGIOCGLOWPASS: /* not supported, no internal filtering */ return -EINVAL; diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 3e6ce64b81..8d6dc0672e 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -180,56 +180,62 @@ public: LSM303D(int bus, const char* path, spi_dev_e device); virtual ~LSM303D(); - virtual int init(); + virtual int init(); virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); /** * Diagnostics - print some basic information about the driver. */ - void print_info(); + void print_info(); protected: - virtual int probe(); + virtual int probe(); friend class LSM303D_mag; virtual ssize_t mag_read(struct file *filp, char *buffer, size_t buflen); - virtual int mag_ioctl(struct file *filp, int cmd, unsigned long arg); + virtual int mag_ioctl(struct file *filp, int cmd, unsigned long arg); private: - LSM303D_mag *_mag; + LSM303D_mag *_mag; struct hrt_call _accel_call; struct hrt_call _mag_call; - unsigned _call_accel_interval; - unsigned _call_mag_interval; + unsigned _call_accel_interval; + unsigned _call_mag_interval; - unsigned _num_accel_reports; + unsigned _num_accel_reports; volatile unsigned _next_accel_report; volatile unsigned _oldest_accel_report; struct accel_report *_accel_reports; - struct accel_scale _accel_scale; - float _accel_range_scale; - float _accel_range_m_s2; - orb_advert_t _accel_topic; - - unsigned _current_samplerate; - - unsigned _num_mag_reports; + unsigned _num_mag_reports; volatile unsigned _next_mag_report; volatile unsigned _oldest_mag_report; struct mag_report *_mag_reports; + struct accel_scale _accel_scale; + unsigned _accel_range_g; + float _accel_range_m_s2; + float _accel_range_scale; + unsigned _accel_samplerate; + unsigned _accel_filter_bandwith; + struct mag_scale _mag_scale; - float _mag_range_scale; - float _mag_range_ga; + unsigned _mag_range_ga; + float _mag_range_scale; + unsigned _mag_samplerate; + + orb_advert_t _accel_topic; orb_advert_t _mag_topic; + unsigned _accel_read; + unsigned _mag_read; + perf_counter_t _accel_sample_perf; perf_counter_t _mag_sample_perf; @@ -240,12 +246,19 @@ private: /** * Start automatic measurement. */ - void start(); + void start(); /** * Stop automatic measurement. */ - void stop(); + void stop(); + + /** + * Reset chip. + * + * Resets the chip and measurements ranges, but not scale and offset. + */ + void reset(); /** * Static trampoline from the hrt_call context; because we don't have a @@ -256,24 +269,38 @@ private: * * @param arg Instance pointer for the driver that is polling. */ - static void measure_trampoline(void *arg); + static void measure_trampoline(void *arg); /** * Static trampoline for the mag because it runs at a lower rate * * @param arg Instance pointer for the driver that is polling. */ - static void mag_measure_trampoline(void *arg); + static void mag_measure_trampoline(void *arg); /** * Fetch accel measurements from the sensor and update the report ring. */ - void measure(); + void measure(); /** * Fetch mag measurements from the sensor and update the report ring. */ - void mag_measure(); + void mag_measure(); + + /** + * Accel self test + * + * @return 0 on success, 1 on failure + */ + int accel_self_test(); + + /** + * Mag self test + * + * @return 0 on success, 1 on failure + */ + int mag_self_test(); /** * Read a register from the LSM303D @@ -281,7 +308,7 @@ private: * @param The register to read. * @return The value that was read. */ - uint8_t read_reg(unsigned reg); + uint8_t read_reg(unsigned reg); /** * Write a register in the LSM303D @@ -289,7 +316,7 @@ private: * @param reg The register to write. * @param value The new value to write. */ - void write_reg(unsigned reg, uint8_t value); + void write_reg(unsigned reg, uint8_t value); /** * Modify a register in the LSM303D @@ -300,7 +327,7 @@ private: * @param clearbits Bits in the register to clear. * @param setbits Bits in the register to set. */ - void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); + void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); /** * Set the LSM303D accel measurement range. @@ -309,7 +336,7 @@ private: * Zero selects the maximum supported range. * @return OK if the value can be supported, -ERANGE otherwise. */ - int set_range(unsigned max_g); + int accel_set_range(unsigned max_g); /** * Set the LSM303D mag measurement range. @@ -318,7 +345,7 @@ private: * Zero selects the maximum supported range. * @return OK if the value can be supported, -ERANGE otherwise. */ - int mag_set_range(unsigned max_g); + int mag_set_range(unsigned max_g); /** * Set the LSM303D accel anti-alias filter. @@ -327,15 +354,7 @@ private: * Zero selects the highest bandwidth * @return OK if the value can be supported, -ERANGE otherwise. */ - int set_antialias_filter_bandwidth(unsigned bandwith); - - /** - * Get the LSM303D accel anti-alias filter. - * - * @param bandwidth The anti-alias filter bandwidth in Hz - * @return OK if the value was read and supported, ERROR otherwise. - */ - int get_antialias_filter_bandwidth(unsigned &bandwidth); + int accel_set_antialias_filter_bandwidth(unsigned bandwith); /** * Set the LSM303D internal accel sampling frequency. @@ -345,7 +364,7 @@ private: * Zero selects the maximum rate supported. * @return OK if the value can be supported. */ - int set_samplerate(unsigned frequency); + int accel_set_samplerate(unsigned frequency); /** * Set the LSM303D internal mag sampling frequency. @@ -355,7 +374,7 @@ private: * Zero selects the maximum rate supported. * @return OK if the value can be supported. */ - int mag_set_samplerate(unsigned frequency); + int mag_set_samplerate(unsigned frequency); }; /** @@ -396,16 +415,22 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _next_accel_report(0), _oldest_accel_report(0), _accel_reports(nullptr), - _accel_range_scale(0.0f), - _accel_range_m_s2(0.0f), - _accel_topic(-1), - _current_samplerate(0), _num_mag_reports(0), _next_mag_report(0), _oldest_mag_report(0), _mag_reports(nullptr), + _accel_range_g(8), + _accel_range_m_s2(0.0f), + _accel_range_scale(0.0f), + _accel_samplerate(800), + _accel_filter_bandwith(50), + _mag_range_ga(2), _mag_range_scale(0.0f), - _mag_range_ga(0.0f), + _mag_samplerate(100), + _accel_topic(-1), + _mag_topic(-1), + _accel_read(0), + _mag_read(0), _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")), _accel_filter_x(800, 30), @@ -416,18 +441,18 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _debug_enabled = true; // default scale factors - _accel_scale.x_offset = 0; + _accel_scale.x_offset = 0.0f; _accel_scale.x_scale = 1.0f; - _accel_scale.y_offset = 0; + _accel_scale.y_offset = 0.0f; _accel_scale.y_scale = 1.0f; - _accel_scale.z_offset = 0; + _accel_scale.z_offset = 0.0f; _accel_scale.z_scale = 1.0f; - _mag_scale.x_offset = 0; + _mag_scale.x_offset = 0.0f; _mag_scale.x_scale = 1.0f; - _mag_scale.y_offset = 0; + _mag_scale.y_offset = 0.0f; _mag_scale.y_scale = 1.0f; - _mag_scale.z_offset = 0; + _mag_scale.z_offset = 0.0f; _mag_scale.z_scale = 1.0f; } @@ -478,27 +503,12 @@ LSM303D::init() if (_mag_reports == nullptr) goto out; + reset(); + /* advertise mag topic */ memset(&_mag_reports[0], 0, sizeof(_mag_reports[0])); _mag_topic = orb_advertise(ORB_ID(sensor_mag), &_mag_reports[0]); - /* enable accel, XXX do this with an ioctl? */ - write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE); - - /* enable mag, XXX do this with an ioctl? */ - write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); - write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); - - /* XXX should we enable FIFO? */ - - set_range(8); /* XXX 16G mode seems wrong (shows 6 instead of 9.8m/s^2, therefore use 8G for now */ - set_antialias_filter_bandwidth(50); /* available bandwidths: 50, 194, 362 or 773 Hz */ - set_samplerate(400); /* max sample rate */ - - mag_set_range(4); /* XXX: take highest sensor range of 12GA? */ - mag_set_samplerate(100); - - /* XXX test this when another mag is used */ /* do CDev init for the mag device node, keep it optional */ mag_ret = _mag->init(); @@ -511,6 +521,24 @@ out: return ret; } +void +LSM303D::reset() +{ + /* enable accel*/ + write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE); + + /* enable mag */ + write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); + write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); + + accel_set_range(_accel_range_g); + accel_set_samplerate(_accel_samplerate); + accel_set_antialias_filter_bandwidth(_accel_filter_bandwith); + + mag_set_range(_mag_range_ga); + mag_set_samplerate(_mag_samplerate); +} + int LSM303D::probe() { @@ -612,64 +640,60 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) switch (cmd) { case SENSORIOCSPOLLRATE: { - switch (arg) { + switch (arg) { - /* switching to manual polling */ + /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); _call_accel_interval = 0; return OK; - /* external signalling not supported */ + /* external signalling not supported */ case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ + /* zero would be bad */ case 0: return -EINVAL; - /* set default/max polling rate */ + /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: return ioctl(filp, SENSORIOCSPOLLRATE, 1600); case SENSOR_POLLRATE_DEFAULT: - /* With internal low pass filters enabled, 250 Hz is sufficient */ - /* XXX for vibration tests with 800 Hz */ + /* Use 800Hz as it is filtered in the driver as well*/ return ioctl(filp, SENSORIOCSPOLLRATE, 800); /* adjust to a legal polling interval in Hz */ default: { - /* do we need to start internal polling? */ - bool want_start = (_call_accel_interval == 0); + /* do we need to start internal polling? */ + bool want_start = (_call_accel_interval == 0); - /* convert hz to hrt interval via microseconds */ - unsigned ticks = 1000000 / arg; + /* convert hz to hrt interval via microseconds */ + unsigned ticks = 1000000 / arg; - /* check against maximum sane rate */ - if (ticks < 1000) - return -EINVAL; + /* check against maximum sane rate */ + if (ticks < 500) + return -EINVAL; - /* adjust sample rate of sensor */ - set_samplerate(arg); + /* adjust filters */ + float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq(); - // adjust filters - float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq(); - float sample_rate = 1.0e6f/ticks; - _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - _accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + _accel_filter_x.set_cutoff_frequency((float)arg, cutoff_freq_hz); + _accel_filter_y.set_cutoff_frequency((float)arg, cutoff_freq_hz); + _accel_filter_z.set_cutoff_frequency((float)arg, cutoff_freq_hz); - /* update interval for next measurement */ - /* XXX this is a bit shady, but no other way to adjust... */ - _accel_call.period = _call_accel_interval = ticks; + /* update interval for next measurement */ + /* XXX this is a bit shady, but no other way to adjust... */ + _accel_call.period = _call_accel_interval = ticks; - /* if we need to start the poll state machine, do it */ - if (want_start) - start(); + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); - return OK; - } + return OK; } } + } case SENSORIOCGPOLLRATE: if (_call_accel_interval == 0) @@ -678,66 +702,78 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) return 1000000 / _call_accel_interval; case SENSORIOCSQUEUEDEPTH: { - /* account for sentinel in the ring */ - arg++; + /* account for sentinel in the ring */ + arg++; - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 2) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 2) || (arg > 100)) + return -EINVAL; - /* allocate new buffer */ - struct accel_report *buf = new struct accel_report[arg]; + /* allocate new buffer */ + struct accel_report *buf = new struct accel_report[arg]; - if (nullptr == buf) - return -ENOMEM; + if (nullptr == buf) + return -ENOMEM; - /* reset the measurement state machine with the new buffer, free the old */ - stop(); - delete[] _accel_reports; - _num_accel_reports = arg; - _accel_reports = buf; - start(); + /* reset the measurement state machine with the new buffer, free the old */ + stop(); + delete[] _accel_reports; + _num_accel_reports = arg; + _accel_reports = buf; + start(); - return OK; - } + return OK; + } case SENSORIOCGQUEUEDEPTH: return _num_accel_reports - 1; case SENSORIOCRESET: - /* XXX implement */ - return -EINVAL; + reset(); + return OK; + + case ACCELIOCSSAMPLERATE: + return accel_set_samplerate(arg); + + case ACCELIOCGSAMPLERATE: + return _accel_samplerate; case ACCELIOCSLOWPASS: { - float cutoff_freq_hz = arg; - float sample_rate = 1.0e6f / _call_accel_interval; - _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - _accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - return OK; - } + _accel_filter_x.set_cutoff_frequency((float)_accel_samplerate, (float)arg); + _accel_filter_y.set_cutoff_frequency((float)_accel_samplerate, (float)arg); + _accel_filter_z.set_cutoff_frequency((float)_accel_samplerate, (float)arg); + return OK; + } case ACCELIOCGLOWPASS: - return _accel_filter_x.get_cutoff_freq(); + return _accel_filter_x.get_cutoff_freq(); - case ACCELIOCSSCALE: - { - /* copy scale, but only if off by a few percent */ - struct accel_scale *s = (struct accel_scale *) arg; - float sum = s->x_scale + s->y_scale + s->z_scale; - if (sum > 2.0f && sum < 4.0f) { - memcpy(&_accel_scale, s, sizeof(_accel_scale)); - return OK; - } else { - return -EINVAL; - } + case ACCELIOCSSCALE: { + /* copy scale, but only if off by a few percent */ + struct accel_scale *s = (struct accel_scale *) arg; + float sum = s->x_scale + s->y_scale + s->z_scale; + if (sum > 2.0f && sum < 4.0f) { + memcpy(&_accel_scale, s, sizeof(_accel_scale)); + return OK; + } else { + return -EINVAL; } + } + + case ACCELIOCSRANGE: + return accel_set_range(arg); + + case ACCELIOCGRANGE: + return _accel_range_g; case ACCELIOCGSCALE: /* copy scale out */ memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale)); return OK; + case ACCELIOCSELFTEST: + return accel_self_test(); + default: /* give it to the superclass */ return SPI::ioctl(filp, cmd, arg); @@ -768,7 +804,7 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: - /* 50 Hz is max for mag */ + /* 100 Hz is max for mag */ return mag_ioctl(filp, SENSORIOCSPOLLRATE, 100); /* adjust to a legal polling interval in Hz */ @@ -783,9 +819,6 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) if (ticks < 1000) return -EINVAL; - /* adjust sample rate of sensor */ - mag_set_samplerate(arg); - /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ _mag_call.period = _call_mag_interval = ticks; @@ -833,17 +866,18 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) return _num_mag_reports - 1; case SENSORIOCRESET: - return ioctl(filp, cmd, arg); + reset(); + return OK; case MAGIOCSSAMPLERATE: -// case MAGIOCGSAMPLERATE: - /* XXX not implemented */ - return -EINVAL; + return mag_set_samplerate(arg); + + case MAGIOCGSAMPLERATE: + return _mag_samplerate; case MAGIOCSLOWPASS: -// case MAGIOCGLOWPASS: - /* XXX not implemented */ -// _set_dlpf_filter((uint16_t)arg); + case MAGIOCGLOWPASS: + /* not supported, no internal filtering */ return -EINVAL; case MAGIOCSSCALE: @@ -857,17 +891,13 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) return OK; case MAGIOCSRANGE: -// case MAGIOCGRANGE: - /* XXX not implemented */ - // XXX change these two values on set: - // _mag_range_scale = xx - // _mag_range_ga = xx - return -EINVAL; + return mag_set_range(arg); + + case MAGIOCGRANGE: + return _mag_range_ga; case MAGIOCSELFTEST: - /* XXX not implemented */ -// return self_test(); - return -EINVAL; + return mag_self_test(); case MAGIOCGEXTERNAL: /* no external mag board yet */ @@ -879,6 +909,53 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) } } +int +LSM303D::accel_self_test() +{ + if (_accel_read == 0) + return 1; + + /* inspect accel offsets */ + if (fabsf(_accel_scale.x_offset) < 0.000001f) + return 1; + if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f) + return 1; + + if (fabsf(_accel_scale.y_offset) < 0.000001f) + return 1; + if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f) + return 1; + + if (fabsf(_accel_scale.z_offset) < 0.000001f) + return 1; + if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f) + return 1; + + return 0; +} + +int +LSM303D::mag_self_test() +{ + if (_mag_read == 0) + return 1; + + /** + * inspect mag offsets + * don't check mag scale because it seems this is calibrated on chip + */ + if (fabsf(_mag_scale.x_offset) < 0.000001f) + return 1; + + if (fabsf(_mag_scale.y_offset) < 0.000001f) + return 1; + + if (fabsf(_mag_scale.z_offset) < 0.000001f) + return 1; + + return 0; +} + uint8_t LSM303D::read_reg(unsigned reg) { @@ -914,38 +991,37 @@ LSM303D::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) } int -LSM303D::set_range(unsigned max_g) +LSM303D::accel_set_range(unsigned max_g) { uint8_t setbits = 0; uint8_t clearbits = REG2_FULL_SCALE_BITS_A; - float new_range_g = 0.0f; float new_scale_g_digit = 0.0f; if (max_g == 0) max_g = 16; if (max_g <= 2) { - new_range_g = 2.0f; + _accel_range_g = 2; setbits |= REG2_FULL_SCALE_2G_A; new_scale_g_digit = 0.061e-3f; } else if (max_g <= 4) { - new_range_g = 4.0f; + _accel_range_g = 4; setbits |= REG2_FULL_SCALE_4G_A; new_scale_g_digit = 0.122e-3f; } else if (max_g <= 6) { - new_range_g = 6.0f; + _accel_range_g = 6; setbits |= REG2_FULL_SCALE_6G_A; new_scale_g_digit = 0.183e-3f; } else if (max_g <= 8) { - new_range_g = 8.0f; + _accel_range_g = 8; setbits |= REG2_FULL_SCALE_8G_A; new_scale_g_digit = 0.244e-3f; } else if (max_g <= 16) { - new_range_g = 16.0f; + _accel_range_g = 16; setbits |= REG2_FULL_SCALE_16G_A; new_scale_g_digit = 0.732e-3f; @@ -953,7 +1029,7 @@ LSM303D::set_range(unsigned max_g) return -EINVAL; } - _accel_range_m_s2 = new_range_g * 9.80665f; + _accel_range_m_s2 = _accel_range_g * 9.80665f; _accel_range_scale = new_scale_g_digit * 9.80665f; modify_reg(ADDR_CTRL_REG2, clearbits, setbits); @@ -966,29 +1042,28 @@ LSM303D::mag_set_range(unsigned max_ga) { uint8_t setbits = 0; uint8_t clearbits = REG6_FULL_SCALE_BITS_M; - float new_range = 0.0f; float new_scale_ga_digit = 0.0f; if (max_ga == 0) max_ga = 12; if (max_ga <= 2) { - new_range = 2.0f; + _mag_range_ga = 2; setbits |= REG6_FULL_SCALE_2GA_M; new_scale_ga_digit = 0.080e-3f; } else if (max_ga <= 4) { - new_range = 4.0f; + _mag_range_ga = 4; setbits |= REG6_FULL_SCALE_4GA_M; new_scale_ga_digit = 0.160e-3f; } else if (max_ga <= 8) { - new_range = 8.0f; + _mag_range_ga = 8; setbits |= REG6_FULL_SCALE_8GA_M; new_scale_ga_digit = 0.320e-3f; } else if (max_ga <= 12) { - new_range = 12.0f; + _mag_range_ga = 12; setbits |= REG6_FULL_SCALE_12GA_M; new_scale_ga_digit = 0.479e-3f; @@ -996,7 +1071,6 @@ LSM303D::mag_set_range(unsigned max_ga) return -EINVAL; } - _mag_range_ga = new_range; _mag_range_scale = new_scale_ga_digit; modify_reg(ADDR_CTRL_REG6, clearbits, setbits); @@ -1005,7 +1079,7 @@ LSM303D::mag_set_range(unsigned max_ga) } int -LSM303D::set_antialias_filter_bandwidth(unsigned bandwidth) +LSM303D::accel_set_antialias_filter_bandwidth(unsigned bandwidth) { uint8_t setbits = 0; uint8_t clearbits = REG2_ANTIALIAS_FILTER_BW_BITS_A; @@ -1015,15 +1089,19 @@ LSM303D::set_antialias_filter_bandwidth(unsigned bandwidth) if (bandwidth <= 50) { setbits |= REG2_AA_FILTER_BW_50HZ_A; + _accel_filter_bandwith = 50; } else if (bandwidth <= 194) { setbits |= REG2_AA_FILTER_BW_194HZ_A; + _accel_filter_bandwith = 194; } else if (bandwidth <= 362) { setbits |= REG2_AA_FILTER_BW_362HZ_A; + _accel_filter_bandwith = 362; } else if (bandwidth <= 773) { setbits |= REG2_AA_FILTER_BW_773HZ_A; + _accel_filter_bandwith = 773; } else { return -EINVAL; @@ -1035,26 +1113,7 @@ LSM303D::set_antialias_filter_bandwidth(unsigned bandwidth) } int -LSM303D::get_antialias_filter_bandwidth(unsigned &bandwidth) -{ - uint8_t readbits = read_reg(ADDR_CTRL_REG2); - - if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_50HZ_A) - bandwidth = 50; - else if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_194HZ_A) - bandwidth = 194; - else if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_362HZ_A) - bandwidth = 362; - else if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_773HZ_A) - bandwidth = 773; - else - return ERROR; - - return OK; -} - -int -LSM303D::set_samplerate(unsigned frequency) +LSM303D::accel_set_samplerate(unsigned frequency) { uint8_t setbits = 0; uint8_t clearbits = REG1_RATE_BITS_A; @@ -1064,23 +1123,23 @@ LSM303D::set_samplerate(unsigned frequency) if (frequency <= 100) { setbits |= REG1_RATE_100HZ_A; - _current_samplerate = 100; + _accel_samplerate = 100; } else if (frequency <= 200) { setbits |= REG1_RATE_200HZ_A; - _current_samplerate = 200; + _accel_samplerate = 200; } else if (frequency <= 400) { setbits |= REG1_RATE_400HZ_A; - _current_samplerate = 400; + _accel_samplerate = 400; } else if (frequency <= 800) { setbits |= REG1_RATE_800HZ_A; - _current_samplerate = 800; + _accel_samplerate = 800; } else if (frequency <= 1600) { setbits |= REG1_RATE_1600HZ_A; - _current_samplerate = 1600; + _accel_samplerate = 1600; } else { return -EINVAL; @@ -1102,13 +1161,15 @@ LSM303D::mag_set_samplerate(unsigned frequency) if (frequency <= 25) { setbits |= REG5_RATE_25HZ_M; + _mag_samplerate = 25; } else if (frequency <= 50) { setbits |= REG5_RATE_50HZ_M; + _mag_samplerate = 50; } else if (frequency <= 100) { setbits |= REG5_RATE_100HZ_M; - + _mag_samplerate = 100; } else { return -EINVAL; @@ -1229,6 +1290,8 @@ LSM303D::measure() /* publish for subscribers */ orb_publish(ORB_ID(sensor_accel), _accel_topic, accel_report); + _accel_read++; + /* stop the perf counter */ perf_end(_accel_sample_perf); } @@ -1281,7 +1344,7 @@ LSM303D::mag_measure() mag_report->y = ((mag_report->y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; mag_report->z = ((mag_report->z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; mag_report->scaling = _mag_range_scale; - mag_report->range_ga = _mag_range_ga; + mag_report->range_ga = (float)_mag_range_ga; /* post a report to the ring - note, not locked */ INCREMENT(_next_mag_report, _num_mag_reports); @@ -1297,6 +1360,8 @@ LSM303D::mag_measure() /* publish for subscribers */ orb_publish(ORB_ID(sensor_mag), _mag_topic, mag_report); + _mag_read++; + /* stop the perf counter */ perf_end(_mag_sample_perf); } @@ -1304,6 +1369,8 @@ LSM303D::mag_measure() void LSM303D::print_info() { + printf("accel reads: %u\n", _accel_read); + printf("mag reads: %u\n", _mag_read); perf_print_counter(_accel_sample_perf); printf("report queue: %u (%u/%u @ %p)\n", _num_accel_reports, _oldest_accel_report, _next_accel_report, _accel_reports); @@ -1466,7 +1533,7 @@ test() /* check if mag is onboard or external */ if ((ret = ioctl(fd_mag, MAGIOCGEXTERNAL, 0)) < 0) errx(1, "failed to get if mag is onboard or external"); - warnx("device active: %s", ret ? "external" : "onboard"); + warnx("mag device active: %s", ret ? "external" : "onboard"); /* do a simple demand read */ sz = read(fd_mag, &m_report, sizeof(m_report)); @@ -1484,7 +1551,7 @@ test() /* XXX add poll-rate tests here too */ -// reset(); + reset(); errx(0, "PASS"); } @@ -1503,7 +1570,16 @@ reset() err(1, "driver reset failed"); if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) - err(1, "driver poll restart failed"); + err(1, "accel driver poll rate reset failed"); + + int fd_mag = open(MAG_DEVICE_PATH, O_RDONLY); + + if (fd_mag < 0) { + warnx("could not open to mag " MAG_DEVICE_PATH); + } else { + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + err(1, "mag driver poll rate reset failed"); + } exit(0); } diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 198da9f0ae..7ea1ae0f30 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -936,6 +936,7 @@ void Sensors::mag_init() { int fd; + int ret; fd = open(MAG_DEVICE_PATH, 0); @@ -944,13 +945,27 @@ Sensors::mag_init() errx(1, "FATAL: no magnetometer found"); } - /* set the mag internal poll rate to at least 150Hz */ - ioctl(fd, MAGIOCSSAMPLERATE, 150); + /* try different mag sampling rates */ - /* set the driver to poll at 150Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, 150); +#if 0 + ret = ioctl(fd, MAGIOCSSAMPLERATE, 150); + if (ret == OK) { + /* set the pollrate accordingly */ + ioctl(fd, SENSORIOCSPOLLRATE, 150); + } else { + ret = ioctl(fd, MAGIOCSSAMPLERATE, 100); + /* if the slower sampling rate still fails, something is wrong */ + if (ret == OK) { + /* set the driver to poll also at the slower rate */ + ioctl(fd, SENSORIOCSPOLLRATE, 100); + } else { + errx(1, "FATAL: mag sampling rate could not be set"); + } + } +#endif + - int ret = ioctl(fd, MAGIOCGEXTERNAL, 0); + ret = ioctl(fd, MAGIOCGEXTERNAL, 0); if (ret < 0) errx(1, "FATAL: unknown if magnetometer is external or onboard"); else if (ret == 1) diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index 766598ddd6..262a52d200 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -90,6 +90,7 @@ static void do_gyro(int argc, char *argv[]) { int fd; + int ret; fd = open(GYRO_DEVICE_PATH, 0); @@ -102,20 +103,29 @@ do_gyro(int argc, char *argv[]) if (argc == 2 && !strcmp(argv[0], "sampling")) { /* set the gyro internal sampling rate up to at least i Hz */ - ioctl(fd, GYROIOCSSAMPLERATE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, GYROIOCSSAMPLERATE, strtoul(argv[1], NULL, 0)); + + if (ret) + errx(ret,"sampling rate could not be set"); } else if (argc == 2 && !strcmp(argv[0], "rate")) { /* set the driver to poll at i Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0)); + + if (ret) + errx(ret,"pollrate could not be set"); } else if (argc == 2 && !strcmp(argv[0], "range")) { /* set the range to i dps */ - ioctl(fd, GYROIOCSRANGE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, GYROIOCSRANGE, strtoul(argv[1], NULL, 0)); + + if (ret) + errx(ret,"range could not be set"); } else if(argc == 1 && !strcmp(argv[0], "check")) { - int ret = ioctl(fd, GYROIOCSELFTEST, 0); + ret = ioctl(fd, GYROIOCSELFTEST, 0); if (ret) { warnx("gyro self test FAILED! Check calibration:"); @@ -147,6 +157,7 @@ static void do_mag(int argc, char *argv[]) { int fd; + int ret; fd = open(MAG_DEVICE_PATH, 0); @@ -156,8 +167,32 @@ do_mag(int argc, char *argv[]) } else { - if(argc == 1 && !strcmp(argv[0], "check")) { - int ret = ioctl(fd, MAGIOCSELFTEST, 0); + if (argc == 2 && !strcmp(argv[0], "sampling")) { + + /* set the mag internal sampling rate up to at least i Hz */ + ret = ioctl(fd, MAGIOCSSAMPLERATE, strtoul(argv[1], NULL, 0)); + + if (ret) + errx(ret,"sampling rate could not be set"); + + } else if (argc == 2 && !strcmp(argv[0], "rate")) { + + /* set the driver to poll at i Hz */ + ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0)); + + if (ret) + errx(ret,"pollrate could not be set"); + + } else if (argc == 2 && !strcmp(argv[0], "range")) { + + /* set the range to i G */ + ret = ioctl(fd, MAGIOCSRANGE, strtoul(argv[1], NULL, 0)); + + if (ret) + errx(ret,"range could not be set"); + + } else if(argc == 1 && !strcmp(argv[0], "check")) { + ret = ioctl(fd, MAGIOCSELFTEST, 0); if (ret) { warnx("mag self test FAILED! Check calibration:"); @@ -173,9 +208,9 @@ do_mag(int argc, char *argv[]) errx(1, "wrong or no arguments given. Try: \n\n\t'check' for the self test\n\t"); } - int srate = -1; //ioctl(fd, MAGIOCGSAMPLERATE, 0); + int srate = ioctl(fd, MAGIOCGSAMPLERATE, 0); int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0); - int range = -1; //ioctl(fd, MAGIOCGRANGE, 0); + int range = ioctl(fd, MAGIOCGRANGE, 0); warnx("mag: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d gauss", srate, prate, range); @@ -189,6 +224,7 @@ static void do_accel(int argc, char *argv[]) { int fd; + int ret; fd = open(ACCEL_DEVICE_PATH, 0); @@ -201,20 +237,29 @@ do_accel(int argc, char *argv[]) if (argc == 2 && !strcmp(argv[0], "sampling")) { /* set the accel internal sampling rate up to at least i Hz */ - ioctl(fd, ACCELIOCSSAMPLERATE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, ACCELIOCSSAMPLERATE, strtoul(argv[1], NULL, 0)); + + if (ret) + errx(ret,"sampling rate could not be set"); } else if (argc == 2 && !strcmp(argv[0], "rate")) { /* set the driver to poll at i Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0)); + + if (ret) + errx(ret,"pollrate could not be set"); } else if (argc == 2 && !strcmp(argv[0], "range")) { - /* set the range to i m/s^2 */ - ioctl(fd, ACCELIOCSRANGE, strtoul(argv[1], NULL, 0)); + /* set the range to i G */ + ret = ioctl(fd, ACCELIOCSRANGE, strtoul(argv[1], NULL, 0)); + + if (ret) + errx(ret,"range could not be set"); } else if(argc == 1 && !strcmp(argv[0], "check")) { - int ret = ioctl(fd, ACCELIOCSELFTEST, 0); + ret = ioctl(fd, ACCELIOCSELFTEST, 0); if (ret) { warnx("accel self test FAILED! Check calibration:"); From 408b29ba618e1c2c7375d6acd488c223d796186f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 21 Aug 2013 08:40:51 +0200 Subject: [PATCH 423/486] Don't store m/s^2 and G at the same time --- src/drivers/lsm303d/lsm303d.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 8d6dc0672e..2b77699925 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -220,7 +220,6 @@ private: struct accel_scale _accel_scale; unsigned _accel_range_g; - float _accel_range_m_s2; float _accel_range_scale; unsigned _accel_samplerate; unsigned _accel_filter_bandwith; @@ -420,7 +419,6 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _oldest_mag_report(0), _mag_reports(nullptr), _accel_range_g(8), - _accel_range_m_s2(0.0f), _accel_range_scale(0.0f), _accel_samplerate(800), _accel_filter_bandwith(50), @@ -1029,7 +1027,6 @@ LSM303D::accel_set_range(unsigned max_g) return -EINVAL; } - _accel_range_m_s2 = _accel_range_g * 9.80665f; _accel_range_scale = new_scale_g_digit * 9.80665f; modify_reg(ADDR_CTRL_REG2, clearbits, setbits); @@ -1275,7 +1272,7 @@ LSM303D::measure() accel_report->z = _accel_filter_z.apply(z_in_new); accel_report->scaling = _accel_range_scale; - accel_report->range_m_s2 = _accel_range_m_s2; + accel_report->range_m_s2 = _accel_range_g * 9.80665f; /* post a report to the ring - note, not locked */ INCREMENT(_next_accel_report, _num_accel_reports); From 658276e1cc5f4639fb09ff41a20b422e6ce33fe3 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 21 Aug 2013 09:23:21 +0200 Subject: [PATCH 424/486] Add reset and samplerate ioctl to HMC5883 driver --- src/drivers/hmc5883/hmc5883.cpp | 117 ++++++++++++++++++-------------- 1 file changed, 65 insertions(+), 52 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 1afc16a9a6..cb708db4a5 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -191,6 +191,11 @@ private: */ void stop(); + /** + * Reset the device + */ + int reset(); + /** * Perform the on-sensor scale calibration routine. * @@ -365,6 +370,9 @@ HMC5883::init() if (I2C::init() != OK) goto out; + /* reset the device configuration */ + reset(); + /* allocate basic report buffers */ _num_reports = 2; _reports = new struct mag_report[_num_reports]; @@ -381,9 +389,6 @@ HMC5883::init() if (_mag_topic < 0) debug("failed to create sensor_mag object"); - /* set range */ - set_range(_range_ga); - ret = OK; /* sensor is ok, but not calibrated */ _sensor_ok = true; @@ -542,62 +547,61 @@ int HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { - case SENSORIOCSPOLLRATE: { - switch (arg) { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return OK; + + /* external signalling (DRDY) not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(HMC5883_CONVERSION_INTERVAL); + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); - /* switching to manual polling */ - case SENSOR_POLLRATE_MANUAL: - stop(); - _measure_ticks = 0; return OK; + } - /* external signalling (DRDY) not supported */ - case SENSOR_POLLRATE_EXTERNAL: + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); - /* zero would be bad */ - case 0: - return -EINVAL; + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); - /* set default/max polling rate */ - case SENSOR_POLLRATE_MAX: - case SENSOR_POLLRATE_DEFAULT: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); + /* check against maximum rate */ + if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL)) + return -EINVAL; - /* set interval for next measurement to minimum legal value */ - _measure_ticks = USEC2TICK(HMC5883_CONVERSION_INTERVAL); + /* update interval for next measurement */ + _measure_ticks = ticks; - /* if we need to start the poll state machine, do it */ - if (want_start) - start(); + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); - return OK; - } - - /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); - - /* convert hz to tick interval via microseconds */ - unsigned ticks = USEC2TICK(1000000 / arg); - - /* check against maximum rate */ - if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL)) - return -EINVAL; - - /* update interval for next measurement */ - _measure_ticks = ticks; - - /* if we need to start the poll state machine, do it */ - if (want_start) - start(); - - return OK; - } + return OK; } } + } case SENSORIOCGPOLLRATE: if (_measure_ticks == 0) @@ -633,13 +637,15 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) return _num_reports - 1; case SENSORIOCRESET: - /* XXX implement this */ - return -EINVAL; + return reset(); case MAGIOCSSAMPLERATE: + /* same as pollrate because device is in single measurement mode*/ + return ioctl(filp, SENSORIOCSPOLLRATE, arg); + case MAGIOCGSAMPLERATE: - /* not supported, always 1 sample per poll */ - return -EINVAL; + /* same as pollrate because device is in single measurement mode*/ + return ioctl(filp, SENSORIOCGPOLLRATE, 0); case MAGIOCSRANGE: return set_range(arg); @@ -702,6 +708,13 @@ HMC5883::stop() work_cancel(HPWORK, &_work); } +int +HMC5883::reset() +{ + /* set range */ + return set_range(_range_ga); +} + void HMC5883::cycle_trampoline(void *arg) { From 9762cf86a081f44e7f7ea48f160d556003bf5be9 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 21 Aug 2013 09:52:21 +0200 Subject: [PATCH 425/486] Forgot to comment mag init in sensors.cpp back back in --- src/modules/sensors/sensors.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 7ea1ae0f30..dda558b4c9 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -947,7 +947,7 @@ Sensors::mag_init() /* try different mag sampling rates */ -#if 0 + ret = ioctl(fd, MAGIOCSSAMPLERATE, 150); if (ret == OK) { /* set the pollrate accordingly */ @@ -962,7 +962,7 @@ Sensors::mag_init() errx(1, "FATAL: mag sampling rate could not be set"); } } -#endif + ret = ioctl(fd, MAGIOCGEXTERNAL, 0); From 3875df2fe07d33d00331efd02394201d684655c7 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 21 Aug 2013 10:44:47 +0200 Subject: [PATCH 426/486] Workaround to get the HMC5883 default rate right --- src/drivers/hmc5883/hmc5883.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index cb708db4a5..d77f03bb72 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -77,8 +77,8 @@ #define HMC5883L_ADDRESS PX4_I2C_OBDEV_HMC5883 -/* Max measurement rate is 160Hz */ -#define HMC5883_CONVERSION_INTERVAL (1000000 / 160) /* microseconds */ +/* Max measurement rate is 160Hz, however with 160 it will be set to 166 Hz, therefore workaround using 150 */ +#define HMC5883_CONVERSION_INTERVAL (1000000 / 150) /* microseconds */ #define ADDR_CONF_A 0x00 #define ADDR_CONF_B 0x01 @@ -607,7 +607,7 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) if (_measure_ticks == 0) return SENSOR_POLLRATE_MANUAL; - return (1000 / _measure_ticks); + return 1000000/TICK2USEC(_measure_ticks); case SENSORIOCSQUEUEDEPTH: { /* add one to account for the sentinel in the ring */ @@ -645,7 +645,7 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) case MAGIOCGSAMPLERATE: /* same as pollrate because device is in single measurement mode*/ - return ioctl(filp, SENSORIOCGPOLLRATE, 0); + return 1000000/TICK2USEC(_measure_ticks); case MAGIOCSRANGE: return set_range(arg); From 5fbee2394522d8b0c7a78d2751783845d011b56d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 Aug 2013 11:17:29 +0200 Subject: [PATCH 427/486] Added flag to disable RC evaluation onboard of IO (raw values still forwarded) --- src/drivers/px4io/px4io.cpp | 31 ++++++++++++++++++++------- src/modules/px4iofirmware/mixer.cpp | 3 ++- src/modules/px4iofirmware/protocol.h | 3 ++- src/modules/px4iofirmware/registers.c | 10 ++++++++- 4 files changed, 36 insertions(+), 11 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index cebe339967..afbd4e6959 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -206,7 +206,8 @@ private: unsigned _max_relays; /// Date: Wed, 21 Aug 2013 12:37:06 +0200 Subject: [PATCH 428/486] Changed range handling of LSM303D once again, added defines for default values --- src/drivers/lsm303d/lsm303d.cpp | 63 ++++++++++++++++++++------------- 1 file changed, 39 insertions(+), 24 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 2b77699925..803cd658f7 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -54,6 +54,7 @@ #include #include +#include #include #include @@ -168,6 +169,14 @@ static const int ERROR = -1; #define INT_CTRL_M 0x12 #define INT_SRC_M 0x13 +/* default values for this device */ +#define ACCEL_DEFAULT_RANGE_G 8 +#define ACCEL_DEFAULT_SAMPLERATE 800 +#define ACCEL_DEFAULT_ONCHIP_FILTER_FREQ 50 +#define ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 + +#define MAG_DEFAULT_RANGE_GA 2 +#define MAG_DEFAULT_SAMPLERATE 100 extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); } @@ -219,7 +228,7 @@ private: struct mag_report *_mag_reports; struct accel_scale _accel_scale; - unsigned _accel_range_g; + unsigned _accel_range_m_s2; float _accel_range_scale; unsigned _accel_samplerate; unsigned _accel_filter_bandwith; @@ -418,22 +427,22 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _next_mag_report(0), _oldest_mag_report(0), _mag_reports(nullptr), - _accel_range_g(8), + _accel_range_m_s2(0.0f), _accel_range_scale(0.0f), - _accel_samplerate(800), - _accel_filter_bandwith(50), - _mag_range_ga(2), + _accel_samplerate(0), + _accel_filter_bandwith(0), + _mag_range_ga(0.0f), _mag_range_scale(0.0f), - _mag_samplerate(100), + _mag_samplerate(0), _accel_topic(-1), _mag_topic(-1), _accel_read(0), _mag_read(0), _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")), - _accel_filter_x(800, 30), - _accel_filter_y(800, 30), - _accel_filter_z(800, 30) + _accel_filter_x(0, 0), + _accel_filter_y(0, 0), + _accel_filter_z(0, 0) { // enable debug() calls _debug_enabled = true; @@ -529,12 +538,17 @@ LSM303D::reset() write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); - accel_set_range(_accel_range_g); - accel_set_samplerate(_accel_samplerate); - accel_set_antialias_filter_bandwidth(_accel_filter_bandwith); + _accel_filter_bandwith = ACCEL_DEFAULT_DRIVER_FILTER_FREQ; - mag_set_range(_mag_range_ga); - mag_set_samplerate(_mag_samplerate); + accel_set_range(ACCEL_DEFAULT_RANGE_G); + accel_set_samplerate(ACCEL_DEFAULT_SAMPLERATE); + accel_set_antialias_filter_bandwidth(ACCEL_DEFAULT_ONCHIP_FILTER_FREQ); + + mag_set_range(MAG_DEFAULT_RANGE_GA); + mag_set_samplerate(MAG_DEFAULT_SAMPLERATE); + + _accel_read = 0; + _mag_read = 0; } int @@ -658,8 +672,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) return ioctl(filp, SENSORIOCSPOLLRATE, 1600); case SENSOR_POLLRATE_DEFAULT: - /* Use 800Hz as it is filtered in the driver as well*/ - return ioctl(filp, SENSORIOCSPOLLRATE, 800); + return ioctl(filp, SENSORIOCSPOLLRATE, ACCEL_DEFAULT_SAMPLERATE); /* adjust to a legal polling interval in Hz */ default: { @@ -759,10 +772,12 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) } case ACCELIOCSRANGE: + /* arg needs to be in G */ return accel_set_range(arg); case ACCELIOCGRANGE: - return _accel_range_g; + /* convert to m/s^2 and return rounded in G */ + return (unsigned long)((_accel_range_m_s2)/CONSTANTS_ONE_G + 0.5f); case ACCELIOCGSCALE: /* copy scale out */ @@ -999,27 +1014,27 @@ LSM303D::accel_set_range(unsigned max_g) max_g = 16; if (max_g <= 2) { - _accel_range_g = 2; + _accel_range_m_s2 = 2.0f*CONSTANTS_ONE_G; setbits |= REG2_FULL_SCALE_2G_A; new_scale_g_digit = 0.061e-3f; } else if (max_g <= 4) { - _accel_range_g = 4; + _accel_range_m_s2 = 4.0f*CONSTANTS_ONE_G; setbits |= REG2_FULL_SCALE_4G_A; new_scale_g_digit = 0.122e-3f; } else if (max_g <= 6) { - _accel_range_g = 6; + _accel_range_m_s2 = 6.0f*CONSTANTS_ONE_G; setbits |= REG2_FULL_SCALE_6G_A; new_scale_g_digit = 0.183e-3f; } else if (max_g <= 8) { - _accel_range_g = 8; + _accel_range_m_s2 = 8.0f*CONSTANTS_ONE_G; setbits |= REG2_FULL_SCALE_8G_A; new_scale_g_digit = 0.244e-3f; } else if (max_g <= 16) { - _accel_range_g = 16; + _accel_range_m_s2 = 16.0f*CONSTANTS_ONE_G; setbits |= REG2_FULL_SCALE_16G_A; new_scale_g_digit = 0.732e-3f; @@ -1027,7 +1042,7 @@ LSM303D::accel_set_range(unsigned max_g) return -EINVAL; } - _accel_range_scale = new_scale_g_digit * 9.80665f; + _accel_range_scale = new_scale_g_digit * CONSTANTS_ONE_G; modify_reg(ADDR_CTRL_REG2, clearbits, setbits); @@ -1272,7 +1287,7 @@ LSM303D::measure() accel_report->z = _accel_filter_z.apply(z_in_new); accel_report->scaling = _accel_range_scale; - accel_report->range_m_s2 = _accel_range_g * 9.80665f; + accel_report->range_m_s2 = _accel_range_m_s2; /* post a report to the ring - note, not locked */ INCREMENT(_next_accel_report, _num_accel_reports); From 64b8f5232bcd12a819cb72e862158db6db5a0a66 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 Aug 2013 13:54:37 +0200 Subject: [PATCH 429/486] Build fix, added command line switch norc to disable RC --- src/drivers/px4io/px4io.cpp | 26 ++++++++++++++++++++++++++ 1 file changed, 26 insertions(+) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index afbd4e6959..2e8946264a 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -175,6 +175,11 @@ public: */ void print_status(); + /** + * Disable RC input handling + */ + int disable_rc_handling(); + /** * Set the DSM VCC is controlled by relay one flag * @@ -276,6 +281,11 @@ private: */ int io_get_status(); + /** + * Disable RC input handling + */ + int io_disable_rc_handling(); + /** * Fetch RC inputs from IO. * @@ -853,6 +863,12 @@ PX4IO::io_set_arming_state() return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set); } +int +PX4IO::io_disable_rc_handling() +{ + return io_disable_rc_handling(); +} + int PX4IO::io_disable_rc_handling() { @@ -1785,6 +1801,16 @@ start(int argc, char *argv[]) errx(1, "driver init failed"); } + /* disable RC handling on request */ + if (argc > 0 && !strcmp(argv[0], "norc")) { + + if(g_dev->disable_rc_handling()) + warnx("Failed disabling RC handling"); + + } else { + warnx("unknown argument: %s", argv[0]); + } + int dsm_vcc_ctl; if (param_get(param_find("RC_RL1_DSM_VCC"), &dsm_vcc_ctl) == OK) { From 7db420b9b222e6114e2cb6ffb5d726a946dd07c6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 21 Aug 2013 14:20:42 +0200 Subject: [PATCH 430/486] Get units right in config --- src/systemcmds/config/config.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index 262a52d200..188dafa4ef 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -212,7 +212,7 @@ do_mag(int argc, char *argv[]) int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0); int range = ioctl(fd, MAGIOCGRANGE, 0); - warnx("mag: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d gauss", srate, prate, range); + warnx("mag: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", srate, prate, range); close(fd); } @@ -279,7 +279,7 @@ do_accel(int argc, char *argv[]) int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0); int range = ioctl(fd, ACCELIOCGRANGE, 0); - warnx("accel: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d m/s", srate, prate, range); + warnx("accel: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d G", srate, prate, range); close(fd); } From 8083efb60c97ffce5be8dcbf3956ab67cc17d729 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 21 Aug 2013 14:21:11 +0200 Subject: [PATCH 431/486] Use gyro at correct rate --- src/modules/sensors/sensors.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index dda558b4c9..2ffa5f698c 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -919,11 +919,11 @@ Sensors::gyro_init() #else - /* set the gyro internal sampling rate up to at leat 800Hz */ - ioctl(fd, GYROIOCSSAMPLERATE, 800); + /* set the gyro internal sampling rate up to at least 760Hz */ + ioctl(fd, GYROIOCSSAMPLERATE, 760); - /* set the driver to poll at 800Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, 800); + /* set the driver to poll at 760Hz */ + ioctl(fd, SENSORIOCSPOLLRATE, 760); #endif From 1ede95d252f5401f3bcf94265c42a060833ca8ca Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 21 Aug 2013 14:21:54 +0200 Subject: [PATCH 432/486] L3GD20 and LSM303D reset and range config working properly now --- src/drivers/l3gd20/l3gd20.cpp | 131 ++++++++++++++++++++------------ src/drivers/lsm303d/lsm303d.cpp | 92 ++++++++++++---------- 2 files changed, 136 insertions(+), 87 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index de6b753f12..5e0a2119a1 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -154,6 +154,10 @@ static const int ERROR = -1; #define FIFO_CTRL_STREAM_TO_FIFO_MODE (3<<5) #define FIFO_CTRL_BYPASS_TO_STREAM_MODE (1<<7) +#define L3GD20_DEFAULT_RATE 760 +#define L3GD20_DEFAULT_RANGE_DPS 2000 +#define L3GD20_DEFAULT_FILTER_FREQ 30 + extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); } class L3GD20 : public device::SPI @@ -191,9 +195,10 @@ private: orb_advert_t _gyro_topic; unsigned _current_rate; - unsigned _current_range; unsigned _orientation; + unsigned _read; + perf_counter_t _sample_perf; math::LowPassFilter2p _gyro_filter_x; @@ -210,6 +215,11 @@ private: */ void stop(); + /** + * Reset the driver + */ + void reset(); + /** * Static trampoline from the hrt_call context; because we don't have a * generic hrt wrapper yet. @@ -273,6 +283,14 @@ private: */ int set_samplerate(unsigned frequency); + /** + * Set the lowpass filter of the driver + * + * @param samplerate The current samplerate + * @param frequency The cutoff frequency for the lowpass filter + */ + void set_driver_lowpass_filter(float samplerate, float bandwidth); + /** * Self test * @@ -296,12 +314,12 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : _gyro_range_rad_s(0.0f), _gyro_topic(-1), _current_rate(0), - _current_range(0), _orientation(SENSOR_BOARD_ROTATION_270_DEG), - _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")), - _gyro_filter_x(250, 30), - _gyro_filter_y(250, 30), - _gyro_filter_z(250, 30) + _read(0), + _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")), + _gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), + _gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), + _gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ) { // enable debug() calls _debug_enabled = true; @@ -349,22 +367,7 @@ L3GD20::init() memset(&_reports[0], 0, sizeof(_reports[0])); _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_reports[0]); - /* set default configuration */ - write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); - write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ - write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */ - write_reg(ADDR_CTRL_REG4, REG4_BDU); - write_reg(ADDR_CTRL_REG5, 0); - - write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */ - - /* disable FIFO. This makes things simpler and ensures we - * aren't getting stale data. It means we must run the hrt - * callback fast enough to not miss data. */ - write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE); - - set_range(2000); /* default to 2000dps */ - set_samplerate(0); /* max sample rate */ + reset(); ret = OK; out: @@ -464,8 +467,7 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: - /* With internal low pass filters enabled, 250 Hz is sufficient */ - return ioctl(filp, SENSORIOCSPOLLRATE, 250); + return ioctl(filp, SENSORIOCSPOLLRATE, L3GD20_DEFAULT_RATE); /* adjust to a legal polling interval in Hz */ default: { @@ -483,12 +485,10 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) /* XXX this is a bit shady, but no other way to adjust... */ _call.period = _call_interval = ticks; - // adjust filters - float cutoff_freq_hz = _gyro_filter_x.get_cutoff_freq(); - float sample_rate = 1.0e6f/ticks; - _gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - _gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - _gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + /* adjust filters */ + float cutoff_freq_hz = _gyro_filter_x.get_cutoff_freq(); + float sample_rate = 1.0e6f/ticks; + set_driver_lowpass_filter(sample_rate, cutoff_freq_hz); /* if we need to start the poll state machine, do it */ if (want_start) @@ -533,8 +533,8 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) return _num_reports - 1; case SENSORIOCRESET: - /* XXX implement */ - return -EINVAL; + reset(); + return OK; case GYROIOCSSAMPLERATE: return set_samplerate(arg); @@ -543,16 +543,15 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) return _current_rate; case GYROIOCSLOWPASS: { - float cutoff_freq_hz = arg; - float sample_rate = 1.0e6f / _call_interval; - _gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - _gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - _gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - return OK; - } + float cutoff_freq_hz = arg; + float sample_rate = 1.0e6f / _call_interval; + set_driver_lowpass_filter(sample_rate, cutoff_freq_hz); + + return OK; + } case GYROIOCGLOWPASS: - return _gyro_filter_x.get_cutoff_freq(); + return _gyro_filter_x.get_cutoff_freq(); case GYROIOCSSCALE: /* copy scale in */ @@ -565,10 +564,12 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) return OK; case GYROIOCSRANGE: + /* arg should be in dps */ return set_range(arg); case GYROIOCGRANGE: - return _current_range; + /* convert to dps and round */ + return (unsigned long)(_gyro_range_rad_s * 180.0f / M_PI_F + 0.5f); case GYROIOCSELFTEST: return self_test(); @@ -618,22 +619,23 @@ L3GD20::set_range(unsigned max_dps) { uint8_t bits = REG4_BDU; float new_range_scale_dps_digit; + float new_range; if (max_dps == 0) { max_dps = 2000; } if (max_dps <= 250) { - _current_range = 250; + new_range = 250; bits |= RANGE_250DPS; new_range_scale_dps_digit = 8.75e-3f; } else if (max_dps <= 500) { - _current_range = 500; + new_range = 500; bits |= RANGE_500DPS; new_range_scale_dps_digit = 17.5e-3f; } else if (max_dps <= 2000) { - _current_range = 2000; + new_range = 2000; bits |= RANGE_2000DPS; new_range_scale_dps_digit = 70e-3f; @@ -641,7 +643,7 @@ L3GD20::set_range(unsigned max_dps) return -EINVAL; } - _gyro_range_rad_s = _current_range / 180.0f * M_PI_F; + _gyro_range_rad_s = new_range / 180.0f * M_PI_F; _gyro_range_scale = new_range_scale_dps_digit / 180.0f * M_PI_F; write_reg(ADDR_CTRL_REG4, bits); @@ -656,7 +658,7 @@ L3GD20::set_samplerate(unsigned frequency) if (frequency == 0) frequency = 760; - // use limits good for H or non-H models + /* use limits good for H or non-H models */ if (frequency <= 100) { _current_rate = 95; bits |= RATE_95HZ_LP_25HZ; @@ -682,6 +684,14 @@ L3GD20::set_samplerate(unsigned frequency) return OK; } +void +L3GD20::set_driver_lowpass_filter(float samplerate, float bandwidth) +{ + _gyro_filter_x.set_cutoff_frequency(samplerate, bandwidth); + _gyro_filter_y.set_cutoff_frequency(samplerate, bandwidth); + _gyro_filter_z.set_cutoff_frequency(samplerate, bandwidth); +} + void L3GD20::start() { @@ -701,6 +711,30 @@ L3GD20::stop() hrt_cancel(&_call); } +void +L3GD20::reset() +{ + /* set default configuration */ + write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); + write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ + write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */ + write_reg(ADDR_CTRL_REG4, REG4_BDU); + write_reg(ADDR_CTRL_REG5, 0); + + write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */ + + /* disable FIFO. This makes things simpler and ensures we + * aren't getting stale data. It means we must run the hrt + * callback fast enough to not miss data. */ + write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE); + + set_samplerate(L3GD20_DEFAULT_RATE); + set_range(L3GD20_DEFAULT_RANGE_DPS); + set_driver_lowpass_filter(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ); + + _read = 0; +} + void L3GD20::measure_trampoline(void *arg) { @@ -804,6 +838,8 @@ L3GD20::measure() if (_gyro_topic > 0) orb_publish(ORB_ID(sensor_gyro), _gyro_topic, report); + _read++; + /* stop the perf counter */ perf_end(_sample_perf); } @@ -811,6 +847,7 @@ L3GD20::measure() void L3GD20::print_info() { + printf("gyro reads: %u\n", _read); perf_print_counter(_sample_perf); printf("report queue: %u (%u/%u @ %p)\n", _num_reports, _oldest_report, _next_report, _reports); @@ -949,7 +986,7 @@ reset() err(1, "driver reset failed"); if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) - err(1, "driver poll restart failed"); + err(1, "accel pollrate reset failed"); exit(0); } diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 803cd658f7..efe7cf8eb6 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -170,13 +170,13 @@ static const int ERROR = -1; #define INT_SRC_M 0x13 /* default values for this device */ -#define ACCEL_DEFAULT_RANGE_G 8 -#define ACCEL_DEFAULT_SAMPLERATE 800 -#define ACCEL_DEFAULT_ONCHIP_FILTER_FREQ 50 -#define ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 +#define LSM303D_ACCEL_DEFAULT_RANGE_G 8 +#define LSM303D_ACCEL_DEFAULT_RATE 800 +#define LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ 50 +#define LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 -#define MAG_DEFAULT_RANGE_GA 2 -#define MAG_DEFAULT_SAMPLERATE 100 +#define LSM303D_MAG_DEFAULT_RANGE_GA 2 +#define LSM303D_MAG_DEFAULT_RATE 100 extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); } @@ -231,7 +231,7 @@ private: unsigned _accel_range_m_s2; float _accel_range_scale; unsigned _accel_samplerate; - unsigned _accel_filter_bandwith; + unsigned _accel_onchip_filter_bandwith; struct mag_scale _mag_scale; unsigned _mag_range_ga; @@ -356,13 +356,22 @@ private: int mag_set_range(unsigned max_g); /** - * Set the LSM303D accel anti-alias filter. + * Set the LSM303D on-chip anti-alias filter bandwith. * * @param bandwidth The anti-alias filter bandwidth in Hz * Zero selects the highest bandwidth * @return OK if the value can be supported, -ERANGE otherwise. */ - int accel_set_antialias_filter_bandwidth(unsigned bandwith); + int accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth); + + /** + * Set the driver lowpass filter bandwidth. + * + * @param bandwidth The anti-alias filter bandwidth in Hz + * Zero selects the highest bandwidth + * @return OK if the value can be supported, -ERANGE otherwise. + */ + int accel_set_driver_lowpass_filter(float samplerate, float bandwidth); /** * Set the LSM303D internal accel sampling frequency. @@ -430,7 +439,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _accel_range_m_s2(0.0f), _accel_range_scale(0.0f), _accel_samplerate(0), - _accel_filter_bandwith(0), + _accel_onchip_filter_bandwith(0), _mag_range_ga(0.0f), _mag_range_scale(0.0f), _mag_samplerate(0), @@ -440,9 +449,9 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _mag_read(0), _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")), - _accel_filter_x(0, 0), - _accel_filter_y(0, 0), - _accel_filter_z(0, 0) + _accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ) { // enable debug() calls _debug_enabled = true; @@ -538,14 +547,13 @@ LSM303D::reset() write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); - _accel_filter_bandwith = ACCEL_DEFAULT_DRIVER_FILTER_FREQ; + accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); + accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE); + accel_set_driver_lowpass_filter((float)LSM303D_ACCEL_DEFAULT_RATE, (float)LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ); + accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ); - accel_set_range(ACCEL_DEFAULT_RANGE_G); - accel_set_samplerate(ACCEL_DEFAULT_SAMPLERATE); - accel_set_antialias_filter_bandwidth(ACCEL_DEFAULT_ONCHIP_FILTER_FREQ); - - mag_set_range(MAG_DEFAULT_RANGE_GA); - mag_set_samplerate(MAG_DEFAULT_SAMPLERATE); + mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA); + mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE); _accel_read = 0; _mag_read = 0; @@ -672,7 +680,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) return ioctl(filp, SENSORIOCSPOLLRATE, 1600); case SENSOR_POLLRATE_DEFAULT: - return ioctl(filp, SENSORIOCSPOLLRATE, ACCEL_DEFAULT_SAMPLERATE); + return ioctl(filp, SENSORIOCSPOLLRATE, LSM303D_ACCEL_DEFAULT_RATE); /* adjust to a legal polling interval in Hz */ default: { @@ -687,11 +695,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; /* adjust filters */ - float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq(); - - _accel_filter_x.set_cutoff_frequency((float)arg, cutoff_freq_hz); - _accel_filter_y.set_cutoff_frequency((float)arg, cutoff_freq_hz); - _accel_filter_z.set_cutoff_frequency((float)arg, cutoff_freq_hz); + accel_set_driver_lowpass_filter((float)arg, _accel_filter_x.get_cutoff_freq()); /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ @@ -750,10 +754,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) return _accel_samplerate; case ACCELIOCSLOWPASS: { - _accel_filter_x.set_cutoff_frequency((float)_accel_samplerate, (float)arg); - _accel_filter_y.set_cutoff_frequency((float)_accel_samplerate, (float)arg); - _accel_filter_z.set_cutoff_frequency((float)_accel_samplerate, (float)arg); - return OK; + return accel_set_driver_lowpass_filter((float)_accel_samplerate, (float)arg); } case ACCELIOCGLOWPASS: @@ -1091,7 +1092,7 @@ LSM303D::mag_set_range(unsigned max_ga) } int -LSM303D::accel_set_antialias_filter_bandwidth(unsigned bandwidth) +LSM303D::accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth) { uint8_t setbits = 0; uint8_t clearbits = REG2_ANTIALIAS_FILTER_BW_BITS_A; @@ -1101,19 +1102,19 @@ LSM303D::accel_set_antialias_filter_bandwidth(unsigned bandwidth) if (bandwidth <= 50) { setbits |= REG2_AA_FILTER_BW_50HZ_A; - _accel_filter_bandwith = 50; + _accel_onchip_filter_bandwith = 50; } else if (bandwidth <= 194) { setbits |= REG2_AA_FILTER_BW_194HZ_A; - _accel_filter_bandwith = 194; + _accel_onchip_filter_bandwith = 194; } else if (bandwidth <= 362) { setbits |= REG2_AA_FILTER_BW_362HZ_A; - _accel_filter_bandwith = 362; + _accel_onchip_filter_bandwith = 362; } else if (bandwidth <= 773) { setbits |= REG2_AA_FILTER_BW_773HZ_A; - _accel_filter_bandwith = 773; + _accel_onchip_filter_bandwith = 773; } else { return -EINVAL; @@ -1124,6 +1125,16 @@ LSM303D::accel_set_antialias_filter_bandwidth(unsigned bandwidth) return OK; } +int +LSM303D::accel_set_driver_lowpass_filter(float samplerate, float bandwidth) +{ + _accel_filter_x.set_cutoff_frequency(samplerate, bandwidth); + _accel_filter_y.set_cutoff_frequency(samplerate, bandwidth); + _accel_filter_z.set_cutoff_frequency(samplerate, bandwidth); + + return OK; +} + int LSM303D::accel_set_samplerate(unsigned frequency) { @@ -1582,15 +1593,16 @@ reset() err(1, "driver reset failed"); if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) - err(1, "accel driver poll rate reset failed"); + err(1, "accel pollrate reset failed"); - int fd_mag = open(MAG_DEVICE_PATH, O_RDONLY); + fd = open(MAG_DEVICE_PATH, O_RDONLY); - if (fd_mag < 0) { - warnx("could not open to mag " MAG_DEVICE_PATH); + if (fd < 0) { + warnx("mag could not be opened, external mag might be used"); } else { + /* no need to reset the mag as well, the reset() is the same */ if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) - err(1, "mag driver poll rate reset failed"); + err(1, "mag pollrate reset failed"); } exit(0); From 4f51f333a9a125ce137abe689c3fc0ce7943701b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 21 Aug 2013 14:52:20 +0200 Subject: [PATCH 433/486] Adapted the MPU6000 to have the same get range ioctls and defines for defaults --- src/drivers/mpu6000/mpu6000.cpp | 85 ++++++++++++++++++--------------- 1 file changed, 47 insertions(+), 38 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index bfc74c73e9..0e65923db7 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -149,6 +149,15 @@ #define MPU6000_REV_D9 0x59 #define MPU6000_REV_D10 0x5A +#define MPU6000_ACCEL_DEFAULT_RANGE_G 8 +#define MPU6000_ACCEL_DEFAULT_RATE 1000 +#define MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 + +#define MPU6000_GYRO_DEFAULT_RANGE_G 8 +#define MPU6000_GYRO_DEFAULT_RATE 1000 +#define MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30 + +#define MPU6000_DEFAULT_ONCHIP_FILTER_FREQ 42 class MPU6000_gyro; @@ -357,12 +366,12 @@ MPU6000::MPU6000(int bus, spi_dev_e device) : _reads(0), _sample_rate(1000), _sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")), - _accel_filter_x(1000, 30), - _accel_filter_y(1000, 30), - _accel_filter_z(1000, 30), - _gyro_filter_x(1000, 30), - _gyro_filter_y(1000, 30), - _gyro_filter_z(1000, 30) + _accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _gyro_filter_x(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), + _gyro_filter_y(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), + _gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ) { // disable debug() calls _debug_enabled = false; @@ -485,14 +494,13 @@ void MPU6000::reset() up_udelay(1000); // SAMPLE RATE - //write_reg(MPUREG_SMPLRT_DIV, 0x04); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz - _set_sample_rate(_sample_rate); // default sample rate = 200Hz + _set_sample_rate(_sample_rate); usleep(1000); // FS & DLPF FS=2000 deg/s, DLPF = 20Hz (low pass filter) // was 90 Hz, but this ruins quality and does not improve the // system response - _set_dlpf_filter(42); + _set_dlpf_filter(MPU6000_DEFAULT_ONCHIP_FILTER_FREQ); usleep(1000); // Gyro scale 2000 deg/s () write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS); @@ -535,8 +543,8 @@ void MPU6000::reset() // Correct accel scale factors of 4096 LSB/g // scale to m/s^2 ( 1g = 9.81 m/s^2) - _accel_range_scale = (9.81f / 4096.0f); - _accel_range_m_s2 = 8.0f * 9.81f; + _accel_range_scale = (CONSTANTS_ONE_G / 4096.0f); + _accel_range_m_s2 = 8.0f * CONSTANTS_ONE_G; usleep(1000); @@ -777,9 +785,10 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: + return ioctl(filp, SENSORIOCSPOLLRATE, 1000); + case SENSOR_POLLRATE_DEFAULT: - /* set to same as sample rate per default */ - return ioctl(filp, SENSORIOCSPOLLRATE, _sample_rate); + return ioctl(filp, SENSORIOCSPOLLRATE, MPU6000_ACCEL_DEFAULT_RATE); /* adjust to a legal polling interval in Hz */ default: { @@ -867,9 +876,9 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) // XXX decide on relationship of both filters // i.e. disable the on-chip filter //_set_dlpf_filter((uint16_t)arg); - _accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); - _accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); - _accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); + _accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); + _accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); + _accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); return OK; case ACCELIOCSSCALE: @@ -897,7 +906,7 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) // _accel_range_m_s2 = 8.0f * 9.81f; return -EINVAL; case ACCELIOCGRANGE: - return _accel_range_m_s2; + return (unsigned long)((_accel_range_m_s2)/CONSTANTS_ONE_G + 0.5f); case ACCELIOCSELFTEST: return accel_self_test(); @@ -920,29 +929,29 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) return ioctl(filp, cmd, arg); case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) + return -EINVAL; - /* allocate new buffer */ - GyroReportBuffer *buf = new GyroReportBuffer(arg); + /* allocate new buffer */ + GyroReportBuffer *buf = new GyroReportBuffer(arg); - if (nullptr == buf) - return -ENOMEM; - if (buf->size() == 0) { - delete buf; - return -ENOMEM; - } - - /* reset the measurement state machine with the new buffer, free the old */ - stop(); - delete _gyro_reports; - _gyro_reports = buf; - start(); - - return OK; + if (nullptr == buf) + return -ENOMEM; + if (buf->size() == 0) { + delete buf; + return -ENOMEM; } + /* reset the measurement state machine with the new buffer, free the old */ + stop(); + delete _gyro_reports; + _gyro_reports = buf; + start(); + + return OK; + } + case SENSORIOCGQUEUEDEPTH: return _gyro_reports->size(); @@ -980,7 +989,7 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) // _gyro_range_rad_s = xx return -EINVAL; case GYROIOCGRANGE: - return _gyro_range_rad_s; + return (unsigned long)(_gyro_range_rad_s * 180.0f / M_PI_F + 0.5f); case GYROIOCSELFTEST: return gyro_self_test(); @@ -1400,7 +1409,7 @@ test() warnx("acc y: \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw); warnx("acc z: \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw); warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2, - (double)(a_report.range_m_s2 / 9.81f)); + (double)(a_report.range_m_s2 / CONSTANTS_ONE_G)); /* do a simple demand read */ sz = read(fd_gyro, &g_report, sizeof(g_report)); From 5be2f4a792dab32a5fa25f4faaff1edf05143cd9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 Aug 2013 14:54:57 +0200 Subject: [PATCH 434/486] Moved mavlink log to system lib --- src/drivers/px4io/px4io.cpp | 4 ++-- src/modules/mavlink/module.mk | 1 - .../{mavlink => systemlib}/mavlink_log.c | 21 ++++++------------- src/modules/systemlib/module.mk | 3 ++- 4 files changed, 10 insertions(+), 19 deletions(-) rename src/modules/{mavlink => systemlib}/mavlink_log.c (81%) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 2e8946264a..c00816a127 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -864,7 +864,7 @@ PX4IO::io_set_arming_state() } int -PX4IO::io_disable_rc_handling() +PX4IO::disable_rc_handling() { return io_disable_rc_handling(); } @@ -1803,7 +1803,7 @@ start(int argc, char *argv[]) /* disable RC handling on request */ if (argc > 0 && !strcmp(argv[0], "norc")) { - + if(g_dev->disable_rc_handling()) warnx("Failed disabling RC handling"); diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index bfccb2d389..5d3d6a73c1 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -39,7 +39,6 @@ MODULE_COMMAND = mavlink SRCS += mavlink.c \ missionlib.c \ mavlink_parameters.c \ - mavlink_log.c \ mavlink_receiver.cpp \ orb_listener.c \ waypoints.c diff --git a/src/modules/mavlink/mavlink_log.c b/src/modules/systemlib/mavlink_log.c similarity index 81% rename from src/modules/mavlink/mavlink_log.c rename to src/modules/systemlib/mavlink_log.c index 1921958568..27608bdbfe 100644 --- a/src/modules/mavlink/mavlink_log.c +++ b/src/modules/systemlib/mavlink_log.c @@ -46,28 +46,25 @@ #include -static FILE* text_recorder_fd = NULL; - -void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size) +__EXPORT void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size) { lb->size = size; lb->start = 0; lb->count = 0; lb->elems = (struct mavlink_logmessage *)calloc(lb->size, sizeof(struct mavlink_logmessage)); - text_recorder_fd = fopen("/fs/microsd/text_recorder.txt", "w"); } -int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb) +__EXPORT int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb) { return lb->count == (int)lb->size; } -int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb) +__EXPORT int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb) { return lb->count == 0; } -void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem) +__EXPORT void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem) { int end = (lb->start + lb->count) % lb->size; memcpy(&(lb->elems[end]), elem, sizeof(struct mavlink_logmessage)); @@ -80,19 +77,13 @@ void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_ } } -int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem) +__EXPORT int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem) { if (!mavlink_logbuffer_is_empty(lb)) { memcpy(elem, &(lb->elems[lb->start]), sizeof(struct mavlink_logmessage)); lb->start = (lb->start + 1) % lb->size; --lb->count; - if (text_recorder_fd) { - fwrite(elem->text, 1, strnlen(elem->text, 50), text_recorder_fd); - fputc("\n", text_recorder_fd); - fsync(text_recorder_fd); - } - return 0; } else { @@ -100,7 +91,7 @@ int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessa } } -void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...) +__EXPORT void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...) { va_list ap; va_start(ap, fmt); diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index b470c12271..cbf8291225 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -48,4 +48,5 @@ SRCS = err.c \ geo/geo.c \ systemlib.c \ airspeed.c \ - system_params.c + system_params.c \ + mavlink_log.c From 2bcf4385f66b2bcdb2917d2edf60d40813e207df Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 24 Jun 2013 12:39:33 +1000 Subject: [PATCH 435/486] build: use unqualified com port names on windows the qualified names were not working with current versions of python --- makefiles/upload.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/upload.mk b/makefiles/upload.mk index c55e3f1883..470ddfdf16 100644 --- a/makefiles/upload.mk +++ b/makefiles/upload.mk @@ -18,7 +18,7 @@ ifeq ($(SYSTYPE),Linux) SERIAL_PORTS ?= "/dev/ttyACM5,/dev/ttyACM4,/dev/ttyACM3,/dev/ttyACM2,/dev/ttyACM1,/dev/ttyACM0" endif ifeq ($(SERIAL_PORTS),) -SERIAL_PORTS = "\\\\.\\COM32,\\\\.\\COM31,\\\\.\\COM30,\\\\.\\COM29,\\\\.\\COM28,\\\\.\\COM27,\\\\.\\COM26,\\\\.\\COM25,\\\\.\\COM24,\\\\.\\COM23,\\\\.\\COM22,\\\\.\\COM21,\\\\.\\COM20,\\\\.\\COM19,\\\\.\\COM18,\\\\.\\COM17,\\\\.\\COM16,\\\\.\\COM15,\\\\.\\COM14,\\\\.\\COM13,\\\\.\\COM12,\\\\.\\COM11,\\\\.\\COM10,\\\\.\\COM9,\\\\.\\COM8,\\\\.\\COM7,\\\\.\\COM6,\\\\.\\COM5,\\\\.\\COM4,\\\\.\\COM3,\\\\.\\COM2,\\\\.\\COM1,\\\\.\\COM0" +SERIAL_PORTS = "COM32,COM31,COM30,COM29,COM28,COM27,COM26,COM25,COM24,COM23,COM22,COM21,COM20,COM19,COM18,COM17,COM16,COM15,COM14,COM13,COM12,COM11,COM10,COM9,COM8,COM7,COM6,COM5,COM4,COM3,COM2,COM1,COM0" endif .PHONY: all upload-$(METHOD)-$(BOARD) From f665ace38cfa4613fb911cb68f6662b15720ffea Mon Sep 17 00:00:00 2001 From: Kevin Hester Date: Sun, 11 Aug 2013 11:34:19 -1000 Subject: [PATCH 436/486] Add scripts for debugging with openocd Note: We now use the version of stm32f4x that comes with openocd 0.7.0 or later --- Debug/olimex-px4fmu-debug.cfg | 22 ++++++++++++ Debug/openocd.gdbinit | 7 ++++ Debug/px4fmu-v1-board.cfg | 6 ++++ Debug/runopenocd.sh | 1 + Debug/stm32f4x.cfg | 64 ----------------------------------- 5 files changed, 36 insertions(+), 64 deletions(-) create mode 100644 Debug/olimex-px4fmu-debug.cfg create mode 100644 Debug/openocd.gdbinit create mode 100644 Debug/px4fmu-v1-board.cfg create mode 100755 Debug/runopenocd.sh delete mode 100644 Debug/stm32f4x.cfg diff --git a/Debug/olimex-px4fmu-debug.cfg b/Debug/olimex-px4fmu-debug.cfg new file mode 100644 index 0000000000..61d70070db --- /dev/null +++ b/Debug/olimex-px4fmu-debug.cfg @@ -0,0 +1,22 @@ +# program a bootable device load on a mavstation +# To run type openocd -f mavprogram.cfg + +source [find interface/olimex-arm-usb-ocd-h.cfg] +source [find px4fmu-v1-board.cfg] + +init +halt + +# Find the flash inside this CPU +flash probe 0 + +# erase it (128 pages) then program and exit + +#flash erase_sector 0 0 127 +# stm32f1x mass_erase 0 + +# It seems that Pat's image has a start address offset of 0x1000 but the vectors need to be at zero, so fixbin.sh moves things around +#flash write_bank 0 fixed.bin 0 +#flash write_image firmware.elf +#shutdown + diff --git a/Debug/openocd.gdbinit b/Debug/openocd.gdbinit new file mode 100644 index 0000000000..4d2dc4c869 --- /dev/null +++ b/Debug/openocd.gdbinit @@ -0,0 +1,7 @@ +target remote :3333 +mon reset halt +mon poll +mon cortex_m maskisr auto +set mem inaccessible-by-default off +set print pretty +source Debug/PX4 \ No newline at end of file diff --git a/Debug/px4fmu-v1-board.cfg b/Debug/px4fmu-v1-board.cfg new file mode 100644 index 0000000000..ce1cca571f --- /dev/null +++ b/Debug/px4fmu-v1-board.cfg @@ -0,0 +1,6 @@ +# The latest defaults in OpenOCD 0.7.0 are actually prettymuch correct for the px4fmu + +# increase working area to 32KB for faster flash programming +set WORKAREASIZE 0x8000 + +source [find target/stm32f4x.cfg] diff --git a/Debug/runopenocd.sh b/Debug/runopenocd.sh new file mode 100755 index 0000000000..291c5b0f50 --- /dev/null +++ b/Debug/runopenocd.sh @@ -0,0 +1 @@ +openocd -f interface/olimex-arm-usb-ocd-h.cfg -f Debug/stm32f4x.cfg diff --git a/Debug/stm32f4x.cfg b/Debug/stm32f4x.cfg deleted file mode 100644 index 28bfcfbbbc..0000000000 --- a/Debug/stm32f4x.cfg +++ /dev/null @@ -1,64 +0,0 @@ -# script for stm32f2xxx - -if { [info exists CHIPNAME] } { - set _CHIPNAME $CHIPNAME -} else { - set _CHIPNAME stm32f4xxx -} - -if { [info exists ENDIAN] } { - set _ENDIAN $ENDIAN -} else { - set _ENDIAN little -} - -# Work-area is a space in RAM used for flash programming -# By default use 64kB -if { [info exists WORKAREASIZE] } { - set _WORKAREASIZE $WORKAREASIZE -} else { - set _WORKAREASIZE 0x10000 -} - -# JTAG speed should be <= F_CPU/6. F_CPU after reset is 8MHz, so use F_JTAG = 1MHz -# -# Since we may be running of an RC oscilator, we crank down the speed a -# bit more to be on the safe side. Perhaps superstition, but if are -# running off a crystal, we can run closer to the limit. Note -# that there can be a pretty wide band where things are more or less stable. -jtag_khz 1000 - -jtag_nsrst_delay 100 -jtag_ntrst_delay 100 - -#jtag scan chain -if { [info exists CPUTAPID ] } { - set _CPUTAPID $CPUTAPID -} else { - # See STM Document RM0033 - # Section 32.6.3 - corresponds to Cortex-M3 r2p0 - set _CPUTAPID 0x4ba00477 -} -jtag newtap $_CHIPNAME cpu -irlen 4 -ircapture 0x1 -irmask 0xf -expected-id $_CPUTAPID - -if { [info exists BSTAPID ] } { - set _BSTAPID $BSTAPID -} else { - # See STM Document RM0033 - # Section 32.6.2 - # - set _BSTAPID 0x06413041 -} -jtag newtap $_CHIPNAME bs -irlen 5 -expected-id $_BSTAPID - -set _TARGETNAME $_CHIPNAME.cpu -target create $_TARGETNAME cortex_m3 -endian $_ENDIAN -chain-position $_TARGETNAME -rtos auto - -$_TARGETNAME configure -work-area-phys 0x20000000 -work-area-size $_WORKAREASIZE -work-area-backup 0 - -set _FLASHNAME $_CHIPNAME.flash -flash bank $_FLASHNAME stm32f2x 0 0 0 0 $_TARGETNAME - -# if srst is not fitted use SYSRESETREQ to -# perform a soft reset -cortex_m3 reset_config sysresetreq From fa8f8f2a0255d743494e17120955421677e76567 Mon Sep 17 00:00:00 2001 From: Kevin Hester Date: Sun, 11 Aug 2013 11:38:00 -1000 Subject: [PATCH 437/486] add step hooks to make stepping work correctly for non isrs Conflicts: Debug/openocd.gdbinit Debug/px4fmu-v1-board.cfg --- Debug/openocd.gdbinit | 18 ++++++++++++++++-- Debug/px4fmu-v1-board.cfg | 32 ++++++++++++++++++++++++++++++++ 2 files changed, 48 insertions(+), 2 deletions(-) diff --git a/Debug/openocd.gdbinit b/Debug/openocd.gdbinit index 4d2dc4c869..92d78b58db 100644 --- a/Debug/openocd.gdbinit +++ b/Debug/openocd.gdbinit @@ -1,7 +1,21 @@ target remote :3333 -mon reset halt + +# Don't let GDB get confused while stepping +define hook-step + mon cortex_m maskisr on +end +define hookpost-step + mon cortex_m maskisr off +end + +mon init +mon stm32_init +# mon reset halt mon poll mon cortex_m maskisr auto set mem inaccessible-by-default off set print pretty -source Debug/PX4 \ No newline at end of file +source Debug/PX4 + +echo PX4 resumed, press ctrl-c to interrupt\n +continue diff --git a/Debug/px4fmu-v1-board.cfg b/Debug/px4fmu-v1-board.cfg index ce1cca571f..19b862a2d1 100644 --- a/Debug/px4fmu-v1-board.cfg +++ b/Debug/px4fmu-v1-board.cfg @@ -4,3 +4,35 @@ set WORKAREASIZE 0x8000 source [find target/stm32f4x.cfg] + +# needed for px4 +reset_config trst_only + +proc stm32_reset {} { + reset halt +# FIXME - needed to init periphs on reset +# 0x40023800 RCC base +# 0x24 RCC_APB2 0x75933 +# RCC_APB2 0 +} + +# perform init that is required on each connection to the target +proc stm32_init {} { + + # force jtag to not shutdown during sleep + #uint32_t cr = getreg32(STM32_DBGMCU_CR); + #cr |= DBGMCU_CR_STANDBY | DBGMCU_CR_STOP | DBGMCU_CR_SLEEP; + #putreg32(cr, STM32_DBGMCU_CR); + mww 0xe0042004 00000007 +} + +# if srst is not fitted use SYSRESETREQ to +# perform a soft reset +cortex_m reset_config sysresetreq + +# Let GDB directly program elf binaries +gdb_memory_map enable + +# doesn't work yet +gdb_flash_program disable + From 1c371a6cf81d3a791237d1969ce84f512670c6c9 Mon Sep 17 00:00:00 2001 From: Kevin Hester Date: Sun, 11 Aug 2013 17:17:29 -1000 Subject: [PATCH 438/486] openocd: lost change from my cherry-picking --- Debug/runopenocd.sh | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/Debug/runopenocd.sh b/Debug/runopenocd.sh index 291c5b0f50..6258fccfb6 100755 --- a/Debug/runopenocd.sh +++ b/Debug/runopenocd.sh @@ -1 +1,5 @@ -openocd -f interface/olimex-arm-usb-ocd-h.cfg -f Debug/stm32f4x.cfg +#!/bin/bash + +DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" + +openocd -f interface/olimex-arm-usb-ocd-h.cfg -f $DIR/px4fmu-v1-board.cfg From 5a8dc9c504f70b4ce1b45f91b3bdd9b7126ef0d3 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Mon, 19 Aug 2013 22:58:50 +0200 Subject: [PATCH 439/486] Added TBS script --- ROMFS/px4fmu_common/init.d/10_io_f330 | 9 +- ROMFS/px4fmu_common/init.d/15_io_tbs | 138 ++++++++++++++++++++++++++ ROMFS/px4fmu_common/init.d/rcS | 5 + 3 files changed, 150 insertions(+), 2 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/15_io_tbs diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_io_f330 index 48636292c2..0e6d3f5d54 100644 --- a/ROMFS/px4fmu_common/init.d/10_io_f330 +++ b/ROMFS/px4fmu_common/init.d/10_io_f330 @@ -1,12 +1,17 @@ #!nsh # -# Flight startup script for PX4FMU+PX4IO +# Flight startup script for PX4FMU+PX4IO on an F330 quad. # # disable USB and autostart set USB no set MODE custom +# +# Start the ORB (first app to start) +# +uorb start + # # Load default params for this platform # @@ -49,7 +54,7 @@ usleep 5000 # Start the commander (depends on orb, mavlink) # commander start - + # # Start PX4IO interface (depends on orb, commander) # diff --git a/ROMFS/px4fmu_common/init.d/15_io_tbs b/ROMFS/px4fmu_common/init.d/15_io_tbs new file mode 100644 index 0000000000..237bb4267c --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/15_io_tbs @@ -0,0 +1,138 @@ +#!nsh +# +# Flight startup script for PX4FMU+PX4IO on a Team Blacksheep Discovery quad +# with stock DJI ESCs, motors and props. +# + +# disable USB and autostart +set USB no +set MODE custom + +# +# Start the ORB (first app to start) +# +uorb start + +# +# Load microSD params +# +echo "[init] loading microSD params" +param select /fs/microsd/params +if [ -f /fs/microsd/params ] +then + param load /fs/microsd/params +fi + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set SYS_AUTOCONFIG 0 + + param set MC_ATTRATE_D 0.006 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_P 0.17 + param set MC_ATT_D 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_P 5.0 + param set MC_RCLOSS_THR 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWPOS_I 0.15 + param set MC_YAWPOS_P 0.5 + param set MC_YAWRATE_D 0.0 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_P 0.2 + + param save /fs/microsd/params +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor +# see https://pixhawk.ethz.ch/mavlink/ +# +param set MAV_TYPE 2 + +# +# Start MAVLink (depends on orb) +# +mavlink start +usleep 5000 + +# +# Start the commander (depends on orb, mavlink) +# +commander start + +# +# Start PX4IO interface (depends on orb, commander) +# +px4io start +pwm -u 400 -m 0xff + +# +# Allow PX4IO to recover from midair restarts. +# This is very unlikely, but quite safe and robust. +px4io recovery + +# +# This sets a PWM right after startup (regardless of safety button) +# +px4io idle 900 900 900 900 + +# +# The values are for spinning motors when armed using DJI ESCs +# +px4io min 1180 1180 1180 1180 + +# +# Upper limits could be higher, this is on the safe side +# +px4io max 1800 1800 1800 1800 + +# +# Load the mixer for a quad with wide arms +# +mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix + +# +# Start the sensors (depends on orb, px4io) +# +sh /etc/init.d/rc.sensors + +# +# Start GPS interface (depends on orb) +# +gps start + +# +# Start the attitude estimator (depends on orb) +# +attitude_estimator_ekf start + +# +# Start the controllers (depends on orb) +# +multirotor_att_control start + +# +# Disable px4io topic limiting and start logging +# +if [ $BOARD == fmuv1 ] +then + px4io limit 200 + sdlog2 start -r 50 -a -b 16 + if blinkm start + then + blinkm systemstate + fi +else + px4io limit 400 + sdlog2 start -r 200 -a -b 16 + if rgbled start + then + #rgbled systemstate + fi +fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 7f04095194..650224cf17 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -150,6 +150,11 @@ then sh /etc/init.d/10_io_f330 fi +if param compare SYS_AUTOSTART 15 +then + sh /etc/init.d/15_io_tbs +fi + if param compare SYS_AUTOSTART 30 then sh /etc/init.d/30_io_camflyer From fab110d21f147e5064ff140aadac649017fa466e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 Aug 2013 18:13:01 +0200 Subject: [PATCH 440/486] Moved math library to library dir, improved sensor-level HIL, cleaned up geo / conversion libs --- ROMFS/px4fmu_common/init.d/rc.hil | 12 +-- makefiles/setup.mk | 4 +- src/drivers/hott/messages.cpp | 2 +- src/drivers/lsm303d/lsm303d.cpp | 17 +-- src/drivers/mpu6000/mpu6000.cpp | 10 +- src/examples/fixedwing_control/main.c | 2 +- src/{modules/systemlib => lib}/geo/geo.c | 2 +- src/{modules/systemlib => lib}/geo/geo.h | 0 src/lib/geo/module.mk | 38 +++++++ .../CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h | 0 .../CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h | 0 .../mathlib/CMSIS/Include/arm_common_tables.h | 0 .../mathlib/CMSIS/Include/arm_const_structs.h | 0 .../mathlib/CMSIS/Include/arm_math.h | 0 .../mathlib/CMSIS/Include/core_cm3.h | 0 .../mathlib/CMSIS/Include/core_cm4.h | 0 .../mathlib/CMSIS/Include/core_cm4_simd.h | 0 .../mathlib/CMSIS/Include/core_cmFunc.h | 0 .../mathlib/CMSIS/Include/core_cmInstr.h | 0 .../mathlib/CMSIS/libarm_cortexM3l_math.a | Bin .../mathlib/CMSIS/libarm_cortexM4l_math.a | Bin .../mathlib/CMSIS/libarm_cortexM4lf_math.a | Bin src/{modules => lib}/mathlib/CMSIS/library.mk | 0 .../mathlib/CMSIS/license.txt | 0 src/{modules => lib}/mathlib/math/Dcm.cpp | 0 src/{modules => lib}/mathlib/math/Dcm.hpp | 0 .../mathlib/math/EulerAngles.cpp | 0 .../mathlib/math/EulerAngles.hpp | 0 src/{modules => lib}/mathlib/math/Limits.cpp | 0 src/{modules => lib}/mathlib/math/Limits.hpp | 0 src/{modules => lib}/mathlib/math/Matrix.cpp | 0 src/{modules => lib}/mathlib/math/Matrix.hpp | 0 .../mathlib/math/Quaternion.cpp | 0 .../mathlib/math/Quaternion.hpp | 0 src/{modules => lib}/mathlib/math/Vector.cpp | 0 src/{modules => lib}/mathlib/math/Vector.hpp | 0 .../mathlib/math/Vector2f.cpp | 0 .../mathlib/math/Vector2f.hpp | 0 src/{modules => lib}/mathlib/math/Vector3.cpp | 0 src/{modules => lib}/mathlib/math/Vector3.hpp | 0 .../mathlib/math/arm/Matrix.cpp | 0 .../mathlib/math/arm/Matrix.hpp | 0 .../mathlib/math/arm/Vector.cpp | 0 .../mathlib/math/arm/Vector.hpp | 0 .../mathlib/math/filter/LowPassFilter2p.cpp | 0 .../mathlib/math/filter/LowPassFilter2p.hpp | 0 .../mathlib/math/filter/module.mk | 0 .../mathlib/math/generic/Matrix.cpp | 0 .../mathlib/math/generic/Matrix.hpp | 0 .../mathlib/math/generic/Vector.cpp | 0 .../mathlib/math/generic/Vector.hpp | 0 .../mathlib/math/nasa_rotation_def.pdf | Bin .../mathlib/math/test/test.cpp | 0 .../mathlib/math/test/test.hpp | 0 .../mathlib/math/test_math.sce | 0 src/{modules => lib}/mathlib/mathlib.h | 0 src/{modules => lib}/mathlib/module.mk | 0 .../commander/accelerometer_calibration.cpp | 2 +- src/modules/controllib/uorb/blocks.hpp | 2 +- src/modules/mavlink/mavlink.c | 3 + src/modules/mavlink/mavlink_receiver.cpp | 88 +++++++++++++++- src/modules/mavlink/orb_listener.c | 50 +++++++-- src/modules/mavlink/orb_topics.h | 1 + .../position_estimator_inav_main.c | 3 +- src/modules/systemlib/airspeed.c | 16 +-- src/modules/systemlib/airspeed.h | 8 ++ src/modules/systemlib/conversions.c | 97 ------------------ src/modules/systemlib/conversions.h | 29 ------ src/modules/systemlib/module.mk | 1 - 69 files changed, 216 insertions(+), 171 deletions(-) rename src/{modules/systemlib => lib}/geo/geo.c (99%) rename src/{modules/systemlib => lib}/geo/geo.h (100%) create mode 100644 src/lib/geo/module.mk rename src/{modules => lib}/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h (100%) rename src/{modules => lib}/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h (100%) rename src/{modules => lib}/mathlib/CMSIS/Include/arm_common_tables.h (100%) rename src/{modules => lib}/mathlib/CMSIS/Include/arm_const_structs.h (100%) rename src/{modules => lib}/mathlib/CMSIS/Include/arm_math.h (100%) rename src/{modules => lib}/mathlib/CMSIS/Include/core_cm3.h (100%) rename src/{modules => lib}/mathlib/CMSIS/Include/core_cm4.h (100%) rename src/{modules => lib}/mathlib/CMSIS/Include/core_cm4_simd.h (100%) rename src/{modules => lib}/mathlib/CMSIS/Include/core_cmFunc.h (100%) rename src/{modules => lib}/mathlib/CMSIS/Include/core_cmInstr.h (100%) rename src/{modules => lib}/mathlib/CMSIS/libarm_cortexM3l_math.a (100%) rename src/{modules => lib}/mathlib/CMSIS/libarm_cortexM4l_math.a (100%) rename src/{modules => lib}/mathlib/CMSIS/libarm_cortexM4lf_math.a (100%) rename src/{modules => lib}/mathlib/CMSIS/library.mk (100%) rename src/{modules => lib}/mathlib/CMSIS/license.txt (100%) rename src/{modules => lib}/mathlib/math/Dcm.cpp (100%) rename src/{modules => lib}/mathlib/math/Dcm.hpp (100%) rename src/{modules => lib}/mathlib/math/EulerAngles.cpp (100%) rename src/{modules => lib}/mathlib/math/EulerAngles.hpp (100%) rename src/{modules => lib}/mathlib/math/Limits.cpp (100%) rename src/{modules => lib}/mathlib/math/Limits.hpp (100%) rename src/{modules => lib}/mathlib/math/Matrix.cpp (100%) rename src/{modules => lib}/mathlib/math/Matrix.hpp (100%) rename src/{modules => lib}/mathlib/math/Quaternion.cpp (100%) rename src/{modules => lib}/mathlib/math/Quaternion.hpp (100%) rename src/{modules => lib}/mathlib/math/Vector.cpp (100%) rename src/{modules => lib}/mathlib/math/Vector.hpp (100%) rename src/{modules => lib}/mathlib/math/Vector2f.cpp (100%) rename src/{modules => lib}/mathlib/math/Vector2f.hpp (100%) rename src/{modules => lib}/mathlib/math/Vector3.cpp (100%) rename src/{modules => lib}/mathlib/math/Vector3.hpp (100%) rename src/{modules => lib}/mathlib/math/arm/Matrix.cpp (100%) rename src/{modules => lib}/mathlib/math/arm/Matrix.hpp (100%) rename src/{modules => lib}/mathlib/math/arm/Vector.cpp (100%) rename src/{modules => lib}/mathlib/math/arm/Vector.hpp (100%) rename src/{modules => lib}/mathlib/math/filter/LowPassFilter2p.cpp (100%) rename src/{modules => lib}/mathlib/math/filter/LowPassFilter2p.hpp (100%) rename src/{modules => lib}/mathlib/math/filter/module.mk (100%) rename src/{modules => lib}/mathlib/math/generic/Matrix.cpp (100%) rename src/{modules => lib}/mathlib/math/generic/Matrix.hpp (100%) rename src/{modules => lib}/mathlib/math/generic/Vector.cpp (100%) rename src/{modules => lib}/mathlib/math/generic/Vector.hpp (100%) rename src/{modules => lib}/mathlib/math/nasa_rotation_def.pdf (100%) rename src/{modules => lib}/mathlib/math/test/test.cpp (100%) rename src/{modules => lib}/mathlib/math/test/test.hpp (100%) rename src/{modules => lib}/mathlib/math/test_math.sce (100%) rename src/{modules => lib}/mathlib/mathlib.h (100%) rename src/{modules => lib}/mathlib/module.mk (100%) diff --git a/ROMFS/px4fmu_common/init.d/rc.hil b/ROMFS/px4fmu_common/init.d/rc.hil index 3517a5bd82..a843b7ffbc 100644 --- a/ROMFS/px4fmu_common/init.d/rc.hil +++ b/ROMFS/px4fmu_common/init.d/rc.hil @@ -8,7 +8,7 @@ echo "[HIL] starting.." uorb start # Tell MAVLink that this link is "fast" -mavlink start -b 230400 -d /dev/console +mavlink start -b 230400 -d /dev/ttyS1 # Create a fake HIL /dev/pwm_output interface hil mode_pwm @@ -38,13 +38,13 @@ commander start # # Check if we got an IO # -if [ px4io start ] +if px4io start then echo "IO started" else fmu mode_serial echo "FMU started" -end +fi # # Start the sensors (depends on orb, px4io) @@ -60,9 +60,7 @@ att_pos_estimator_ekf start # Load mixer and start controllers (depends on px4io) # mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix -fixedwing_backside start +fw_pos_control_l1 start +fw_att_control start echo "[HIL] setup done, running" - -# Exit shell to make it available to MAVLink -exit diff --git a/makefiles/setup.mk b/makefiles/setup.mk index 168e41a5cb..42f9a8a7fe 100644 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -43,6 +43,7 @@ # export PX4_INCLUDE_DIR = $(abspath $(PX4_BASE)/src/include)/ export PX4_MODULE_SRC = $(abspath $(PX4_BASE)/src)/ +export PX4_LIB_DIR = $(abspath $(PX4_BASE)/src/lib)/ export PX4_MK_DIR = $(abspath $(PX4_BASE)/makefiles)/ export NUTTX_SRC = $(abspath $(PX4_BASE)/NuttX/nuttx)/ export NUTTX_APP_SRC = $(abspath $(PX4_BASE)/NuttX/apps)/ @@ -57,7 +58,8 @@ export ARCHIVE_DIR = $(abspath $(PX4_BASE)/Archives)/ # export INCLUDE_DIRS := $(PX4_MODULE_SRC) \ $(PX4_MODULE_SRC)/modules/ \ - $(PX4_INCLUDE_DIR) + $(PX4_INCLUDE_DIR) \ + $(PX4_LIB_DIR) # # Tools diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp index 57c2563394..bb8d45beab 100644 --- a/src/drivers/hott/messages.cpp +++ b/src/drivers/hott/messages.cpp @@ -42,7 +42,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index efe7cf8eb6..cf5f8d94ce 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -54,7 +54,6 @@ #include #include -#include #include #include @@ -178,6 +177,8 @@ static const int ERROR = -1; #define LSM303D_MAG_DEFAULT_RANGE_GA 2 #define LSM303D_MAG_DEFAULT_RATE 100 +#define LSM303D_ONE_G 9.80665f + extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); } @@ -778,7 +779,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) case ACCELIOCGRANGE: /* convert to m/s^2 and return rounded in G */ - return (unsigned long)((_accel_range_m_s2)/CONSTANTS_ONE_G + 0.5f); + return (unsigned long)((_accel_range_m_s2)/LSM303D_ONE_G + 0.5f); case ACCELIOCGSCALE: /* copy scale out */ @@ -1015,27 +1016,27 @@ LSM303D::accel_set_range(unsigned max_g) max_g = 16; if (max_g <= 2) { - _accel_range_m_s2 = 2.0f*CONSTANTS_ONE_G; + _accel_range_m_s2 = 2.0f*LSM303D_ONE_G; setbits |= REG2_FULL_SCALE_2G_A; new_scale_g_digit = 0.061e-3f; } else if (max_g <= 4) { - _accel_range_m_s2 = 4.0f*CONSTANTS_ONE_G; + _accel_range_m_s2 = 4.0f*LSM303D_ONE_G; setbits |= REG2_FULL_SCALE_4G_A; new_scale_g_digit = 0.122e-3f; } else if (max_g <= 6) { - _accel_range_m_s2 = 6.0f*CONSTANTS_ONE_G; + _accel_range_m_s2 = 6.0f*LSM303D_ONE_G; setbits |= REG2_FULL_SCALE_6G_A; new_scale_g_digit = 0.183e-3f; } else if (max_g <= 8) { - _accel_range_m_s2 = 8.0f*CONSTANTS_ONE_G; + _accel_range_m_s2 = 8.0f*LSM303D_ONE_G; setbits |= REG2_FULL_SCALE_8G_A; new_scale_g_digit = 0.244e-3f; } else if (max_g <= 16) { - _accel_range_m_s2 = 16.0f*CONSTANTS_ONE_G; + _accel_range_m_s2 = 16.0f*LSM303D_ONE_G; setbits |= REG2_FULL_SCALE_16G_A; new_scale_g_digit = 0.732e-3f; @@ -1043,7 +1044,7 @@ LSM303D::accel_set_range(unsigned max_g) return -EINVAL; } - _accel_range_scale = new_scale_g_digit * CONSTANTS_ONE_G; + _accel_range_scale = new_scale_g_digit * LSM303D_ONE_G; modify_reg(ADDR_CTRL_REG2, clearbits, setbits); diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 0e65923db7..14f8f44b82 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -159,6 +159,8 @@ #define MPU6000_DEFAULT_ONCHIP_FILTER_FREQ 42 +#define MPU6000_ONE_G 9.80665f + class MPU6000_gyro; class MPU6000 : public device::SPI @@ -543,8 +545,8 @@ void MPU6000::reset() // Correct accel scale factors of 4096 LSB/g // scale to m/s^2 ( 1g = 9.81 m/s^2) - _accel_range_scale = (CONSTANTS_ONE_G / 4096.0f); - _accel_range_m_s2 = 8.0f * CONSTANTS_ONE_G; + _accel_range_scale = (MPU6000_ONE_G / 4096.0f); + _accel_range_m_s2 = 8.0f * MPU6000_ONE_G; usleep(1000); @@ -906,7 +908,7 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) // _accel_range_m_s2 = 8.0f * 9.81f; return -EINVAL; case ACCELIOCGRANGE: - return (unsigned long)((_accel_range_m_s2)/CONSTANTS_ONE_G + 0.5f); + return (unsigned long)((_accel_range_m_s2)/MPU6000_ONE_G + 0.5f); case ACCELIOCSELFTEST: return accel_self_test(); @@ -1409,7 +1411,7 @@ test() warnx("acc y: \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw); warnx("acc z: \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw); warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2, - (double)(a_report.range_m_s2 / CONSTANTS_ONE_G)); + (double)(a_report.range_m_s2 / MPU6000_ONE_G)); /* do a simple demand read */ sz = read(fd_gyro, &g_report, sizeof(g_report)); diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c index e29f768775..b286e00077 100644 --- a/src/examples/fixedwing_control/main.c +++ b/src/examples/fixedwing_control/main.c @@ -66,7 +66,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/src/modules/systemlib/geo/geo.c b/src/lib/geo/geo.c similarity index 99% rename from src/modules/systemlib/geo/geo.c rename to src/lib/geo/geo.c index 6463e6489e..63792dda52 100644 --- a/src/modules/systemlib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -44,7 +44,7 @@ * @author Lorenz Meier */ -#include +#include #include #include #include diff --git a/src/modules/systemlib/geo/geo.h b/src/lib/geo/geo.h similarity index 100% rename from src/modules/systemlib/geo/geo.h rename to src/lib/geo/geo.h diff --git a/src/lib/geo/module.mk b/src/lib/geo/module.mk new file mode 100644 index 0000000000..30a2dc99fd --- /dev/null +++ b/src/lib/geo/module.mk @@ -0,0 +1,38 @@ +############################################################################ +# +# Copyright (C) 2012 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. +# +############################################################################ + +# +# Geo library +# + +SRCS = geo.c diff --git a/src/modules/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h b/src/lib/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h similarity index 100% rename from src/modules/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h rename to src/lib/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h diff --git a/src/modules/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h b/src/lib/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h similarity index 100% rename from src/modules/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h rename to src/lib/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h diff --git a/src/modules/mathlib/CMSIS/Include/arm_common_tables.h b/src/lib/mathlib/CMSIS/Include/arm_common_tables.h similarity index 100% rename from src/modules/mathlib/CMSIS/Include/arm_common_tables.h rename to src/lib/mathlib/CMSIS/Include/arm_common_tables.h diff --git a/src/modules/mathlib/CMSIS/Include/arm_const_structs.h b/src/lib/mathlib/CMSIS/Include/arm_const_structs.h similarity index 100% rename from src/modules/mathlib/CMSIS/Include/arm_const_structs.h rename to src/lib/mathlib/CMSIS/Include/arm_const_structs.h diff --git a/src/modules/mathlib/CMSIS/Include/arm_math.h b/src/lib/mathlib/CMSIS/Include/arm_math.h similarity index 100% rename from src/modules/mathlib/CMSIS/Include/arm_math.h rename to src/lib/mathlib/CMSIS/Include/arm_math.h diff --git a/src/modules/mathlib/CMSIS/Include/core_cm3.h b/src/lib/mathlib/CMSIS/Include/core_cm3.h similarity index 100% rename from src/modules/mathlib/CMSIS/Include/core_cm3.h rename to src/lib/mathlib/CMSIS/Include/core_cm3.h diff --git a/src/modules/mathlib/CMSIS/Include/core_cm4.h b/src/lib/mathlib/CMSIS/Include/core_cm4.h similarity index 100% rename from src/modules/mathlib/CMSIS/Include/core_cm4.h rename to src/lib/mathlib/CMSIS/Include/core_cm4.h diff --git a/src/modules/mathlib/CMSIS/Include/core_cm4_simd.h b/src/lib/mathlib/CMSIS/Include/core_cm4_simd.h similarity index 100% rename from src/modules/mathlib/CMSIS/Include/core_cm4_simd.h rename to src/lib/mathlib/CMSIS/Include/core_cm4_simd.h diff --git a/src/modules/mathlib/CMSIS/Include/core_cmFunc.h b/src/lib/mathlib/CMSIS/Include/core_cmFunc.h similarity index 100% rename from src/modules/mathlib/CMSIS/Include/core_cmFunc.h rename to src/lib/mathlib/CMSIS/Include/core_cmFunc.h diff --git a/src/modules/mathlib/CMSIS/Include/core_cmInstr.h b/src/lib/mathlib/CMSIS/Include/core_cmInstr.h similarity index 100% rename from src/modules/mathlib/CMSIS/Include/core_cmInstr.h rename to src/lib/mathlib/CMSIS/Include/core_cmInstr.h diff --git a/src/modules/mathlib/CMSIS/libarm_cortexM3l_math.a b/src/lib/mathlib/CMSIS/libarm_cortexM3l_math.a similarity index 100% rename from src/modules/mathlib/CMSIS/libarm_cortexM3l_math.a rename to src/lib/mathlib/CMSIS/libarm_cortexM3l_math.a diff --git a/src/modules/mathlib/CMSIS/libarm_cortexM4l_math.a b/src/lib/mathlib/CMSIS/libarm_cortexM4l_math.a similarity index 100% rename from src/modules/mathlib/CMSIS/libarm_cortexM4l_math.a rename to src/lib/mathlib/CMSIS/libarm_cortexM4l_math.a diff --git a/src/modules/mathlib/CMSIS/libarm_cortexM4lf_math.a b/src/lib/mathlib/CMSIS/libarm_cortexM4lf_math.a similarity index 100% rename from src/modules/mathlib/CMSIS/libarm_cortexM4lf_math.a rename to src/lib/mathlib/CMSIS/libarm_cortexM4lf_math.a diff --git a/src/modules/mathlib/CMSIS/library.mk b/src/lib/mathlib/CMSIS/library.mk similarity index 100% rename from src/modules/mathlib/CMSIS/library.mk rename to src/lib/mathlib/CMSIS/library.mk diff --git a/src/modules/mathlib/CMSIS/license.txt b/src/lib/mathlib/CMSIS/license.txt similarity index 100% rename from src/modules/mathlib/CMSIS/license.txt rename to src/lib/mathlib/CMSIS/license.txt diff --git a/src/modules/mathlib/math/Dcm.cpp b/src/lib/mathlib/math/Dcm.cpp similarity index 100% rename from src/modules/mathlib/math/Dcm.cpp rename to src/lib/mathlib/math/Dcm.cpp diff --git a/src/modules/mathlib/math/Dcm.hpp b/src/lib/mathlib/math/Dcm.hpp similarity index 100% rename from src/modules/mathlib/math/Dcm.hpp rename to src/lib/mathlib/math/Dcm.hpp diff --git a/src/modules/mathlib/math/EulerAngles.cpp b/src/lib/mathlib/math/EulerAngles.cpp similarity index 100% rename from src/modules/mathlib/math/EulerAngles.cpp rename to src/lib/mathlib/math/EulerAngles.cpp diff --git a/src/modules/mathlib/math/EulerAngles.hpp b/src/lib/mathlib/math/EulerAngles.hpp similarity index 100% rename from src/modules/mathlib/math/EulerAngles.hpp rename to src/lib/mathlib/math/EulerAngles.hpp diff --git a/src/modules/mathlib/math/Limits.cpp b/src/lib/mathlib/math/Limits.cpp similarity index 100% rename from src/modules/mathlib/math/Limits.cpp rename to src/lib/mathlib/math/Limits.cpp diff --git a/src/modules/mathlib/math/Limits.hpp b/src/lib/mathlib/math/Limits.hpp similarity index 100% rename from src/modules/mathlib/math/Limits.hpp rename to src/lib/mathlib/math/Limits.hpp diff --git a/src/modules/mathlib/math/Matrix.cpp b/src/lib/mathlib/math/Matrix.cpp similarity index 100% rename from src/modules/mathlib/math/Matrix.cpp rename to src/lib/mathlib/math/Matrix.cpp diff --git a/src/modules/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp similarity index 100% rename from src/modules/mathlib/math/Matrix.hpp rename to src/lib/mathlib/math/Matrix.hpp diff --git a/src/modules/mathlib/math/Quaternion.cpp b/src/lib/mathlib/math/Quaternion.cpp similarity index 100% rename from src/modules/mathlib/math/Quaternion.cpp rename to src/lib/mathlib/math/Quaternion.cpp diff --git a/src/modules/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp similarity index 100% rename from src/modules/mathlib/math/Quaternion.hpp rename to src/lib/mathlib/math/Quaternion.hpp diff --git a/src/modules/mathlib/math/Vector.cpp b/src/lib/mathlib/math/Vector.cpp similarity index 100% rename from src/modules/mathlib/math/Vector.cpp rename to src/lib/mathlib/math/Vector.cpp diff --git a/src/modules/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp similarity index 100% rename from src/modules/mathlib/math/Vector.hpp rename to src/lib/mathlib/math/Vector.hpp diff --git a/src/modules/mathlib/math/Vector2f.cpp b/src/lib/mathlib/math/Vector2f.cpp similarity index 100% rename from src/modules/mathlib/math/Vector2f.cpp rename to src/lib/mathlib/math/Vector2f.cpp diff --git a/src/modules/mathlib/math/Vector2f.hpp b/src/lib/mathlib/math/Vector2f.hpp similarity index 100% rename from src/modules/mathlib/math/Vector2f.hpp rename to src/lib/mathlib/math/Vector2f.hpp diff --git a/src/modules/mathlib/math/Vector3.cpp b/src/lib/mathlib/math/Vector3.cpp similarity index 100% rename from src/modules/mathlib/math/Vector3.cpp rename to src/lib/mathlib/math/Vector3.cpp diff --git a/src/modules/mathlib/math/Vector3.hpp b/src/lib/mathlib/math/Vector3.hpp similarity index 100% rename from src/modules/mathlib/math/Vector3.hpp rename to src/lib/mathlib/math/Vector3.hpp diff --git a/src/modules/mathlib/math/arm/Matrix.cpp b/src/lib/mathlib/math/arm/Matrix.cpp similarity index 100% rename from src/modules/mathlib/math/arm/Matrix.cpp rename to src/lib/mathlib/math/arm/Matrix.cpp diff --git a/src/modules/mathlib/math/arm/Matrix.hpp b/src/lib/mathlib/math/arm/Matrix.hpp similarity index 100% rename from src/modules/mathlib/math/arm/Matrix.hpp rename to src/lib/mathlib/math/arm/Matrix.hpp diff --git a/src/modules/mathlib/math/arm/Vector.cpp b/src/lib/mathlib/math/arm/Vector.cpp similarity index 100% rename from src/modules/mathlib/math/arm/Vector.cpp rename to src/lib/mathlib/math/arm/Vector.cpp diff --git a/src/modules/mathlib/math/arm/Vector.hpp b/src/lib/mathlib/math/arm/Vector.hpp similarity index 100% rename from src/modules/mathlib/math/arm/Vector.hpp rename to src/lib/mathlib/math/arm/Vector.hpp diff --git a/src/modules/mathlib/math/filter/LowPassFilter2p.cpp b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp similarity index 100% rename from src/modules/mathlib/math/filter/LowPassFilter2p.cpp rename to src/lib/mathlib/math/filter/LowPassFilter2p.cpp diff --git a/src/modules/mathlib/math/filter/LowPassFilter2p.hpp b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp similarity index 100% rename from src/modules/mathlib/math/filter/LowPassFilter2p.hpp rename to src/lib/mathlib/math/filter/LowPassFilter2p.hpp diff --git a/src/modules/mathlib/math/filter/module.mk b/src/lib/mathlib/math/filter/module.mk similarity index 100% rename from src/modules/mathlib/math/filter/module.mk rename to src/lib/mathlib/math/filter/module.mk diff --git a/src/modules/mathlib/math/generic/Matrix.cpp b/src/lib/mathlib/math/generic/Matrix.cpp similarity index 100% rename from src/modules/mathlib/math/generic/Matrix.cpp rename to src/lib/mathlib/math/generic/Matrix.cpp diff --git a/src/modules/mathlib/math/generic/Matrix.hpp b/src/lib/mathlib/math/generic/Matrix.hpp similarity index 100% rename from src/modules/mathlib/math/generic/Matrix.hpp rename to src/lib/mathlib/math/generic/Matrix.hpp diff --git a/src/modules/mathlib/math/generic/Vector.cpp b/src/lib/mathlib/math/generic/Vector.cpp similarity index 100% rename from src/modules/mathlib/math/generic/Vector.cpp rename to src/lib/mathlib/math/generic/Vector.cpp diff --git a/src/modules/mathlib/math/generic/Vector.hpp b/src/lib/mathlib/math/generic/Vector.hpp similarity index 100% rename from src/modules/mathlib/math/generic/Vector.hpp rename to src/lib/mathlib/math/generic/Vector.hpp diff --git a/src/modules/mathlib/math/nasa_rotation_def.pdf b/src/lib/mathlib/math/nasa_rotation_def.pdf similarity index 100% rename from src/modules/mathlib/math/nasa_rotation_def.pdf rename to src/lib/mathlib/math/nasa_rotation_def.pdf diff --git a/src/modules/mathlib/math/test/test.cpp b/src/lib/mathlib/math/test/test.cpp similarity index 100% rename from src/modules/mathlib/math/test/test.cpp rename to src/lib/mathlib/math/test/test.cpp diff --git a/src/modules/mathlib/math/test/test.hpp b/src/lib/mathlib/math/test/test.hpp similarity index 100% rename from src/modules/mathlib/math/test/test.hpp rename to src/lib/mathlib/math/test/test.hpp diff --git a/src/modules/mathlib/math/test_math.sce b/src/lib/mathlib/math/test_math.sce similarity index 100% rename from src/modules/mathlib/math/test_math.sce rename to src/lib/mathlib/math/test_math.sce diff --git a/src/modules/mathlib/mathlib.h b/src/lib/mathlib/mathlib.h similarity index 100% rename from src/modules/mathlib/mathlib.h rename to src/lib/mathlib/mathlib.h diff --git a/src/modules/mathlib/module.mk b/src/lib/mathlib/module.mk similarity index 100% rename from src/modules/mathlib/module.mk rename to src/lib/mathlib/module.mk diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index b6217a414f..ddd2e23d95 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -116,7 +116,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp index 9c0720aa5e..46dc1bec25 100644 --- a/src/modules/controllib/uorb/blocks.hpp +++ b/src/modules/controllib/uorb/blocks.hpp @@ -58,7 +58,7 @@ #include extern "C" { -#include +#include } #include "../blocks.hpp" diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 3d34346704..f39bbaa729 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -279,6 +279,9 @@ int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_m orb_set_interval(subs->act_2_sub, min_interval); orb_set_interval(subs->act_3_sub, min_interval); orb_set_interval(subs->actuators_sub, min_interval); + orb_set_interval(subs->actuators_effective_sub, min_interval); + orb_set_interval(subs->spa_sub, min_interval); + orb_set_interval(mavlink_subs.rates_setpoint_sub, min_interval); break; case MAVLINK_MSG_ID_MANUAL_CONTROL: diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 01bbabd46d..28f7af33cb 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -50,6 +50,10 @@ #include #include #include +#include +#include +#include +#include #include #include #include @@ -101,6 +105,10 @@ static orb_advert_t pub_hil_global_pos = -1; static orb_advert_t pub_hil_attitude = -1; static orb_advert_t pub_hil_gps = -1; static orb_advert_t pub_hil_sensors = -1; +static orb_advert_t pub_hil_gyro = -1; +static orb_advert_t pub_hil_accel = -1; +static orb_advert_t pub_hil_mag = -1; +static orb_advert_t pub_hil_baro = -1; static orb_advert_t pub_hil_airspeed = -1; static orb_advert_t cmd_pub = -1; @@ -412,12 +420,12 @@ handle_message(mavlink_message_t *msg) /* airspeed from differential pressure, ambient pressure and temp */ struct airspeed_s airspeed; - airspeed.timestamp = hrt_absolute_time(); float ias = calc_indicated_airspeed(imu.diff_pressure); // XXX need to fix this float tas = ias; + airspeed.timestamp = hrt_absolute_time(); airspeed.indicated_airspeed_m_s = ias; airspeed.true_airspeed_m_s = tas; @@ -428,7 +436,67 @@ handle_message(mavlink_message_t *msg) } //warnx("SENSOR: IAS: %6.2f TAS: %6.2f", airspeed.indicated_airspeed_m_s, airspeed.true_airspeed_m_s); - /* publish */ + /* individual sensor publications */ + struct gyro_report gyro; + gyro.x_raw = imu.xgyro / mrad2rad; + gyro.y_raw = imu.ygyro / mrad2rad; + gyro.z_raw = imu.zgyro / mrad2rad; + gyro.x = imu.xgyro; + gyro.y = imu.ygyro; + gyro.z = imu.zgyro; + gyro.temperature = imu.temperature; + gyro.timestamp = hrt_absolute_time(); + + if (pub_hil_gyro < 0) { + pub_hil_gyro = orb_advertise(ORB_ID(sensor_gyro), &gyro); + } else { + orb_publish(ORB_ID(sensor_gyro), pub_hil_gyro, &gyro); + } + + struct accel_report accel; + accel.x_raw = imu.xacc / mg2ms2; + accel.y_raw = imu.yacc / mg2ms2; + accel.z_raw = imu.zacc / mg2ms2; + accel.x = imu.xacc; + accel.y = imu.yacc; + accel.z = imu.zacc; + accel.temperature = imu.temperature; + accel.timestamp = hrt_absolute_time(); + + if (pub_hil_accel < 0) { + pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel); + } else { + orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel); + } + + struct mag_report mag; + mag.x_raw = imu.xmag / mga2ga; + mag.y_raw = imu.ymag / mga2ga; + mag.z_raw = imu.zmag / mga2ga; + mag.x = imu.xmag; + mag.y = imu.ymag; + mag.z = imu.zmag; + mag.timestamp = hrt_absolute_time(); + + if (pub_hil_mag < 0) { + pub_hil_mag = orb_advertise(ORB_ID(sensor_mag), &mag); + } else { + orb_publish(ORB_ID(sensor_mag), pub_hil_mag, &mag); + } + + struct baro_report baro; + baro.pressure = imu.abs_pressure; + baro.altitude = imu.pressure_alt; + baro.temperature = imu.temperature; + baro.timestamp = hrt_absolute_time(); + + if (pub_hil_baro < 0) { + pub_hil_baro = orb_advertise(ORB_ID(sensor_baro), &baro); + } else { + orb_publish(ORB_ID(sensor_baro), pub_hil_baro, &baro); + } + + /* publish combined sensor topic */ if (pub_hil_sensors > 0) { orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors); } else { @@ -552,6 +620,22 @@ handle_message(mavlink_message_t *msg) } else { pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); } + + struct accel_report accel; + accel.x_raw = hil_state.xacc / 9.81f * 1e3f; + accel.y_raw = hil_state.yacc / 9.81f * 1e3f; + accel.z_raw = hil_state.zacc / 9.81f * 1e3f; + accel.x = hil_state.xacc; + accel.y = hil_state.yacc; + accel.z = hil_state.zacc; + accel.temperature = 25.0f; + accel.timestamp = hrt_absolute_time(); + + if (pub_hil_accel < 0) { + pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel); + } else { + orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel); + } } if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) { diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 2a260861d3..c711b18032 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -71,7 +71,8 @@ struct vehicle_status_s v_status; struct rc_channels_s rc; struct rc_input_values rc_raw; struct actuator_armed_s armed; -struct actuator_controls_effective_s actuators_0; +struct actuator_controls_effective_s actuators_effective_0; +struct actuator_controls_s actuators_0; struct vehicle_attitude_s att; struct mavlink_subscriptions mavlink_subs; @@ -112,6 +113,7 @@ static void l_attitude_setpoint(const struct listener *l); static void l_actuator_outputs(const struct listener *l); static void l_actuator_armed(const struct listener *l); static void l_manual_control_setpoint(const struct listener *l); +static void l_vehicle_attitude_controls_effective(const struct listener *l); static void l_vehicle_attitude_controls(const struct listener *l); static void l_debug_key_value(const struct listener *l); static void l_optical_flow(const struct listener *l); @@ -138,6 +140,7 @@ static const struct listener listeners[] = { {l_actuator_armed, &mavlink_subs.armed_sub, 0}, {l_manual_control_setpoint, &mavlink_subs.man_control_sp_sub, 0}, {l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0}, + {l_vehicle_attitude_controls_effective, &mavlink_subs.actuators_effective_sub, 0}, {l_debug_key_value, &mavlink_subs.debug_key_value, 0}, {l_optical_flow, &mavlink_subs.optical_flow, 0}, {l_vehicle_rates_setpoint, &mavlink_subs.rates_setpoint_sub, 0}, @@ -567,28 +570,54 @@ l_manual_control_setpoint(const struct listener *l) } void -l_vehicle_attitude_controls(const struct listener *l) +l_vehicle_attitude_controls_effective(const struct listener *l) { - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_sub, &actuators_0); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_effective_sub, &actuators_effective_0); if (gcs_link) { /* send, add spaces so that string buffer is at least 10 chars long */ mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "eff ctrl0 ", - actuators_0.control_effective[0]); + actuators_effective_0.control_effective[0]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "eff ctrl1 ", - actuators_0.control_effective[1]); + actuators_effective_0.control_effective[1]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "eff ctrl2 ", - actuators_0.control_effective[2]); + actuators_effective_0.control_effective[2]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "eff ctrl3 ", - actuators_0.control_effective[3]); + actuators_effective_0.control_effective[3]); + } +} + +void +l_vehicle_attitude_controls(const struct listener *l) +{ + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators_0); + + if (gcs_link) { + /* send, add spaces so that string buffer is at least 10 chars long */ + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl0 ", + actuators_0.control[0]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl1 ", + actuators_0.control[1]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl2 ", + actuators_0.control[2]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl3 ", + actuators_0.control[3]); } } @@ -637,7 +666,7 @@ l_airspeed(const struct listener *l) orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed); float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); - float throttle = actuators_0.control_effective[3] * (UINT16_MAX - 1); + float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1); float alt = global_pos.alt; float climb = global_pos.vz; @@ -773,7 +802,10 @@ uorb_receive_start(void) orb_set_interval(mavlink_subs.man_control_sp_sub, 100); /* 10Hz updates */ /* --- ACTUATOR CONTROL VALUE --- */ - mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); + mavlink_subs.actuators_effective_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); + orb_set_interval(mavlink_subs.actuators_effective_sub, 100); /* 10Hz updates */ + + mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); orb_set_interval(mavlink_subs.actuators_sub, 100); /* 10Hz updates */ /* --- DEBUG VALUE OUTPUT --- */ diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h index 506f73105d..e2e6300463 100644 --- a/src/modules/mavlink/orb_topics.h +++ b/src/modules/mavlink/orb_topics.h @@ -80,6 +80,7 @@ struct mavlink_subscriptions { int safety_sub; int actuators_sub; int armed_sub; + int actuators_effective_sub; int local_pos_sub; int spa_sub; int spl_sub; diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 3466841c4c..760e598bce 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -64,9 +64,8 @@ #include #include #include -#include +#include #include -#include #include #include "position_estimator_inav_params.h" diff --git a/src/modules/systemlib/airspeed.c b/src/modules/systemlib/airspeed.c index e01cc4ddaa..310fbf60ff 100644 --- a/src/modules/systemlib/airspeed.c +++ b/src/modules/systemlib/airspeed.c @@ -42,7 +42,7 @@ #include #include -#include "conversions.h" +#include #include "airspeed.h" @@ -95,17 +95,21 @@ float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_am float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius) { float density = get_air_density(static_pressure, temperature_celsius); + if (density < 0.0001f || !isfinite(density)) { - density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C; -// printf("[airspeed] Invalid air density, using density at sea level\n"); + density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C; } float pressure_difference = total_pressure - static_pressure; - if(pressure_difference > 0) { + if (pressure_difference > 0) { return sqrtf((2.0f*(pressure_difference)) / density); - } else - { + } else { return -sqrtf((2.0f*fabsf(pressure_difference)) / density); } } + +float get_air_density(float static_pressure, float temperature_celsius) +{ + return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS)); +} diff --git a/src/modules/systemlib/airspeed.h b/src/modules/systemlib/airspeed.h index def53f0c1b..8dccaab9c5 100644 --- a/src/modules/systemlib/airspeed.h +++ b/src/modules/systemlib/airspeed.h @@ -85,6 +85,14 @@ */ __EXPORT float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius); + /** + * Calculates air density. + * + * @param static_pressure ambient pressure in millibar + * @param temperature_celcius air / ambient temperature in celcius + */ +__EXPORT float get_air_density(float static_pressure, float temperature_celsius); + __END_DECLS #endif diff --git a/src/modules/systemlib/conversions.c b/src/modules/systemlib/conversions.c index ac94252c57..9105d83cbe 100644 --- a/src/modules/systemlib/conversions.c +++ b/src/modules/systemlib/conversions.c @@ -55,100 +55,3 @@ int16_t_from_bytes(uint8_t bytes[]) return u.w; } - -void rot2quat(const float R[9], float Q[4]) -{ - float q0_2; - float q1_2; - float q2_2; - float q3_2; - int32_t idx; - - /* conversion of rotation matrix to quaternion - * choose the largest component to begin with */ - q0_2 = (((1.0F + R[0]) + R[4]) + R[8]) / 4.0F; - q1_2 = (((1.0F + R[0]) - R[4]) - R[8]) / 4.0F; - q2_2 = (((1.0F - R[0]) + R[4]) - R[8]) / 4.0F; - q3_2 = (((1.0F - R[0]) - R[4]) + R[8]) / 4.0F; - - idx = 0; - - if (q0_2 < q1_2) { - q0_2 = q1_2; - - idx = 1; - } - - if (q0_2 < q2_2) { - q0_2 = q2_2; - idx = 2; - } - - if (q0_2 < q3_2) { - q0_2 = q3_2; - idx = 3; - } - - q0_2 = sqrtf(q0_2); - - /* solve for the remaining three components */ - if (idx == 0) { - q1_2 = q0_2; - q2_2 = (R[5] - R[7]) / 4.0F / q0_2; - q3_2 = (R[6] - R[2]) / 4.0F / q0_2; - q0_2 = (R[1] - R[3]) / 4.0F / q0_2; - - } else if (idx == 1) { - q2_2 = q0_2; - q1_2 = (R[5] - R[7]) / 4.0F / q0_2; - q3_2 = (R[3] + R[1]) / 4.0F / q0_2; - q0_2 = (R[6] + R[2]) / 4.0F / q0_2; - - } else if (idx == 2) { - q3_2 = q0_2; - q1_2 = (R[6] - R[2]) / 4.0F / q0_2; - q2_2 = (R[3] + R[1]) / 4.0F / q0_2; - q0_2 = (R[7] + R[5]) / 4.0F / q0_2; - - } else { - q1_2 = (R[1] - R[3]) / 4.0F / q0_2; - q2_2 = (R[6] + R[2]) / 4.0F / q0_2; - q3_2 = (R[7] + R[5]) / 4.0F / q0_2; - } - - /* return values */ - Q[0] = q1_2; - Q[1] = q2_2; - Q[2] = q3_2; - Q[3] = q0_2; -} - -void quat2rot(const float Q[4], float R[9]) -{ - float q0_2; - float q1_2; - float q2_2; - float q3_2; - - memset(&R[0], 0, 9U * sizeof(float)); - - q0_2 = Q[0] * Q[0]; - q1_2 = Q[1] * Q[1]; - q2_2 = Q[2] * Q[2]; - q3_2 = Q[3] * Q[3]; - - R[0] = ((q0_2 + q1_2) - q2_2) - q3_2; - R[3] = 2.0F * (Q[1] * Q[2] - Q[0] * Q[3]); - R[6] = 2.0F * (Q[1] * Q[3] + Q[0] * Q[2]); - R[1] = 2.0F * (Q[1] * Q[2] + Q[0] * Q[3]); - R[4] = ((q0_2 + q2_2) - q1_2) - q3_2; - R[7] = 2.0F * (Q[2] * Q[3] - Q[0] * Q[1]); - R[2] = 2.0F * (Q[1] * Q[3] - Q[0] * Q[2]); - R[5] = 2.0F * (Q[2] * Q[3] + Q[0] * Q[1]); - R[8] = ((q0_2 + q3_2) - q1_2) - q2_2; -} - -float get_air_density(float static_pressure, float temperature_celsius) -{ - return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS)); -} diff --git a/src/modules/systemlib/conversions.h b/src/modules/systemlib/conversions.h index 064426f218..dc383e770f 100644 --- a/src/modules/systemlib/conversions.h +++ b/src/modules/systemlib/conversions.h @@ -43,7 +43,6 @@ #define CONVERSIONS_H_ #include #include -#include __BEGIN_DECLS @@ -57,34 +56,6 @@ __BEGIN_DECLS */ __EXPORT int16_t int16_t_from_bytes(uint8_t bytes[]); -/** - * Converts a 3 x 3 rotation matrix to an unit quaternion. - * - * All orientations are expressed in NED frame. - * - * @param R rotation matrix to convert - * @param Q quaternion to write back to - */ -__EXPORT void rot2quat(const float R[9], float Q[4]); - -/** - * Converts an unit quaternion to a 3 x 3 rotation matrix. - * - * All orientations are expressed in NED frame. - * - * @param Q quaternion to convert - * @param R rotation matrix to write back to - */ -__EXPORT void quat2rot(const float Q[4], float R[9]); - -/** - * Calculates air density. - * - * @param static_pressure ambient pressure in millibar - * @param temperature_celcius air / ambient temperature in celcius - */ -__EXPORT float get_air_density(float static_pressure, float temperature_celsius); - __END_DECLS #endif /* CONVERSIONS_H_ */ diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index cbf8291225..94c744c039 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -45,7 +45,6 @@ SRCS = err.c \ getopt_long.c \ up_cxxinitialize.c \ pid/pid.c \ - geo/geo.c \ systemlib.c \ airspeed.c \ system_params.c \ From 3005c8aae0ed0c74d2d611712d9e7d659f4ed453 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 Aug 2013 18:36:10 +0200 Subject: [PATCH 441/486] Added missing config files, compiling --- makefiles/config_px4fmu-v1_default.mk | 15 +++++++-------- makefiles/config_px4fmu-v2_default.mk | 17 +++++++++-------- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index cca5185522..a556d0b073 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -70,8 +70,6 @@ MODULES += modules/gpio_led # Estimation modules (EKF / other filters) # MODULES += modules/attitude_estimator_ekf -MODULES += modules/attitude_estimator_so3_comp -MODULES += modules/position_estimator MODULES += modules/att_pos_estimator_ekf MODULES += modules/position_estimator_inav MODULES += examples/flow_position_estimator @@ -80,9 +78,8 @@ MODULES += examples/flow_position_estimator # Vehicle Control # #MODULES += modules/segway # XXX needs state machine update -MODULES += modules/fixedwing_backside -MODULES += modules/fixedwing_att_control -MODULES += modules/fixedwing_pos_control +#MODULES += modules/fw_pos_control_l1 +#MODULES += modules/fw_att_control MODULES += modules/multirotor_att_control MODULES += modules/multirotor_pos_control MODULES += examples/flow_position_control @@ -98,15 +95,17 @@ MODULES += modules/sdlog2 # MODULES += modules/systemlib MODULES += modules/systemlib/mixer -MODULES += modules/mathlib -MODULES += modules/mathlib/math/filter MODULES += modules/controllib MODULES += modules/uORB # # Libraries # -LIBRARIES += modules/mathlib/CMSIS +LIBRARIES += lib/mathlib/CMSIS +MODULES += lib/mathlib +MODULES += lib/mathlib/math/filter +#MODULES += lib/ecl +MODULES += lib/geo # # Demo apps diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 101b497125..20dbe717fd 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -66,16 +66,15 @@ MODULES += modules/mavlink_onboard # Estimation modules (EKF / other filters) # MODULES += modules/attitude_estimator_ekf -MODULES += modules/position_estimator_mc -MODULES += modules/position_estimator MODULES += modules/att_pos_estimator_ekf +MODULES += modules/position_estimator_inav +MODULES += examples/flow_position_estimator # # Vehicle Control # -MODULES += modules/fixedwing_backside -MODULES += modules/fixedwing_att_control -MODULES += modules/fixedwing_pos_control +#MODULES += modules/fw_pos_control_l1 +#MODULES += modules/fw_att_control MODULES += modules/multirotor_att_control MODULES += modules/multirotor_pos_control @@ -89,15 +88,17 @@ MODULES += modules/sdlog2 # MODULES += modules/systemlib MODULES += modules/systemlib/mixer -MODULES += modules/mathlib -MODULES += modules/mathlib/math/filter MODULES += modules/controllib MODULES += modules/uORB # # Libraries # -LIBRARIES += modules/mathlib/CMSIS +LIBRARIES += lib/mathlib/CMSIS +MODULES += lib/mathlib +MODULES += lib/mathlib/math/filter +#MODULES += lib/ecl +MODULES += lib/geo # # Demo apps From cfa9054aa4e6accae1fefaad1a13316301ce56fc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 22 Aug 2013 09:25:13 +0200 Subject: [PATCH 442/486] Moved to USART1 for the main console, starting a 2nd NSH instance on USB if needed, reworked start scripts to not fall over --- ROMFS/px4fmu_common/init.d/01_fmu_quad_x | 14 +++--- ROMFS/px4fmu_common/init.d/02_io_quad_x | 2 +- ROMFS/px4fmu_common/init.d/08_ardrone | 25 +++-------- ROMFS/px4fmu_common/init.d/09_ardrone_flow | 15 ++++--- ROMFS/px4fmu_common/init.d/10_io_f330 | 51 +++++++++++----------- ROMFS/px4fmu_common/init.d/rcS | 47 +++++++++++++++++++- nuttx-configs/px4fmu-v1/nsh/defconfig | 8 ++-- 7 files changed, 97 insertions(+), 65 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/01_fmu_quad_x b/ROMFS/px4fmu_common/init.d/01_fmu_quad_x index 58a970eba5..c1f3187f93 100644 --- a/ROMFS/px4fmu_common/init.d/01_fmu_quad_x +++ b/ROMFS/px4fmu_common/init.d/01_fmu_quad_x @@ -97,11 +97,9 @@ multirotor_att_control start # Start logging # sdlog2 start -r 50 -a -b 14 - -# -# Start system state -# -if blinkm start -then - blinkm systemstate -fi + +# Try to get an USB console +nshterm /dev/ttyACM0 & + +# Exit, because /dev/ttyS0 is needed for MAVLink +exit diff --git a/ROMFS/px4fmu_common/init.d/02_io_quad_x b/ROMFS/px4fmu_common/init.d/02_io_quad_x index c63e92f6d3..49483d14fe 100644 --- a/ROMFS/px4fmu_common/init.d/02_io_quad_x +++ b/ROMFS/px4fmu_common/init.d/02_io_quad_x @@ -88,7 +88,7 @@ pwm -u 400 -m 0xff multirotor_att_control start # -# Start logging +# Start logging once armed # sdlog2 start -r 50 -a -b 14 diff --git a/ROMFS/px4fmu_common/init.d/08_ardrone b/ROMFS/px4fmu_common/init.d/08_ardrone index f55ac2ae34..5bb1491e91 100644 --- a/ROMFS/px4fmu_common/init.d/08_ardrone +++ b/ROMFS/px4fmu_common/init.d/08_ardrone @@ -2,12 +2,6 @@ # # Flight startup script for PX4FMU on PX4IOAR carrier board. # - -# Disable the USB interface -set USB no - -# Disable autostarting other apps -set MODE ardrone echo "[init] doing PX4IOAR startup..." @@ -70,25 +64,14 @@ multirotor_att_control start ardrone_interface start -d /dev/ttyS1 # -# Start logging +# Start logging once armed # -sdlog start -s 10 +sdlog2 start -r 50 -a -b 14 # # Start GPS capture # gps start - -# -# Start system state -# -if blinkm start -then - echo "using BlinkM for state indication" - blinkm systemstate -else - echo "no BlinkM found, OK." -fi # # startup is done; we don't want the shell because we @@ -96,4 +79,8 @@ fi # echo "[init] startup done" +# Try to get an USB console +nshterm /dev/ttyACM0 & + +# Exit, because /dev/ttyS0 is needed for MAVLink exit diff --git a/ROMFS/px4fmu_common/init.d/09_ardrone_flow b/ROMFS/px4fmu_common/init.d/09_ardrone_flow index e7173f6e6e..a223ef7aab 100644 --- a/ROMFS/px4fmu_common/init.d/09_ardrone_flow +++ b/ROMFS/px4fmu_common/init.d/09_ardrone_flow @@ -2,12 +2,6 @@ # # Flight startup script for PX4FMU on PX4IOAR carrier board. # - -# Disable the USB interface -set USB no - -# Disable autostarting other apps -set MODE ardrone echo "[init] doing PX4IOAR startup..." @@ -84,6 +78,11 @@ flow_speed_control start # Fire up the AR.Drone interface. # ardrone_interface start -d /dev/ttyS1 + +# +# Start logging once armed +# +sdlog2 start -r 50 -a -b 14 # # startup is done; we don't want the shell because we @@ -91,4 +90,8 @@ ardrone_interface start -d /dev/ttyS1 # echo "[init] startup done" +# Try to get an USB console +nshterm /dev/ttyACM0 & + +# Exit, because /dev/ttyS0 is needed for MAVLink exit diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_io_f330 index 0e6d3f5d54..b2fc0c96f1 100644 --- a/ROMFS/px4fmu_common/init.d/10_io_f330 +++ b/ROMFS/px4fmu_common/init.d/10_io_f330 @@ -2,10 +2,6 @@ # # Flight startup script for PX4FMU+PX4IO on an F330 quad. # - -# disable USB and autostart -set USB no -set MODE custom # # Start the ORB (first app to start) @@ -60,34 +56,37 @@ commander start # if px4io start then + # + # This sets a PWM right after startup (regardless of safety button) + # + px4io idle 900 900 900 900 + pwm -u 400 -m 0xff + + # + # Allow PX4IO to recover from midair restarts. + # this is very unlikely, but quite safe and robust. + px4io recovery + + + + # + # The values are for spinning motors when armed using DJI ESCs + # + px4io min 1200 1200 1200 1200 + + # + # Upper limits could be higher, this is on the safe side + # + px4io max 1800 1800 1800 1800 + + mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix + else # SOS tone_alarm 6 fi -# -# Allow PX4IO to recover from midair restarts. -# this is very unlikely, but quite safe and robust. -px4io recovery - -# -# This sets a PWM right after startup (regardless of safety button) -# -px4io idle 900 900 900 900 - -# -# The values are for spinning motors when armed using DJI ESCs -# -px4io min 1200 1200 1200 1200 - -# -# Upper limits could be higher, this is on the safe side -# -px4io max 1800 1800 1800 1800 - -mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix - # # Start the sensors (depends on orb, px4io) # diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 650224cf17..8674c3c042 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -61,7 +61,13 @@ then # # Start terminal # -sercon +if sercon +then + echo "USB connected" +else + # second attempt + sercon & +fi # # Start the ORB (first app to start) @@ -128,45 +134,84 @@ fi if param compare SYS_AUTOSTART 1 then sh /etc/init.d/01_fmu_quad_x + set MODE custom fi if param compare SYS_AUTOSTART 2 then sh /etc/init.d/02_io_quad_x + set MODE custom fi if param compare SYS_AUTOSTART 8 then sh /etc/init.d/08_ardrone + set MODE custom fi if param compare SYS_AUTOSTART 9 then sh /etc/init.d/09_ardrone_flow + set MODE custom fi if param compare SYS_AUTOSTART 10 then sh /etc/init.d/10_io_f330 + set MODE custom fi if param compare SYS_AUTOSTART 15 then sh /etc/init.d/15_io_tbs + set MODE custom fi if param compare SYS_AUTOSTART 30 then sh /etc/init.d/30_io_camflyer + set MODE custom fi if param compare SYS_AUTOSTART 31 then sh /etc/init.d/31_io_phantom + set MODE custom fi # Try to get an USB console nshterm /dev/ttyACM0 & +# If none of the autostart scripts triggered, get a minimal setup +if [ $MODE == autostart ] +then + # Telemetry port is on both FMU boards ttyS1 + mavlink start -b 57600 -d /dev/ttyS1 + + # Start commander + commander start + + # Start px4io if present + if px4io start + then + echo "PX4IO driver started" + else + if fmu mode_serial + then + echo "FMU driver started" + fi + fi + + # Start sensors + sh /etc/init.d/rc.sensors + + # Start one of the estimators + attitude_estimator_ekf start + + # Start GPS + gps start + +fi + # End of autostart fi diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index a7a6725c65..412a29fbe6 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -368,7 +368,7 @@ CONFIG_TASK_NAME_SIZE=24 CONFIG_START_YEAR=1970 CONFIG_START_MONTH=1 CONFIG_START_DAY=1 -# CONFIG_DEV_CONSOLE is not set +CONFIG_DEV_CONSOLE=y # CONFIG_MUTEX_TYPES is not set CONFIG_PRIORITY_INHERITANCE=y CONFIG_SEM_PREALLOCHOLDERS=0 @@ -476,11 +476,11 @@ CONFIG_ARCH_HAVE_USART6=y CONFIG_MCU_SERIAL=y CONFIG_STANDARD_SERIAL=y CONFIG_SERIAL_NPOLLWAITERS=2 -# CONFIG_USART1_SERIAL_CONSOLE is not set +CONFIG_USART1_SERIAL_CONSOLE=y # CONFIG_USART2_SERIAL_CONSOLE is not set # CONFIG_UART5_SERIAL_CONSOLE is not set # CONFIG_USART6_SERIAL_CONSOLE is not set -CONFIG_NO_SERIAL_CONSOLE=y +# CONFIG_NO_SERIAL_CONSOLE is not set # # USART1 Configuration @@ -550,7 +550,7 @@ CONFIG_USBDEV_MAXPOWER=500 # CONFIG_USBDEV_COMPOSITE is not set # CONFIG_PL2303 is not set CONFIG_CDCACM=y -CONFIG_CDCACM_CONSOLE=y +CONFIG_CDCACM_CONSOLE=n CONFIG_CDCACM_EP0MAXPACKET=64 CONFIG_CDCACM_EPINTIN=1 CONFIG_CDCACM_EPINTIN_FSSIZE=64 From 11257cbade5d89d3d2de8101daec11d32f7f74ce Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 22 Aug 2013 10:13:47 +0200 Subject: [PATCH 443/486] Fixed commandline handling --- src/drivers/px4io/px4io.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index c00816a127..9fc07392cc 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1802,13 +1802,15 @@ start(int argc, char *argv[]) } /* disable RC handling on request */ - if (argc > 0 && !strcmp(argv[0], "norc")) { + if (argc > 1) { + if (!strcmp(argv[1], "norc")) { - if(g_dev->disable_rc_handling()) - warnx("Failed disabling RC handling"); + if(g_dev->disable_rc_handling()) + warnx("Failed disabling RC handling"); - } else { - warnx("unknown argument: %s", argv[0]); + } else { + warnx("unknown argument: %s", argv[1]); + } } int dsm_vcc_ctl; From 85eafa323aec397e4ed5c394f25d48ce6d878f9f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 22 Aug 2013 10:43:19 +0200 Subject: [PATCH 444/486] Fix to RC param updates on IO --- ROMFS/px4fmu_common/init.d/rcS | 1 + src/modules/px4iofirmware/registers.c | 8 ++++++-- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 8674c3c042..6b624b2780 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -187,6 +187,7 @@ if [ $MODE == autostart ] then # Telemetry port is on both FMU boards ttyS1 mavlink start -b 57600 -d /dev/ttyS1 + usleep 5000 # Start commander commander start diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 655a0c7a85..9c95fd1c52 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -499,8 +499,12 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_PAGE_RC_CONFIG: { - /* do not allow a RC config change while outputs armed */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { + /** + * do not allow a RC config change while outputs armed + */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || + (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) || + (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { break; } From ab5ec0da0b4afcb3f68eaf70efebe691513f74ce Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 22 Aug 2013 15:53:19 +0200 Subject: [PATCH 445/486] Changed the default PID gains for the F330 --- ROMFS/px4fmu_common/init.d/10_io_f330 | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_io_f330 index b2fc0c96f1..50842764a6 100644 --- a/ROMFS/px4fmu_common/init.d/10_io_f330 +++ b/ROMFS/px4fmu_common/init.d/10_io_f330 @@ -16,19 +16,19 @@ then # Set all params here, then disable autoconfig param set SYS_AUTOCONFIG 0 - param set MC_ATTRATE_D 0.005 + param set MC_ATTRATE_D 0.004 param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.1 + param set MC_ATTRATE_P 0.12 param set MC_ATT_D 0.0 param set MC_ATT_I 0.0 - param set MC_ATT_P 4.5 + param set MC_ATT_P 7.0 param set MC_RCLOSS_THR 0.0 param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.3 - param set MC_YAWPOS_P 0.6 - param set MC_YAWRATE_D 0.0 - param set MC_YAWRATE_I 0.0 - param set MC_YAWRATE_P 0.1 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_P 2.0 + param set MC_YAWRATE_D 0.005 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_P 0.3 param save /fs/microsd/params fi From ca96140b217971d527e2306922a87a1592e6f90d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 22 Aug 2013 15:53:46 +0200 Subject: [PATCH 446/486] Allow the tone alarms to be interrupted --- src/drivers/stm32/tone_alarm/tone_alarm.cpp | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index b06920a765..ad21f7143f 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -879,14 +879,9 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg) _tune = nullptr; _next = nullptr; } else { - /* don't interrupt alarms unless they are repeated */ - if (_tune != nullptr && !_repeat) { - /* abort and let the current tune finish */ - result = -EBUSY; - } else if (_repeat && _default_tune_number == arg) { - /* requested repeating tune already playing */ - } else { - // play the selected tune + /* always interrupt alarms, unless they are repeating and already playing */ + if (!(_repeat && _default_tune_number == arg)) { + /* play the selected tune */ _default_tune_number = arg; start_tune(_default_tunes[arg - 1]); } From 6c3da5aeddf929f5a4f19f6bd1b75c911c2a414c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 22 Aug 2013 15:55:33 +0200 Subject: [PATCH 447/486] Reset yaw position when disarmed in multirotor controller --- .../multirotor_att_control_main.c | 18 ++---------------- 1 file changed, 2 insertions(+), 16 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index c057ef3647..2d16d49219 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -144,10 +144,6 @@ mc_thread_main(int argc, char *argv[]) /* welcome user */ warnx("starting"); - /* store last control mode to detect mode switches */ - bool flag_control_manual_enabled = false; - bool flag_control_attitude_enabled = false; - bool control_yaw_position = true; bool reset_yaw_sp = true; @@ -232,12 +228,6 @@ mc_thread_main(int argc, char *argv[]) } else if (control_mode.flag_control_manual_enabled) { /* direct manual input */ if (control_mode.flag_control_attitude_enabled) { - /* control attitude, update attitude setpoint depending on mode */ - /* initialize to current yaw if switching to manual or att control */ - if (control_mode.flag_control_attitude_enabled != flag_control_attitude_enabled || - control_mode.flag_control_manual_enabled != flag_control_manual_enabled) { - att_sp.yaw_body = att.yaw; - } static bool rc_loss_first_time = true; @@ -283,12 +273,12 @@ mc_thread_main(int argc, char *argv[]) /* control yaw in all manual / assisted modes */ /* set yaw if arming or switching to attitude stabilized mode */ - if (!flag_control_attitude_enabled) { + if (!control_mode.flag_armed) { reset_yaw_sp = true; } /* only move setpoint if manual input is != 0 */ - if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { // TODO use landed status instead of throttle + if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.1f) { // TODO use landed status instead of throttle rates_sp.yaw = manual.yaw; control_yaw_position = false; reset_yaw_sp = true; @@ -377,10 +367,6 @@ mc_thread_main(int argc, char *argv[]) actuators.timestamp = hrt_absolute_time(); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - /* update state */ - flag_control_attitude_enabled = control_mode.flag_control_attitude_enabled; - flag_control_manual_enabled = control_mode.flag_control_manual_enabled; - perf_end(mc_loop_perf); } /* end of poll call for attitude updates */ } /* end of poll return value check */ From 5f1004117f8086c4bba5b4031f3aebd73411682c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 22 Aug 2013 15:57:17 +0200 Subject: [PATCH 448/486] Restore proper feedback (mavlink and tone) for calibration commands, etc --- .../commander/accelerometer_calibration.cpp | 6 +- .../commander/accelerometer_calibration.h | 2 +- .../commander/airspeed_calibration.cpp | 18 +- src/modules/commander/airspeed_calibration.h | 4 +- src/modules/commander/baro_calibration.cpp | 10 +- src/modules/commander/baro_calibration.h | 4 +- src/modules/commander/commander.cpp | 198 ++++++++++-------- src/modules/commander/gyro_calibration.cpp | 31 +-- src/modules/commander/gyro_calibration.h | 4 +- src/modules/commander/mag_calibration.cpp | 17 +- src/modules/commander/mag_calibration.h | 4 +- src/modules/commander/rc_calibration.cpp | 13 +- src/modules/commander/rc_calibration.h | 4 +- 13 files changed, 179 insertions(+), 136 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index ddd2e23d95..c2b2e92581 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -133,7 +133,7 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp int mat_invert3(float src[3][3], float dst[3][3]); int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g); -void do_accel_calibration(int mavlink_fd) { +int do_accel_calibration(int mavlink_fd) { /* announce change */ mavlink_log_info(mavlink_fd, "accel calibration started"); @@ -176,11 +176,11 @@ void do_accel_calibration(int mavlink_fd) { } mavlink_log_info(mavlink_fd, "accel calibration done"); - tune_positive(); + return OK; } else { /* measurements error */ mavlink_log_info(mavlink_fd, "accel calibration aborted"); - tune_negative(); + return ERROR; } /* exit accel calibration mode */ diff --git a/src/modules/commander/accelerometer_calibration.h b/src/modules/commander/accelerometer_calibration.h index 3275d94612..1cf9c09775 100644 --- a/src/modules/commander/accelerometer_calibration.h +++ b/src/modules/commander/accelerometer_calibration.h @@ -45,6 +45,6 @@ #include -void do_accel_calibration(int mavlink_fd); +int do_accel_calibration(int mavlink_fd); #endif /* ACCELEROMETER_CALIBRATION_H_ */ diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index df08292e32..e414e5f707 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -49,7 +49,13 @@ #include #include -void do_airspeed_calibration(int mavlink_fd) +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +int do_airspeed_calibration(int mavlink_fd) { /* give directions */ mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still"); @@ -79,7 +85,7 @@ void do_airspeed_calibration(int mavlink_fd) } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ mavlink_log_info(mavlink_fd, "airspeed calibration aborted"); - return; + return ERROR; } } @@ -89,6 +95,7 @@ void do_airspeed_calibration(int mavlink_fd) if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { mavlink_log_critical(mavlink_fd, "Setting offs failed!"); + return ERROR; } /* auto-save to EEPROM */ @@ -96,6 +103,8 @@ void do_airspeed_calibration(int mavlink_fd) if (save_ret != 0) { warn("WARNING: auto-save of params to storage failed"); + mavlink_log_info(mavlink_fd, "FAILED storing calibration"); + return ERROR; } //char buf[50]; @@ -103,11 +112,10 @@ void do_airspeed_calibration(int mavlink_fd) //mavlink_log_info(mavlink_fd, buf); mavlink_log_info(mavlink_fd, "airspeed calibration done"); - tune_positive(); + return OK; } else { mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); + return ERROR; } - - close(diff_pres_sub); } diff --git a/src/modules/commander/airspeed_calibration.h b/src/modules/commander/airspeed_calibration.h index 92f5651ec1..71c701fc2e 100644 --- a/src/modules/commander/airspeed_calibration.h +++ b/src/modules/commander/airspeed_calibration.h @@ -41,6 +41,6 @@ #include -void do_airspeed_calibration(int mavlink_fd); +int do_airspeed_calibration(int mavlink_fd); -#endif /* AIRSPEED_CALIBRATION_H_ */ \ No newline at end of file +#endif /* AIRSPEED_CALIBRATION_H_ */ diff --git a/src/modules/commander/baro_calibration.cpp b/src/modules/commander/baro_calibration.cpp index d7515b3d9b..3123c40876 100644 --- a/src/modules/commander/baro_calibration.cpp +++ b/src/modules/commander/baro_calibration.cpp @@ -47,8 +47,14 @@ #include #include -void do_baro_calibration(int mavlink_fd) +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +int do_baro_calibration(int mavlink_fd) { // TODO implement this - return; + return ERROR; } diff --git a/src/modules/commander/baro_calibration.h b/src/modules/commander/baro_calibration.h index ac0f4be368..bc42bc6cfc 100644 --- a/src/modules/commander/baro_calibration.h +++ b/src/modules/commander/baro_calibration.h @@ -41,6 +41,6 @@ #include -void do_baro_calibration(int mavlink_fd); +int do_baro_calibration(int mavlink_fd); -#endif /* BARO_CALIBRATION_H_ */ \ No newline at end of file +#endif /* BARO_CALIBRATION_H_ */ diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 17db0f9c88..66b9272de6 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -502,7 +502,13 @@ int commander_thread_main(int argc, char *argv[]) mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); if (mavlink_fd < 0) { - warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first."); + /* try again later */ + usleep(20000); + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + + if (mavlink_fd < 0) { + warnx("ERROR: Failed to open MAVLink log stream again, start mavlink app first."); + } } /* Main state machine */ @@ -1628,6 +1634,33 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v return res; } +void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result) +{ + switch (result) { + case VEHICLE_CMD_RESULT_ACCEPTED: + tune_positive(); + break; + case VEHICLE_CMD_RESULT_DENIED: + mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command); + tune_negative(); + break; + case VEHICLE_CMD_RESULT_FAILED: + mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command); + tune_negative(); + break; + case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: + mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd.command); + tune_negative(); + break; + case VEHICLE_CMD_RESULT_UNSUPPORTED: + mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command); + tune_negative(); + break; + default: + break; + } +} + void *commander_low_prio_loop(void *arg) { /* Set thread name */ @@ -1668,125 +1701,110 @@ void *commander_low_prio_loop(void *arg) cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM) continue; - /* result of the command */ - uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; - /* only handle low-priority commands here */ switch (cmd.command) { case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: - if (is_safe(&status, &safety, &armed)) { + if (is_safe(&status, &safety, &armed)) { - if (((int)(cmd.param1)) == 1) { - /* reboot */ - systemreset(false); - } else if (((int)(cmd.param1)) == 3) { - /* reboot to bootloader */ - systemreset(true); - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - + if (((int)(cmd.param1)) == 1) { + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + usleep(1000000); + /* reboot */ + systemreset(false); + } else if (((int)(cmd.param1)) == 3) { + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + usleep(1000000); + /* reboot to bootloader */ + systemreset(true); } else { - result = VEHICLE_CMD_RESULT_DENIED; + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); } - break; + + } else { + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + } + break; case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { - /* try to go to INIT/PREFLIGHT arming state */ + int calib_ret = ERROR; - // XXX disable interrupts in arming_state_transition - if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) { - result = VEHICLE_CMD_RESULT_DENIED; - break; - } - - if ((int)(cmd.param1) == 1) { - /* gyro calibration */ - do_gyro_calibration(mavlink_fd); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else if ((int)(cmd.param2) == 1) { - /* magnetometer calibration */ - do_mag_calibration(mavlink_fd); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else if ((int)(cmd.param3) == 1) { - /* zero-altitude pressure calibration */ - result = VEHICLE_CMD_RESULT_DENIED; - - } else if ((int)(cmd.param4) == 1) { - /* RC calibration */ - result = VEHICLE_CMD_RESULT_DENIED; - - } else if ((int)(cmd.param5) == 1) { - /* accelerometer calibration */ - do_accel_calibration(mavlink_fd); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else if ((int)(cmd.param6) == 1) { - /* airspeed calibration */ - do_airspeed_calibration(mavlink_fd); - result = VEHICLE_CMD_RESULT_ACCEPTED; - } - - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + /* try to go to INIT/PREFLIGHT arming state */ + // XXX disable interrupts in arming_state_transition + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) { + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); break; } - case VEHICLE_CMD_PREFLIGHT_STORAGE: { + if ((int)(cmd.param1) == 1) { + /* gyro calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_gyro_calibration(mavlink_fd); - if (((int)(cmd.param1)) == 0) { - if (0 == param_load_default()) { - mavlink_log_info(mavlink_fd, "[cmd] parameters loaded"); - result = VEHICLE_CMD_RESULT_ACCEPTED; + } else if ((int)(cmd.param2) == 1) { + /* magnetometer calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_mag_calibration(mavlink_fd); - } else { - mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); - tune_error(); - result = VEHICLE_CMD_RESULT_FAILED; - } + } else if ((int)(cmd.param3) == 1) { + /* zero-altitude pressure calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); - } else if (((int)(cmd.param1)) == 1) { - if (0 == param_save_default()) { - mavlink_log_info(mavlink_fd, "[cmd] parameters saved"); - result = VEHICLE_CMD_RESULT_ACCEPTED; + } else if ((int)(cmd.param4) == 1) { + /* RC calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); - } else { - mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); - tune_error(); - result = VEHICLE_CMD_RESULT_FAILED; - } - } + } else if ((int)(cmd.param5) == 1) { + /* accelerometer calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_accel_calibration(mavlink_fd); - break; + } else if ((int)(cmd.param6) == 1) { + /* airspeed calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_airspeed_calibration(mavlink_fd); } - default: + if (calib_ret == OK) + tune_positive(); + else + tune_negative(); + + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + break; } - /* supported command handling stop */ - if (result == VEHICLE_CMD_RESULT_ACCEPTED) { - tune_positive(); + case VEHICLE_CMD_PREFLIGHT_STORAGE: { - } else { - tune_negative(); + if (((int)(cmd.param1)) == 0) { + if (0 == param_load_default()) { + mavlink_log_info(mavlink_fd, "[cmd] parameters loaded"); + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - if (result == VEHICLE_CMD_RESULT_DENIED) { - mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command); + } else { + mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); + answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); + } - } else if (result == VEHICLE_CMD_RESULT_FAILED) { - mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command); + } else if (((int)(cmd.param1)) == 1) { + if (0 == param_save_default()) { + mavlink_log_info(mavlink_fd, "[cmd] parameters saved"); + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - } else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) { - mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd.command); - - } else if (result == VEHICLE_CMD_RESULT_UNSUPPORTED) { - mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command); + } else { + mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); + answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); + } } + break; + } + + default: + answer_command(cmd, VEHICLE_CMD_RESULT_UNSUPPORTED); + break; } /* send any requested ACKs */ diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index f1afb0107e..9cd2f3a19c 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -50,8 +50,13 @@ #include #include +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; -void do_gyro_calibration(int mavlink_fd) +int do_gyro_calibration(int mavlink_fd) { mavlink_log_info(mavlink_fd, "gyro calibration starting, hold still"); @@ -98,7 +103,7 @@ void do_gyro_calibration(int mavlink_fd) } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); - return; + return ERROR; } } @@ -137,18 +142,17 @@ void do_gyro_calibration(int mavlink_fd) if (save_ret != 0) { warnx("WARNING: auto-save of params to storage failed"); mavlink_log_critical(mavlink_fd, "gyro store failed"); - // XXX negative tune - return; + return ERROR; } mavlink_log_info(mavlink_fd, "gyro calibration done"); - tune_positive(); + tune_neutral(); /* third beep by cal end routine */ } else { mavlink_log_info(mavlink_fd, "offset cal FAILED (NaN)"); - return; + return ERROR; } @@ -180,8 +184,7 @@ void do_gyro_calibration(int mavlink_fd) && (hrt_absolute_time() - start_time > 5 * 1e6)) { mavlink_log_info(mavlink_fd, "gyro scale calibration skipped"); mavlink_log_info(mavlink_fd, "gyro calibration done"); - tune_positive(); - return; + return OK; } /* wait blocking for new data */ @@ -221,7 +224,7 @@ void do_gyro_calibration(int mavlink_fd) // operating near the 1e6/1e8 max sane resolution of float. gyro_integral += (raw.gyro_rad_s[2] * dt_ms) / 1e3f; - warnx("dbg: b: %6.4f, g: %6.4f", baseline_integral, gyro_integral); +// warnx("dbg: b: %6.4f, g: %6.4f", (double)baseline_integral, (double)gyro_integral); // } else if (poll_ret == 0) { // /* any poll failure for 1s is a reason to abort */ @@ -232,8 +235,8 @@ void do_gyro_calibration(int mavlink_fd) float gyro_scale = baseline_integral / gyro_integral; float gyro_scales[] = { gyro_scale, gyro_scale, gyro_scale }; - warnx("gyro scale: yaw (z): %6.4f", gyro_scale); - mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", gyro_scale); + warnx("gyro scale: yaw (z): %6.4f", (double)gyro_scale); + mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", (double)gyro_scale); if (isfinite(gyro_scales[0]) && isfinite(gyro_scales[1]) && isfinite(gyro_scales[2])) { @@ -272,12 +275,10 @@ void do_gyro_calibration(int mavlink_fd) // mavlink_log_info(mavlink_fd, buf); mavlink_log_info(mavlink_fd, "gyro calibration done"); - tune_positive(); /* third beep by cal end routine */ - + return OK; } else { mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)"); + return ERROR; } - - close(sub_sensor_combined); } diff --git a/src/modules/commander/gyro_calibration.h b/src/modules/commander/gyro_calibration.h index cd262507db..59c32a15e9 100644 --- a/src/modules/commander/gyro_calibration.h +++ b/src/modules/commander/gyro_calibration.h @@ -41,6 +41,6 @@ #include -void do_gyro_calibration(int mavlink_fd); +int do_gyro_calibration(int mavlink_fd); -#endif /* GYRO_CALIBRATION_H_ */ \ No newline at end of file +#endif /* GYRO_CALIBRATION_H_ */ diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 9a25103f8c..9263c6a159 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -53,8 +53,13 @@ #include #include +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; -void do_mag_calibration(int mavlink_fd) +int do_mag_calibration(int mavlink_fd) { mavlink_log_info(mavlink_fd, "mag calibration starting, hold still"); @@ -113,7 +118,7 @@ void do_mag_calibration(int mavlink_fd) warnx("mag cal failed: out of memory"); mavlink_log_info(mavlink_fd, "mag cal failed: out of memory"); warnx("x:%p y:%p z:%p\n", x, y, z); - return; + return ERROR; } while (hrt_absolute_time() < calibration_deadline && @@ -252,6 +257,7 @@ void do_mag_calibration(int mavlink_fd) if (save_ret != 0) { warn("WARNING: auto-save of params to storage failed"); mavlink_log_info(mavlink_fd, "FAILED storing calibration"); + return ERROR; } warnx("\tscale: %.6f %.6f %.6f\n \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n", @@ -269,12 +275,11 @@ void do_mag_calibration(int mavlink_fd) mavlink_log_info(mavlink_fd, "mag calibration done"); - tune_positive(); + return OK; /* third beep by cal end routine */ } else { mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)"); + return ERROR; } - - close(sub_mag); -} \ No newline at end of file +} diff --git a/src/modules/commander/mag_calibration.h b/src/modules/commander/mag_calibration.h index fd2085f14a..a101cd7b1f 100644 --- a/src/modules/commander/mag_calibration.h +++ b/src/modules/commander/mag_calibration.h @@ -41,6 +41,6 @@ #include -void do_mag_calibration(int mavlink_fd); +int do_mag_calibration(int mavlink_fd); -#endif /* MAG_CALIBRATION_H_ */ \ No newline at end of file +#endif /* MAG_CALIBRATION_H_ */ diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp index 0de411713b..fe87a3323b 100644 --- a/src/modules/commander/rc_calibration.cpp +++ b/src/modules/commander/rc_calibration.cpp @@ -46,8 +46,13 @@ #include #include +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; -void do_rc_calibration(int mavlink_fd) +int do_rc_calibration(int mavlink_fd) { mavlink_log_info(mavlink_fd, "trim calibration starting"); @@ -75,9 +80,9 @@ void do_rc_calibration(int mavlink_fd) if (save_ret != 0) { mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed"); + return ERROR; } - tune_positive(); - mavlink_log_info(mavlink_fd, "trim calibration done"); -} \ No newline at end of file + return OK; +} diff --git a/src/modules/commander/rc_calibration.h b/src/modules/commander/rc_calibration.h index 6505c73647..9aa6faafa8 100644 --- a/src/modules/commander/rc_calibration.h +++ b/src/modules/commander/rc_calibration.h @@ -41,6 +41,6 @@ #include -void do_rc_calibration(int mavlink_fd); +int do_rc_calibration(int mavlink_fd); -#endif /* RC_CALIBRATION_H_ */ \ No newline at end of file +#endif /* RC_CALIBRATION_H_ */ From b5bb20995be8c0f55bed4f2f2bd6cee9efdcf03e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 22 Aug 2013 17:31:59 +0200 Subject: [PATCH 449/486] multirotor_att_control: yaw setpoint reset fix --- .../multirotor_att_control_main.c | 101 +++++++++--------- 1 file changed, 52 insertions(+), 49 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index c057ef3647..2d46bf4387 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -81,6 +81,8 @@ __EXPORT int multirotor_att_control_main(int argc, char *argv[]); static bool thread_should_exit; static int mc_task; static bool motor_test_mode = false; +static const float min_takeoff_throttle = 0.3f; +static const float yaw_deadzone = 0.01f; static int mc_thread_main(int argc, char *argv[]) @@ -147,14 +149,14 @@ mc_thread_main(int argc, char *argv[]) /* store last control mode to detect mode switches */ bool flag_control_manual_enabled = false; bool flag_control_attitude_enabled = false; - bool control_yaw_position = true; bool reset_yaw_sp = true; + bool failsafe_first_time = true; /* prepare the handle for the failsafe throttle */ param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR"); float failsafe_throttle = 0.0f; - + param_get(failsafe_throttle_handle, &failsafe_throttle); while (!thread_should_exit) { @@ -176,7 +178,7 @@ mc_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(parameter_update), param_sub, &update); /* update parameters */ - // XXX no params here yet + param_get(failsafe_throttle_handle, &failsafe_throttle); } /* only run controller if attitude changed */ @@ -208,6 +210,9 @@ mc_thread_main(int argc, char *argv[]) /* get a local copy of the current sensor values */ orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); + /* set flag to safe value */ + control_yaw_position = true; + /* define which input is the dominating control input */ if (control_mode.flag_control_offboard_enabled) { /* offboard inputs */ @@ -225,47 +230,40 @@ mc_thread_main(int argc, char *argv[]) att_sp.yaw_body = offboard_sp.p3; att_sp.thrust = offboard_sp.p4; att_sp.timestamp = hrt_absolute_time(); - /* STEP 2: publish the result to the vehicle actuators */ + /* publish the result to the vehicle actuators */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); } } else if (control_mode.flag_control_manual_enabled) { - /* direct manual input */ + /* manual input */ if (control_mode.flag_control_attitude_enabled) { /* control attitude, update attitude setpoint depending on mode */ - /* initialize to current yaw if switching to manual or att control */ - if (control_mode.flag_control_attitude_enabled != flag_control_attitude_enabled || - control_mode.flag_control_manual_enabled != flag_control_manual_enabled) { - att_sp.yaw_body = att.yaw; - } - - static bool rc_loss_first_time = true; - /* if the RC signal is lost, try to stay level and go slowly back down to ground */ if (control_mode.failsave_highlevel) { + failsafe_first_time = false; + if (!control_mode.flag_control_velocity_enabled) { - /* Don't reset attitude setpoint in position control mode, it's handled by position controller. */ + /* don't reset attitude setpoint in position control mode, it's handled by position controller. */ att_sp.roll_body = 0.0f; att_sp.pitch_body = 0.0f; + } - if (!control_mode.flag_control_climb_rate_enabled) { - /* Don't touch throttle in modes with altitude hold, it's handled by position controller. - * - * Only go to failsafe throttle if last known throttle was - * high enough to create some lift to make hovering state likely. - * - * This is to prevent that someone landing, but not disarming his - * multicopter (throttle = 0) does not make it jump up in the air - * if shutting down his remote. - */ - if (isfinite(manual.throttle) && manual.throttle > 0.2f) { // TODO use landed status instead of throttle - /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ - param_get(failsafe_throttle_handle, &failsafe_throttle); - att_sp.thrust = failsafe_throttle; + if (!control_mode.flag_control_climb_rate_enabled) { + /* don't touch throttle in modes with altitude hold, it's handled by position controller. + * + * Only go to failsafe throttle if last known throttle was + * high enough to create some lift to make hovering state likely. + * + * This is to prevent that someone landing, but not disarming his + * multicopter (throttle = 0) does not make it jump up in the air + * if shutting down his remote. + */ + if (isfinite(manual.throttle) && manual.throttle > min_takeoff_throttle) { // TODO use landed status instead of throttle + /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ + att_sp.thrust = failsafe_throttle; - } else { - att_sp.thrust = 0.0f; - } + } else { + att_sp.thrust = 0.0f; } } @@ -273,46 +271,48 @@ mc_thread_main(int argc, char *argv[]) * since if the pilot regains RC control, he will be lost regarding * the current orientation. */ - if (rc_loss_first_time) - att_sp.yaw_body = att.yaw; - - rc_loss_first_time = false; + if (failsafe_first_time) { + reset_yaw_sp = true; + } } else { - rc_loss_first_time = true; + failsafe_first_time = true; /* control yaw in all manual / assisted modes */ /* set yaw if arming or switching to attitude stabilized mode */ - if (!flag_control_attitude_enabled) { + if (!flag_control_manual_enabled || !flag_control_attitude_enabled || !control_mode.flag_armed) { reset_yaw_sp = true; } /* only move setpoint if manual input is != 0 */ - if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { // TODO use landed status instead of throttle - rates_sp.yaw = manual.yaw; + // TODO review yaw restpoint reset + if ((manual.yaw < -yaw_deadzone || yaw_deadzone < manual.yaw) && manual.throttle > min_takeoff_throttle) { + /* control yaw rate */ control_yaw_position = false; - reset_yaw_sp = true; + rates_sp.yaw = manual.yaw; + reset_yaw_sp = true; // has no effect on control, just for beautiful log } else { - if (reset_yaw_sp) { - att_sp.yaw_body = att.yaw; - reset_yaw_sp = false; - } control_yaw_position = true; } if (!control_mode.flag_control_velocity_enabled) { - /* don't update attitude setpoint in position control mode */ + /* update attitude setpoint if not in position control mode */ att_sp.roll_body = manual.roll; att_sp.pitch_body = manual.pitch; if (!control_mode.flag_control_climb_rate_enabled) { - /* don't set throttle in altitude hold modes */ + /* pass throttle directly if not in altitude control mode */ att_sp.thrust = manual.throttle; } } - att_sp.timestamp = hrt_absolute_time(); + } + + /* reset yaw setpint to current position if needed */ + if (reset_yaw_sp) { + att_sp.yaw_body = att.yaw; + reset_yaw_sp = false; } if (motor_test_mode) { @@ -321,10 +321,11 @@ mc_thread_main(int argc, char *argv[]) att_sp.pitch_body = 0.0f; att_sp.yaw_body = 0.0f; att_sp.thrust = 0.1f; - att_sp.timestamp = hrt_absolute_time(); } - /* STEP 2: publish the controller output */ + att_sp.timestamp = hrt_absolute_time(); + + /* publish the controller output */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); } else { @@ -367,6 +368,7 @@ mc_thread_main(int argc, char *argv[]) rates[1] = att.pitchspeed; rates[2] = att.yawspeed; multirotor_control_rates(&rates_sp, rates, &actuators, reset_integral); + } else { /* rates controller disabled, set actuators to zero for safety */ actuators.control[0] = 0.0f; @@ -374,6 +376,7 @@ mc_thread_main(int argc, char *argv[]) actuators.control[2] = 0.0f; actuators.control[3] = 0.0f; } + actuators.timestamp = hrt_absolute_time(); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); From bb91484b2648800923a135be28924119a1382ba6 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 22 Aug 2013 17:34:59 +0200 Subject: [PATCH 450/486] Default flight mode switches parameters changed. --- src/modules/sensors/sensor_params.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 2bd869263c..8c2beb4561 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -167,8 +167,8 @@ PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3); PARAM_DEFINE_INT32(RC_MAP_YAW, 4); PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5); -PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 6); -PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 6); PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); //PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); From 41fac46ff08787d9b2e4d902045d65c205389abd Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 22 Aug 2013 18:05:30 +0200 Subject: [PATCH 451/486] mavlink VFR_HUD message fixed, minor fixes and cleanup --- src/modules/commander/commander.cpp | 4 ++-- src/modules/mavlink/orb_listener.c | 10 +++++----- .../position_estimator_inav_main.c | 2 +- .../uORB/topics/vehicle_global_position.h | 18 +++++++++--------- 4 files changed, 17 insertions(+), 17 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 04e6dd2cb2..4580c57bc5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -825,9 +825,9 @@ int commander_thread_main(int argc, char *argv[]) status.condition_landed = local_position.landed; status_changed = true; if (status.condition_landed) { - mavlink_log_info(mavlink_fd, "[cmd] LANDED"); + mavlink_log_critical(mavlink_fd, "[cmd] LANDED"); } else { - mavlink_log_info(mavlink_fd, "[cmd] IN AIR"); + mavlink_log_critical(mavlink_fd, "[cmd] IN AIR"); } } } diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 2a260861d3..97cf571e52 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -637,12 +637,12 @@ l_airspeed(const struct listener *l) orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed); float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); - float throttle = actuators_0.control_effective[3] * (UINT16_MAX - 1); - float alt = global_pos.alt; - float climb = global_pos.vz; + uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f; + uint16_t throttle = actuators_0.control_effective[3] * (UINT16_MAX - 1); + float alt = global_pos.relative_alt; + float climb = -global_pos.vz; - mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, - ((att.yaw + M_PI_F) / M_PI_F) * 180.0f, throttle, alt, climb); + mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, alt, climb); } static void * diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 3466841c4c..d35755b4f3 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -610,7 +610,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (local_pos.v_xy_valid) { global_pos.vx = local_pos.vx; global_pos.vy = local_pos.vy; - global_pos.hdg = atan2f(local_pos.vy, local_pos.vx); + global_pos.hdg = atan2f(local_pos.vy, local_pos.vx); // TODO is it correct? } if (local_pos.z_valid) { diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index f036c72237..822c323cfc 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -62,17 +62,17 @@ struct vehicle_global_position_s { uint64_t timestamp; /**< time of this estimate, in microseconds since system start */ - uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ + uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ bool valid; /**< true if position satisfies validity criteria of estimator */ - int32_t lat; /**< Latitude in 1E7 degrees LOGME */ - int32_t lon; /**< Longitude in 1E7 degrees LOGME */ - float alt; /**< Altitude in meters LOGME */ - float relative_alt; /**< Altitude above home position in meters, LOGME */ - float vx; /**< Ground X Speed (Latitude), m/s in ENU LOGME */ - float vy; /**< Ground Y Speed (Longitude), m/s in ENU LOGME */ - float vz; /**< Ground Z Speed (Altitude), m/s in ENU LOGME */ - float hdg; /**< Compass heading in radians -PI..+PI. */ + int32_t lat; /**< Latitude in 1E7 degrees */ + int32_t lon; /**< Longitude in 1E7 degrees */ + float alt; /**< Altitude in meters */ + float relative_alt; /**< Altitude above home position in meters, */ + float vx; /**< Ground X velocity, m/s in NED */ + float vy; /**< Ground Y velocity, m/s in NED */ + float vz; /**< Ground Z velocity, m/s in NED */ + float hdg; /**< Compass heading in radians -PI..+PI. */ }; From 330908225e5fcb1731df20e740dbfe403a7b30b9 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 22 Aug 2013 18:23:42 +0200 Subject: [PATCH 452/486] sdlog2: free buffer on exit --- src/modules/sdlog2/sdlog2.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 7f8648d95d..2b21bb5a5a 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1182,6 +1182,8 @@ int sdlog2_thread_main(int argc, char *argv[]) pthread_mutex_destroy(&logbuffer_mutex); pthread_cond_destroy(&logbuffer_cond); + free(lb.data); + warnx("exiting."); thread_running = false; From ca9aabf48bbf4ab050a13f12e8152490e2b1dbde Mon Sep 17 00:00:00 2001 From: tstellanova Date: Thu, 22 Aug 2013 17:29:06 -0700 Subject: [PATCH 453/486] add placeholder autoconfig file for X550 --- ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 | 107 +++++++++++++++++++ 1 file changed, 107 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 diff --git a/ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 b/ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 new file mode 100644 index 0000000000..41593fa6c0 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 @@ -0,0 +1,107 @@ +#!nsh +# +# Flight startup script for PX4FMU with PWM outputs. +# + +# disable USB and autostart +set USB no +set MODE custom + +echo "[init] doing PX4FMU Quad startup 666_fmu_quad_X550..." + +# +# Start the ORB +# +uorb start + +# +# Load microSD params +# +echo "[init] loading microSD params" +param select /fs/microsd/params +if [ -f /fs/microsd/params ] +then + param load /fs/microsd/params +fi + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set MC_ATTRATE_P 0.14 + param set MC_ATTRATE_I 0 + param set MC_ATTRATE_D 0.006 + param set MC_ATT_P 5.5 + param set MC_ATT_I 0 + param set MC_ATT_D 0 + param set MC_YAWPOS_D 0 + param set MC_YAWPOS_I 0 + param set MC_YAWPOS_P 0.6 + param set MC_YAWRATE_D 0 + param set MC_YAWRATE_I 0 + param set MC_YAWRATE_P 0.08 + param set RC_SCALE_PITCH 1 + param set RC_SCALE_ROLL 1 + param set RC_SCALE_YAW 3 + + param set SYS_AUTOCONFIG 0 + param save /fs/microsd/params +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor +# see https://pixhawk.ethz.ch/mavlink/ +# +param set MAV_TYPE 2 + +# +# Start MAVLink +# +mavlink start -d /dev/ttyS0 -b 57600 +usleep 5000 + +# +# Start the sensors and test them. +# +sh /etc/init.d/rc.sensors + +# +# Start the commander. +# +commander start + +# +# Start GPS interface (depends on orb) +# +gps start + +# +# Start the attitude estimator +# +attitude_estimator_ekf start + +echo "[init] starting PWM output" +fmu mode_pwm +mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix +pwm -u 400 -m 0xff + +# +# Start attitude control +# +multirotor_att_control start + +# +# Start logging +# +sdlog2 start -r 50 -a -b 14 + +# +# Start system state +# +if blinkm start +then + blinkm systemstate +fi From 64f402ce839f619572bb45a7b1a3126a2cd96d88 Mon Sep 17 00:00:00 2001 From: tstellanova Date: Thu, 22 Aug 2013 17:29:06 -0700 Subject: [PATCH 454/486] add placeholder autoconfig file for X550 --- ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 | 107 +++++++++++++++++++ 1 file changed, 107 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 diff --git a/ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 b/ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 new file mode 100644 index 0000000000..41593fa6c0 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 @@ -0,0 +1,107 @@ +#!nsh +# +# Flight startup script for PX4FMU with PWM outputs. +# + +# disable USB and autostart +set USB no +set MODE custom + +echo "[init] doing PX4FMU Quad startup 666_fmu_quad_X550..." + +# +# Start the ORB +# +uorb start + +# +# Load microSD params +# +echo "[init] loading microSD params" +param select /fs/microsd/params +if [ -f /fs/microsd/params ] +then + param load /fs/microsd/params +fi + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set MC_ATTRATE_P 0.14 + param set MC_ATTRATE_I 0 + param set MC_ATTRATE_D 0.006 + param set MC_ATT_P 5.5 + param set MC_ATT_I 0 + param set MC_ATT_D 0 + param set MC_YAWPOS_D 0 + param set MC_YAWPOS_I 0 + param set MC_YAWPOS_P 0.6 + param set MC_YAWRATE_D 0 + param set MC_YAWRATE_I 0 + param set MC_YAWRATE_P 0.08 + param set RC_SCALE_PITCH 1 + param set RC_SCALE_ROLL 1 + param set RC_SCALE_YAW 3 + + param set SYS_AUTOCONFIG 0 + param save /fs/microsd/params +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor +# see https://pixhawk.ethz.ch/mavlink/ +# +param set MAV_TYPE 2 + +# +# Start MAVLink +# +mavlink start -d /dev/ttyS0 -b 57600 +usleep 5000 + +# +# Start the sensors and test them. +# +sh /etc/init.d/rc.sensors + +# +# Start the commander. +# +commander start + +# +# Start GPS interface (depends on orb) +# +gps start + +# +# Start the attitude estimator +# +attitude_estimator_ekf start + +echo "[init] starting PWM output" +fmu mode_pwm +mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix +pwm -u 400 -m 0xff + +# +# Start attitude control +# +multirotor_att_control start + +# +# Start logging +# +sdlog2 start -r 50 -a -b 14 + +# +# Start system state +# +if blinkm start +then + blinkm systemstate +fi From 201bda457941fa55961eea4f13ec9fc9c24c0c61 Mon Sep 17 00:00:00 2001 From: tstellanova Date: Thu, 22 Aug 2013 17:36:10 -0700 Subject: [PATCH 455/486] start position estimator and position control --- ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 b/ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 index 41593fa6c0..a7ffffaeec 100644 --- a/ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 +++ b/ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 @@ -82,6 +82,10 @@ gps start # Start the attitude estimator # attitude_estimator_ekf start +# +# Start the position estimator +# +position_estimator_inav start echo "[init] starting PWM output" fmu mode_pwm @@ -93,6 +97,11 @@ pwm -u 400 -m 0xff # multirotor_att_control start +# +# Start position control +# +multirotor_pos_control start + # # Start logging # From f70a4b3b7045a24baf70e642e9a354a13e5fa7d1 Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 22 Aug 2013 23:47:55 -0700 Subject: [PATCH 456/486] Add support for adding extra files to the ROMFS from the config. If there is an IO firmware image already built when we build the corresponding FMU ROMFS, copy it into the ROMFS. Note - due to there being no fixed build ordering, to be certain that you have the most current IO firmware, you must build the IO firmware explicitly first. --- Makefile | 26 +++++++++++++------------- makefiles/config_px4fmu-v1_default.mk | 1 + makefiles/config_px4fmu-v2_default.mk | 4 +++- makefiles/firmware.mk | 22 +++++++++++++++++++--- makefiles/setup.mk | 1 + 5 files changed, 37 insertions(+), 17 deletions(-) diff --git a/Makefile b/Makefile index f7cab05c86..c34945ecab 100644 --- a/Makefile +++ b/Makefile @@ -114,8 +114,8 @@ $(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4: @$(ECHO) %%%% @$(ECHO) %%%% Building $(config) in $(work_dir) @$(ECHO) %%%% - $(Q) mkdir -p $(work_dir) - $(Q) make -r -C $(work_dir) \ + $(Q) $(MKDIR) -p $(work_dir) + $(Q) $(MAKE) -r -C $(work_dir) \ -f $(PX4_MK_DIR)firmware.mk \ CONFIG=$(config) \ WORK_DIR=$(work_dir) \ @@ -147,12 +147,12 @@ $(ARCHIVE_DIR)%.export: configuration = nsh $(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) @$(ECHO) %% Configuring NuttX for $(board) $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) - $(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean + $(Q) $(MAKE) -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean $(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(board) .) $(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(board)/$(configuration)) @$(ECHO) %% Exporting NuttX for $(board) - $(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) CONFIG_ARCH_BOARD=$(board) export - $(Q) mkdir -p $(dir $@) + $(Q) $(MAKE) -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) CONFIG_ARCH_BOARD=$(board) export + $(Q) $(MKDIR) -p $(dir $@) $(Q) $(COPY) $(NUTTX_SRC)nuttx-export.zip $@ $(Q) (cd $(NUTTX_SRC)/configs && $(RMDIR) $(board)) @@ -168,11 +168,11 @@ BOARD = $(BOARDS) menuconfig: $(NUTTX_SRC) @$(ECHO) %% Configuring NuttX for $(BOARD) $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) - $(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean + $(Q) $(MAKE) -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean $(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(BOARD) .) $(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(BOARD)/nsh) @$(ECHO) %% Running menuconfig for $(BOARD) - $(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) menuconfig + $(Q) $(MAKE) -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) menuconfig @$(ECHO) %% Saving configuration file $(Q)$(COPY) $(NUTTX_SRC).config $(PX4_BASE)nuttx-configs/$(BOARD)/nsh/defconfig else @@ -191,7 +191,7 @@ $(NUTTX_SRC): # Testing targets # testbuild: - $(Q) (cd $(PX4_BASE) && make distclean && make archives && make -j8) + $(Q) (cd $(PX4_BASE) && $(MAKE) distclean && $(MAKE) archives && $(MAKE) -j8) # # Cleanup targets. 'clean' should remove all built products and force @@ -206,8 +206,8 @@ clean: .PHONY: distclean distclean: clean $(Q) $(REMOVE) $(ARCHIVE_DIR)*.export - $(Q) make -C $(NUTTX_SRC) -r $(MQUIET) distclean - $(Q) (cd $(NUTTX_SRC)/configs && find . -maxdepth 1 -type l -delete) + $(Q) $(MAKE) -C $(NUTTX_SRC) -r $(MQUIET) distclean + $(Q) (cd $(NUTTX_SRC)/configs && $(FIND) . -maxdepth 1 -type l -delete) # # Print some help text @@ -229,9 +229,9 @@ help: @$(ECHO) " A limited set of configs can be built with CONFIGS=" @$(ECHO) "" @for config in $(CONFIGS); do \ - echo " $$config"; \ - echo " Build just the $$config firmware configuration."; \ - echo ""; \ + $(ECHO) " $$config"; \ + $(ECHO) " Build just the $$config firmware configuration."; \ + $(ECHO) ""; \ done @$(ECHO) " clean" @$(ECHO) " Remove all firmware build pieces." diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 8f2ade8dc8..0e4617f1db 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -6,6 +6,7 @@ # Use the configuration's ROMFS. # ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common +ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v1_default.bin # # Board support modules diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 101b497125..61df64a78c 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -3,9 +3,11 @@ # # -# Use the configuration's ROMFS. +# Use the configuration's ROMFS, copy the px4iov2 firmware into +# the ROMFS if it's available # ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common +ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v2_default.bin # # Board support modules diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index 8a027a0a81..b3e50501c7 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -322,7 +322,7 @@ endif # a root from several templates. That would be a nice feature. # -# Add dependencies on anything in the ROMFS root +# Add dependencies on anything in the ROMFS root directory ROMFS_FILES += $(wildcard \ $(ROMFS_ROOT)/* \ $(ROMFS_ROOT)/*/* \ @@ -334,7 +334,14 @@ ifeq ($(ROMFS_FILES),) $(error ROMFS_ROOT $(ROMFS_ROOT) specifies a directory containing no files) endif ROMFS_DEPS += $(ROMFS_FILES) + +# Extra files that may be copied into the ROMFS /extras directory +# ROMFS_EXTRA_FILES are required, ROMFS_OPTIONAL_FILES are optional +ROMFS_EXTRA_FILES += $(wildcard $(ROMFS_OPTIONAL_FILES)) +ROMFS_DEPS += $(ROMFS_EXTRA_FILES) + ROMFS_IMG = romfs.img +ROMFS_SCRATCH = romfs_scratch ROMFS_CSRC = $(ROMFS_IMG:.img=.c) ROMFS_OBJ = $(ROMFS_CSRC:.c=.o) LIBS += $(ROMFS_OBJ) @@ -345,9 +352,18 @@ $(ROMFS_OBJ): $(ROMFS_IMG) $(GLOBAL_DEPS) $(call BIN_TO_OBJ,$<,$@,romfs_img) # Generate the ROMFS image from the root -$(ROMFS_IMG): $(ROMFS_DEPS) $(GLOBAL_DEPS) +$(ROMFS_IMG): $(ROMFS_SCRATCH) $(ROMFS_DEPS) $(GLOBAL_DEPS) @$(ECHO) "ROMFS: $@" - $(Q) $(GENROMFS) -f $@ -d $(ROMFS_ROOT) -V "NSHInitVol" + $(Q) $(GENROMFS) -f $@ -d $(ROMFS_SCRATCH) -V "NSHInitVol" + +# Construct the ROMFS scratch root from the canonical root +$(ROMFS_SCRATCH): $(ROMFS_DEPS) $(GLOBAL_DEPS) + $(Q) $(MKDIR) -p $(ROMFS_SCRATCH) + $(Q) $(COPYDIR) $(ROMFS_ROOT)/* $(ROMFS_SCRATCH) +ifneq ($(ROMFS_EXTRA_FILES),) + $(Q) $(MKDIR) -p $(ROMFS_SCRATCH)/extras + $(Q) $(COPY) $(ROMFS_EXTRA_FILES) $(ROMFS_SCRATCH)/extras +endif EXTRA_CLEANS += $(ROMGS_OBJ) $(ROMFS_IMG) diff --git a/makefiles/setup.mk b/makefiles/setup.mk index 168e41a5cb..51f830b6ee 100644 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -71,6 +71,7 @@ export RMDIR = rm -rf export GENROMFS = genromfs export TOUCH = touch export MKDIR = mkdir +export FIND = find export ECHO = echo export UNZIP_CMD = unzip export PYTHON = python From 54711bbcfe52fe33c213b9e15a1da9ad978d3535 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 23 Aug 2013 00:23:32 -0700 Subject: [PATCH 457/486] In order to save people from themselves, force a given FMU version to depend on the corresponding _default IO version. This avoids the risk of building a new FMU ROMFS with an old IO firmware, at the cost of the sanity of anyone reading this. --- Makefile | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/Makefile b/Makefile index c34945ecab..cbf0da6c97 100644 --- a/Makefile +++ b/Makefile @@ -121,6 +121,19 @@ $(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4: WORK_DIR=$(work_dir) \ $(FIRMWARE_GOAL) +# +# Make FMU firmwares depend on pre-packaged IO binaries. +# +# This is a pretty vile hack, since it hard-codes knowledge of the FMU->IO dependency +# and forces the _default config in all cases. There has to be a better way to do this... +# +FMU_VERSION = $(patsubst px4fmu-%,%,$(word 1, $(subst _, ,$(1)))) +define FMU_DEP +$(BUILD_DIR)$(1).build/firmware.px4: $(IMAGE_DIR)px4io-$(call FMU_VERSION,$(1))_default.px4 +endef +FMU_CONFIGS := $(filter px4fmu%,$(CONFIGS)) +$(foreach config,$(FMU_CONFIGS),$(eval $(call FMU_DEP,$(config)))) + # # Build the NuttX export archives. # From d1fd1bbbf7c7fd6bc76137257d6ecb91e8b6f3d4 Mon Sep 17 00:00:00 2001 From: tstellanova Date: Fri, 23 Aug 2013 13:27:16 -0700 Subject: [PATCH 458/486] Fix timestamp on rates_sp --- .../multirotor_att_control/multirotor_attitude_control.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index 12d16f7c73..c78232f11c 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -247,6 +247,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s } rates_sp->thrust = att_sp->thrust; + //need to update the timestamp now that we've touched rates_sp + rates_sp->timestamp = hrt_absolute_time(); motor_skip_counter++; } From 5e9b508ea0ec799ab6f8723d114c999beffc347e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 23 Aug 2013 23:03:59 +0200 Subject: [PATCH 459/486] Indicate AUTO submodes in mavlink custom_mode. --- src/modules/commander/commander.cpp | 74 +++++++++++++----------- src/modules/commander/px4_custom_mode.h | 29 ++++++++-- src/modules/mavlink/mavlink.c | 24 ++++++-- src/modules/mavlink/mavlink_receiver.cpp | 5 +- 4 files changed, 87 insertions(+), 45 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 4580c57bc5..8bde6b7a90 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -274,7 +274,8 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe switch (cmd->command) { case VEHICLE_CMD_DO_SET_MODE: { uint8_t base_mode = (uint8_t) cmd->param1; - uint32_t custom_mode = (uint32_t) cmd->param2; + union px4_custom_mode custom_mode; + custom_mode.data_float = cmd->param2; // TODO remove debug code mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_mode); @@ -307,19 +308,19 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { /* use autopilot-specific mode */ - if (custom_mode == PX4_CUSTOM_MODE_MANUAL) { + if (custom_mode.main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { /* MANUAL */ main_res = main_state_transition(status, MAIN_STATE_MANUAL); - } else if (custom_mode == PX4_CUSTOM_MODE_SEATBELT) { + } else if (custom_mode.main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) { /* SEATBELT */ main_res = main_state_transition(status, MAIN_STATE_SEATBELT); - } else if (custom_mode == PX4_CUSTOM_MODE_EASY) { + } else if (custom_mode.main_mode == PX4_CUSTOM_MAIN_MODE_EASY) { /* EASY */ main_res = main_state_transition(status, MAIN_STATE_EASY); - } else if (custom_mode == PX4_CUSTOM_MODE_AUTO) { + } else if (custom_mode.main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ main_res = main_state_transition(status, MAIN_STATE_AUTO); } @@ -1544,43 +1545,46 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v break; case MAIN_STATE_AUTO: - if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - /* don't act while taking off */ - res = TRANSITION_NOT_CHANGED; - - } else { - if (current_status->condition_landed) { - /* if landed: transitions only to AUTO_READY are allowed */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); - // TRANSITION_DENIED is not possible here - break; + if (current_status->arming_state == ARMING_STATE_ARMED || current_status->arming_state == ARMING_STATE_ARMED_ERROR) { + if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + /* don't act while taking off */ + res = TRANSITION_NOT_CHANGED; } else { - /* if not landed: */ - if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) { - /* act depending on switches when manual control enabled */ - if (current_status->return_switch == RETURN_SWITCH_RETURN) { - /* RTL */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); - - } else { - if (current_status->mission_switch == MISSION_SWITCH_MISSION) { - /* MISSION */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); - - } else { - /* LOITER */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); - } - } + if (current_status->condition_landed) { + /* if landed: transitions only to AUTO_READY are allowed */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); + // TRANSITION_DENIED is not possible here + break; } else { - /* always switch to loiter in air when no RC control */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + /* if not landed: */ + if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) { + /* act depending on switches when manual control enabled */ + if (current_status->return_switch == RETURN_SWITCH_RETURN) { + /* RTL */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); + + } else { + if (current_status->mission_switch == MISSION_SWITCH_MISSION) { + /* MISSION */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); + + } else { + /* LOITER */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + } + } + + } else { + /* always switch to loiter in air when no RC control */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + } } } + } else { + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); } - break; default: diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index 4d4859a5c5..b60a7e0b9c 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -8,11 +8,30 @@ #ifndef PX4_CUSTOM_MODE_H_ #define PX4_CUSTOM_MODE_H_ -enum PX4_CUSTOM_MODE { - PX4_CUSTOM_MODE_MANUAL = 1, - PX4_CUSTOM_MODE_SEATBELT, - PX4_CUSTOM_MODE_EASY, - PX4_CUSTOM_MODE_AUTO, +enum PX4_CUSTOM_MAIN_MODE { + PX4_CUSTOM_MAIN_MODE_MANUAL = 1, + PX4_CUSTOM_MAIN_MODE_SEATBELT, + PX4_CUSTOM_MAIN_MODE_EASY, + PX4_CUSTOM_MAIN_MODE_AUTO, +}; + +enum PX4_CUSTOM_SUB_MODE_AUTO { + PX4_CUSTOM_SUB_MODE_AUTO_READY = 1, + PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF, + PX4_CUSTOM_SUB_MODE_AUTO_LOITER, + PX4_CUSTOM_SUB_MODE_AUTO_MISSION, + PX4_CUSTOM_SUB_MODE_AUTO_RTL, + PX4_CUSTOM_SUB_MODE_AUTO_LAND, +}; + +union px4_custom_mode { + struct { + uint16_t reserved; + uint8_t main_mode; + uint8_t sub_mode; + }; + uint32_t data; + float data_float; }; #endif /* PX4_CUSTOM_MODE_H_ */ diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 6d9ca1120e..93ec36d05f 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -205,19 +205,35 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u /* main state */ *mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; + union px4_custom_mode custom_mode; + custom_mode.data = 0; if (v_status.main_state == MAIN_STATE_MANUAL) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); - *mavlink_custom_mode = PX4_CUSTOM_MODE_MANUAL; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; } else if (v_status.main_state == MAIN_STATE_SEATBELT) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; - *mavlink_custom_mode = PX4_CUSTOM_MODE_SEATBELT; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; } else if (v_status.main_state == MAIN_STATE_EASY) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - *mavlink_custom_mode = PX4_CUSTOM_MODE_EASY; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY; } else if (v_status.main_state == MAIN_STATE_AUTO) { *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - *mavlink_custom_mode = PX4_CUSTOM_MODE_AUTO; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + if (v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF; + } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; + } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; + } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; + } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; + } } + *mavlink_custom_mode = custom_mode.data; /** * Set mavlink state diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 01bbabd46d..86fa736562 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -67,6 +67,7 @@ #include #include #include +#include __BEGIN_DECLS @@ -188,9 +189,11 @@ handle_message(mavlink_message_t *msg) mavlink_set_mode_t new_mode; mavlink_msg_set_mode_decode(msg, &new_mode); + union px4_custom_mode custom_mode; + custom_mode.data = new_mode.custom_mode; /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ vcmd.param1 = new_mode.base_mode; - vcmd.param2 = new_mode.custom_mode; + vcmd.param2 = custom_mode.data_float; vcmd.param3 = 0; vcmd.param4 = 0; vcmd.param5 = 0; From 8579d0b7c9f77c84fa9afa87f7ab2f353443a242 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 24 Aug 2013 20:31:01 +0200 Subject: [PATCH 460/486] Allow disarm by RC in assisted modes if landed and in AUTO_READY state. --- src/modules/commander/commander.cpp | 55 ++++++++++++++--------------- 1 file changed, 26 insertions(+), 29 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3654839fbb..f5a9d4088d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1017,46 +1017,42 @@ int commander_thread_main(int argc, char *argv[]) /* arm/disarm by RC */ res = TRANSITION_NOT_CHANGED; - /* check if left stick is in lower left position and we are in MANUAL or AUTO mode -> disarm + /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm * do it only for rotary wings */ if (status.is_rotary_wing && (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && - (status.main_state == MAIN_STATE_MANUAL || status.navigation_state == NAVIGATION_STATE_AUTO_READY)) { - if (sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { - if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ - arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - res = arming_state_transition(&status, &safety, new_arming_state, &armed); - stick_off_counter = 0; - - } else { - stick_off_counter++; - } - - stick_on_counter = 0; + (status.main_state == MAIN_STATE_MANUAL || status.navigation_state == NAVIGATION_STATE_AUTO_READY || + (status.condition_landed && ( + status.navigation_state == NAVIGATION_STATE_ALTHOLD || + status.navigation_state == NAVIGATION_STATE_VECTOR + ))) && sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { + /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ + arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); + res = arming_state_transition(&status, &safety, new_arming_state, &armed); + stick_off_counter = 0; } else { - stick_off_counter = 0; + stick_off_counter++; } + } else { + stick_off_counter = 0; } - /* check if left stick is in lower right position and we're in manual mode -> arm */ + /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ if (status.arming_state == ARMING_STATE_STANDBY && - status.main_state == MAIN_STATE_MANUAL) { - if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { - if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); - stick_on_counter = 0; - - } else { - stick_on_counter++; - } - - stick_off_counter = 0; + status.main_state == MAIN_STATE_MANUAL && + sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { + res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); + stick_on_counter = 0; } else { - stick_on_counter = 0; + stick_on_counter++; } + + } else { + stick_on_counter = 0; } if (res == TRANSITION_CHANGED) { @@ -1702,7 +1698,8 @@ void *commander_low_prio_loop(void *arg) /* ignore commands the high-prio loop handles */ if (cmd.command == VEHICLE_CMD_DO_SET_MODE || - cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM) + cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM || + cmd.command == VEHICLE_CMD_NAV_TAKEOFF) continue; /* only handle low-priority commands here */ From 41dfdfb1a4b974b5d32788852768513d0dac7a67 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 24 Aug 2013 20:32:46 +0200 Subject: [PATCH 461/486] Use common rc.multirotor script (now only in 01_fmu_quad_x). --- ROMFS/px4fmu_common/init.d/01_fmu_quad_x | 33 +++------------- ROMFS/px4fmu_common/init.d/rc.multirotor | 49 ++++++++++++++++++++++++ 2 files changed, 54 insertions(+), 28 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/rc.multirotor diff --git a/ROMFS/px4fmu_common/init.d/01_fmu_quad_x b/ROMFS/px4fmu_common/init.d/01_fmu_quad_x index c1f3187f93..8223b3ea5a 100644 --- a/ROMFS/px4fmu_common/init.d/01_fmu_quad_x +++ b/ROMFS/px4fmu_common/init.d/01_fmu_quad_x @@ -62,44 +62,21 @@ param set MAV_TYPE 2 # mavlink start -d /dev/ttyS0 -b 57600 usleep 5000 - -# -# Start the sensors and test them. -# -sh /etc/init.d/rc.sensors - -# -# Start the commander. -# -commander start # -# Start GPS interface (depends on orb) +# Start common for all multirotors apps # -gps start - +sh /etc/init.d/rc.multirotor + # -# Start the attitude estimator +# Start PWM output # -attitude_estimator_ekf start - -echo "[init] starting PWM output" fmu mode_pwm mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix pwm -u 400 -m 0xff -# -# Start attitude control -# -multirotor_att_control start - -# -# Start logging -# -sdlog2 start -r 50 -a -b 14 - # Try to get an USB console nshterm /dev/ttyACM0 & # Exit, because /dev/ttyS0 is needed for MAVLink -exit +#exit diff --git a/ROMFS/px4fmu_common/init.d/rc.multirotor b/ROMFS/px4fmu_common/init.d/rc.multirotor new file mode 100644 index 0000000000..81184f3632 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.multirotor @@ -0,0 +1,49 @@ +#!nsh +# +# Standard everything needed for multirotors except mixer, output and mavlink +# + +# +# Start the sensors and test them. +# +sh /etc/init.d/rc.sensors + +# +# Start the commander. +# +commander start + +# +# Start GPS interface (depends on orb) +# +gps start + +# +# Start the attitude estimator +# +attitude_estimator_ekf start + +# +# Start position estimator +# +position_estimator_inav start + +# +# Start attitude control +# +multirotor_att_control start + +# +# Start position control +# +multirotor_pos_control start + +# +# Start logging +# +if [ $BOARD == fmuv1 ] +then + sdlog2 start -r 50 -a -b 16 +else + sdlog2 start -r 200 -a -b 16 +fi From e119bbb0f1161c71b8c2dcbbbc150d40b356c4b1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Aug 2013 16:33:14 +0200 Subject: [PATCH 462/486] A lot more on calibration and RC checks. Needs more testing, but no known issues --- ROMFS/px4fmu_common/init.d/rcS | 6 + .../commander/accelerometer_calibration.cpp | 18 ++- src/modules/commander/commander.cpp | 6 + src/modules/commander/mag_calibration.cpp | 10 +- src/modules/sensors/sensor_params.c | 10 +- src/modules/sensors/sensors.cpp | 37 +---- src/modules/systemlib/module.mk | 3 +- src/modules/systemlib/rc_check.c | 148 ++++++++++++++++++ src/modules/systemlib/rc_check.h | 52 ++++++ .../preflight_check/preflight_check.c | 97 +----------- 10 files changed, 246 insertions(+), 141 deletions(-) create mode 100644 src/modules/systemlib/rc_check.c create mode 100644 src/modules/systemlib/rc_check.h diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 6b624b2780..30edf679ad 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -182,6 +182,12 @@ fi # Try to get an USB console nshterm /dev/ttyACM0 & +# Start any custom extensions that might be missing +if [ -f /fs/microsd/etc/rc.local ] +then + sh /fs/microsd/etc/rc.local +fi + # If none of the autostart scripts triggered, get a minimal setup if [ $MODE == autostart ] then diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index c2b2e92581..82df7c37d6 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -136,6 +136,7 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo int do_accel_calibration(int mavlink_fd) { /* announce change */ mavlink_log_info(mavlink_fd, "accel calibration started"); + mavlink_log_info(mavlink_fd, "accel cal progress <0> percent"); /* measure and calculate offsets & scales */ float accel_offs[3]; @@ -211,17 +212,28 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float } int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); + + unsigned done_count = 0; + while (true) { bool done = true; - char str[80]; + char str[60]; int str_ptr; str_ptr = sprintf(str, "keep vehicle still:"); + unsigned old_done_count = done_count; + done_count = 0; for (int i = 0; i < 6; i++) { if (!data_collected[i]) { str_ptr += sprintf(&(str[str_ptr]), " %s", orientation_strs[i]); done = false; + } else { + done_count++; } } + + if (old_done_count != done_count) + mavlink_log_info(mavlink_fd, "accel cal progress <%u> percent", 17 * done_count); + if (done) break; mavlink_log_info(mavlink_fd, str); @@ -236,8 +248,8 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float continue; } - sprintf(str, "meas started: %s", orientation_strs[orient]); - mavlink_log_info(mavlink_fd, str); + // sprintf(str, + mavlink_log_info(mavlink_fd, "accel meas started: %s", orientation_strs[orient]); read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num); str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient], (double)accel_ref[orient][0], diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3654839fbb..e3d314881c 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -84,6 +84,7 @@ #include #include #include +#include #include "px4_custom_mode.h" #include "commander_helper.h" @@ -617,6 +618,8 @@ int commander_thread_main(int argc, char *argv[]) bool updated = false; + bool rc_calibration_ok = (OK == rc_calibration_check()); + /* Subscribe to safety topic */ int safety_sub = orb_subscribe(ORB_ID(safety)); memset(&safety, 0, sizeof(safety)); @@ -727,6 +730,9 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_system_id, &(status.system_id)); param_get(_param_component_id, &(status.component_id)); status_changed = true; + + /* Re-check RC calibration */ + rc_calibration_ok = (OK == rc_calibration_check()); } } diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 9263c6a159..42f0190c7e 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -100,6 +100,8 @@ int do_mag_calibration(int mavlink_fd) close(fd); + mavlink_log_info(mavlink_fd, "mag cal progress <20> percent"); + /* calibrate offsets */ // uint64_t calibration_start = hrt_absolute_time(); @@ -135,9 +137,8 @@ int do_mag_calibration(int mavlink_fd) axis_index++; - char buf[50]; - sprintf(buf, "please rotate around %c", axislabels[axis_index]); - mavlink_log_info(mavlink_fd, buf); + mavlink_log_info(mavlink_fd, "please rotate in a figure 8 or around %c", axislabels[axis_index]); + mavlink_log_info(mavlink_fd, "mag cal progress <%u> percent", 20 + (calibration_maxcount * 50) / calibration_counter); tune_neutral(); axis_deadline += calibration_interval / 3; @@ -251,6 +252,8 @@ int do_mag_calibration(int mavlink_fd) warnx("Setting Z mag scale failed!\n"); } + mavlink_log_info(mavlink_fd, "mag cal progress <90> percent"); + /* auto-save to EEPROM */ int save_ret = param_save_default(); @@ -274,6 +277,7 @@ int do_mag_calibration(int mavlink_fd) mavlink_log_info(mavlink_fd, buf); mavlink_log_info(mavlink_fd, "mag calibration done"); + mavlink_log_info(mavlink_fd, "mag cal progress <100> percent"); return OK; /* third beep by cal end routine */ diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index b45317dbef..fd2a6318eb 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -157,7 +157,6 @@ PARAM_DEFINE_FLOAT(RC14_MAX, 2000); PARAM_DEFINE_FLOAT(RC14_REV, 1.0f); PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f); -PARAM_DEFINE_INT32(RC_TYPE, 1); /** 1 = FUTABA, 2 = Spektrum, 3 = Graupner HoTT, 4 = Turnigy 9x */ PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */ PARAM_DEFINE_INT32(RC_DSM_BIND, 0); /* 0 = Idle, 1 = Start DSM2 bind, 2 = Start DSMX bind */ @@ -184,10 +183,7 @@ PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0); PARAM_DEFINE_INT32(RC_MAP_AUX1, 0); /**< default function: camera yaw / azimuth */ PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /**< default function: camera pitch / tilt */ -PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); /**< default function: camera trigger */ -PARAM_DEFINE_INT32(RC_MAP_AUX4, 0); /**< default function: camera roll */ -PARAM_DEFINE_INT32(RC_MAP_AUX5, 0); /**< default function: payload drop */ -PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.4f); -PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.4f); -PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 1.0f); +PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f); +PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f); +PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index ded39c4651..e857b1c2f7 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -305,8 +305,6 @@ private: int board_rotation; int external_mag_rotation; - int rc_type; - int rc_map_roll; int rc_map_pitch; int rc_map_yaw; @@ -342,9 +340,6 @@ private: param_t max[_rc_max_chan_count]; param_t rev[_rc_max_chan_count]; param_t dz[_rc_max_chan_count]; - param_t rc_type; - - param_t rc_demix; param_t gyro_offset[3]; param_t gyro_scale[3]; @@ -566,8 +561,6 @@ Sensors::Sensors() : } - _parameter_handles.rc_type = param_find("RC_TYPE"); - /* mandatory input switched, mapped to channels 1-4 per default */ _parameter_handles.rc_map_roll = param_find("RC_MAP_ROLL"); _parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH"); @@ -692,11 +685,6 @@ Sensors::parameters_update() if (!rc_valid) warnx("WARNING WARNING WARNING\n\nRC CALIBRATION NOT SANE!\n\n"); - /* remote control type */ - if (param_get(_parameter_handles.rc_type, &(_parameters.rc_type)) != OK) { - warnx("Failed getting remote control type"); - } - /* channel mapping */ if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) { warnx("Failed getting roll chan index"); @@ -738,26 +726,11 @@ Sensors::parameters_update() // warnx("Failed getting offboard control mode sw chan index"); // } - if (param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)) != OK) { - warnx("Failed getting mode aux 1 index"); - } - - if (param_get(_parameter_handles.rc_map_aux2, &(_parameters.rc_map_aux2)) != OK) { - warnx("Failed getting mode aux 2 index"); - } - - if (param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)) != OK) { - warnx("Failed getting mode aux 3 index"); - } - - if (param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4)) != OK) { - warnx("Failed getting mode aux 4 index"); - } - - if (param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)) != OK) { - warnx("Failed getting mode aux 5 index"); - } - + param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)); + param_get(_parameter_handles.rc_map_aux2, &(_parameters.rc_map_aux2)); + param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)); + param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4)); + param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)); param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll)); param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch)); param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw)); diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index 94c744c039..843cda7225 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -48,4 +48,5 @@ SRCS = err.c \ systemlib.c \ airspeed.c \ system_params.c \ - mavlink_log.c + mavlink_log.c \ + rc_check.c diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c new file mode 100644 index 0000000000..9d47d8000d --- /dev/null +++ b/src/modules/systemlib/rc_check.c @@ -0,0 +1,148 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file rc_check.c + * + * RC calibration check + */ + +#include + +#include +#include + +#include +#include +#include +#include + +int rc_calibration_check(void) { + + char nbuf[20]; + param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max, + _parameter_handles_rev, _parameter_handles_dz; + + int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + + float param_min, param_max, param_trim, param_rev, param_dz; + + /* first check channel mappings */ + /* check which map param applies */ + // if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) { + // mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1); + // /* give system time to flush error message in case there are more */ + // usleep(100000); + // count++; + // } + + + + for (int i = 0; i < RC_CHANNELS_MAX; i++) { + /* should the channel be enabled? */ + uint8_t count = 0; + + /* min values */ + sprintf(nbuf, "RC%d_MIN", i + 1); + _parameter_handles_min = param_find(nbuf); + param_get(_parameter_handles_min, ¶m_min); + + /* trim values */ + sprintf(nbuf, "RC%d_TRIM", i + 1); + _parameter_handles_trim = param_find(nbuf); + param_get(_parameter_handles_trim, ¶m_trim); + + /* max values */ + sprintf(nbuf, "RC%d_MAX", i + 1); + _parameter_handles_max = param_find(nbuf); + param_get(_parameter_handles_max, ¶m_max); + + /* channel reverse */ + sprintf(nbuf, "RC%d_REV", i + 1); + _parameter_handles_rev = param_find(nbuf); + param_get(_parameter_handles_rev, ¶m_rev); + + /* channel deadzone */ + sprintf(nbuf, "RC%d_DZ", i + 1); + _parameter_handles_dz = param_find(nbuf); + param_get(_parameter_handles_dz, ¶m_dz); + + /* assert min..center..max ordering */ + if (param_min < 500) { + count++; + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MIN < 500", i+1); + /* give system time to flush error message in case there are more */ + usleep(100000); + } + if (param_max > 2500) { + count++; + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAX > 2500", i+1); + /* give system time to flush error message in case there are more */ + usleep(100000); + } + if (param_trim < param_min) { + count++; + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN", i+1); + /* give system time to flush error message in case there are more */ + usleep(100000); + } + if (param_trim > param_max) { + count++; + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM > MAX", i+1); + /* give system time to flush error message in case there are more */ + usleep(100000); + } + + /* assert deadzone is sane */ + if (param_dz > 500) { + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_DZ > 500", i+1); + /* give system time to flush error message in case there are more */ + usleep(100000); + count++; + } + + /* check which map param applies */ + // if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) { + // mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1); + // /* give system time to flush error message in case there are more */ + // usleep(100000); + // count++; + // } + + /* sanity checks pass, enable channel */ + if (count) { + mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); + usleep(100000); + } + } +} diff --git a/src/modules/systemlib/rc_check.h b/src/modules/systemlib/rc_check.h new file mode 100644 index 0000000000..e2238d1516 --- /dev/null +++ b/src/modules/systemlib/rc_check.h @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file rc_check.h + * + * RC calibration check + */ + +#pragma once + + __BEGIN_DECLS + +/** + * Check the RC calibration + * + * @return 0 / OK if RC calibration is ok, index + 1 of the first + * channel that failed else (so 1 == first channel failed) + */ +__EXPORT int rc_calibration_check(void); + +__END_DECLS diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index e7d9ce85fb..4c19dcffb6 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -57,6 +57,7 @@ #include #include +#include __EXPORT int preflight_check_main(int argc, char *argv[]); static int led_toggle(int leds, int led); @@ -139,101 +140,7 @@ int preflight_check_main(int argc, char *argv[]) /* ---- RC CALIBRATION ---- */ - param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max, - _parameter_handles_rev, _parameter_handles_dz; - - float param_min, param_max, param_trim, param_rev, param_dz; - - bool rc_ok = true; - char nbuf[20]; - - /* first check channel mappings */ - /* check which map param applies */ - // if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) { - // mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1); - // /* give system time to flush error message in case there are more */ - // usleep(100000); - // count++; - // } - - for (int i = 0; i < 12; i++) { - /* should the channel be enabled? */ - uint8_t count = 0; - - /* min values */ - sprintf(nbuf, "RC%d_MIN", i + 1); - _parameter_handles_min = param_find(nbuf); - param_get(_parameter_handles_min, ¶m_min); - - /* trim values */ - sprintf(nbuf, "RC%d_TRIM", i + 1); - _parameter_handles_trim = param_find(nbuf); - param_get(_parameter_handles_trim, ¶m_trim); - - /* max values */ - sprintf(nbuf, "RC%d_MAX", i + 1); - _parameter_handles_max = param_find(nbuf); - param_get(_parameter_handles_max, ¶m_max); - - /* channel reverse */ - sprintf(nbuf, "RC%d_REV", i + 1); - _parameter_handles_rev = param_find(nbuf); - param_get(_parameter_handles_rev, ¶m_rev); - - /* channel deadzone */ - sprintf(nbuf, "RC%d_DZ", i + 1); - _parameter_handles_dz = param_find(nbuf); - param_get(_parameter_handles_dz, ¶m_dz); - - /* assert min..center..max ordering */ - if (param_min < 500) { - count++; - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MIN < 500", i+1); - /* give system time to flush error message in case there are more */ - usleep(100000); - } - if (param_max > 2500) { - count++; - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAX > 2500", i+1); - /* give system time to flush error message in case there are more */ - usleep(100000); - } - if (param_trim < param_min) { - count++; - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN", i+1); - /* give system time to flush error message in case there are more */ - usleep(100000); - } - if (param_trim > param_max) { - count++; - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM > MAX", i+1); - /* give system time to flush error message in case there are more */ - usleep(100000); - } - - /* assert deadzone is sane */ - if (param_dz > 500) { - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_DZ > 500", i+1); - /* give system time to flush error message in case there are more */ - usleep(100000); - count++; - } - - /* check which map param applies */ - // if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) { - // mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1); - // /* give system time to flush error message in case there are more */ - // usleep(100000); - // count++; - // } - - /* sanity checks pass, enable channel */ - if (count) { - mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); - usleep(100000); - rc_ok = false; - } - } + bool rc_ok = (OK == rc_calibration_check()); /* require RC ok to keep system_ok */ system_ok &= rc_ok; From 557d3f22de25a392995f2803363f32fdbc6ce843 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 25 Aug 2013 19:27:11 +0200 Subject: [PATCH 463/486] Startup scripts major cleanup --- ROMFS/px4fmu_common/init.d/01_fmu_quad_x | 58 ++-- ROMFS/px4fmu_common/init.d/02_io_quad_x | 90 ++---- ROMFS/px4fmu_common/init.d/08_ardrone | 79 ++--- ROMFS/px4fmu_common/init.d/09_ardrone_flow | 67 ++-- ROMFS/px4fmu_common/init.d/10_io_f330 | 90 ++---- ROMFS/px4fmu_common/init.d/15_io_tbs | 91 +----- ROMFS/px4fmu_common/init.d/30_io_camflyer | 64 +--- ROMFS/px4fmu_common/init.d/31_io_phantom | 93 +----- ROMFS/px4fmu_common/init.d/40_io_segway | 84 +---- .../{666_fmu_quad_x550 => 666_fmu_q_x550} | 84 +---- ROMFS/px4fmu_common/init.d/rc.io | 23 ++ ROMFS/px4fmu_common/init.d/rc.logging | 7 +- ROMFS/px4fmu_common/init.d/rc.multirotor | 10 - ROMFS/px4fmu_common/init.d/rcS | 301 +++++++++--------- 14 files changed, 338 insertions(+), 803 deletions(-) rename ROMFS/px4fmu_common/init.d/{666_fmu_quad_x550 => 666_fmu_q_x550} (50%) create mode 100644 ROMFS/px4fmu_common/init.d/rc.io diff --git a/ROMFS/px4fmu_common/init.d/01_fmu_quad_x b/ROMFS/px4fmu_common/init.d/01_fmu_quad_x index 8223b3ea5a..f57e4bd68d 100644 --- a/ROMFS/px4fmu_common/init.d/01_fmu_quad_x +++ b/ROMFS/px4fmu_common/init.d/01_fmu_quad_x @@ -1,28 +1,6 @@ #!nsh -# -# Flight startup script for PX4FMU with PWM outputs. -# - -# disable USB and autostart -set USB no -set MODE custom - -echo "[init] doing PX4FMU Quad startup..." - -# -# Start the ORB -# -uorb start - -# -# Load microSD params -# -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] -then - param load /fs/microsd/params -fi + +echo "[init] 01_fmu_quad_x: PX4FMU Quad X with PWM outputs" # # Load default params for this platform @@ -52,31 +30,35 @@ fi # # Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ +# MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 - + # # Start MAVLink # mavlink start -d /dev/ttyS0 -b 57600 usleep 5000 -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor - # # Start PWM output # fmu mode_pwm -mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix -pwm -u 400 -m 0xff - -# Try to get an USB console -nshterm /dev/ttyACM0 & +# +# Load mixer +# +mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix + +# +# Set PWM output frequency +# +pwm -u 400 -m 0xff + +# +# Start common for all multirotors apps +# +sh /etc/init.d/rc.multirotor + # Exit, because /dev/ttyS0 is needed for MAVLink -#exit +exit diff --git a/ROMFS/px4fmu_common/init.d/02_io_quad_x b/ROMFS/px4fmu_common/init.d/02_io_quad_x index 49483d14fe..a37c26ad1d 100644 --- a/ROMFS/px4fmu_common/init.d/02_io_quad_x +++ b/ROMFS/px4fmu_common/init.d/02_io_quad_x @@ -1,26 +1,6 @@ #!nsh -# -# Flight startup script for PX4FMU+PX4IO -# - -# disable USB and autostart -set USB no -set MODE custom - -# -# Start the ORB (first app to start) -# -uorb start - -# -# Load microSD params -# -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] -then - param load /fs/microsd/params -fi + +echo "[init] 02_io_quad_x: PX4FMU+PX4IO Quad X with PWM outputs" # # Load default params for this platform @@ -28,74 +8,40 @@ fi if param compare SYS_AUTOCONFIG 1 then # Set all params here, then disable autoconfig + # TODO + param set SYS_AUTOCONFIG 0 param save /fs/microsd/params fi - + # # Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ +# MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 # -# Start MAVLink (depends on orb) +# Start MAVLink # mavlink start -d /dev/ttyS1 -b 57600 usleep 5000 - -# -# Start the commander (depends on orb, mavlink) -# -commander start - -# -# Start PX4IO interface (depends on orb, commander) -# -px4io start - -# -# Allow PX4IO to recover from midair restarts. -# this is very unlikely, but quite safe and robust. -px4io recovery # -# Disable px4io topic limiting +# Start and configure PX4IO interface # -px4io limit 200 - +sh /etc/init.d/rc.io + # -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Start GPS interface (depends on orb) -# -gps start - -# -# Start the attitude estimator (depends on orb) -# -attitude_estimator_ekf start - -# -# Load mixer and start controllers (depends on px4io) +# Load mixer # mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix + +# +# Set PWM output frequency +# pwm -u 400 -m 0xff -multirotor_att_control start - + # -# Start logging once armed +# Start common for all multirotors apps # -sdlog2 start -r 50 -a -b 14 - -# -# Start system state -# -if blinkm start -then - blinkm systemstate -fi +sh /etc/init.d/rc.multirotor diff --git a/ROMFS/px4fmu_common/init.d/08_ardrone b/ROMFS/px4fmu_common/init.d/08_ardrone index 5bb1491e91..eb9f82f771 100644 --- a/ROMFS/px4fmu_common/init.d/08_ardrone +++ b/ROMFS/px4fmu_common/init.d/08_ardrone @@ -1,86 +1,45 @@ #!nsh -# -# Flight startup script for PX4FMU on PX4IOAR carrier board. -# - -echo "[init] doing PX4IOAR startup..." + +echo "[init] 08_ardrone: PX4FMU on PX4IOAR carrier board" # -# Start the ORB +# Load default params for this platform # -uorb start - -# -# Load microSD params -# -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] +if param compare SYS_AUTOCONFIG 1 then - param load /fs/microsd/params + # Set all params here, then disable autoconfig + # TODO + + param set SYS_AUTOCONFIG 0 + param save /fs/microsd/params fi # # Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ +# MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 +# +# Start MAVLink +# +mavlink start -d /dev/ttyS0 -b 57600 +usleep 5000 + # # Configure PX4FMU for operation with PX4IOAR # fmu mode_gpio_serial -# -# Start the sensors. -# -sh /etc/init.d/rc.sensors - -# -# Start MAVLink -# -mavlink start -d /dev/ttyS0 -b 57600 -usleep 5000 - -# -# Start the commander. -# -commander start - -# -# Start the attitude estimator -# -attitude_estimator_ekf start - -# -# Fire up the multi rotor attitude controller -# -multirotor_att_control start - # # Fire up the AR.Drone interface. # ardrone_interface start -d /dev/ttyS1 - -# -# Start logging once armed -# -sdlog2 start -r 50 -a -b 14 - -# -# Start GPS capture -# -gps start # -# startup is done; we don't want the shell because we -# use the same UART for telemetry +# Start common for all multirotors apps # -echo "[init] startup done" - -# Try to get an USB console -nshterm /dev/ttyACM0 & - +sh /etc/init.d/rc.multirotor + # Exit, because /dev/ttyS0 is needed for MAVLink exit diff --git a/ROMFS/px4fmu_common/init.d/09_ardrone_flow b/ROMFS/px4fmu_common/init.d/09_ardrone_flow index a223ef7aab..44fbb79b7b 100644 --- a/ROMFS/px4fmu_common/init.d/09_ardrone_flow +++ b/ROMFS/px4fmu_common/init.d/09_ardrone_flow @@ -1,49 +1,47 @@ #!nsh + +echo "[init] 09_ardrone_flow: PX4FMU on PX4IOAR carrier board with PX4FLOW" + # -# Flight startup script for PX4FMU on PX4IOAR carrier board. +# Load default params for this platform # - -echo "[init] doing PX4IOAR startup..." - -# -# Start the ORB -# -uorb start - -# -# Load microSD params -# -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] +if param compare SYS_AUTOCONFIG 1 then - param load /fs/microsd/params + # Set all params here, then disable autoconfig + # TODO + + param set SYS_AUTOCONFIG 0 + param save /fs/microsd/params fi # # Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ +# MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 +# +# Start MAVLink and MAVLink Onboard (PX4FLOW Sensor) +# +mavlink start -d /dev/ttyS0 -b 57600 +mavlink_onboard start -d /dev/ttyS3 -b 115200 +usleep 5000 + # # Configure PX4FMU for operation with PX4IOAR # fmu mode_gpio_serial + +# +# Fire up the AR.Drone interface. +# +ardrone_interface start -d /dev/ttyS1 # # Start the sensors. # sh /etc/init.d/rc.sensors -# -# Start MAVLink and MAVLink Onboard (Flow Sensor) -# -mavlink start -d /dev/ttyS0 -b 57600 -mavlink_onboard start -d /dev/ttyS3 -b 115200 -usleep 5000 - # # Start the commander. # @@ -73,25 +71,6 @@ flow_position_control start # Fire up the flow speed controller # flow_speed_control start - -# -# Fire up the AR.Drone interface. -# -ardrone_interface start -d /dev/ttyS1 - -# -# Start logging once armed -# -sdlog2 start -r 50 -a -b 14 -# -# startup is done; we don't want the shell because we -# use the same UART for telemetry -# -echo "[init] startup done" - -# Try to get an USB console -nshterm /dev/ttyACM0 & - # Exit, because /dev/ttyS0 is needed for MAVLink exit diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_io_f330 index 50842764a6..7b6509bf88 100644 --- a/ROMFS/px4fmu_common/init.d/10_io_f330 +++ b/ROMFS/px4fmu_common/init.d/10_io_f330 @@ -1,12 +1,6 @@ #!nsh -# -# Flight startup script for PX4FMU+PX4IO on an F330 quad. -# -# -# Start the ORB (first app to start) -# -uorb start +echo "[init] 10_io_f330: PX4FMU+PX4IO on a DJI F330 quad frame" # # Load default params for this platform @@ -35,8 +29,7 @@ fi # # Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ +# MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 @@ -47,71 +40,28 @@ mavlink start usleep 5000 # -# Start the commander (depends on orb, mavlink) +# Start and configure PX4IO interface # -commander start +sh /etc/init.d/rc.io # -# Start PX4IO interface (depends on orb, commander) +# Set PWM values for DJI ESCs # -if px4io start -then - # - # This sets a PWM right after startup (regardless of safety button) - # - px4io idle 900 900 900 900 - - pwm -u 400 -m 0xff - - # - # Allow PX4IO to recover from midair restarts. - # this is very unlikely, but quite safe and robust. - px4io recovery - - - - # - # The values are for spinning motors when armed using DJI ESCs - # - px4io min 1200 1200 1200 1200 - - # - # Upper limits could be higher, this is on the safe side - # - px4io max 1800 1800 1800 1800 - - mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix - -else - # SOS - tone_alarm 6 -fi - -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Start GPS interface (depends on orb) -# -gps start - -# -# Start the attitude estimator (depends on orb) -# -attitude_estimator_ekf start - -multirotor_att_control start +px4io idle 900 900 900 900 +px4io min 1200 1200 1200 1200 +px4io max 1800 1800 1800 1800 # -# Disable px4io topic limiting and start logging +# Load mixer # -if [ $BOARD == fmuv1 ] -then - px4io limit 200 - sdlog2 start -r 50 -a -b 16 -else - px4io limit 400 - sdlog2 start -r 200 -a -b 16 -fi +mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix + +# +# Set PWM output frequency +# +pwm -u 400 -m 0xff + +# +# Start common for all multirotors apps +# +sh /etc/init.d/rc.multirotor diff --git a/ROMFS/px4fmu_common/init.d/15_io_tbs b/ROMFS/px4fmu_common/init.d/15_io_tbs index 237bb4267c..b4f063e524 100644 --- a/ROMFS/px4fmu_common/init.d/15_io_tbs +++ b/ROMFS/px4fmu_common/init.d/15_io_tbs @@ -1,27 +1,6 @@ #!nsh -# -# Flight startup script for PX4FMU+PX4IO on a Team Blacksheep Discovery quad -# with stock DJI ESCs, motors and props. -# - -# disable USB and autostart -set USB no -set MODE custom -# -# Start the ORB (first app to start) -# -uorb start - -# -# Load microSD params -# -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] -then - param load /fs/microsd/params -fi +echo "[init] 15_io_tbs: PX4FMU+PX4IO on a Team Blacksheep Discovery quad" # # Load default params for this platform @@ -47,11 +26,10 @@ then param save /fs/microsd/params fi - + # # Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ +# MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 @@ -62,77 +40,28 @@ mavlink start usleep 5000 # -# Start the commander (depends on orb, mavlink) +# Start and configure PX4IO interface # -commander start +sh /etc/init.d/rc.io # -# Start PX4IO interface (depends on orb, commander) -# -px4io start -pwm -u 400 -m 0xff - -# -# Allow PX4IO to recover from midair restarts. -# This is very unlikely, but quite safe and robust. -px4io recovery - -# -# This sets a PWM right after startup (regardless of safety button) +# Set PWM values for DJI ESCs # px4io idle 900 900 900 900 - -# -# The values are for spinning motors when armed using DJI ESCs -# px4io min 1180 1180 1180 1180 - -# -# Upper limits could be higher, this is on the safe side -# px4io max 1800 1800 1800 1800 # # Load the mixer for a quad with wide arms # mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix - -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Start GPS interface (depends on orb) -# -gps start - -# -# Start the attitude estimator (depends on orb) -# -attitude_estimator_ekf start # -# Start the controllers (depends on orb) +# Set PWM output frequency # -multirotor_att_control start +pwm -u 400 -m 0xff # -# Disable px4io topic limiting and start logging +# Start common for all multirotors apps # -if [ $BOARD == fmuv1 ] -then - px4io limit 200 - sdlog2 start -r 50 -a -b 16 - if blinkm start - then - blinkm systemstate - fi -else - px4io limit 400 - sdlog2 start -r 200 -a -b 16 - if rgbled start - then - #rgbled systemstate - fi -fi +sh /etc/init.d/rc.multirotor diff --git a/ROMFS/px4fmu_common/init.d/30_io_camflyer b/ROMFS/px4fmu_common/init.d/30_io_camflyer index 5090b98a42..c9d5d66326 100644 --- a/ROMFS/px4fmu_common/init.d/30_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/30_io_camflyer @@ -1,26 +1,6 @@ #!nsh -# -# Flight startup script for PX4FMU+PX4IO -# - -# disable USB and autostart -set USB no -set MODE custom - -# -# Start the ORB (first app to start) -# -uorb start - -# -# Load microSD params -# -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] -then - param load /fs/microsd/params -fi + +cho "[init] 30_io_camflyer: PX4FMU+PX4IO on Camflyer" # # Load default params for this platform @@ -28,39 +8,18 @@ fi if param compare SYS_AUTOCONFIG 1 then # Set all params here, then disable autoconfig + # TODO + param set SYS_AUTOCONFIG 0 param save /fs/microsd/params fi # # Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ +# MAV_TYPE 1 = fixed wing # param set MAV_TYPE 1 -# -# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) -# -if [ -f /fs/microsd/px4io.bin ] -then - echo "PX4IO Firmware found. Checking Upgrade.." - if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current - then - echo "No newer version, skipping upgrade." - else - echo "Loading /fs/microsd/px4io.bin" - if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io_update.log - then - cp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current - echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io_update.log - else - echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io_update.log - echo "Failed to upgrade PX4IO firmware - check if PX4IO is in bootloader mode" - fi - fi -fi - # # Start MAVLink (depends on orb) # @@ -106,16 +65,3 @@ kalman_demo start # mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix control_demo start - -# -# Start logging -# -sdlog2 start -r 50 -a -b 14 - -# -# Start system state -# -if blinkm start -then - blinkm systemstate -fi diff --git a/ROMFS/px4fmu_common/init.d/31_io_phantom b/ROMFS/px4fmu_common/init.d/31_io_phantom index 5090b98a42..0deffe3f17 100644 --- a/ROMFS/px4fmu_common/init.d/31_io_phantom +++ b/ROMFS/px4fmu_common/init.d/31_io_phantom @@ -1,26 +1,6 @@ #!nsh -# -# Flight startup script for PX4FMU+PX4IO -# - -# disable USB and autostart -set USB no -set MODE custom - -# -# Start the ORB (first app to start) -# -uorb start - -# -# Load microSD params -# -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] -then - param load /fs/microsd/params -fi + +echo "[init] 31_io_phantom: PX4FMU+PX4IO on Phantom" # # Load default params for this platform @@ -28,76 +8,50 @@ fi if param compare SYS_AUTOCONFIG 1 then # Set all params here, then disable autoconfig + # TODO + param set SYS_AUTOCONFIG 0 param save /fs/microsd/params fi # # Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ +# MAV_TYPE 1 = fixed wing # param set MAV_TYPE 1 - -# -# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) -# -if [ -f /fs/microsd/px4io.bin ] -then - echo "PX4IO Firmware found. Checking Upgrade.." - if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current - then - echo "No newer version, skipping upgrade." - else - echo "Loading /fs/microsd/px4io.bin" - if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io_update.log - then - cp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current - echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io_update.log - else - echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io_update.log - echo "Failed to upgrade PX4IO firmware - check if PX4IO is in bootloader mode" - fi - fi -fi - + # # Start MAVLink (depends on orb) # mavlink start -d /dev/ttyS1 -b 57600 usleep 5000 - + # -# Start the commander (depends on orb, mavlink) +# Start and configure PX4IO interface # -commander start - -# -# Start PX4IO interface (depends on orb, commander) -# -px4io start - -# -# Allow PX4IO to recover from midair restarts. -# this is very unlikely, but quite safe and robust. -px4io recovery +sh /etc/init.d/rc.io # # Set actuator limit to 100 Hz update (50 Hz PWM) px4io limit 100 # -# Start the sensors (depends on orb, px4io) +# Start the commander +# +commander start + +# +# Start the sensors # sh /etc/init.d/rc.sensors # -# Start GPS interface (depends on orb) +# Start GPS interface # gps start # -# Start the attitude estimator (depends on orb) +# Start the attitude estimator # kalman_demo start @@ -106,16 +60,3 @@ kalman_demo start # mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix control_demo start - -# -# Start logging -# -sdlog2 start -r 50 -a -b 14 - -# -# Start system state -# -if blinkm start -then - blinkm systemstate -fi diff --git a/ROMFS/px4fmu_common/init.d/40_io_segway b/ROMFS/px4fmu_common/init.d/40_io_segway index 5742d685aa..2890f43bec 100644 --- a/ROMFS/px4fmu_common/init.d/40_io_segway +++ b/ROMFS/px4fmu_common/init.d/40_io_segway @@ -1,26 +1,4 @@ #!nsh -# -# Flight startup script for PX4FMU+PX4IO -# - -# disable USB and autostart -set USB no -set MODE custom - -# -# Start the ORB (first app to start) -# -uorb start - -# -# Load microSD params -# -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] -then - param load /fs/microsd/params -fi # # Load default params for this platform @@ -28,65 +6,34 @@ fi if param compare SYS_AUTOCONFIG 1 then # Set all params here, then disable autoconfig + # TODO + param set SYS_AUTOCONFIG 0 param save /fs/microsd/params fi # # Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ +# MAV_TYPE 10 = ground rover # param set MAV_TYPE 10 - -# -# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) -# -if [ -f /fs/microsd/px4io.bin ] -then - echo "PX4IO Firmware found. Checking Upgrade.." - if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current - then - echo "No newer version, skipping upgrade." - else - echo "Loading /fs/microsd/px4io.bin" - if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io_update.log - then - cp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current - echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io_update.log - else - echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io_update.log - echo "Failed to upgrade PX4IO firmware - check if PX4IO is in bootloader mode" - fi - fi -fi - + # # Start MAVLink (depends on orb) # mavlink start -d /dev/ttyS1 -b 57600 usleep 5000 +# +# Start and configure PX4IO interface +# +sh /etc/init.d/rc.io + # # Start the commander (depends on orb, mavlink) # commander start - -# -# Start PX4IO interface (depends on orb, commander) -# -px4io start - -# -# Allow PX4IO to recover from midair restarts. -# this is very unlikely, but quite safe and robust. -px4io recovery -# -# Disable px4io topic limiting -# -px4io limit 200 - # # Start the sensors (depends on orb, px4io) # @@ -107,16 +54,3 @@ attitude_estimator_ekf start # md25 start 3 0x58 segway start - -# -# Start logging -# -sdlog2 start -r 50 -a -b 14 - -# -# Start system state -# -if blinkm start -then - blinkm systemstate -fi diff --git a/ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 similarity index 50% rename from ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 rename to ROMFS/px4fmu_common/init.d/666_fmu_q_x550 index a7ffffaeec..2c82180131 100644 --- a/ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 +++ b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 @@ -1,28 +1,6 @@ #!nsh -# -# Flight startup script for PX4FMU with PWM outputs. -# - -# disable USB and autostart -set USB no -set MODE custom - -echo "[init] doing PX4FMU Quad startup 666_fmu_quad_X550..." - -# -# Start the ORB -# -uorb start - -# -# Load microSD params -# -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] -then - param load /fs/microsd/params -fi + +echo "[init] 666_fmu_q_x550: PX4FMU Quad X550 with PWM outputs" # # Load default params for this platform @@ -52,8 +30,7 @@ fi # # Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ +# MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 @@ -62,55 +39,26 @@ param set MAV_TYPE 2 # mavlink start -d /dev/ttyS0 -b 57600 usleep 5000 - -# -# Start the sensors and test them. -# -sh /etc/init.d/rc.sensors - -# -# Start the commander. -# -commander start # -# Start GPS interface (depends on orb) +# Start PWM output # -gps start - -# -# Start the attitude estimator -# -attitude_estimator_ekf start -# -# Start the position estimator -# -position_estimator_inav start - -echo "[init] starting PWM output" fmu mode_pwm + +# +# Load mixer +# mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix + +# +# Set PWM output frequency +# pwm -u 400 -m 0xff - -# -# Start attitude control -# -multirotor_att_control start # -# Start position control +# Start common for all multirotors apps # -multirotor_pos_control start - -# -# Start logging -# -sdlog2 start -r 50 -a -b 14 +sh /etc/init.d/rc.multirotor -# -# Start system state -# -if blinkm start -then - blinkm systemstate -fi +# Exit, because /dev/ttyS0 is needed for MAVLink +exit diff --git a/ROMFS/px4fmu_common/init.d/rc.io b/ROMFS/px4fmu_common/init.d/rc.io new file mode 100644 index 0000000000..85f00e582e --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.io @@ -0,0 +1,23 @@ +# +# Start PX4IO interface (depends on orb, commander) +# +if px4io start +then + # + # Allow PX4IO to recover from midair restarts. + # this is very unlikely, but quite safe and robust. + px4io recovery + + # + # Disable px4io topic limiting + # + if [ $BOARD == fmuv1 ] + then + px4io limit 200 + else + px4io limit 400 + fi +else + # SOS + tone_alarm 6 +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index 09c2d00d19..dc4be8055a 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -5,5 +5,10 @@ if [ -d /fs/microsd ] then - sdlog start + if [ $BOARD == fmuv1 ] + then + sdlog2 start -r 50 -a -b 16 + else + sdlog2 start -r 200 -a -b 16 + fi fi diff --git a/ROMFS/px4fmu_common/init.d/rc.multirotor b/ROMFS/px4fmu_common/init.d/rc.multirotor index 81184f3632..73f1b37429 100644 --- a/ROMFS/px4fmu_common/init.d/rc.multirotor +++ b/ROMFS/px4fmu_common/init.d/rc.multirotor @@ -37,13 +37,3 @@ multirotor_att_control start # Start position control # multirotor_pos_control start - -# -# Start logging -# -if [ $BOARD == fmuv1 ] -then - sdlog2 start -r 50 -a -b 16 -else - sdlog2 start -r 200 -a -b 16 -fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 6b624b2780..d92a3ce5e3 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -57,162 +57,165 @@ fi if [ $MODE == autostart ] then - -# -# Start terminal -# -if sercon -then - echo "USB connected" -else - # second attempt - sercon & -fi - -# -# Start the ORB (first app to start) -# -uorb start - -# -# Load microSD params -# -if ramtron start -then - param select /ramtron/params - if [ -f /ramtron/params ] + # + # Start terminal + # + if sercon then - param load /ramtron/params - fi -else - param select /fs/microsd/params - if [ -f /fs/microsd/params ] - then - param load /fs/microsd/params - fi -fi - -# -# Start system state indicator -# -if rgbled start -then - echo "Using external RGB Led" -else - if blinkm start - then - blinkm systemstate - fi -fi - -# -# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) -# -if [ -f /fs/microsd/px4io.bin ] -then - echo "PX4IO Firmware found. Checking Upgrade.." - if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.cur - then - echo "No newer version, skipping upgrade." + echo "USB connected" else - echo "Loading /fs/microsd/px4io.bin" - if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io.log + # second attempt + sercon & + fi + + # + # Start the ORB (first app to start) + # + uorb start + + # + # Load microSD params + # + if ramtron start + then + param select /ramtron/params + if [ -f /ramtron/params ] then - cp /fs/microsd/px4io.bin /fs/microsd/px4io.cur - echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io.log + param load /ramtron/params + fi + else + param select /fs/microsd/params + if [ -f /fs/microsd/params ] + then + param load /fs/microsd/params + fi + fi + + # + # Start system state indicator + # + if rgbled start + then + echo "Using external RGB Led" + else + if blinkm start + then + blinkm systemstate + fi + fi + + # + # Start logging + # + sh /etc/init.d/rc.logging + + # + # Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) + # + if [ -f /fs/microsd/px4io.bin ] + then + echo "PX4IO Firmware found. Checking Upgrade.." + if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.cur + then + echo "No newer version, skipping upgrade." else - echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io.log - echo "Failed to upgrade px4io firmware - check if px4io is in bootloader mode" + echo "Loading /fs/microsd/px4io.bin" + if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io.log + then + cp /fs/microsd/px4io.bin /fs/microsd/px4io.cur + echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io.log + else + echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io.log + echo "Failed to upgrade px4io firmware - check if px4io is in bootloader mode" + fi fi fi -fi - -# -# Check if auto-setup from one of the standard scripts is wanted -# SYS_AUTOSTART = 0 means no autostart (default) -# -if param compare SYS_AUTOSTART 1 -then - sh /etc/init.d/01_fmu_quad_x - set MODE custom -fi - -if param compare SYS_AUTOSTART 2 -then - sh /etc/init.d/02_io_quad_x - set MODE custom -fi - -if param compare SYS_AUTOSTART 8 -then - sh /etc/init.d/08_ardrone - set MODE custom -fi - -if param compare SYS_AUTOSTART 9 -then - sh /etc/init.d/09_ardrone_flow - set MODE custom -fi - -if param compare SYS_AUTOSTART 10 -then - sh /etc/init.d/10_io_f330 - set MODE custom -fi - -if param compare SYS_AUTOSTART 15 -then - sh /etc/init.d/15_io_tbs - set MODE custom -fi - -if param compare SYS_AUTOSTART 30 -then - sh /etc/init.d/30_io_camflyer - set MODE custom -fi - -if param compare SYS_AUTOSTART 31 -then - sh /etc/init.d/31_io_phantom - set MODE custom -fi - -# Try to get an USB console -nshterm /dev/ttyACM0 & - -# If none of the autostart scripts triggered, get a minimal setup -if [ $MODE == autostart ] -then - # Telemetry port is on both FMU boards ttyS1 - mavlink start -b 57600 -d /dev/ttyS1 - usleep 5000 - - # Start commander - commander start - - # Start px4io if present - if px4io start + + # + # Check if auto-setup from one of the standard scripts is wanted + # SYS_AUTOSTART = 0 means no autostart (default) + # + if param compare SYS_AUTOSTART 1 then - echo "PX4IO driver started" - else - if fmu mode_serial - then - echo "FMU driver started" - fi + sh /etc/init.d/01_fmu_quad_x + set MODE custom + fi + + if param compare SYS_AUTOSTART 2 + then + sh /etc/init.d/02_io_quad_x + set MODE custom + fi + + if param compare SYS_AUTOSTART 8 + then + sh /etc/init.d/08_ardrone + set MODE custom + fi + + if param compare SYS_AUTOSTART 9 + then + sh /etc/init.d/09_ardrone_flow + set MODE custom + fi + + if param compare SYS_AUTOSTART 10 + then + sh /etc/init.d/10_io_f330 + set MODE custom + fi + + if param compare SYS_AUTOSTART 15 + then + sh /etc/init.d/15_io_tbs + set MODE custom + fi + + if param compare SYS_AUTOSTART 30 + then + sh /etc/init.d/30_io_camflyer + set MODE custom + fi + + if param compare SYS_AUTOSTART 31 + then + sh /etc/init.d/31_io_phantom + set MODE custom + fi + + # Try to get an USB console + nshterm /dev/ttyACM0 & + + # If none of the autostart scripts triggered, get a minimal setup + if [ $MODE == autostart ] + then + # Telemetry port is on both FMU boards ttyS1 + mavlink start -b 57600 -d /dev/ttyS1 + usleep 5000 + + # Start commander + commander start + + # Start px4io if present + if px4io start + then + echo "PX4IO driver started" + else + if fmu mode_serial + then + echo "FMU driver started" + fi + fi + + # Start sensors + sh /etc/init.d/rc.sensors + + # Start one of the estimators + attitude_estimator_ekf start + + # Start GPS + gps start + fi - - # Start sensors - sh /etc/init.d/rc.sensors - - # Start one of the estimators - attitude_estimator_ekf start - - # Start GPS - gps start - -fi - # End of autostart fi From 725bb7697c9fc85ac95fdadc9d2fd2ef5b9848f1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 25 Aug 2013 20:17:42 +0200 Subject: [PATCH 464/486] Minor fix in "set mode" command handling. --- src/modules/commander/commander.cpp | 13 ++++++------- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f7ac243412..74cd5e36a4 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -323,11 +323,10 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe switch (cmd->command) { case VEHICLE_CMD_DO_SET_MODE: { uint8_t base_mode = (uint8_t) cmd->param1; - union px4_custom_mode custom_mode; - custom_mode.data_float = cmd->param2; + uint8_t custom_main_mode = (uint8_t) cmd->param2; // TODO remove debug code - mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_mode); + mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode); /* set arming state */ transition_result_t arming_res = TRANSITION_NOT_CHANGED; @@ -357,19 +356,19 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { /* use autopilot-specific mode */ - if (custom_mode.main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { + if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { /* MANUAL */ main_res = main_state_transition(status, MAIN_STATE_MANUAL); - } else if (custom_mode.main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) { + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) { /* SEATBELT */ main_res = main_state_transition(status, MAIN_STATE_SEATBELT); - } else if (custom_mode.main_mode == PX4_CUSTOM_MAIN_MODE_EASY) { + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) { /* EASY */ main_res = main_state_transition(status, MAIN_STATE_EASY); - } else if (custom_mode.main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ main_res = main_state_transition(status, MAIN_STATE_AUTO); } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index eb28af1a10..af43542da4 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -201,7 +201,7 @@ handle_message(mavlink_message_t *msg) custom_mode.data = new_mode.custom_mode; /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ vcmd.param1 = new_mode.base_mode; - vcmd.param2 = custom_mode.data_float; + vcmd.param2 = custom_mode.main_mode; vcmd.param3 = 0; vcmd.param4 = 0; vcmd.param5 = 0; From 548f322493fae95c41f3769192fa0c9b28d44d26 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sun, 25 Aug 2013 22:43:01 +0200 Subject: [PATCH 465/486] Added a simple unit test framework and initial testing some of the commander state machines. --- makefiles/config_px4fmu-v1_default.mk | 7 + makefiles/config_px4fmu-v2_default.mk | 6 + .../commander_tests/commander_tests.cpp | 57 ++++ .../commander/commander_tests/module.mk | 41 +++ .../state_machine_helper_test.cpp | 248 ++++++++++++++++++ .../state_machine_helper_test.h | 43 +++ src/modules/test/foo.c | 4 - src/modules/test/module.mk | 4 - src/modules/unit_test/module.mk | 39 +++ src/modules/unit_test/unit_test.cpp | 65 +++++ src/modules/unit_test/unit_test.h | 93 +++++++ 11 files changed, 599 insertions(+), 8 deletions(-) create mode 100644 src/modules/commander/commander_tests/commander_tests.cpp create mode 100644 src/modules/commander/commander_tests/module.mk create mode 100644 src/modules/commander/commander_tests/state_machine_helper_test.cpp create mode 100644 src/modules/commander/commander_tests/state_machine_helper_test.h delete mode 100644 src/modules/test/foo.c delete mode 100644 src/modules/test/module.mk create mode 100644 src/modules/unit_test/module.mk create mode 100644 src/modules/unit_test/unit_test.cpp create mode 100644 src/modules/unit_test/unit_test.h diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index a556d0b073..245fe8415d 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -90,6 +90,13 @@ MODULES += examples/flow_speed_control # MODULES += modules/sdlog2 +# +# Unit tests +# +MODULES += modules/unit_test +MODULES += modules/commander/commander_tests + + # # Library modules # diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 20dbe717fd..c5131262b0 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -83,6 +83,12 @@ MODULES += modules/multirotor_pos_control # MODULES += modules/sdlog2 +# +# Unit tests +# +MODULES += modules/unit_test +MODULES += modules/commander/commander_tests + # # Library modules # diff --git a/src/modules/commander/commander_tests/commander_tests.cpp b/src/modules/commander/commander_tests/commander_tests.cpp new file mode 100644 index 0000000000..f1a9674aa2 --- /dev/null +++ b/src/modules/commander/commander_tests/commander_tests.cpp @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Petri Tanskanen + * Lorenz Meier + * Thomas Gubler + * Julian Oes + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file commander_tests.cpp + * Commander unit tests. + * + */ + +#include + +#include "state_machine_helper_test.h" + +extern "C" __EXPORT int commander_tests_main(int argc, char *argv[]); + + +int commander_tests_main(int argc, char *argv[]) +{ + state_machine_helper_test(); + //state_machine_test(); + + return 0; +} diff --git a/src/modules/commander/commander_tests/module.mk b/src/modules/commander/commander_tests/module.mk new file mode 100644 index 0000000000..df9b7ac4b0 --- /dev/null +++ b/src/modules/commander/commander_tests/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# System state machine tests. +# + +MODULE_COMMAND = commander_tests +SRCS = commander_tests.cpp \ + state_machine_helper_test.cpp \ + ../state_machine_helper.cpp diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp new file mode 100644 index 0000000000..7101b455ae --- /dev/null +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -0,0 +1,248 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Simon Wilks + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file state_machine_helper_test.cpp + * System state machine unit test. + * + */ + +#include "state_machine_helper_test.h" + +#include "../state_machine_helper.h" +#include + +class StateMachineHelperTest : public UnitTest +{ +public: + StateMachineHelperTest(); + virtual ~StateMachineHelperTest(); + + virtual const char* run_tests(); + +private: + const char* arming_state_transition_test(); + const char* arming_state_transition_arm_disarm_test(); + const char* main_state_transition_test(); + const char* is_safe_test(); +}; + +StateMachineHelperTest::StateMachineHelperTest() { +} + +StateMachineHelperTest::~StateMachineHelperTest() { +} + +const char* +StateMachineHelperTest::arming_state_transition_test() +{ + struct vehicle_status_s status; + struct safety_s safety; + arming_state_t new_arming_state; + struct actuator_armed_s armed; + + // Identical states. + status.arming_state = ARMING_STATE_INIT; + new_arming_state = ARMING_STATE_INIT; + mu_assert("no transition: identical states", + TRANSITION_NOT_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)); + + // INIT to STANDBY. + armed.armed = false; + armed.ready_to_arm = false; + status.arming_state = ARMING_STATE_INIT; + status.condition_system_sensors_initialized = true; + new_arming_state = ARMING_STATE_STANDBY; + mu_assert("transition: init to standby", + TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)); + mu_assert("current state: standby", ARMING_STATE_STANDBY == status.arming_state); + mu_assert("not armed", !armed.armed); + mu_assert("ready to arm", armed.ready_to_arm); + + // INIT to STANDBY, sensors not initialized. + armed.armed = false; + armed.ready_to_arm = false; + status.arming_state = ARMING_STATE_INIT; + status.condition_system_sensors_initialized = false; + new_arming_state = ARMING_STATE_STANDBY; + mu_assert("no transition: sensors not initialized", + TRANSITION_DENIED == arming_state_transition(&status, &safety, new_arming_state, &armed)); + mu_assert("current state: init", ARMING_STATE_INIT == status.arming_state); + mu_assert("not armed", !armed.armed); + mu_assert("not ready to arm", !armed.ready_to_arm); + + return 0; +} + +const char* +StateMachineHelperTest::arming_state_transition_arm_disarm_test() +{ + struct vehicle_status_s status; + struct safety_s safety; + arming_state_t new_arming_state; + struct actuator_armed_s armed; + + // TODO(sjwilks): ARM then DISARM. + return 0; +} + +const char* +StateMachineHelperTest::main_state_transition_test() +{ + struct vehicle_status_s current_state; + main_state_t new_main_state; + + // Identical states. + current_state.main_state = MAIN_STATE_MANUAL; + new_main_state = MAIN_STATE_MANUAL; + mu_assert("no transition: identical states", + TRANSITION_NOT_CHANGED == main_state_transition(¤t_state, new_main_state)); + mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); + + // AUTO to MANUAL. + current_state.main_state = MAIN_STATE_AUTO; + new_main_state = MAIN_STATE_MANUAL; + mu_assert("transition changed: auto to manual", + TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); + mu_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state); + + // MANUAL to SEATBELT. + current_state.main_state = MAIN_STATE_MANUAL; + current_state.condition_local_altitude_valid = true; + new_main_state = MAIN_STATE_SEATBELT; + mu_assert("tranisition: manual to seatbelt", + TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); + mu_assert("new state: seatbelt", MAIN_STATE_SEATBELT == current_state.main_state); + + // MANUAL to SEATBELT, invalid local altitude. + current_state.main_state = MAIN_STATE_MANUAL; + current_state.condition_local_altitude_valid = false; + new_main_state = MAIN_STATE_SEATBELT; + mu_assert("no transition: invalid local altitude", + TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); + mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); + + // MANUAL to EASY. + current_state.main_state = MAIN_STATE_MANUAL; + current_state.condition_local_position_valid = true; + new_main_state = MAIN_STATE_EASY; + mu_assert("transition: manual to easy", + TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); + mu_assert("current state: easy", MAIN_STATE_EASY == current_state.main_state); + + // MANUAL to EASY, invalid local position. + current_state.main_state = MAIN_STATE_MANUAL; + current_state.condition_local_position_valid = false; + new_main_state = MAIN_STATE_EASY; + mu_assert("no transition: invalid position", + TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); + mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); + + // MANUAL to AUTO. + current_state.main_state = MAIN_STATE_MANUAL; + current_state.condition_global_position_valid = true; + new_main_state = MAIN_STATE_AUTO; + mu_assert("transition: manual to auto", + TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); + mu_assert("current state: auto", MAIN_STATE_AUTO == current_state.main_state); + + // MANUAL to AUTO, invalid global position. + current_state.main_state = MAIN_STATE_MANUAL; + current_state.condition_global_position_valid = false; + new_main_state = MAIN_STATE_AUTO; + mu_assert("no transition: invalid global position", + TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); + mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); + + return 0; +} + +const char* +StateMachineHelperTest::is_safe_test() +{ + struct vehicle_status_s current_state; + struct safety_s safety; + struct actuator_armed_s armed; + + armed.armed = false; + armed.lockdown = false; + safety.safety_switch_available = true; + safety.safety_off = false; + mu_assert("is safe: not armed", is_safe(¤t_state, &safety, &armed)); + + armed.armed = false; + armed.lockdown = true; + safety.safety_switch_available = true; + safety.safety_off = true; + mu_assert("is safe: software lockdown", is_safe(¤t_state, &safety, &armed)); + + armed.armed = true; + armed.lockdown = false; + safety.safety_switch_available = true; + safety.safety_off = true; + mu_assert("not safe: safety off", !is_safe(¤t_state, &safety, &armed)); + + armed.armed = true; + armed.lockdown = false; + safety.safety_switch_available = true; + safety.safety_off = false; + mu_assert("is safe: safety off", is_safe(¤t_state, &safety, &armed)); + + armed.armed = true; + armed.lockdown = false; + safety.safety_switch_available = false; + safety.safety_off = false; + mu_assert("not safe: no safety switch", !is_safe(¤t_state, &safety, &armed)); + + return 0; +} + +const char* +StateMachineHelperTest::run_tests() +{ + mu_run_test(arming_state_transition_test); + mu_run_test(arming_state_transition_arm_disarm_test); + mu_run_test(main_state_transition_test); + mu_run_test(is_safe_test); + + return 0; +} + +void +state_machine_helper_test() +{ + StateMachineHelperTest* test = new StateMachineHelperTest(); + test->UnitTest::print_results(test->run_tests()); +} + diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.h b/src/modules/commander/commander_tests/state_machine_helper_test.h new file mode 100644 index 0000000000..339b58d22b --- /dev/null +++ b/src/modules/commander/commander_tests/state_machine_helper_test.h @@ -0,0 +1,43 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file state_machine_helper_test.h + */ + +#ifndef STATE_MACHINE_HELPER_TEST_H_ +#define STATE_MACHINE_HELPER_TEST_ + +void state_machine_helper_test(); + +#endif /* STATE_MACHINE_HELPER_TEST_H_ */ \ No newline at end of file diff --git a/src/modules/test/foo.c b/src/modules/test/foo.c deleted file mode 100644 index ff6af031f7..0000000000 --- a/src/modules/test/foo.c +++ /dev/null @@ -1,4 +0,0 @@ -int test_main(void) -{ - return 0; -} \ No newline at end of file diff --git a/src/modules/test/module.mk b/src/modules/test/module.mk deleted file mode 100644 index abf80eedf7..0000000000 --- a/src/modules/test/module.mk +++ /dev/null @@ -1,4 +0,0 @@ - -MODULE_NAME = test -SRCS = foo.c - diff --git a/src/modules/unit_test/module.mk b/src/modules/unit_test/module.mk new file mode 100644 index 0000000000..f00b0f5926 --- /dev/null +++ b/src/modules/unit_test/module.mk @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Makefile to build the unit test library. +# + +SRCS = unit_test.cpp + diff --git a/src/modules/unit_test/unit_test.cpp b/src/modules/unit_test/unit_test.cpp new file mode 100644 index 0000000000..64ee544a27 --- /dev/null +++ b/src/modules/unit_test/unit_test.cpp @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Simon Wilks + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file unit_test.cpp + * A unit test library. + * + */ + +#include "unit_test.h" + +#include + + +UnitTest::UnitTest() +{ +} + +UnitTest::~UnitTest() +{ +} + +void +UnitTest::print_results(const char* result) +{ + if (result != 0) { + warnx("Failed: %s:%d", mu_last_test(), mu_line()); + warnx("%s", result); + } else { + warnx("ALL TESTS PASSED"); + warnx(" Tests run : %d", mu_tests_run()); + warnx(" Assertion : %d", mu_assertion()); + } +} diff --git a/src/modules/unit_test/unit_test.h b/src/modules/unit_test/unit_test.h new file mode 100644 index 0000000000..5d4c3e46d8 --- /dev/null +++ b/src/modules/unit_test/unit_test.h @@ -0,0 +1,93 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Simon Wilks + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file unit_test.h + * A unit test library based on MinUnit (http://www.jera.com/techinfo/jtns/jtn002.html). + * + */ + +#ifndef UNIT_TEST_H_ +#define UNIT_TEST_ + +#include + + +class __EXPORT UnitTest +{ + +public: +#define xstr(s) str(s) +#define str(s) #s +#define INLINE_GLOBAL(type,func) inline type& func() { static type x; return x; } + +INLINE_GLOBAL(int, mu_tests_run) +INLINE_GLOBAL(int, mu_assertion) +INLINE_GLOBAL(int, mu_line) +INLINE_GLOBAL(const char*, mu_last_test) + +#define mu_assert(message, test) \ + do \ + { \ + if (!(test)) \ + return __FILE__ ":" xstr(__LINE__) " " message " (" #test ")"; \ + else \ + mu_assertion()++; \ + } while (0) + + +#define mu_run_test(test) \ +do \ +{ \ + const char *message; \ + mu_last_test() = #test; \ + mu_line() = __LINE__; \ + message = test(); \ + mu_tests_run()++; \ + if (message) \ + return message; \ +} while (0) + + +public: + UnitTest(); + virtual ~UnitTest(); + + virtual const char* run_tests() = 0; + virtual void print_results(const char* result); +}; + + + +#endif /* UNIT_TEST_H_ */ \ No newline at end of file From e25f2ff44f9579da3e5bef1e6d1baa3822ec40df Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sun, 25 Aug 2013 22:54:31 +0200 Subject: [PATCH 466/486] Whitespace and formatting cleanup. --- makefiles/config_px4fmu-v1_default.mk | 1 - src/modules/commander/commander_tests/commander_tests.cpp | 8 +++----- src/modules/commander/commander_tests/module.mk | 2 +- .../commander_tests/state_machine_helper_test.cpp | 1 - .../commander/commander_tests/state_machine_helper_test.h | 3 ++- src/modules/unit_test/unit_test.h | 2 +- 6 files changed, 7 insertions(+), 10 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 245fe8415d..bb0c1550f2 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -96,7 +96,6 @@ MODULES += modules/sdlog2 MODULES += modules/unit_test MODULES += modules/commander/commander_tests - # # Library modules # diff --git a/src/modules/commander/commander_tests/commander_tests.cpp b/src/modules/commander/commander_tests/commander_tests.cpp index f1a9674aa2..6e72cf0d9c 100644 --- a/src/modules/commander/commander_tests/commander_tests.cpp +++ b/src/modules/commander/commander_tests/commander_tests.cpp @@ -1,10 +1,7 @@ /**************************************************************************** * * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Petri Tanskanen - * Lorenz Meier - * Thomas Gubler - * Julian Oes + * Author: Simon Wilks * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,7 +34,8 @@ /** * @file commander_tests.cpp - * Commander unit tests. + * Commander unit tests. Run the tests as follows: + * nsh> commander_tests * */ diff --git a/src/modules/commander/commander_tests/module.mk b/src/modules/commander/commander_tests/module.mk index df9b7ac4b0..4d10275d1e 100644 --- a/src/modules/commander/commander_tests/module.mk +++ b/src/modules/commander/commander_tests/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2013 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 7101b455ae..40bedd9f31 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -245,4 +245,3 @@ state_machine_helper_test() StateMachineHelperTest* test = new StateMachineHelperTest(); test->UnitTest::print_results(test->run_tests()); } - diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.h b/src/modules/commander/commander_tests/state_machine_helper_test.h index 339b58d22b..10a68e6028 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.h +++ b/src/modules/commander/commander_tests/state_machine_helper_test.h @@ -1,6 +1,7 @@ /**************************************************************************** * * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Simon Wilks * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -40,4 +41,4 @@ void state_machine_helper_test(); -#endif /* STATE_MACHINE_HELPER_TEST_H_ */ \ No newline at end of file +#endif /* STATE_MACHINE_HELPER_TEST_H_ */ diff --git a/src/modules/unit_test/unit_test.h b/src/modules/unit_test/unit_test.h index 5d4c3e46d8..3020734f4b 100644 --- a/src/modules/unit_test/unit_test.h +++ b/src/modules/unit_test/unit_test.h @@ -90,4 +90,4 @@ public: -#endif /* UNIT_TEST_H_ */ \ No newline at end of file +#endif /* UNIT_TEST_H_ */ From 07f7fd1585dab0a0256b0e8dda48ea1a1bd65388 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 25 Aug 2013 22:26:47 -0700 Subject: [PATCH 467/486] Fix the firmware build rules so that we always know how to build all the firmwares and thus we can have dependencies between FMU and IO firmware handled a little more sensibly. --- Makefile | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/Makefile b/Makefile index cbf0da6c97..83a6f3ce9f 100644 --- a/Makefile +++ b/Makefile @@ -40,14 +40,16 @@ export PX4_BASE := $(realpath $(dir $(lastword $(MAKEFILE_LIST))))/ include $(PX4_BASE)makefiles/setup.mk # -# Canned firmware configurations that we build. +# Canned firmware configurations that we (know how to) build. # -CONFIGS ?= $(subst config_,,$(basename $(notdir $(wildcard $(PX4_MK_DIR)config_*.mk)))) +KNOWN_CONFIGS := $(subst config_,,$(basename $(notdir $(wildcard $(PX4_MK_DIR)config_*.mk)))) +CONFIGS ?= $(KNOWN_CONFIGS) # -# Boards that we build NuttX export kits for. +# Boards that we (know how to) build NuttX export kits for. # -BOARDS := $(subst board_,,$(basename $(notdir $(wildcard $(PX4_MK_DIR)board_*.mk)))) +KNOWN_BOARDS := $(subst board_,,$(basename $(notdir $(wildcard $(PX4_MK_DIR)board_*.mk)))) +BOARDS ?= $(KNOWN_BOARDS) # # Debugging @@ -87,10 +89,11 @@ endif # # Built products # -STAGED_FIRMWARES = $(foreach config,$(CONFIGS),$(IMAGE_DIR)$(config).px4) -FIRMWARES = $(foreach config,$(CONFIGS),$(BUILD_DIR)$(config).build/firmware.px4) +DESIRED_FIRMWARES = $(foreach config,$(CONFIGS),$(IMAGE_DIR)$(config).px4) +STAGED_FIRMWARES = $(foreach config,$(KNOWN_CONFIGS),$(IMAGE_DIR)$(config).px4) +FIRMWARES = $(foreach config,$(KNOWN_CONFIGS),$(BUILD_DIR)$(config).build/firmware.px4) -all: $(STAGED_FIRMWARES) +all: $(DESIRED_FIRMWARES) # # Copy FIRMWARES into the image directory. @@ -122,7 +125,7 @@ $(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4: $(FIRMWARE_GOAL) # -# Make FMU firmwares depend on pre-packaged IO binaries. +# Make FMU firmwares depend on the corresponding IO firmware. # # This is a pretty vile hack, since it hard-codes knowledge of the FMU->IO dependency # and forces the _default config in all cases. There has to be a better way to do this... From c5731bbc3f29361f3d50ecc54d44a521d2441a48 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 26 Aug 2013 09:12:17 +0200 Subject: [PATCH 468/486] TAKEOFF implemented for multirotors, added altitude check to waypoint navigation. --- .../att_pos_estimator_ekf/KalmanNav.cpp | 2 +- src/modules/commander/commander.cpp | 14 ++- .../commander/state_machine_helper.cpp | 1 + src/modules/mavlink/orb_listener.c | 2 +- src/modules/mavlink/waypoints.c | 40 +++--- .../multirotor_pos_control.c | 115 +++++++++++------- .../position_estimator_inav_main.c | 3 +- .../uORB/topics/vehicle_control_mode.h | 2 + .../uORB/topics/vehicle_global_position.h | 3 +- .../uORB/topics/vehicle_local_position.h | 2 + 10 files changed, 108 insertions(+), 76 deletions(-) diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index 191d20f306..33879892ee 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -325,7 +325,7 @@ void KalmanNav::updatePublications() _pos.vx = vN; _pos.vy = vE; _pos.vz = vD; - _pos.hdg = psi; + _pos.yaw = psi; // attitude publication _att.timestamp = _pubTimeStamp; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 74cd5e36a4..39d74e37aa 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -210,7 +210,7 @@ void print_reject_arm(const char *msg); void print_status(); -transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode); +transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos); /** * Loop that runs at a lower rate and priority for calibration and parameter tasks. @@ -1214,7 +1214,7 @@ int commander_thread_main(int argc, char *argv[]) } /* evaluate the navigation state machine */ - transition_result_t res = check_navigation_state_machine(&status, &control_mode); + transition_result_t res = check_navigation_state_machine(&status, &control_mode, &local_position); if (res == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ @@ -1572,7 +1572,7 @@ print_reject_arm(const char *msg) } transition_result_t -check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode) +check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos) { transition_result_t res = TRANSITION_DENIED; @@ -1592,8 +1592,12 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v case MAIN_STATE_AUTO: if (current_status->arming_state == ARMING_STATE_ARMED || current_status->arming_state == ARMING_STATE_ARMED_ERROR) { if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - /* don't act while taking off */ - res = TRANSITION_NOT_CHANGED; + /* check if takeoff completed */ + if (local_pos->z < -5.0f && !status.condition_landed) { + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + } else { + res = TRANSITION_NOT_CHANGED; + } } else { if (current_status->condition_landed) { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 80ee3db233..fe7e8cc536 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -413,6 +413,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ if (ret == TRANSITION_CHANGED) { current_status->navigation_state = new_navigation_state; + control_mode->auto_state = current_status->navigation_state; navigation_state_changed = true; } } diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index dcdc03281a..53d86ec005 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -341,7 +341,7 @@ l_global_position(const struct listener *l) int16_t vz = (int16_t)(global_pos.vz * 100.0f); /* heading in degrees * 10, from 0 to 36.000) */ - uint16_t hdg = (global_pos.hdg / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f); + uint16_t hdg = (global_pos.yaw / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f); mavlink_msg_global_position_int_send(MAVLINK_COMM_0, timestamp / 1000, diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index eea928a170..16a7c2d355 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -294,16 +294,13 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq) */ float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt) { - //TODO: implement for z once altidude contoller is implemented - static uint16_t counter; -// if(counter % 10 == 0) printf(" x = %.10f, y = %.10f\n", x, y); if (seq < wpm->size) { - mavlink_mission_item_t *cur = &(wpm->waypoints[seq]); + mavlink_mission_item_t *wp = &(wpm->waypoints[seq]); - double current_x_rad = cur->x / 180.0 * M_PI; - double current_y_rad = cur->y / 180.0 * M_PI; + double current_x_rad = wp->x / 180.0 * M_PI; + double current_y_rad = wp->y / 180.0 * M_PI; double x_rad = lat / 180.0 * M_PI; double y_rad = lon / 180.0 * M_PI; @@ -315,7 +312,10 @@ float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float const double radius_earth = 6371000.0; - return radius_earth * c; + float dxy = radius_earth * c; + float dz = alt - wp->z; + + return sqrtf(dxy * dxy + dz * dz); } else { return -1.0f; @@ -383,21 +383,19 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ // XXX TODO } - if (dist >= 0.f && dist <= orbit /*&& wpm->yaw_reached*/) { //TODO implement yaw - + if (dist >= 0.f && dist <= orbit) { wpm->pos_reached = true; - } - -// else -// { -// if(counter % 100 == 0) -// printf("Setpoint not reached yet: %0.4f, orbit: %.4f, coordinate frame: %d\n",dist, orbit, coordinate_frame); -// } + // check if required yaw reached + float yaw_sp = _wrap_pi(wpm->waypoints[wpm->current_active_wp_id].param4 / 180.0f * FM_PI); + float yaw_err = _wrap_pi(yaw_sp - local_pos->yaw); + if (fabsf(yaw_err) < 0.05f) { + wpm->yaw_reached = true; + } } //check if the current waypoint was reached - if (wpm->pos_reached /*wpm->yaw_reached &&*/ && !wpm->idle) { + if (wpm->pos_reached && /*wpm->yaw_reached &&*/ !wpm->idle) { if (wpm->current_active_wp_id < wpm->size) { mavlink_mission_item_t *cur_wp = &(wpm->waypoints[wpm->current_active_wp_id]); @@ -412,11 +410,7 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ bool time_elapsed = false; - if (cur_wp->command == (int)MAV_CMD_NAV_LOITER_TIME) { - if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) { - time_elapsed = true; - } - } else if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) { + if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) { time_elapsed = true; } else if (cur_wp->command == (int)MAV_CMD_NAV_TAKEOFF) { time_elapsed = true; @@ -493,7 +487,7 @@ int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_positio // mavlink_wpm_send_setpoint(wpm->current_active_wp_id); // } - check_waypoints_reached(now, global_position , local_position); + check_waypoints_reached(now, global_position, local_position); return OK; } diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index a6ebeeacb1..e65792c76b 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -54,6 +54,7 @@ #include #include #include +#include #include #include #include @@ -221,11 +222,12 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) bool reset_int_xy = true; bool was_armed = false; bool reset_integral = true; + bool reset_auto_pos = true; hrt_abstime t_prev = 0; - /* integrate in NED frame to estimate wind but not attitude offset */ const float alt_ctl_dz = 0.2f; const float pos_ctl_dz = 0.05f; + const float takeoff_alt_default = 10.0f; float ref_alt = 0.0f; hrt_abstime ref_alt_t = 0; uint64_t local_ref_timestamp = 0; @@ -414,50 +416,76 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } else { /* non-manual mode, use global setpoint */ - /* init local projection using local position ref */ - if (local_pos.ref_timestamp != local_ref_timestamp) { - global_pos_sp_reproject = true; - local_ref_timestamp = local_pos.ref_timestamp; - double lat_home = local_pos.ref_lat * 1e-7; - double lon_home = local_pos.ref_lon * 1e-7; - map_projection_init(lat_home, lon_home); - mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", lat_home, lon_home); - } - - if (global_pos_sp_reproject) { - /* update global setpoint projection */ - global_pos_sp_reproject = false; - - if (global_pos_sp_valid) { - /* global position setpoint valid, use it */ - double sp_lat = global_pos_sp.lat * 1e-7; - double sp_lon = global_pos_sp.lon * 1e-7; - /* project global setpoint to local setpoint */ - map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y)); - - if (global_pos_sp.altitude_is_relative) { - local_pos_sp.z = -global_pos_sp.altitude; - - } else { - local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; - } - - mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); - - } else { - if (!local_pos_sp_valid) { - /* local position setpoint is invalid, - * use current position as setpoint for loiter */ - local_pos_sp.x = local_pos.x; - local_pos_sp.y = local_pos.y; - local_pos_sp.z = local_pos.z; - } - - mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y); + if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + if (reset_auto_pos) { + local_pos_sp.x = local_pos.x; + local_pos_sp.y = local_pos.y; + local_pos_sp.z = -takeoff_alt_default; + att_sp.yaw_body = att.yaw; + reset_auto_pos = false; + } + } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_LOITER) { + if (reset_auto_pos) { + local_pos_sp.x = local_pos.x; + local_pos_sp.y = local_pos.y; + local_pos_sp.z = local_pos.z; + att_sp.yaw_body = att.yaw; + reset_auto_pos = false; + } + } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) { + // TODO + reset_auto_pos = true; + } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_MISSION) { + /* init local projection using local position ref */ + if (local_pos.ref_timestamp != local_ref_timestamp) { + global_pos_sp_reproject = true; + local_ref_timestamp = local_pos.ref_timestamp; + double lat_home = local_pos.ref_lat * 1e-7; + double lon_home = local_pos.ref_lon * 1e-7; + map_projection_init(lat_home, lon_home); + mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", lat_home, lon_home); } - /* publish local position setpoint as projection of global position setpoint */ - orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); + if (global_pos_sp_reproject) { + /* update global setpoint projection */ + global_pos_sp_reproject = false; + if (global_pos_sp_valid) { + /* global position setpoint valid, use it */ + double sp_lat = global_pos_sp.lat * 1e-7; + double sp_lon = global_pos_sp.lon * 1e-7; + /* project global setpoint to local setpoint */ + map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y)); + + if (global_pos_sp.altitude_is_relative) { + local_pos_sp.z = -global_pos_sp.altitude; + + } else { + local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; + } + + mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); + + } else { + if (!local_pos_sp_valid) { + /* local position setpoint is invalid, + * use current position as setpoint for loiter */ + local_pos_sp.x = local_pos.x; + local_pos_sp.y = local_pos.y; + local_pos_sp.z = local_pos.z; + } + + mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y); + } + + /* publish local position setpoint as projection of global position setpoint */ + orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); + } + att_sp.yaw_body = global_pos_sp.yaw; + reset_auto_pos = true; + } + + if (control_mode.auto_state != NAVIGATION_STATE_AUTO_MISSION) { + global_pos_sp_reproject = true; } /* reset setpoints after non-manual modes */ @@ -575,6 +603,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) reset_int_z = true; reset_int_xy = true; global_pos_sp_reproject = true; + reset_auto_pos = true; } /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 1e20f9de9d..af6a704a2e 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -591,6 +591,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.z = z_est[0]; local_pos.vz = z_est[1]; local_pos.landed = landed; + local_pos.yaw = att.yaw; orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); @@ -609,7 +610,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (local_pos.v_xy_valid) { global_pos.vx = local_pos.vx; global_pos.vy = local_pos.vy; - global_pos.hdg = atan2f(local_pos.vy, local_pos.vx); // TODO is it correct? } if (local_pos.z_valid) { @@ -623,6 +623,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (local_pos.v_z_valid) { global_pos.vz = local_pos.vz; } + global_pos.yaw = local_pos.yaw; global_pos.timestamp = t; diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 4f4db5dbcb..e15ddde269 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -82,6 +82,8 @@ struct vehicle_control_mode_s bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ bool failsave_highlevel; /**< Set to true if high-level failsafe mode is enabled */ + + uint8_t auto_state; // TEMP navigation state for AUTO modes }; /** diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index 822c323cfc..0fc0ed5c97 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -72,8 +72,7 @@ struct vehicle_global_position_s float vx; /**< Ground X velocity, m/s in NED */ float vy; /**< Ground Y velocity, m/s in NED */ float vz; /**< Ground Z velocity, m/s in NED */ - float hdg; /**< Compass heading in radians -PI..+PI. */ - + float yaw; /**< Compass heading in radians -PI..+PI. */ }; /** diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index 26e8f335b5..31a0e632b2 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -67,6 +67,8 @@ struct vehicle_local_position_s float vx; /**< Ground X Speed (Latitude), m/s in NED */ float vy; /**< Ground Y Speed (Longitude), m/s in NED */ float vz; /**< Ground Z Speed (Altitude), m/s in NED */ + /* Heading */ + float yaw; /* Reference position in GPS / WGS84 frame */ bool global_xy; /**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */ bool global_z; /**< true if z is valid and has valid global reference (ref_alt) */ From dfde02c82507d183fdc16aa2d45cb8d9cdf6ff0e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Aug 2013 11:53:52 +0200 Subject: [PATCH 469/486] Startup scripts fixup, fixed unmatched dependencies --- ROMFS/px4fmu_common/init.d/30_io_camflyer | 9 +++++++-- ROMFS/px4fmu_common/init.d/31_io_phantom | 7 ++++++- ROMFS/px4fmu_common/init.d/rc.multirotor | 5 +++++ ROMFS/px4fmu_common/init.d/rcS | 11 +++-------- 4 files changed, 21 insertions(+), 11 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/30_io_camflyer b/ROMFS/px4fmu_common/init.d/30_io_camflyer index c9d5d66326..6a0bd4da82 100644 --- a/ROMFS/px4fmu_common/init.d/30_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/30_io_camflyer @@ -1,6 +1,6 @@ #!nsh -cho "[init] 30_io_camflyer: PX4FMU+PX4IO on Camflyer" +echo "[init] 30_io_camflyer: PX4FMU+PX4IO on Camflyer" # # Load default params for this platform @@ -49,6 +49,11 @@ px4io limit 100 # Start the sensors (depends on orb, px4io) # sh /etc/init.d/rc.sensors + +# +# Start logging (depends on sensors) +# +sh /etc/init.d/rc.logging # # Start GPS interface (depends on orb) @@ -64,4 +69,4 @@ kalman_demo start # Load mixer and start controllers (depends on px4io) # mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix -control_demo start +fw_att_control start diff --git a/ROMFS/px4fmu_common/init.d/31_io_phantom b/ROMFS/px4fmu_common/init.d/31_io_phantom index 0deffe3f17..7183138622 100644 --- a/ROMFS/px4fmu_common/init.d/31_io_phantom +++ b/ROMFS/px4fmu_common/init.d/31_io_phantom @@ -44,6 +44,11 @@ commander start # Start the sensors # sh /etc/init.d/rc.sensors + +# +# Start logging (depends on sensors) +# +sh /etc/init.d/rc.logging # # Start GPS interface @@ -59,4 +64,4 @@ kalman_demo start # Load mixer and start controllers (depends on px4io) # mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix -control_demo start +fw_att_control start diff --git a/ROMFS/px4fmu_common/init.d/rc.multirotor b/ROMFS/px4fmu_common/init.d/rc.multirotor index 73f1b37429..e3638e3d1f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.multirotor +++ b/ROMFS/px4fmu_common/init.d/rc.multirotor @@ -8,6 +8,11 @@ # sh /etc/init.d/rc.sensors +# +# Start logging (depends on sensors) +# +sh /etc/init.d/rc.logging + # # Start the commander. # diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 6a82acdcc3..c4abd07d2e 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -103,11 +103,9 @@ then blinkm systemstate fi fi - - # - # Start logging - # - sh /etc/init.d/rc.logging + + # Try to get an USB console + nshterm /dev/ttyACM0 & # # Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) @@ -183,9 +181,6 @@ then set MODE custom fi - # Try to get an USB console - nshterm /dev/ttyACM0 & - # Start any custom extensions that might be missing if [ -f /fs/microsd/etc/rc.local ] then From 3157285254381827bc0312bfb413ff6823feee3b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Aug 2013 13:26:01 +0200 Subject: [PATCH 470/486] Increased the number of max files descriptors considerably --- nuttx-configs/px4fmu-v1/nsh/defconfig | 4 ++-- nuttx-configs/px4fmu-v2/nsh/defconfig | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index a7a6725c65..56ad46c3f5 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -405,8 +405,8 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=32 -CONFIG_NFILE_STREAMS=25 +CONFIG_NFILE_DESCRIPTORS=64 +CONFIG_NFILE_STREAMS=50 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 CONFIG_MQ_MAXMSGSIZE=32 diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 3e2a481087..f99973aec5 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -451,8 +451,8 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=32 -CONFIG_NFILE_STREAMS=25 +CONFIG_NFILE_DESCRIPTORS=64 +CONFIG_NFILE_STREAMS=50 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 CONFIG_MQ_MAXMSGSIZE=32 From 00a2a0370eedf84f68dcda5995f4e34495aaf887 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 26 Aug 2013 12:43:36 +0200 Subject: [PATCH 471/486] accelerometer_calibration fix --- src/modules/commander/accelerometer_calibration.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 82df7c37d6..7a7fa6f4e4 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -298,7 +298,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { /* set accel error threshold to 5m/s^2 */ float accel_err_thr = 5.0f; /* still time required in us */ - int64_t still_time = 2000000; + hrt_abstime still_time = 2000000; struct pollfd fds[1]; fds[0].fd = sub_sensor_combined; fds[0].events = POLLIN; @@ -342,7 +342,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { t_timeout = t + timeout; } else { /* still since t_still */ - if ((int64_t) t - (int64_t) t_still > still_time) { + if (t > t_still + still_time) { /* vehicle is still, exit from the loop to detection of its orientation */ break; } From bf9282c9882882a5fc4adf680a6ecc5e655bb0e8 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 26 Aug 2013 13:52:10 +0200 Subject: [PATCH 472/486] position_estimator_inav: requre EPH < 5m to set GPS reference --- .../position_estimator_inav/position_estimator_inav_main.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index af6a704a2e..bc6e0b0209 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -429,7 +429,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (fds[6].revents & POLLIN) { orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout) { + if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout && gps.eph_m < 5.0f) { /* initialize reference position if needed */ if (!ref_xy_inited) { if (ref_xy_init_start == 0) { From baa2cab69dd7ba4021bad294cb4e3c49d12a0f9a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 26 Aug 2013 13:52:55 +0200 Subject: [PATCH 473/486] commander: do AUTO_MISSION after takeoff --- src/modules/commander/commander.cpp | 58 ++++++++++++++--------------- 1 file changed, 28 insertions(+), 30 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 39d74e37aa..d900086331 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1592,43 +1592,41 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v case MAIN_STATE_AUTO: if (current_status->arming_state == ARMING_STATE_ARMED || current_status->arming_state == ARMING_STATE_ARMED_ERROR) { if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - /* check if takeoff completed */ - if (local_pos->z < -5.0f && !status.condition_landed) { - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); - } else { + /* don't switch to other states until takeoff not completed */ + if (local_pos->z > -5.0f || status.condition_landed) { res = TRANSITION_NOT_CHANGED; + break; } + } + /* check again, state can be changed */ + if (current_status->condition_landed) { + /* if landed: transitions only to AUTO_READY are allowed */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); + // TRANSITION_DENIED is not possible here + break; } else { - if (current_status->condition_landed) { - /* if landed: transitions only to AUTO_READY are allowed */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); - // TRANSITION_DENIED is not possible here - break; - - } else { - /* if not landed: */ - if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) { - /* act depending on switches when manual control enabled */ - if (current_status->return_switch == RETURN_SWITCH_RETURN) { - /* RTL */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); - - } else { - if (current_status->mission_switch == MISSION_SWITCH_MISSION) { - /* MISSION */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); - - } else { - /* LOITER */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); - } - } + /* if not landed: */ + if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) { + /* act depending on switches when manual control enabled */ + if (current_status->return_switch == RETURN_SWITCH_RETURN) { + /* RTL */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); } else { - /* always switch to loiter in air when no RC control */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + if (current_status->mission_switch == MISSION_SWITCH_MISSION) { + /* MISSION */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); + + } else { + /* LOITER */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + } } + + } else { + /* switch to mission in air when no RC control */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); } } } else { From 7326f8a4215fe736aedd2e2cdb2ab51d06e20f80 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 26 Aug 2013 13:53:30 +0200 Subject: [PATCH 474/486] multirotor_pos_control: fixes, set local_position_sp.yaw --- .../multirotor_pos_control.c | 25 +++++++++++++------ 1 file changed, 18 insertions(+), 7 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index e65792c76b..385892f04e 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -405,30 +405,38 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } } - /* publish local position setpoint */ - orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); + local_pos_sp.yaw = att_sp.yaw_body; /* local position setpoint is valid and can be used for loiter after position controlled mode */ local_pos_sp_valid = control_mode.flag_control_position_enabled; + reset_auto_pos = true; + /* force reprojection of global setpoint after manual mode */ global_pos_sp_reproject = true; } else { /* non-manual mode, use global setpoint */ - if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + if (control_mode.auto_state == NAVIGATION_STATE_AUTO_READY) { + reset_auto_pos = true; + } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) { if (reset_auto_pos) { local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; local_pos_sp.z = -takeoff_alt_default; + local_pos_sp.yaw = att.yaw; + local_pos_sp_valid = true; att_sp.yaw_body = att.yaw; reset_auto_pos = false; + mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", local_pos_sp.x, local_pos_sp.y, local_pos_sp.z); } } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_LOITER) { if (reset_auto_pos) { local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; local_pos_sp.z = local_pos.z; + local_pos_sp.yaw = att.yaw; + local_pos_sp_valid = true; att_sp.yaw_body = att.yaw; reset_auto_pos = false; } @@ -462,6 +470,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } else { local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; } + local_pos_sp.yaw = global_pos_sp.yaw; + att_sp.yaw_body = global_pos_sp.yaw; mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); @@ -472,15 +482,13 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; local_pos_sp.z = local_pos.z; + local_pos_sp.yaw = att.yaw; + local_pos_sp_valid = true; } mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y); } - - /* publish local position setpoint as projection of global position setpoint */ - orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); } - att_sp.yaw_body = global_pos_sp.yaw; reset_auto_pos = true; } @@ -493,6 +501,9 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) reset_sp_z = true; } + /* publish local position setpoint */ + orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); + /* run position & altitude controllers, calculate velocity setpoint */ if (control_mode.flag_control_altitude_enabled) { global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2]; From e88d63ef272124e8c0ee9574506d14866feadb8b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Aug 2013 15:03:58 +0200 Subject: [PATCH 475/486] Increased USB buffer size to cope with fast transfers --- nuttx-configs/px4fmu-v1/nsh/defconfig | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index b6f14490e0..3e02c204e1 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -564,8 +564,8 @@ CONFIG_CDCACM_EPBULKIN_HSSIZE=512 CONFIG_CDCACM_NWRREQS=4 CONFIG_CDCACM_NRDREQS=4 CONFIG_CDCACM_BULKIN_REQLEN=96 -CONFIG_CDCACM_RXBUFSIZE=256 -CONFIG_CDCACM_TXBUFSIZE=256 +CONFIG_CDCACM_RXBUFSIZE=512 +CONFIG_CDCACM_TXBUFSIZE=512 CONFIG_CDCACM_VENDORID=0x26ac CONFIG_CDCACM_PRODUCTID=0x0010 CONFIG_CDCACM_VENDORSTR="3D Robotics" From b29d13347a10156bf1690b67938e2850d4e1e4c5 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 26 Aug 2013 22:08:56 +0200 Subject: [PATCH 476/486] position_estimator_inav: reset reference altitude on arming. --- .../position_estimator_inav_main.c | 22 ++++++++++++++----- 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index bc6e0b0209..ef13ade886 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -75,8 +75,11 @@ static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int position_estimator_inav_task; /**< Handle of deamon task / thread */ static bool verbose_mode = false; -static hrt_abstime gps_timeout = 1000000; // GPS timeout = 1s -static hrt_abstime flow_timeout = 1000000; // optical flow timeout = 1s + +static const hrt_abstime gps_timeout = 1000000; // GPS timeout = 1s +static const hrt_abstime flow_timeout = 1000000; // optical flow timeout = 1s +static const uint32_t updates_counter_len = 1000000; +static const uint32_t pub_interval = 4000; // limit publish rate to 250 Hz __EXPORT int position_estimator_inav_main(int argc, char *argv[]); @@ -172,6 +175,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float alt_avg = 0.0f; bool landed = true; hrt_abstime landed_time = 0; + bool flag_armed = false; uint32_t accel_counter = 0; uint32_t baro_counter = 0; @@ -275,10 +279,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) uint16_t flow_updates = 0; hrt_abstime updates_counter_start = hrt_absolute_time(); - uint32_t updates_counter_len = 1000000; - hrt_abstime pub_last = hrt_absolute_time(); - uint32_t pub_interval = 4000; // limit publish rate to 250 Hz /* acceleration in NED frame */ float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G }; @@ -407,7 +408,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else { // new ground level baro_alt0 += sonar_corr; - warnx("new home: alt = %.3f", baro_alt0); mavlink_log_info(mavlink_fd, "[inav] new home: alt = %.3f", baro_alt0); local_pos.ref_alt = baro_alt0; local_pos.ref_timestamp = hrt_absolute_time(); @@ -486,6 +486,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; t_prev = t; + /* reset ground level on arm */ + if (armed.armed && !flag_armed) { + baro_alt0 -= z_est[0]; + z_est[0] = 0.0f; + local_pos.ref_alt = baro_alt0; + local_pos.ref_timestamp = hrt_absolute_time(); + mavlink_log_info(mavlink_fd, "[inav] new home on arm: alt = %.3f", baro_alt0); + } + /* accel bias correction, now only for Z * not strictly correct, but stable and works */ accel_bias[2] += (accel_NED[2] + CONSTANTS_ONE_G) * params.w_acc_bias * dt; @@ -629,6 +638,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos); } + flag_armed = armed.armed; } warnx("exiting."); From bfd0444cb30bf2db3f19a6a1c052d85c2270a090 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 Aug 2013 07:49:36 +0200 Subject: [PATCH 477/486] Revert "Increased the number of max files descriptors considerably" This reverts commit 3157285254381827bc0312bfb413ff6823feee3b. --- nuttx-configs/px4fmu-v1/nsh/defconfig | 4 ++-- nuttx-configs/px4fmu-v2/nsh/defconfig | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index 56ad46c3f5..a7a6725c65 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -405,8 +405,8 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=64 -CONFIG_NFILE_STREAMS=50 +CONFIG_NFILE_DESCRIPTORS=32 +CONFIG_NFILE_STREAMS=25 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 CONFIG_MQ_MAXMSGSIZE=32 diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index f99973aec5..3e2a481087 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -451,8 +451,8 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=64 -CONFIG_NFILE_STREAMS=50 +CONFIG_NFILE_DESCRIPTORS=32 +CONFIG_NFILE_STREAMS=25 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 CONFIG_MQ_MAXMSGSIZE=32 From 7b42d7a0470aaa50e9a8ec5852007ea52165a7ed Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 Aug 2013 07:50:15 +0200 Subject: [PATCH 478/486] Made number of streams more reasonable --- nuttx-configs/px4fmu-v1/nsh/defconfig | 2 +- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index a7a6725c65..b8b98b1496 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -406,7 +406,7 @@ CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 CONFIG_NFILE_DESCRIPTORS=32 -CONFIG_NFILE_STREAMS=25 +CONFIG_NFILE_STREAMS=8 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 CONFIG_MQ_MAXMSGSIZE=32 diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 3e2a481087..e507c89baf 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -452,7 +452,7 @@ CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 CONFIG_NFILE_DESCRIPTORS=32 -CONFIG_NFILE_STREAMS=25 +CONFIG_NFILE_STREAMS=8 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 CONFIG_MQ_MAXMSGSIZE=32 From 94d8ec4a1c1f052a50f4871db7445fb6454057d3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 Aug 2013 09:48:22 +0200 Subject: [PATCH 479/486] Calibration message cleanup --- .../commander/accelerometer_calibration.cpp | 17 ++++++------- src/modules/commander/gyro_calibration.cpp | 15 +++++++---- src/modules/commander/mag_calibration.cpp | 25 +++++++++++++------ 3 files changed, 35 insertions(+), 22 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 7a7fa6f4e4..e1616acd04 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -219,7 +219,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float bool done = true; char str[60]; int str_ptr; - str_ptr = sprintf(str, "keep vehicle still:"); + str_ptr = sprintf(str, "keep the vehicle still in one of these axes:"); unsigned old_done_count = done_count; done_count = 0; for (int i = 0; i < 6; i++) { @@ -236,22 +236,21 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float if (done) break; - mavlink_log_info(mavlink_fd, str); int orient = detect_orientation(mavlink_fd, sensor_combined_sub); if (orient < 0) return ERROR; if (data_collected[orient]) { - sprintf(str, "%s direction already measured, please rotate", orientation_strs[orient]); + sprintf(str, "%s done, please rotate to a different axis", orientation_strs[orient]); mavlink_log_info(mavlink_fd, str); continue; } // sprintf(str, - mavlink_log_info(mavlink_fd, "accel meas started: %s", orientation_strs[orient]); + mavlink_log_info(mavlink_fd, "accel measurement started: %s axis", orientation_strs[orient]); read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num); - str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient], + str_ptr = sprintf(str, "result for %s axis: [ %.2f %.2f %.2f ]", orientation_strs[orient], (double)accel_ref[orient][0], (double)accel_ref[orient][1], (double)accel_ref[orient][2]); @@ -265,7 +264,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float float accel_T[3][3]; int res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); if (res != 0) { - mavlink_log_info(mavlink_fd, "ERROR: calibration values calc error"); + mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error"); return ERROR; } @@ -337,7 +336,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, "still..."); + mavlink_log_info(mavlink_fd, "detected rest position, waiting..."); t_still = t; t_timeout = t + timeout; } else { @@ -352,7 +351,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { accel_disp[2] > still_thr2 * 2.0f) { /* not still, reset still start time */ if (t_still != 0) { - mavlink_log_info(mavlink_fd, "moving..."); + mavlink_log_info(mavlink_fd, "detected motion, please hold still..."); t_still = 0; } } @@ -364,7 +363,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { } if (poll_errcount > 1000) { - mavlink_log_info(mavlink_fd, "ERROR: failed reading accel"); + mavlink_log_info(mavlink_fd, "ERROR: Failed reading sensor"); return -1; } } diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 9cd2f3a19c..33566d4e53 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -58,7 +58,7 @@ static const int ERROR = -1; int do_gyro_calibration(int mavlink_fd) { - mavlink_log_info(mavlink_fd, "gyro calibration starting, hold still"); + mavlink_log_info(mavlink_fd, "Gyro calibration starting, do not move unit."); const int calibration_count = 5000; @@ -84,6 +84,8 @@ int do_gyro_calibration(int mavlink_fd) close(fd); + unsigned poll_errcount = 0; + while (calibration_counter < calibration_count) { /* wait blocking for new data */ @@ -93,16 +95,19 @@ int do_gyro_calibration(int mavlink_fd) int poll_ret = poll(fds, 1, 1000); - if (poll_ret) { + if (poll_ret > 0) { orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); gyro_offset[0] += raw.gyro_rad_s[0]; gyro_offset[1] += raw.gyro_rad_s[1]; gyro_offset[2] += raw.gyro_rad_s[2]; calibration_counter++; - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); + } else { + poll_errcount++; + } + + if (poll_errcount > 1000) { + mavlink_log_info(mavlink_fd, "ERROR: Failed reading gyro sensor"); return ERROR; } } diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 42f0190c7e..e386160271 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -61,7 +61,7 @@ static const int ERROR = -1; int do_mag_calibration(int mavlink_fd) { - mavlink_log_info(mavlink_fd, "mag calibration starting, hold still"); + mavlink_log_info(mavlink_fd, "please put the system in a rest position and wait."); int sub_mag = orb_subscribe(ORB_ID(sensor_mag)); struct mag_report mag; @@ -123,6 +123,10 @@ int do_mag_calibration(int mavlink_fd) return ERROR; } + mavlink_log_info(mavlink_fd, "scale calibration completed, dynamic calibration starting.."); + + unsigned poll_errcount = 0; + while (hrt_absolute_time() < calibration_deadline && calibration_counter < calibration_maxcount) { @@ -137,7 +141,7 @@ int do_mag_calibration(int mavlink_fd) axis_index++; - mavlink_log_info(mavlink_fd, "please rotate in a figure 8 or around %c", axislabels[axis_index]); + mavlink_log_info(mavlink_fd, "please rotate in a figure 8 or around %c axis.", axislabels[axis_index]); mavlink_log_info(mavlink_fd, "mag cal progress <%u> percent", 20 + (calibration_maxcount * 50) / calibration_counter); tune_neutral(); @@ -158,7 +162,7 @@ int do_mag_calibration(int mavlink_fd) int poll_ret = poll(fds, 1, 1000); - if (poll_ret) { + if (poll_ret > 0) { orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); x[calibration_counter] = mag.x; @@ -190,11 +194,16 @@ int do_mag_calibration(int mavlink_fd) calibration_counter++; - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "mag cal canceled (timed out)"); - break; + } else { + poll_errcount++; } + + if (poll_errcount > 1000) { + mavlink_log_info(mavlink_fd, "ERROR: Failed reading mag sensor"); + return ERROR; + } + + } float sphere_x; @@ -276,7 +285,7 @@ int do_mag_calibration(int mavlink_fd) (double)mscale.y_scale, (double)mscale.z_scale); mavlink_log_info(mavlink_fd, buf); - mavlink_log_info(mavlink_fd, "mag calibration done"); + mavlink_log_info(mavlink_fd, "magnetometer calibration completed"); mavlink_log_info(mavlink_fd, "mag cal progress <100> percent"); return OK; From 665a23259269d870e958543c6e6517323793c1dc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 Aug 2013 10:15:17 +0200 Subject: [PATCH 480/486] More calibration polishing --- .../commander/accelerometer_calibration.cpp | 23 ++++++++++--------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index e1616acd04..30ac07e332 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -217,20 +217,23 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float while (true) { bool done = true; - char str[60]; - int str_ptr; - str_ptr = sprintf(str, "keep the vehicle still in one of these axes:"); unsigned old_done_count = done_count; done_count = 0; + for (int i = 0; i < 6; i++) { if (!data_collected[i]) { - str_ptr += sprintf(&(str[str_ptr]), " %s", orientation_strs[i]); done = false; - } else { - done_count++; } } + mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s", + (!data_collected[0]) ? "x+ " : "", + (!data_collected[0]) ? "x- " : "", + (!data_collected[0]) ? "y+ " : "", + (!data_collected[0]) ? "y- " : "", + (!data_collected[0]) ? "z+ " : "", + (!data_collected[0]) ? "z- " : ""); + if (old_done_count != done_count) mavlink_log_info(mavlink_fd, "accel cal progress <%u> percent", 17 * done_count); @@ -242,19 +245,17 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float return ERROR; if (data_collected[orient]) { - sprintf(str, "%s done, please rotate to a different axis", orientation_strs[orient]); - mavlink_log_info(mavlink_fd, str); + mavlink_log_info(mavlink_fd, "%s done, please rotate to a different axis", orientation_strs[orient]); continue; } - // sprintf(str, mavlink_log_info(mavlink_fd, "accel measurement started: %s axis", orientation_strs[orient]); read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num); - str_ptr = sprintf(str, "result for %s axis: [ %.2f %.2f %.2f ]", orientation_strs[orient], + mavlink_log_info(mavlink_fd, "result for %s axis: [ %.2f %.2f %.2f ]", orientation_strs[orient], (double)accel_ref[orient][0], (double)accel_ref[orient][1], (double)accel_ref[orient][2]); - mavlink_log_info(mavlink_fd, str); + data_collected[orient] = true; tune_neutral(); } From 9c58d2c5c6ef96f9ece7f62d5f02d01d8b1e316e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 27 Aug 2013 18:07:31 +1000 Subject: [PATCH 481/486] airspeed: retry initial I2C probe 4 times this fixes a problem with detecting a MS4525D0 at boot --- src/drivers/airspeed/airspeed.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 5a8157debc..277d8249a3 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -146,7 +146,14 @@ out: int Airspeed::probe() { - return measure(); + /* on initial power up the device needs more than one retry + for detection. Once it is running then retries aren't + needed + */ + _retries = 4; + int ret = measure(); + _retries = 0; + return ret; } int From 33c73429098fee0650bdf2042a2c85e18975afca Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 Aug 2013 10:36:43 +0200 Subject: [PATCH 482/486] Minor fixes for calibration, UI language much more readable now --- src/modules/commander/accelerometer_calibration.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 30ac07e332..ed6707f045 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -228,11 +228,11 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s", (!data_collected[0]) ? "x+ " : "", - (!data_collected[0]) ? "x- " : "", - (!data_collected[0]) ? "y+ " : "", - (!data_collected[0]) ? "y- " : "", - (!data_collected[0]) ? "z+ " : "", - (!data_collected[0]) ? "z- " : ""); + (!data_collected[1]) ? "x- " : "", + (!data_collected[2]) ? "y+ " : "", + (!data_collected[3]) ? "y- " : "", + (!data_collected[4]) ? "z+ " : "", + (!data_collected[5]) ? "z- " : ""); if (old_done_count != done_count) mavlink_log_info(mavlink_fd, "accel cal progress <%u> percent", 17 * done_count); From b9d6981cee9878158435b9b1daa955d5b25c0389 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 27 Aug 2013 13:40:18 +0200 Subject: [PATCH 483/486] multirotor_att_control: yaw control bug fixed --- .../multirotor_att_control_main.c | 31 ++++++++++--------- 1 file changed, 17 insertions(+), 14 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 09955ec507..b3669d9ff6 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -147,8 +147,6 @@ mc_thread_main(int argc, char *argv[]) warnx("starting"); /* store last control mode to detect mode switches */ - bool flag_control_manual_enabled = false; - bool flag_control_attitude_enabled = false; bool control_yaw_position = true; bool reset_yaw_sp = true; bool failsafe_first_time = true; @@ -213,6 +211,11 @@ mc_thread_main(int argc, char *argv[]) /* set flag to safe value */ control_yaw_position = true; + /* reset yaw setpoint if not armed */ + if (!control_mode.flag_armed) { + reset_yaw_sp = true; + } + /* define which input is the dominating control input */ if (control_mode.flag_control_offboard_enabled) { /* offboard inputs */ @@ -234,10 +237,12 @@ mc_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); } + /* reset yaw setpoint after offboard control */ + reset_yaw_sp = true; + } else if (control_mode.flag_control_manual_enabled) { /* manual input */ if (control_mode.flag_control_attitude_enabled) { - /* control attitude, update attitude setpoint depending on mode */ /* if the RC signal is lost, try to stay level and go slowly back down to ground */ if (control_mode.failsave_highlevel) { @@ -279,17 +284,8 @@ mc_thread_main(int argc, char *argv[]) } else { failsafe_first_time = true; - /* control yaw in all manual / assisted modes */ - /* set yaw if arming or switching to attitude stabilized mode */ - - if (!flag_control_manual_enabled || !flag_control_attitude_enabled || !control_mode.flag_armed) { - reset_yaw_sp = true; - } - - /* only move setpoint if manual input is != 0 */ - - // TODO review yaw restpoint reset - if ((manual.yaw < -yaw_deadzone || yaw_deadzone < manual.yaw) && manual.throttle > min_takeoff_throttle) { + /* only move yaw setpoint if manual input is != 0 and not landed */ + if (manual.yaw < -yaw_deadzone || yaw_deadzone < manual.yaw) { /* control yaw rate */ control_yaw_position = false; rates_sp.yaw = manual.yaw; @@ -340,7 +336,14 @@ mc_thread_main(int argc, char *argv[]) rates_sp.thrust = manual.throttle; rates_sp.timestamp = hrt_absolute_time(); } + + /* reset yaw setpoint after ACRO */ + reset_yaw_sp = true; } + + } else { + /* reset yaw setpoint after non-manual control */ + reset_yaw_sp = true; } /* check if we should we reset integrals */ From 3380d40a7d9a8d1946510fab42abdc7a3a6f1525 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 Aug 2013 15:37:04 +0200 Subject: [PATCH 484/486] Tighter configs to save RAM --- nuttx-configs/px4fmu-v1/nsh/defconfig | 2 +- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index a6f914a647..feb4a393df 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -405,7 +405,7 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=32 +CONFIG_NFILE_DESCRIPTORS=24 CONFIG_NFILE_STREAMS=8 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index e507c89baf..e1bc867e53 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -451,7 +451,7 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=32 +CONFIG_NFILE_DESCRIPTORS=24 CONFIG_NFILE_STREAMS=8 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 From 864c1d048c6d9390d6bdf5a8058d48df9d36e973 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 27 Aug 2013 20:16:51 +0200 Subject: [PATCH 485/486] Revert "Tighter configs to save RAM" This reverts commit 3380d40a7d9a8d1946510fab42abdc7a3a6f1525. --- nuttx-configs/px4fmu-v1/nsh/defconfig | 2 +- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index feb4a393df..a6f914a647 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -405,7 +405,7 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=24 +CONFIG_NFILE_DESCRIPTORS=32 CONFIG_NFILE_STREAMS=8 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index e1bc867e53..e507c89baf 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -451,7 +451,7 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=24 +CONFIG_NFILE_DESCRIPTORS=32 CONFIG_NFILE_STREAMS=8 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 From 66c61fbe96e11ee7099431a8370d84f862543810 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 27 Aug 2013 23:08:00 +0200 Subject: [PATCH 486/486] Full failsafe rewrite. --- src/modules/commander/commander.cpp | 629 +++++++++--------- src/modules/commander/commander_params.c | 2 +- .../commander/state_machine_helper.cpp | 30 +- src/modules/commander/state_machine_helper.h | 6 +- src/modules/mavlink/mavlink.c | 6 +- .../multirotor_att_control_main.c | 98 ++- .../multirotor_pos_control.c | 59 +- .../position_estimator_inav_main.c | 2 +- .../uORB/topics/vehicle_control_mode.h | 4 +- src/modules/uORB/topics/vehicle_status.h | 13 +- 10 files changed, 415 insertions(+), 434 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d900086331..a548f943e2 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -147,8 +147,6 @@ static bool thread_should_exit = false; /**< daemon exit flag */ static bool thread_running = false; /**< daemon status flag */ static int daemon_task; /**< Handle of daemon task / thread */ -/* timout until lowlevel failsafe */ -static unsigned int failsafe_lowlevel_timeout_ms; static unsigned int leds_counter; /* To remember when last notification was sent */ static uint64_t last_print_mode_reject_time = 0; @@ -199,6 +197,7 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode int commander_thread_main(int argc, char *argv[]); void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed); + 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 *current_status); @@ -206,17 +205,20 @@ void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicl transition_result_t check_main_state_machine(struct vehicle_status_s *current_status); void print_reject_mode(const char *msg); + void print_reject_arm(const char *msg); void print_status(); -transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode, 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); /** * Loop that runs at a lower rate and priority for calibration and parameter tasks. */ void *commander_low_prio_loop(void *arg); +void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result); + int commander_main(int argc, char *argv[]) { @@ -271,7 +273,8 @@ void usage(const char *reason) exit(1); } -void print_status() { +void print_status() +{ warnx("usb powered: %s", (on_usb_power) ? "yes" : "no"); /* read all relevant states */ @@ -279,33 +282,40 @@ void print_status() { struct vehicle_status_s state; orb_copy(ORB_ID(vehicle_status), state_sub, &state); - const char* armed_str; + const char *armed_str; switch (state.arming_state) { - case ARMING_STATE_INIT: - armed_str = "INIT"; - break; - case ARMING_STATE_STANDBY: - armed_str = "STANDBY"; - break; - case ARMING_STATE_ARMED: - armed_str = "ARMED"; - break; - case ARMING_STATE_ARMED_ERROR: - armed_str = "ARMED_ERROR"; - break; - case ARMING_STATE_STANDBY_ERROR: - armed_str = "STANDBY_ERROR"; - break; - case ARMING_STATE_REBOOT: - armed_str = "REBOOT"; - break; - case ARMING_STATE_IN_AIR_RESTORE: - armed_str = "IN_AIR_RESTORE"; - break; - default: - armed_str = "ERR: UNKNOWN STATE"; - break; + case ARMING_STATE_INIT: + armed_str = "INIT"; + break; + + case ARMING_STATE_STANDBY: + armed_str = "STANDBY"; + break; + + case ARMING_STATE_ARMED: + armed_str = "ARMED"; + break; + + case ARMING_STATE_ARMED_ERROR: + armed_str = "ARMED_ERROR"; + break; + + case ARMING_STATE_STANDBY_ERROR: + armed_str = "STANDBY_ERROR"; + break; + + case ARMING_STATE_REBOOT: + armed_str = "REBOOT"; + break; + + case ARMING_STATE_IN_AIR_RESTORE: + armed_str = "IN_AIR_RESTORE"; + break; + + default: + armed_str = "ERR: UNKNOWN STATE"; + break; } @@ -326,7 +336,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe uint8_t custom_main_mode = (uint8_t) cmd->param2; // TODO remove debug code - mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode); + //mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode); /* set arming state */ transition_result_t arming_res = TRANSITION_NOT_CHANGED; @@ -415,6 +425,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } else { result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } + } else { /* reject TAKEOFF not armed */ result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; @@ -478,9 +489,6 @@ int commander_thread_main(int argc, char *argv[]) bool arm_tune_played = false; /* set parameters */ - failsafe_lowlevel_timeout_ms = 0; - param_get(param_find("SYS_FAILSAVE_LL"), &failsafe_lowlevel_timeout_ms); - param_t _param_sys_type = param_find("MAV_TYPE"); param_t _param_system_id = param_find("MAV_SYS_ID"); param_t _param_component_id = param_find("MAV_COMP_ID"); @@ -599,12 +607,6 @@ int commander_thread_main(int argc, char *argv[]) unsigned stick_off_counter = 0; unsigned stick_on_counter = 0; - /* To remember when last notification was sent */ - uint64_t last_print_control_time = 0; - - enum VEHICLE_BATTERY_WARNING battery_warning_previous = VEHICLE_BATTERY_WARNING_NONE; - bool armed_previous = false; - bool low_battery_voltage_actions_done = false; bool critical_battery_voltage_actions_done = false; @@ -665,7 +667,6 @@ int commander_thread_main(int argc, char *argv[]) int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); struct differential_pressure_s diff_pres; memset(&diff_pres, 0, sizeof(diff_pres)); - uint64_t last_diff_pres_time = 0; /* Subscribe to command topic */ int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -800,12 +801,15 @@ int commander_thread_main(int argc, char *argv[]) /* update condition_local_position_valid and condition_local_altitude_valid */ check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(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) { status.condition_landed = local_position.landed; status_changed = true; + if (status.condition_landed) { mavlink_log_critical(mavlink_fd, "[cmd] LANDED"); + } else { mavlink_log_critical(mavlink_fd, "[cmd] IN AIR"); } @@ -908,6 +912,7 @@ int commander_thread_main(int argc, char *argv[]) // XXX should we still allow to arm with critical battery? //arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); } + status_changed = true; } @@ -943,11 +948,6 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); - - /* check for first, long-term and valid GPS lock -> set home position */ - float hdop_m = gps_position.eph_m; - float vdop_m = gps_position.epv_m; - /* check if GPS fix is ok */ float hdop_threshold_m = 4.0f; float vdop_threshold_m = 8.0f; @@ -963,7 +963,7 @@ int commander_thread_main(int argc, char *argv[]) */ if (!home_position_set && gps_position.fix_type >= 3 && - (hdop_m < hdop_threshold_m) && (vdop_m < vdop_threshold_m) && // XXX note that vdop is 0 for mtk + (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && // XXX note that vdop is 0 for mtk (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed) { /* copy position data to uORB home message, store it locally as well */ // TODO use global position estimate @@ -1013,9 +1013,7 @@ int commander_thread_main(int argc, char *argv[]) } } - status.rc_signal_cutting_off = false; status.rc_signal_lost = false; - status.rc_signal_lost_interval = 0; transition_result_t res; // store all transitions results here @@ -1027,10 +1025,10 @@ int commander_thread_main(int argc, char *argv[]) if (status.is_rotary_wing && (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && (status.main_state == MAIN_STATE_MANUAL || status.navigation_state == NAVIGATION_STATE_AUTO_READY || - (status.condition_landed && ( - status.navigation_state == NAVIGATION_STATE_ALTHOLD || - status.navigation_state == NAVIGATION_STATE_VECTOR - ))) && sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + (status.condition_landed && ( + status.navigation_state == NAVIGATION_STATE_ALTHOLD || + status.navigation_state == NAVIGATION_STATE_VECTOR + ))) && sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); @@ -1040,6 +1038,7 @@ int commander_thread_main(int argc, char *argv[]) } else { stick_off_counter++; } + } else { stick_off_counter = 0; } @@ -1087,7 +1086,7 @@ int commander_thread_main(int argc, char *argv[]) res = check_main_state_machine(&status); if (res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state); + //mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state); tune_positive(); } else if (res == TRANSITION_DENIED) { @@ -1097,122 +1096,14 @@ int commander_thread_main(int argc, char *argv[]) } } else { - - /* print error message for first RC glitch and then every 5s */ - if (!status.rc_signal_cutting_off || (hrt_absolute_time() - last_print_control_time) > PRINT_INTERVAL) { - // TODO remove debug code - if (!status.rc_signal_cutting_off) { - warnx("Reason: not rc_signal_cutting_off\n"); - - } else { - warnx("last print time: %llu\n", last_print_control_time); - } - - /* only complain if the offboard control is NOT active */ - status.rc_signal_cutting_off = true; - mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: NO RC CONTROL"); - - last_print_control_time = hrt_absolute_time(); - } - - /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp; - - /* if the RC signal is gone for a full second, consider it lost */ - if (status.rc_signal_lost_interval > 1000000) { + if (!status.rc_signal_lost) { + mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: RC SIGNAL LOST"); status.rc_signal_lost = true; - status.failsave_lowlevel = true; status_changed = true; } } } - /* END mode switch */ - /* END RC state check */ - - // TODO check this - /* state machine update for offboard control */ - if (!status.rc_signal_found_once && sp_offboard.timestamp != 0) { - if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) { // TODO 5s is too long ? - - // /* decide about attitude control flag, enable in att/pos/vel */ - // bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE || - // sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || - // sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION); - - // /* decide about rate control flag, enable it always XXX (for now) */ - // bool rates_ctrl_enabled = true; - - // /* set up control mode */ - // if (current_status.flag_control_attitude_enabled != attitude_ctrl_enabled) { - // current_status.flag_control_attitude_enabled = attitude_ctrl_enabled; - // state_changed = true; - // } - - // if (current_status.flag_control_rates_enabled != rates_ctrl_enabled) { - // current_status.flag_control_rates_enabled = rates_ctrl_enabled; - // state_changed = true; - // } - - // /* handle the case where offboard control signal was regained */ - // if (!current_status.offboard_control_signal_found_once) { - // current_status.offboard_control_signal_found_once = true; - // /* enable offboard control, disable manual input */ - // current_status.flag_control_manual_enabled = false; - // current_status.flag_control_offboard_enabled = true; - // state_changed = true; - // tune_positive(); - - // mavlink_log_critical(mavlink_fd, "DETECTED OFFBOARD SIGNAL FIRST"); - - // } else { - // if (current_status.offboard_control_signal_lost) { - // mavlink_log_critical(mavlink_fd, "RECOVERY OFFBOARD CONTROL"); - // state_changed = true; - // tune_positive(); - // } - // } - - status.offboard_control_signal_weak = false; - status.offboard_control_signal_lost = false; - status.offboard_control_signal_lost_interval = 0; - - // XXX check if this is correct - /* arm / disarm on request */ - if (sp_offboard.armed && !armed.armed) { - arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); - - } else if (!sp_offboard.armed && armed.armed) { - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); - } - - } else { - - /* print error message for first offboard signal glitch and then every 5s */ - if (!status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_control_time) > PRINT_INTERVAL)) { - status.offboard_control_signal_weak = true; - mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: NO OFFBOARD CONTROL"); - last_print_control_time = hrt_absolute_time(); - } - - /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp; - - /* if the signal is gone for 0.1 seconds, consider it lost */ - if (status.offboard_control_signal_lost_interval > 100000) { - status.offboard_control_signal_lost = true; - status.failsave_lowlevel_start_time = hrt_absolute_time(); - tune_positive(); - - /* kill motors after timeout */ - if (hrt_absolute_time() > status.failsave_lowlevel_start_time + failsafe_lowlevel_timeout_ms * 1000) { - status.failsave_lowlevel = true; - status_changed = true; - } - } - } - } - /* evaluate the navigation state machine */ transition_result_t res = check_navigation_state_machine(&status, &control_mode, &local_position); @@ -1230,6 +1121,7 @@ int commander_thread_main(int argc, char *argv[]) if (arming_state_changed || main_state_changed || navigation_state_changed) { mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state); status_changed = true; + } else { status_changed = false; } @@ -1288,10 +1180,6 @@ int commander_thread_main(int argc, char *argv[]) arm_tune_played = false; } - /* store old modes to detect and act on state transitions */ - battery_warning_previous = status.battery_warning; - armed_previous = armed.armed; - fflush(stdout); counter++; @@ -1343,6 +1231,7 @@ void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed) { #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + /* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */ if (armed->armed) { /* armed, solid */ @@ -1352,11 +1241,13 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang /* ready to arm, blink at 1Hz */ if (leds_counter % 20 == 0) led_toggle(LED_BLUE); + } else { /* not ready to arm, blink at 10Hz */ if (leds_counter % 2 == 0) led_toggle(LED_BLUE); } + #endif if (changed) { @@ -1369,50 +1260,60 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang /* armed, solid */ if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER; + } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; + } else { - pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN :RGBLED_COLOR_GREEN; + pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN : RGBLED_COLOR_GREEN; } + pattern.duration[0] = 1000; } else if (armed->ready_to_arm) { - for (i=0; i<3; i++) { + for (i = 0; i < 3; i++) { if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { - pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER; - } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { - pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; - } else { - pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN : RGBLED_COLOR_GREEN; - } - pattern.duration[i*2] = 200; + pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER; - pattern.color[i*2+1] = RGBLED_COLOR_OFF; - pattern.duration[i*2+1] = 800; - } - if (status->condition_global_position_valid) { - pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE; - pattern.duration[i*2] = 1000; - pattern.color[i*2+1] = RGBLED_COLOR_OFF; - pattern.duration[i*2+1] = 800; - } else { - for (i=3; i<6; i++) { - pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE; - pattern.duration[i*2] = 100; - pattern.color[i*2+1] = RGBLED_COLOR_OFF; - pattern.duration[i*2+1] = 100; + } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { + pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; + + } else { + pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN : RGBLED_COLOR_GREEN; } - pattern.color[6*2] = RGBLED_COLOR_OFF; - pattern.duration[6*2] = 700; + + pattern.duration[i * 2] = 200; + + pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF; + pattern.duration[i * 2 + 1] = 800; + } + + if (status->condition_global_position_valid) { + pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE; + pattern.duration[i * 2] = 1000; + pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF; + pattern.duration[i * 2 + 1] = 800; + + } else { + for (i = 3; i < 6; i++) { + pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE; + pattern.duration[i * 2] = 100; + pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF; + pattern.duration[i * 2 + 1] = 100; + } + + pattern.color[6 * 2] = RGBLED_COLOR_OFF; + pattern.duration[6 * 2] = 700; } } else { - for (i=0; i<3; i++) { - pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; - pattern.duration[i*2] = 200; - pattern.color[i*2+1] = RGBLED_COLOR_OFF; - pattern.duration[i*2+1] = 200; + for (i = 0; i < 3; i++) { + pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; + pattern.duration[i * 2] = 200; + pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF; + pattern.duration[i * 2 + 1] = 200; } + /* not ready to arm, blink at 10Hz */ } @@ -1423,6 +1324,7 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang if (status->load > 0.95f) { if (leds_counter % 2 == 0) led_toggle(LED_AMBER); + } else { led_off(LED_AMBER); } @@ -1572,70 +1474,124 @@ print_reject_arm(const char *msg) } transition_result_t -check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos) +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 res = TRANSITION_DENIED; - switch (current_status->main_state) { - case MAIN_STATE_MANUAL: - res = navigation_state_transition(current_status, current_status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode); - break; - - case MAIN_STATE_SEATBELT: - res = navigation_state_transition(current_status, NAVIGATION_STATE_ALTHOLD, control_mode); - break; - - case MAIN_STATE_EASY: - res = navigation_state_transition(current_status, NAVIGATION_STATE_VECTOR, control_mode); - break; - - case MAIN_STATE_AUTO: - if (current_status->arming_state == ARMING_STATE_ARMED || current_status->arming_state == ARMING_STATE_ARMED_ERROR) { - if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + if (status->main_state == MAIN_STATE_AUTO) { + if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) { + if (status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { /* don't switch to other states until takeoff not completed */ - if (local_pos->z > -5.0f || status.condition_landed) { + if (local_pos->z > -5.0f || status->condition_landed) { res = TRANSITION_NOT_CHANGED; - break; } } - /* check again, state can be changed */ - if (current_status->condition_landed) { - /* if landed: transitions only to AUTO_READY are allowed */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); - // TRANSITION_DENIED is not possible here - break; - } else { - /* if not landed: */ - if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) { - /* act depending on switches when manual control enabled */ - if (current_status->return_switch == RETURN_SWITCH_RETURN) { - /* RTL */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); - - } else { - if (current_status->mission_switch == MISSION_SWITCH_MISSION) { - /* MISSION */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); - - } else { - /* LOITER */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); - } - } + if (res != TRANSITION_NOT_CHANGED) { + /* check again, state can be changed */ + if (status->condition_landed) { + /* if landed: transitions only to AUTO_READY are allowed */ + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode); + // TRANSITION_DENIED is not possible here } else { - /* switch to mission in air when no RC control */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); + /* not landed */ + if (status->rc_signal_found_once && !status->rc_signal_lost) { + /* act depending on switches when manual control enabled */ + if (status->return_switch == RETURN_SWITCH_RETURN) { + /* RTL */ + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_RTL, control_mode); + + } else { + if (status->mission_switch == MISSION_SWITCH_MISSION) { + /* MISSION */ + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode); + + } else { + /* LOITER */ + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + } + } + + } else { + /* switch to MISSION in air when no RC control */ + if (status->navigation_state == NAVIGATION_STATE_AUTO_LOITER || + status->navigation_state == NAVIGATION_STATE_AUTO_MISSION || + status->navigation_state == NAVIGATION_STATE_AUTO_RTL || + status->navigation_state == NAVIGATION_STATE_AUTO_LAND) { + res = TRANSITION_NOT_CHANGED; + + } else { + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode); + } + } } } - } else { - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); - } - break; - default: - break; + } else { + /* disarmed, always switch to AUTO_READY */ + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode); + } + + } else { + /* manual control modes */ + if (status->rc_signal_lost && (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR)) { + /* switch to failsafe mode */ + bool manual_control_old = control_mode->flag_control_manual_enabled; + + if (!status->condition_landed) { + /* in air: try to hold position */ + res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode); + + } else { + /* landed: don't try to hold position but land (if taking off) */ + res = TRANSITION_DENIED; + } + + if (res == TRANSITION_DENIED) { + res = navigation_state_transition(status, NAVIGATION_STATE_ALTHOLD, control_mode); + } + + control_mode->flag_control_manual_enabled = false; + + if (res == TRANSITION_NOT_CHANGED && manual_control_old) { + /* mark navigation state as changed to force immediate flag publishing */ + set_navigation_state_changed(); + res = TRANSITION_CHANGED; + } + + if (res == TRANSITION_CHANGED) { + if (control_mode->flag_control_position_enabled) { + mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: POS HOLD"); + + } else { + if (status->condition_landed) { + mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: ALT HOLD (LAND)"); + + } else { + mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: ALT HOLD"); + } + } + } + + } else { + switch (status->main_state) { + case MAIN_STATE_MANUAL: + res = navigation_state_transition(status, status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode); + break; + + case MAIN_STATE_SEATBELT: + res = navigation_state_transition(status, NAVIGATION_STATE_ALTHOLD, control_mode); + break; + + case MAIN_STATE_EASY: + res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode); + break; + + default: + break; + } + } } return res; @@ -1644,27 +1600,32 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result) { switch (result) { - case VEHICLE_CMD_RESULT_ACCEPTED: + case VEHICLE_CMD_RESULT_ACCEPTED: tune_positive(); - break; - case VEHICLE_CMD_RESULT_DENIED: - mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command); - tune_negative(); - break; - case VEHICLE_CMD_RESULT_FAILED: - mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command); - tune_negative(); - break; - case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: - mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd.command); - tune_negative(); - break; - case VEHICLE_CMD_RESULT_UNSUPPORTED: - mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command); - tune_negative(); - break; - default: - break; + break; + + case VEHICLE_CMD_RESULT_DENIED: + mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command); + tune_negative(); + break; + + case VEHICLE_CMD_RESULT_FAILED: + mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command); + tune_negative(); + break; + + case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: + mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd.command); + tune_negative(); + break; + + case VEHICLE_CMD_RESULT_UNSUPPORTED: + mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command); + tune_negative(); + break; + + default: + break; } } @@ -1720,11 +1681,13 @@ void *commander_low_prio_loop(void *arg) usleep(1000000); /* reboot */ systemreset(false); + } else if (((int)(cmd.param1)) == 3) { answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); usleep(1000000); /* reboot to bootloader */ systemreset(true); + } else { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); } @@ -1732,83 +1695,85 @@ void *commander_low_prio_loop(void *arg) } else { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); } - break; + + break; case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { - int calib_ret = ERROR; + int calib_ret = ERROR; - /* try to go to INIT/PREFLIGHT arming state */ + /* try to go to INIT/PREFLIGHT arming state */ + + // XXX disable interrupts in arming_state_transition + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) { + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + break; + } + + if ((int)(cmd.param1) == 1) { + /* gyro calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_gyro_calibration(mavlink_fd); + + } else if ((int)(cmd.param2) == 1) { + /* magnetometer calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_mag_calibration(mavlink_fd); + + } else if ((int)(cmd.param3) == 1) { + /* zero-altitude pressure calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + + } else if ((int)(cmd.param4) == 1) { + /* RC calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + + } else if ((int)(cmd.param5) == 1) { + /* accelerometer calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_accel_calibration(mavlink_fd); + + } else if ((int)(cmd.param6) == 1) { + /* airspeed calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_airspeed_calibration(mavlink_fd); + } + + if (calib_ret == OK) + tune_positive(); + else + tune_negative(); + + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); - // XXX disable interrupts in arming_state_transition - if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) { - answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); break; } - if ((int)(cmd.param1) == 1) { - /* gyro calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - calib_ret = do_gyro_calibration(mavlink_fd); - - } else if ((int)(cmd.param2) == 1) { - /* magnetometer calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - calib_ret = do_mag_calibration(mavlink_fd); - - } else if ((int)(cmd.param3) == 1) { - /* zero-altitude pressure calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); - - } else if ((int)(cmd.param4) == 1) { - /* RC calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); - - } else if ((int)(cmd.param5) == 1) { - /* accelerometer calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - calib_ret = do_accel_calibration(mavlink_fd); - - } else if ((int)(cmd.param6) == 1) { - /* airspeed calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - calib_ret = do_airspeed_calibration(mavlink_fd); - } - - if (calib_ret == OK) - tune_positive(); - else - tune_negative(); - - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); - - break; - } - case VEHICLE_CMD_PREFLIGHT_STORAGE: { - if (((int)(cmd.param1)) == 0) { - if (0 == param_load_default()) { - mavlink_log_info(mavlink_fd, "[cmd] parameters loaded"); - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + if (((int)(cmd.param1)) == 0) { + if (0 == param_load_default()) { + mavlink_log_info(mavlink_fd, "[cmd] parameters loaded"); + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - } else { - mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); - answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); + } else { + mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); + answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); + } + + } else if (((int)(cmd.param1)) == 1) { + if (0 == param_save_default()) { + mavlink_log_info(mavlink_fd, "[cmd] parameters saved"); + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + + } else { + mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); + answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); + } } - } else if (((int)(cmd.param1)) == 1) { - if (0 == param_save_default()) { - mavlink_log_info(mavlink_fd, "[cmd] parameters saved"); - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - - } else { - mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); - answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); - } + break; } - break; - } default: answer_command(cmd, VEHICLE_CMD_RESULT_UNSUPPORTED); @@ -1817,7 +1782,7 @@ void *commander_low_prio_loop(void *arg) /* send any requested ACKs */ if (cmd.confirmation > 0 && cmd.command != VEHICLE_CMD_DO_SET_MODE - && cmd.command != VEHICLE_CMD_COMPONENT_ARM_DISARM) { + && cmd.command != VEHICLE_CMD_COMPONENT_ARM_DISARM) { /* send acknowledge command */ // XXX TODO } diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 6832fc5ce3..0a1703b2e0 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -45,7 +45,7 @@ #include #include -PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */ +PARAM_DEFINE_INT32(SYS_FAILSAFE_LL, 0); /**< Go into low-level failsafe after 0 ms */ PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index fe7e8cc536..674f3feda3 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -184,7 +184,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * return ret; } -bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed) +bool is_safe(const struct vehicle_status_s *status, const struct safety_s *safety, const struct actuator_armed_s *armed) { // System is safe if: // 1) Not armed @@ -276,12 +276,12 @@ check_main_state_changed() } transition_result_t -navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode) +navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode) { transition_result_t ret = TRANSITION_DENIED; /* only check transition if the new state is actually different from the current one */ - if (new_navigation_state == current_status->navigation_state) { + if (new_navigation_state == status->navigation_state) { ret = TRANSITION_NOT_CHANGED; } else { @@ -296,6 +296,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_altitude_enabled = false; control_mode->flag_control_climb_rate_enabled = false; control_mode->flag_control_manual_enabled = true; + control_mode->flag_control_auto_enabled = false; break; case NAVIGATION_STATE_STABILIZE: @@ -307,6 +308,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_altitude_enabled = false; control_mode->flag_control_climb_rate_enabled = false; control_mode->flag_control_manual_enabled = true; + control_mode->flag_control_auto_enabled = false; break; case NAVIGATION_STATE_ALTHOLD: @@ -318,6 +320,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = true; + control_mode->flag_control_auto_enabled = false; break; case NAVIGATION_STATE_VECTOR: @@ -329,6 +332,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = true; + control_mode->flag_control_auto_enabled = false; break; case NAVIGATION_STATE_AUTO_READY: @@ -340,12 +344,13 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_altitude_enabled = false; control_mode->flag_control_climb_rate_enabled = false; control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_auto_enabled = true; break; case NAVIGATION_STATE_AUTO_TAKEOFF: /* only transitions from AUTO_READY */ - if (current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) { + if (status->navigation_state == NAVIGATION_STATE_AUTO_READY) { ret = TRANSITION_CHANGED; control_mode->flag_control_rates_enabled = true; control_mode->flag_control_attitude_enabled = true; @@ -354,6 +359,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_auto_enabled = true; } break; @@ -367,6 +373,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_auto_enabled = false; break; case NAVIGATION_STATE_AUTO_MISSION: @@ -378,6 +385,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_auto_enabled = true; break; case NAVIGATION_STATE_AUTO_RTL: @@ -389,12 +397,13 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_auto_enabled = true; break; case NAVIGATION_STATE_AUTO_LAND: /* deny transitions from landed state */ - if (current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { + if (status->navigation_state != NAVIGATION_STATE_AUTO_READY) { ret = TRANSITION_CHANGED; control_mode->flag_control_rates_enabled = true; control_mode->flag_control_attitude_enabled = true; @@ -403,6 +412,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_auto_enabled = true; } break; @@ -412,8 +422,8 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ } if (ret == TRANSITION_CHANGED) { - current_status->navigation_state = new_navigation_state; - control_mode->auto_state = current_status->navigation_state; + status->navigation_state = new_navigation_state; + control_mode->auto_state = status->navigation_state; navigation_state_changed = true; } } @@ -433,6 +443,12 @@ check_navigation_state_changed() } } +void +set_navigation_state_changed() +{ + navigation_state_changed = true; +} + /** * Transition from one hil state to another */ diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index a38c2497eb..1641b6f60b 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -58,7 +58,7 @@ typedef enum { } transition_result_t; transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety, - arming_state_t new_arming_state, struct actuator_armed_s *armed); + arming_state_t new_arming_state, struct actuator_armed_s *armed); bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed); @@ -68,10 +68,12 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state bool check_main_state_changed(); -transition_result_t navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode); +transition_result_t navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode); bool check_navigation_state_changed(); +void set_navigation_state_changed(); + int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd); #endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 0a506b1a92..d7b0fa9c7e 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -240,11 +240,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u **/ /* set calibration state */ - if (v_status.preflight_calibration) { - *mavlink_state = MAV_STATE_CALIBRATING; - } else if (v_status.system_emergency) { - *mavlink_state = MAV_STATE_EMERGENCY; - } else if (v_status.arming_state == ARMING_STATE_INIT + if (v_status.arming_state == ARMING_STATE_INIT || v_status.arming_state == ARMING_STATE_IN_AIR_RESTORE || v_status.arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review *mavlink_state = MAV_STATE_UNINIT; diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index b3669d9ff6..04582f2a40 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -2,6 +2,7 @@ * * Copyright (C) 2012 PX4 Development Team. All rights reserved. * Author: Lorenz Meier + * Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -38,6 +39,7 @@ * Implementation of multirotor attitude control main loop. * * @author Lorenz Meier + * @author Anton Babushkin */ #include @@ -63,6 +65,7 @@ #include #include #include +#include #include #include #include @@ -74,8 +77,6 @@ #include "multirotor_attitude_control.h" #include "multirotor_rate_control.h" -PARAM_DEFINE_FLOAT(MC_RCLOSS_THR, 0.0f); // This defines the throttle when the RC signal is lost. - __EXPORT int multirotor_att_control_main(int argc, char *argv[]); static bool thread_should_exit; @@ -102,7 +103,8 @@ mc_thread_main(int argc, char *argv[]) memset(&offboard_sp, 0, sizeof(offboard_sp)); struct vehicle_rates_setpoint_s rates_sp; memset(&rates_sp, 0, sizeof(rates_sp)); - + struct vehicle_status_s status; + memset(&status, 0, sizeof(status)); struct actuator_controls_s actuators; memset(&actuators, 0, sizeof(actuators)); @@ -114,6 +116,8 @@ mc_thread_main(int argc, char *argv[]) int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); + int status_sub = orb_subscribe(ORB_ID(vehicle_status)); /* * Do not rate-limit the loop to prevent aliasing @@ -136,7 +140,6 @@ mc_thread_main(int argc, char *argv[]) orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); orb_advert_t rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); - int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); /* register the perf counter */ perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control_runtime"); @@ -149,12 +152,6 @@ mc_thread_main(int argc, char *argv[]) /* store last control mode to detect mode switches */ bool control_yaw_position = true; bool reset_yaw_sp = true; - bool failsafe_first_time = true; - - /* prepare the handle for the failsafe throttle */ - param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR"); - float failsafe_throttle = 0.0f; - param_get(failsafe_throttle_handle, &failsafe_throttle); while (!thread_should_exit) { @@ -176,7 +173,6 @@ mc_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(parameter_update), param_sub, &update); /* update parameters */ - param_get(failsafe_throttle_handle, &failsafe_throttle); } /* only run controller if attitude changed */ @@ -205,6 +201,13 @@ mc_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(offboard_control_setpoint), setpoint_sub, &offboard_sp); } + /* get a local copy of status */ + orb_check(status_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_status), status_sub, &status); + } + /* get a local copy of the current sensor values */ orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); @@ -244,47 +247,18 @@ mc_thread_main(int argc, char *argv[]) /* manual input */ if (control_mode.flag_control_attitude_enabled) { /* control attitude, update attitude setpoint depending on mode */ - /* if the RC signal is lost, try to stay level and go slowly back down to ground */ - if (control_mode.failsave_highlevel) { - failsafe_first_time = false; + if (att_sp.thrust < 0.1f) { + /* no thrust, don't try to control yaw */ + rates_sp.yaw = 0.0f; + control_yaw_position = false; - if (!control_mode.flag_control_velocity_enabled) { - /* don't reset attitude setpoint in position control mode, it's handled by position controller. */ - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - } - - if (!control_mode.flag_control_climb_rate_enabled) { - /* don't touch throttle in modes with altitude hold, it's handled by position controller. - * - * Only go to failsafe throttle if last known throttle was - * high enough to create some lift to make hovering state likely. - * - * This is to prevent that someone landing, but not disarming his - * multicopter (throttle = 0) does not make it jump up in the air - * if shutting down his remote. - */ - if (isfinite(manual.throttle) && manual.throttle > min_takeoff_throttle) { // TODO use landed status instead of throttle - /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ - att_sp.thrust = failsafe_throttle; - - } else { - att_sp.thrust = 0.0f; - } - } - - /* keep current yaw, do not attempt to go to north orientation, - * since if the pilot regains RC control, he will be lost regarding - * the current orientation. - */ - if (failsafe_first_time) { + if (status.condition_landed) { + /* reset yaw setpoint if on ground */ reset_yaw_sp = true; } } else { - failsafe_first_time = true; - - /* only move yaw setpoint if manual input is != 0 and not landed */ + /* only move yaw setpoint if manual input is != 0 */ if (manual.yaw < -yaw_deadzone || yaw_deadzone < manual.yaw) { /* control yaw rate */ control_yaw_position = false; @@ -294,18 +268,17 @@ mc_thread_main(int argc, char *argv[]) } else { control_yaw_position = true; } + } - if (!control_mode.flag_control_velocity_enabled) { - /* update attitude setpoint if not in position control mode */ - att_sp.roll_body = manual.roll; - att_sp.pitch_body = manual.pitch; + if (!control_mode.flag_control_velocity_enabled) { + /* update attitude setpoint if not in position control mode */ + att_sp.roll_body = manual.roll; + att_sp.pitch_body = manual.pitch; - if (!control_mode.flag_control_climb_rate_enabled) { - /* pass throttle directly if not in altitude control mode */ - att_sp.thrust = manual.throttle; - } + if (!control_mode.flag_control_climb_rate_enabled) { + /* pass throttle directly if not in altitude control mode */ + att_sp.thrust = manual.throttle; } - } /* reset yaw setpint to current position if needed */ @@ -324,7 +297,7 @@ mc_thread_main(int argc, char *argv[]) att_sp.timestamp = hrt_absolute_time(); - /* publish the controller output */ + /* publish the attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); } else { @@ -342,6 +315,17 @@ mc_thread_main(int argc, char *argv[]) } } else { + if (!control_mode.flag_control_auto_enabled) { + /* no control, try to stay on place */ + if (!control_mode.flag_control_velocity_enabled) { + /* no velocity control, reset attitude setpoint */ + att_sp.roll_body = 0.0f; + att_sp.pitch_body = 0.0f; + att_sp.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + } + } + /* reset yaw setpoint after non-manual control */ reset_yaw_sp = true; } diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 385892f04e..8227f76c5e 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -415,10 +415,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* force reprojection of global setpoint after manual mode */ global_pos_sp_reproject = true; - } else { - /* non-manual mode, use global setpoint */ + } else if (control_mode.flag_control_auto_enabled) { + /* AUTO mode, use global setpoint */ if (control_mode.auto_state == NAVIGATION_STATE_AUTO_READY) { reset_auto_pos = true; + } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) { if (reset_auto_pos) { local_pos_sp.x = local_pos.x; @@ -428,21 +429,13 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) local_pos_sp_valid = true; att_sp.yaw_body = att.yaw; reset_auto_pos = false; - mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", local_pos_sp.x, local_pos_sp.y, local_pos_sp.z); - } - } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_LOITER) { - if (reset_auto_pos) { - local_pos_sp.x = local_pos.x; - local_pos_sp.y = local_pos.y; - local_pos_sp.z = local_pos.z; - local_pos_sp.yaw = att.yaw; - local_pos_sp_valid = true; - att_sp.yaw_body = att.yaw; - reset_auto_pos = false; + mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", local_pos_sp.x, local_pos_sp.y, -local_pos_sp.z); } + } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) { // TODO reset_auto_pos = true; + } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_MISSION) { /* init local projection using local position ref */ if (local_pos.ref_timestamp != local_ref_timestamp) { @@ -457,6 +450,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (global_pos_sp_reproject) { /* update global setpoint projection */ global_pos_sp_reproject = false; + if (global_pos_sp_valid) { /* global position setpoint valid, use it */ double sp_lat = global_pos_sp.lat * 1e-7; @@ -470,6 +464,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } else { local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; } + local_pos_sp.yaw = global_pos_sp.yaw; att_sp.yaw_body = global_pos_sp.yaw; @@ -489,6 +484,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y); } } + reset_auto_pos = true; } @@ -496,9 +492,44 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) global_pos_sp_reproject = true; } - /* reset setpoints after non-manual modes */ + /* reset setpoints after AUTO mode */ reset_sp_xy = true; reset_sp_z = true; + + } else { + /* no control, loiter or stay on ground */ + if (local_pos.landed) { + /* landed: move setpoint down */ + /* in air: hold altitude */ + if (local_pos_sp.z < 5.0f) { + /* set altitude setpoint to 5m under ground, + * don't set it too deep to avoid unexpected landing in case of false "landed" signal */ + local_pos_sp.z = 5.0f; + mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", -local_pos_sp.z); + } + + reset_sp_z = true; + + } else { + /* in air: hold altitude */ + if (reset_sp_z) { + reset_sp_z = false; + local_pos_sp.z = local_pos.z; + mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", -local_pos_sp.z); + } + } + + if (control_mode.flag_control_position_enabled) { + if (reset_sp_xy) { + reset_sp_xy = false; + local_pos_sp.x = local_pos.x; + local_pos_sp.y = local_pos.y; + local_pos_sp.yaw = att.yaw; + local_pos_sp_valid = true; + att_sp.yaw_body = att.yaw; + mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", local_pos_sp.x, local_pos_sp.y); + } + } } /* publish local position setpoint */ diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index ef13ade886..4a4572b09c 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -429,7 +429,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (fds[6].revents & POLLIN) { orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout && gps.eph_m < 5.0f) { + if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout && gps.eph_m < 4.0f) { /* initialize reference position if needed */ if (!ref_xy_inited) { if (ref_xy_init_start == 0) { diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index e15ddde269..093c6775d1 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -80,9 +80,7 @@ struct vehicle_control_mode_s bool flag_control_altitude_enabled; /**< true if altitude is controlled */ bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ - bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ - bool failsave_highlevel; /**< Set to true if high-level failsafe mode is enabled */ - + bool flag_control_auto_enabled; // TEMP uint8_t auto_state; // TEMP navigation state for AUTO modes }; diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 9f55bb8742..43218eac4e 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -174,8 +174,6 @@ struct vehicle_status_s uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ - uint64_t failsave_lowlevel_start_time; /**< time when the lowlevel failsafe flag was set */ -// uint64_t failsave_highlevel_begin; TO BE COMPLETED main_state_t main_state; /**< main state machine */ navigation_state_t navigation_state; /**< navigation state machine */ @@ -207,22 +205,13 @@ struct vehicle_status_s bool condition_landed; /**< true if vehicle is landed, always true if disarmed */ bool rc_signal_found_once; - bool rc_signal_lost; /**< true if RC reception is terminally lost */ - bool rc_signal_cutting_off; /**< true if RC reception is weak / cutting off */ - uint64_t rc_signal_lost_interval; /**< interval in microseconds since when no RC signal is available */ + bool rc_signal_lost; /**< true if RC reception lost */ bool offboard_control_signal_found_once; bool offboard_control_signal_lost; bool offboard_control_signal_weak; uint64_t offboard_control_signal_lost_interval; /**< interval in microseconds without an offboard control message */ - bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ - bool failsave_highlevel; - - bool preflight_calibration; - - bool system_emergency; - /* see SYS_STATUS mavlink message for the following */ uint32_t onboard_control_sensors_present; uint32_t onboard_control_sensors_enabled;