From 0e2db0beb9228720a40bd19a7bd8891e5a8fdaba Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 16 Feb 2013 13:40:09 -0800 Subject: [PATCH 001/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/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_onboard/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/Makefile b/src/modules/mavlink/module.mk similarity index 82% rename from apps/mavlink/Makefile rename to src/modules/mavlink/module.mk index 8ddb69b71d..cbf08aeb20 100644 --- a/apps/mavlink/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 -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_onboard/Makefile b/src/modules/mavlink_onboard/module.mk similarity index 85% rename from apps/mavlink_onboard/Makefile rename to src/modules/mavlink_onboard/module.mk index 8b1cb9b2c7..c40fa042e8 100644 --- a/apps/mavlink_onboard/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_onboard -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/872] 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/872] 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/872] 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/872] 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/872] 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/872] Moved all system commands to new build system --- apps/systemcmds/boardinfo/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/boardinfo/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/calibration/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/boardinfo/Makefile b/apps/systemcmds/boardinfo/Makefile deleted file mode 100644 index 6f1be149c6..0000000000 --- a/apps/systemcmds/boardinfo/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. -# -############################################################################ - -# -# Build the boardinfo tool. -# - -APPNAME = boardinfo -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 - -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/calibration/Makefile b/src/systemcmds/preflight_check/module.mk similarity index 86% rename from apps/systemcmds/calibration/Makefile rename to src/systemcmds/preflight_check/module.mk index a1735962e1..7c3c887836 100644 --- a/apps/systemcmds/calibration/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 @@ ############################################################################ # -# Makefile to build the calibration 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 = calibration -PRIORITY = SCHED_PRIORITY_MAX - 1 -STACKSIZE = 4096 - -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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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/872] 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 65d36c44afdacc1ed4762fe63c3e0f8a4a6f1317 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 12 Mar 2013 12:12:23 -0700 Subject: [PATCH 140/872] Prevent flips at high throttle Conflicts: src/drivers/ardrone_interface/ardrone_motor_control.c --- .../ardrone_interface/ardrone_motor_control.c | 84 ++++++++----------- 1 file changed, 36 insertions(+), 48 deletions(-) diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.c b/src/drivers/ardrone_interface/ardrone_motor_control.c index ecd31a073d..d3b414150c 100644 --- a/src/drivers/ardrone_interface/ardrone_motor_control.c +++ b/src/drivers/ardrone_interface/ardrone_motor_control.c @@ -41,6 +41,7 @@ #include #include #include +#include #include #include #include @@ -369,11 +370,9 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls float yaw_control = actuators->control[2]; float motor_thrust = actuators->control[3]; - //printf("AMO: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",roll_control, pitch_control, yaw_control, motor_thrust); - const float min_thrust = 0.02f; /**< 2% minimum thrust */ const float max_thrust = 1.0f; /**< 100% max thrust */ - const float scaling = 500.0f; /**< 100% thrust equals a value of 500 which works, 512 leads to cutoff */ + const float scaling = 510.0f; /**< 100% thrust equals a value of 510 which works, 512 leads to cutoff */ const float min_gas = min_thrust * scaling; /**< value range sent to motors, minimum */ const float max_gas = max_thrust * scaling; /**< value range sent to motors, maximum */ @@ -382,71 +381,56 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls float motor_calc[4] = {0}; float output_band = 0.0f; - float band_factor = 0.75f; const float startpoint_full_control = 0.25f; /**< start full control at 25% thrust */ - float yaw_factor = 1.0f; static bool initialized = false; /* publish effective outputs */ static struct actuator_controls_effective_s actuator_controls_effective; static orb_advert_t actuator_controls_effective_pub; - if (motor_thrust <= min_thrust) { - motor_thrust = min_thrust; - output_band = 0.0f; - } else if (motor_thrust < startpoint_full_control && motor_thrust > min_thrust) { - output_band = band_factor * (motor_thrust - min_thrust); - } else if (motor_thrust >= startpoint_full_control && motor_thrust < max_thrust - band_factor * startpoint_full_control) { - output_band = band_factor * startpoint_full_control; - } else if (motor_thrust >= max_thrust - band_factor * startpoint_full_control) { - output_band = band_factor * (max_thrust - motor_thrust); + /* linearly scale the control inputs from 0 to startpoint_full_control */ + if (motor_thrust < startpoint_full_control) { + output_band = motor_thrust/startpoint_full_control; // linear from 0 to 1 + } else { + output_band = 1.0f; } + roll_control *= output_band; + pitch_control *= output_band; + yaw_control *= output_band; + + //add the yaw, nick and roll components to the basic thrust //TODO:this should be done by the mixer // FRONT (MOTOR 1) motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control); - // RIGHT (MOTOR 2) motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control); - // BACK (MOTOR 3) motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control); - // LEFT (MOTOR 4) motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control); - // if we are not in the output band - if (!(motor_calc[0] < motor_thrust + output_band && motor_calc[0] > motor_thrust - output_band - && motor_calc[1] < motor_thrust + output_band && motor_calc[1] > motor_thrust - output_band - && motor_calc[2] < motor_thrust + output_band && motor_calc[2] > motor_thrust - output_band - && motor_calc[3] < motor_thrust + output_band && motor_calc[3] > motor_thrust - output_band)) { + /* if one motor is saturated, reduce throttle */ + float saturation = fmaxf(fmaxf(motor_calc[0], motor_calc[1]),fmaxf(motor_calc[2], motor_calc[3])) - max_thrust; + + + if (saturation > 0.0f) { + + /* reduce the motor thrust according to the saturation */ + motor_thrust = motor_thrust - saturation; - yaw_factor = 0.5f; - yaw_control *= yaw_factor; // FRONT (MOTOR 1) motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control); - // RIGHT (MOTOR 2) motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control); - // BACK (MOTOR 3) motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control); - // LEFT (MOTOR 4) motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control); } - for (int i = 0; i < 4; i++) { - //check for limits - if (motor_calc[i] < motor_thrust - output_band) { - motor_calc[i] = motor_thrust - output_band; - } - if (motor_calc[i] > motor_thrust + output_band) { - motor_calc[i] = motor_thrust + output_band; - } - } /* publish effective outputs */ actuator_controls_effective.control_effective[0] = roll_control; @@ -467,25 +451,29 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls /* set the motor values */ - /* scale up from 0..1 to 10..512) */ + /* scale up from 0..1 to 10..500) */ motor_pwm[0] = (uint16_t) (motor_calc[0] * ((float)max_gas - min_gas) + min_gas); motor_pwm[1] = (uint16_t) (motor_calc[1] * ((float)max_gas - min_gas) + min_gas); motor_pwm[2] = (uint16_t) (motor_calc[2] * ((float)max_gas - min_gas) + min_gas); motor_pwm[3] = (uint16_t) (motor_calc[3] * ((float)max_gas - min_gas) + min_gas); - /* Keep motors spinning while armed and prevent overflows */ + /* scale up from 0..1 to 10..500) */ + motor_pwm[0] = (uint16_t) (motor_calc[0] * (float)((max_gas - min_gas) + min_gas)); + motor_pwm[1] = (uint16_t) (motor_calc[1] * (float)((max_gas - min_gas) + min_gas)); + motor_pwm[2] = (uint16_t) (motor_calc[2] * (float)((max_gas - min_gas) + min_gas)); + motor_pwm[3] = (uint16_t) (motor_calc[3] * (float)((max_gas - min_gas) + min_gas)); - /* Failsafe logic - should never be necessary */ - motor_pwm[0] = (motor_pwm[0] > 0) ? motor_pwm[0] : 10; - motor_pwm[1] = (motor_pwm[1] > 0) ? motor_pwm[1] : 10; - motor_pwm[2] = (motor_pwm[2] > 0) ? motor_pwm[2] : 10; - motor_pwm[3] = (motor_pwm[3] > 0) ? motor_pwm[3] : 10; + /* Failsafe logic for min values - should never be necessary */ + motor_pwm[0] = (motor_pwm[0] > 0) ? motor_pwm[0] : min_gas; + motor_pwm[1] = (motor_pwm[1] > 0) ? motor_pwm[1] : min_gas; + motor_pwm[2] = (motor_pwm[2] > 0) ? motor_pwm[2] : min_gas; + motor_pwm[3] = (motor_pwm[3] > 0) ? motor_pwm[3] : min_gas; - /* Failsafe logic - should never be necessary */ - motor_pwm[0] = (motor_pwm[0] <= 511) ? motor_pwm[0] : 511; - motor_pwm[1] = (motor_pwm[1] <= 511) ? motor_pwm[1] : 511; - motor_pwm[2] = (motor_pwm[2] <= 511) ? motor_pwm[2] : 511; - motor_pwm[3] = (motor_pwm[3] <= 511) ? motor_pwm[3] : 511; + /* Failsafe logic for max values - should never be necessary */ + motor_pwm[0] = (motor_pwm[0] <= max_gas) ? motor_pwm[0] : max_gas; + motor_pwm[1] = (motor_pwm[1] <= max_gas) ? motor_pwm[1] : max_gas; + motor_pwm[2] = (motor_pwm[2] <= max_gas) ? motor_pwm[2] : max_gas; + motor_pwm[3] = (motor_pwm[3] <= max_gas) ? motor_pwm[3] : max_gas; /* send motors via UART */ ardrone_write_motor_commands(ardrone_write, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]); From 2b9fa731efd08a01effd87a636ae8e53994944f7 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 12 Mar 2013 12:13:02 -0700 Subject: [PATCH 141/872] 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 142/872] 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 143/872] 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 144/872] 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 145/872] 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 146/872] 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 147/872] 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 148/872] 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 149/872] 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 150/872] 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 151/872] 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 152/872] 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 153/872] 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 154/872] 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 155/872] 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 156/872] 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 157/872] 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 158/872] 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 159/872] 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 160/872] 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 161/872] 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 162/872] 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 163/872] 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 164/872] 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 165/872] 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 166/872] 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 167/872] 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 168/872] 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 169/872] 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 170/872] 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 171/872] 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 172/872] 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 173/872] 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 174/872] 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 175/872] 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 176/872] 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 177/872] 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 178/872] 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 179/872] 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 180/872] 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 181/872] 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 182/872] 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 183/872] 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 184/872] 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 185/872] 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 186/872] 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 187/872] 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 188/872] 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 189/872] 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 190/872] 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 191/872] 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 192/872] 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 193/872] 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 194/872] 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 195/872] 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 196/872] 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 197/872] 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 198/872] 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 199/872] 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 200/872] 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 201/872] =?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 202/872] 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 203/872] 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 204/872] 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 205/872] 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 206/872] 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 207/872] 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 208/872] 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 209/872] 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 210/872] 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 211/872] 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 212/872] 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 213/872] 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 214/872] 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 215/872] 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 216/872] 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 217/872] 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 218/872] 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 219/872] 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 220/872] 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 221/872] 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 222/872] 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 223/872] 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 224/872] 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 225/872] 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 226/872] 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 227/872] 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 228/872] 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 229/872] 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 230/872] 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 231/872] 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 232/872] 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 233/872] 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 234/872] 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 235/872] 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 43eca3eb0e0267f1eb314fc2f5ee6d9eaef93d80 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 24 Jun 2013 12:39:33 +1000 Subject: [PATCH 236/872] 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 4b01b447d0..a0671f6adc 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 f5b91e109df755a6171264b59e92099b3ab20dbe Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 10 Jul 2013 23:50:37 -0700 Subject: [PATCH 237/872] 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 238/872] 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 239/872] 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 240/872] 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 241/872] 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 242/872] 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 243/872] 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 244/872] 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 245/872] 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 246/872] 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 247/872] =?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 248/872] 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 249/872] 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 250/872] 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 251/872] 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 252/872] 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 253/872] 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 254/872] 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 255/872] 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 256/872] 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 257/872] 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 258/872] 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 259/872] 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 260/872] 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 261/872] 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 262/872] 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 263/872] 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 264/872] 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 265/872] 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 266/872] 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 267/872] 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 268/872] 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 269/872] 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 270/872] 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 271/872] 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 272/872] 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 273/872] 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 274/872] 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 275/872] 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 276/872] 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 277/872] 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 278/872] 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 279/872] 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 280/872] 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 281/872] 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 282/872] 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 283/872] 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 284/872] 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 285/872] 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 286/872] 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 287/872] 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 288/872] 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 289/872] 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 290/872] 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 291/872] 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 292/872] 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 293/872] 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 294/872] 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 295/872] 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 296/872] 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 297/872] 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 298/872] 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 299/872] 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 300/872] 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 301/872] 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 302/872] 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 303/872] 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 304/872] 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 305/872] 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 306/872] 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 307/872] 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 308/872] 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 309/872] 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 310/872] 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 311/872] 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 312/872] 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 313/872] 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 314/872] 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 315/872] 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 316/872] 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 317/872] 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 318/872] 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 319/872] 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 320/872] 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 321/872] 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 322/872] 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 323/872] 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 324/872] 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 325/872] 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 326/872] 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 327/872] 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 328/872] 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 329/872] 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 330/872] 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 331/872] 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 332/872] 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 333/872] 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 334/872] 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 335/872] 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 336/872] 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 337/872] 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 338/872] 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 339/872] 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 340/872] 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 341/872] 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 342/872] 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 343/872] 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 344/872] 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 345/872] 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 346/872] 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 347/872] 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 dccdc977d5be4f957bcaea036b66d0391b29fd2b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 9 Aug 2013 17:56:32 +0200 Subject: [PATCH 348/872] Made accel / gyro self tests aware of offsets and scales, added support to config command to call these --- src/drivers/l3gd20/l3gd20.cpp | 35 +++++++++- src/drivers/mpu6000/mpu6000.cpp | 69 ++++++++++++++++++- .../commander/accelerometer_calibration.c | 14 +++- src/systemcmds/config/config.c | 63 ++++++++++++++++- 4 files changed, 174 insertions(+), 7 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 1ffca2f43a..744abfa006 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -260,6 +260,13 @@ private: * @return OK if the value can be supported. */ int set_samplerate(unsigned frequency); + + /** + * Self test + * + * @return 0 on success, 1 on failure + */ + int self_test(); }; /* helper macro for handling report buffer indices */ @@ -519,6 +526,9 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) case GYROIOCGRANGE: return _current_range; + case GYROIOCSELFTEST: + return self_test(); + default: /* give it to the superclass */ return SPI::ioctl(filp, cmd, arg); @@ -713,7 +723,8 @@ L3GD20::measure() poll_notify(POLLIN); /* publish for subscribers */ - orb_publish(ORB_ID(sensor_gyro), _gyro_topic, report); + if (_gyro_topic > 0) + orb_publish(ORB_ID(sensor_gyro), _gyro_topic, report); /* stop the perf counter */ perf_end(_sample_perf); @@ -727,6 +738,28 @@ L3GD20::print_info() _num_reports, _oldest_report, _next_report, _reports); } +int +L3GD20::self_test() +{ + /* evaluate gyro offsets, complain if offset -> zero or larger than 6 dps */ + if (fabsf(_gyro_scale.x_offset) > 0.1f || fabsf(_gyro_scale.x_offset) < 0.000001f) + return 1; + if (fabsf(_gyro_scale.x_scale - 1.0f) > 0.3f) + return 1; + + if (fabsf(_gyro_scale.y_offset) > 0.1f || fabsf(_gyro_scale.y_offset) < 0.000001f) + return 1; + if (fabsf(_gyro_scale.y_scale - 1.0f) > 0.3f) + return 1; + + if (fabsf(_gyro_scale.z_offset) > 0.1f || fabsf(_gyro_scale.z_offset) < 0.000001f) + return 1; + if (fabsf(_gyro_scale.z_scale - 1.0f) > 0.3f) + return 1; + + return 0; +} + /** * Local functions in support of the shell command. */ diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index c4e331a30e..ce8fe9feec 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -285,12 +285,26 @@ private: uint16_t swap16(uint16_t val) { return (val >> 8) | (val << 8); } /** - * Self test + * Measurement self test * * @return 0 on success, 1 on failure */ int self_test(); + /** + * Accel self test + * + * @return 0 on success, 1 on failure + */ + int accel_self_test(); + + /** + * Gyro self test + * + * @return 0 on success, 1 on failure + */ + int gyro_self_test(); + /* set low pass filter frequency */ @@ -321,6 +335,7 @@ protected: void parent_poll_notify(); private: MPU6000 *_parent; + }; /** driver 'main' command */ @@ -653,6 +668,54 @@ MPU6000::self_test() return (_reads > 0) ? 0 : 1; } +int +MPU6000::accel_self_test() +{ + if (self_test()) + 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; +} + +int +MPU6000::gyro_self_test() +{ + if (self_test()) + return 1; + + /* evaluate gyro offsets, complain if offset -> zero or larger than 6 dps */ + if (fabsf(_gyro_scale.x_offset) > 0.1f || fabsf(_gyro_scale.x_offset) < 0.000001f) + return 1; + if (fabsf(_gyro_scale.x_scale - 1.0f) > 0.3f) + return 1; + + if (fabsf(_gyro_scale.y_offset) > 0.1f || fabsf(_gyro_scale.y_offset) < 0.000001f) + return 1; + if (fabsf(_gyro_scale.y_scale - 1.0f) > 0.3f) + return 1; + + if (fabsf(_gyro_scale.z_offset) > 0.1f || fabsf(_gyro_scale.z_offset) < 0.000001f) + return 1; + if (fabsf(_gyro_scale.z_scale - 1.0f) > 0.3f) + return 1; + + return 0; +} + ssize_t MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) { @@ -835,7 +898,7 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) return _accel_range_m_s2; case ACCELIOCSELFTEST: - return self_test(); + return accel_self_test(); default: /* give it to the superclass */ @@ -918,7 +981,7 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) return _gyro_range_rad_s; case GYROIOCSELFTEST: - return self_test(); + return gyro_self_test(); default: /* give it to the superclass */ diff --git a/src/modules/commander/accelerometer_calibration.c b/src/modules/commander/accelerometer_calibration.c index 6a304896a3..fbb73d997d 100644 --- a/src/modules/commander/accelerometer_calibration.c +++ b/src/modules/commander/accelerometer_calibration.c @@ -226,6 +226,12 @@ int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float if (orient < 0) return ERROR; + if (data_collected[orient]) { + sprintf(str, "%s direction already measured, please rotate", orientation_strs[orient]); + mavlink_log_info(mavlink_fd, str); + continue; + } + 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); @@ -380,6 +386,8 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp int count = 0; float accel_sum[3] = { 0.0f, 0.0f, 0.0f }; + int errcount = 0; + while (count < samples_num) { int poll_ret = poll(fds, 1, 1000); if (poll_ret == 1) { @@ -389,8 +397,12 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp accel_sum[i] += sensor.accelerometer_m_s2[i]; count++; } else { - return ERROR; + errcount++; + continue; } + + if (errcount > samples_num / 10) + return ERROR; } for (int i = 0; i < 3; i++) { diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index 2dad2261b5..42814f2b2f 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -129,7 +129,19 @@ do_gyro(int argc, char *argv[]) ioctl(fd, GYROIOCSRANGE, i); } - } else if (!(argc > 0 && !strcmp(argv[0], "info"))) { + } else if (argc > 0) { + + if(!strcmp(argv[0], "check")) { + int ret = ioctl(fd, GYROIOCSELFTEST, 0); + + if (ret) { + warnx("gyro self test FAILED! Check calibration."); + } 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"); } @@ -148,6 +160,41 @@ do_gyro(int argc, char *argv[]) static void do_mag(int argc, char *argv[]) { + int fd; + + fd = open(MAG_DEVICE_PATH, 0); + + if (fd < 0) { + warn("%s", MAG_DEVICE_PATH); + errx(1, "FATAL: no magnetometer found"); + + } else { + + if (argc > 0) { + + if (!strcmp(argv[0], "check")) { + int ret = ioctl(fd, MAGIOCSELFTEST, 0); + + if (ret) { + warnx("mag self test FAILED! Check calibration."); + } else { + warnx("mag calibration and self test OK"); + } + } + + } else { + warnx("no arguments given. Try: \n\n\t'check' or 'info'\n\t"); + } + + int srate = -1;//ioctl(fd, MAGIOCGSAMPLERATE, 0); + int prate = ioctl(fd, SENSORIOCGPOLLRATE, 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); + + close(fd); + } + exit(0); } @@ -183,7 +230,19 @@ do_accel(int argc, char *argv[]) /* set the range to i dps */ ioctl(fd, ACCELIOCSRANGE, i); } - } else if (!(argc > 0 && !strcmp(argv[0], "info"))) { + } else if (argc > 0) { + + if (!strcmp(argv[0], "check")) { + int ret = ioctl(fd, ACCELIOCSELFTEST, 0); + + if (ret) { + warnx("accel self test FAILED! Check calibration."); + } 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"); } From 91e54aa5be9930441c85c0585494f8ac8f514681 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 10 Aug 2013 15:51:00 +1000 Subject: [PATCH 349/872] 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 350/872] 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 351/872] 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 352/872] 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 353/872] 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 354/872] 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 355/872] 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 356/872] 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 357/872] 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 358/872] 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 359/872] 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 360/872] 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 361/872] 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 362/872] 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 363/872] 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 364/872] 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 365/872] 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 366/872] 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 367/872] 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 368/872] 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 369/872] 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 370/872] 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 371/872] 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 372/872] 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 373/872] 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 374/872] 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 375/872] 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 376/872] 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 377/872] 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 378/872] 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 379/872] 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 380/872] 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 381/872] 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 382/872] 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 383/872] 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 384/872] 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 385/872] 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 386/872] 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 387/872] 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 388/872] 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 389/872] 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 390/872] 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 391/872] 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 392/872] 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 393/872] 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 394/872] 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 395/872] 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 396/872] 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 397/872] 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 398/872] 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 399/872] 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 400/872] 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 401/872] 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 402/872] 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 403/872] 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 404/872] 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 405/872] 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 406/872] 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 407/872] 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 408/872] 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 409/872] 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 410/872] 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 411/872] 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 412/872] 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 413/872] 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 414/872] 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 415/872] 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 416/872] 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 417/872] 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 418/872] 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 419/872] 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

hr-lLfRyXjo4PfTy8vTOrY;0?Xc<;%aGDeLW>!%nQ@c&MD3ptO z+7DYkEY)FTnOeMRm%bs4v$KD{7HaY~c8-m=?lyybWu-#oM5)R_)B8B<i zg#%4!(1E^e2P%h{Om*Angn!60;ZF)_y_?n-(Yn;ijIghi1S$C(!$_)~)Y!i;^gd_L zc}K?AdY<)oQwX`pe;?i{8+(Un3I(g5MK|4=%-sh&d+7gUY8Wgj4P=Pw$qz39X}w|n z`t1+&z;4Og|JTa&!@Jpe;Q>n_a;>C^^wudWvx{8=BeF?;BrVAPinIUY%JfSkvj2po z5IrO9>3cXf3(O?<(ji^0J?j7LvMZ=V+l3>-@yU0WRy&OLd&$CrUhHyQZ}>p*Uz z&_PB)fjIVoy=ZORDY<5N=B%RJwAfC!Vh*|W5XeQ`PU`n?1DE^XF&RC5j-K8+tFM2< zO0`1fO)+mQht`vacuDOznEMIzyNd5Gxd?b<;6j?#1ATKkRtsjP(^tQs2e!_YEa=xJ zt=Sz1yH*zW$MZ+C24}6l8LMp?+x!ziZSTYThqoTN-+zP~@D;2QbRiVp)T@BFgaC2H=!9ebPc|wU)6|pxIj(A+Ya&Hl zAa9CU_VnOP?sv1ibmu^c{9^kP>E((yW`#PX^*HSVJ$&g;} zvbWMw#5TwKFE0$5ypb%R!7rg-g z8d~O;J%KjcO=kX2aIM=*?0sUKNd7m())IT2SUs^ZVtm7ze~H+w#GWI@^OJv^SU<5x zi0vZw0I`RN-9zlh#QKT7KrBpboLG?9Tf}ZA_7O4OWZisv=C1>GOG|4*Yg@~X4Vq|f zZPCOgg2t9+fK816t-zW$YvQbGv3gBSjaai5U~L`1`gH)S*OPf=9l5nvNzUq8vdsRD z-e}0&pe>7QtAgeos}hNUmKyDr)l$5pTExvn0xvkVc+@n5(5e=jR&;1)e^26D+NKr# zT5lw1_G+P?uo>6liH<}cG~#B+3?#&zv7Usfb#?TF%%G?aM-tsMl!$0Cvm>}ti^d|I z9i5>8EgFd>v|vvh@5sXFX9ibR%Qr|DV)87-EmlpwHRjT=@{KPGaW3&9^z_Nzuj}?*?SnWQCshOrx@4ieyX^5PyKeXFnyM4%RAG6zM>~_>{ z_uB0tyZx2jzH7H1+3iug{nT#7#f1<{7i(>L)Q-;vn{HpIX7FvB9zRFXjW*3c_gFUD zw75ahH#zhpif(gg-dZTPIP?K0fQs)zHv(=1+z7Z4a3kPGz>R<#0XG6}1l$O?5pW~$ z{}h46OSSgJ4MS&CZBt$^t~dBed`IARBhY=jv9qru78KiJzO3R0%~&idvi1B@R(?5e zbmC)jeCa;)fyzf9ho7aTm?kYjJrWaH5nEnnm^mBrYrqeLB5{*M9SG)qh}Q$}isH-n zL>EaNWKSfJke!-9^qRea=m16A6@@essdiA2s2L{S4IH20+b%>wF)u_(KX8>px^#6- zZ4FsN9`X{gKrbdJV#6d8fv!+ryqk1Z(K}6Kpzu98IFBk0#&tN4lOuCdklV#$;2Y%% zqqiF~GXs4I6Tc1UV8pJGlVhxr(Gu78cJzeB^@&(VINoK(jLw0CDYh^dwLoOAAXq_g zm18!0#n6KBh4tdt9~Ln|dNrz(pjZMP)t1{lrkH5x%%V?>LCR#76r7EoO?XX71N^4k z>+w-8>B}XpzGtSKZ_2ztoZne&bmVsbEubN$sw{1Q#q`cH`mQoMRYpHhMl;n}Z0|>9 z^pj=ub7l04W%Qmhdc2J0n}y=>-Y%oxE2F2%=ugV%V`a2Q83$B_i^&CJI;$F$-@S7+ z&qC$92PToZI!u!@#dR(!bB|m%zH{WOk@Hbe>X`_yLj7OZ)bNW1BiIqcZ%2$S{CXtL z<7S*>TN`pC8zr0DDBaeuqz+}tRU>bN5{|ljBum{p_D!Mgh{Ek)-wV!-z*agZZvp50 etCL@H=AFY{IORZ`;|FtLXv&} literal 0 HcmV?d00001 diff --git a/Tools/com.c b/Tools/com.c new file mode 100644 index 0000000000..fa52dcb61e --- /dev/null +++ b/Tools/com.c @@ -0,0 +1,191 @@ +/* + * Building: cc -o com com.c + * Usage : ./com /dev/device [speed] + * Example : ./com /dev/ttyS0 [115200] + * Keys : Ctrl-A - exit, Ctrl-X - display control lines status + * Darcs : darcs get http://tinyserial.sf.net/ + * Homepage: http://tinyserial.sourceforge.net + * Version : 2009-03-05 + * + * Ivan Tikhonov, http://www.brokestream.com, kefeer@brokestream.com + * Patches by Jim Kou, Henry Nestler, Jon Miner, Alan Horstmann + * + */ + + +/* Copyright (C) 2007 Ivan Tikhonov + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + + Ivan Tikhonov, kefeer@brokestream.com + +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +int transfer_byte(int from, int to, int is_control); + +typedef struct {char *name; int flag; } speed_spec; + + +void print_status(int fd) { + int status; + unsigned int arg; + status = ioctl(fd, TIOCMGET, &arg); + fprintf(stderr, "[STATUS]: "); + if(arg & TIOCM_RTS) fprintf(stderr, "RTS "); + if(arg & TIOCM_CTS) fprintf(stderr, "CTS "); + if(arg & TIOCM_DSR) fprintf(stderr, "DSR "); + if(arg & TIOCM_CAR) fprintf(stderr, "DCD "); + if(arg & TIOCM_DTR) fprintf(stderr, "DTR "); + if(arg & TIOCM_RNG) fprintf(stderr, "RI "); + fprintf(stderr, "\r\n"); +} + +int main(int argc, char *argv[]) +{ + int comfd; + struct termios oldtio, newtio; //place for old and new port settings for serial port + struct termios oldkey, newkey; //place tor old and new port settings for keyboard teletype + char *devicename = argv[1]; + int need_exit = 0; + speed_spec speeds[] = + { + {"1200", B1200}, + {"2400", B2400}, + {"4800", B4800}, + {"9600", B9600}, + {"19200", B19200}, + {"38400", B38400}, + {"57600", B57600}, + {"115200", B115200}, + {NULL, 0} + }; + int speed = B9600; + + if(argc < 2) { + fprintf(stderr, "example: %s /dev/ttyS0 [115200]\n", argv[0]); + exit(1); + } + + comfd = open(devicename, O_RDWR | O_NOCTTY | O_NONBLOCK); + if (comfd < 0) + { + perror(devicename); + exit(-1); + } + + if(argc > 2) { + speed_spec *s; + for(s = speeds; s->name; s++) { + if(strcmp(s->name, argv[2]) == 0) { + speed = s->flag; + fprintf(stderr, "setting speed %s\n", s->name); + break; + } + } + } + + fprintf(stderr, "C-a exit, C-x modem lines status\n"); + + tcgetattr(STDIN_FILENO,&oldkey); + newkey.c_cflag = B9600 | CRTSCTS | CS8 | CLOCAL | CREAD; + newkey.c_iflag = IGNPAR; + newkey.c_oflag = 0; + newkey.c_lflag = 0; + newkey.c_cc[VMIN]=1; + newkey.c_cc[VTIME]=0; + tcflush(STDIN_FILENO, TCIFLUSH); + tcsetattr(STDIN_FILENO,TCSANOW,&newkey); + + + tcgetattr(comfd,&oldtio); // save current port settings + newtio.c_cflag = speed | CS8 | CLOCAL | CREAD; + newtio.c_iflag = IGNPAR; + newtio.c_oflag = 0; + newtio.c_lflag = 0; + newtio.c_cc[VMIN]=1; + newtio.c_cc[VTIME]=0; + tcflush(comfd, TCIFLUSH); + tcsetattr(comfd,TCSANOW,&newtio); + + print_status(comfd); + + while(!need_exit) { + fd_set fds; + int ret; + + FD_ZERO(&fds); + FD_SET(STDIN_FILENO, &fds); + FD_SET(comfd, &fds); + + + ret = select(comfd+1, &fds, NULL, NULL, NULL); + if(ret == -1) { + perror("select"); + } else if (ret > 0) { + if(FD_ISSET(STDIN_FILENO, &fds)) { + need_exit = transfer_byte(STDIN_FILENO, comfd, 1); + } + if(FD_ISSET(comfd, &fds)) { + need_exit = transfer_byte(comfd, STDIN_FILENO, 0); + } + } + } + + tcsetattr(comfd,TCSANOW,&oldtio); + tcsetattr(STDIN_FILENO,TCSANOW,&oldkey); + close(comfd); + + return 0; +} + + +int transfer_byte(int from, int to, int is_control) { + char c; + int ret; + do { + ret = read(from, &c, 1); + } while (ret < 0 && errno == EINTR); + if(ret == 1) { + if(is_control) { + if(c == '\x01') { // C-a + return -1; + } else if(c == '\x18') { // C-x + print_status(to); + return 0; + } + } + while(write(to, &c, 1) == -1) { + if(errno!=EAGAIN && errno!=EINTR) { perror("write failed"); break; } + } + } else { + fprintf(stderr, "\nnothing to read. probably port disconnected.\n"); + return -2; + } + return 0; +} + diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index cab3c2fd0e..52d089360a 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -436,7 +436,7 @@ while True: print("attempting reboot on %s..." % port) up.send_reboot() # wait for the reboot, without we might run into Serial I/O Error 5 - time.sleep(1.5) + time.sleep(0.5) continue try: From 30499caecf93496bbe307e5af6b31fea4d3d8cb5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Sep 2013 16:16:22 +0200 Subject: [PATCH 606/872] Make sure to loiter at final waypoint on a best effort basis --- src/modules/mavlink/waypoints.c | 45 ++++++++++++++++++++++++++++++++- 1 file changed, 44 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index c6c2eac68b..84fe0082ba 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -348,6 +348,11 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ { static uint16_t counter; + if (!global_pos->valid && !local_pos->xy_valid) { + /* nothing to check here, return */ + return; + } + if (wpm->current_active_wp_id < wpm->size) { float orbit; @@ -421,15 +426,53 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ if (cur_wp->autocontinue) { cur_wp->current = 0; + float navigation_lat = -1.0f; + float navigation_lon = -1.0f; + float navigation_alt = -1.0f; + int navigation_frame = -1; + + /* initialize to current position in case we don't find a suitable navigation waypoint */ + if (global_pos->valid) { + navigation_lat = global_pos->lat/1e7; + navigation_lon = global_pos->lon/1e7; + navigation_alt = global_pos->alt; + navigation_frame = MAV_FRAME_GLOBAL; + } else if (local_pos->xy_valid && local_pos->z_valid) { + navigation_lat = local_pos->x; + navigation_lon = local_pos->y; + navigation_alt = local_pos->z; + navigation_frame = MAV_FRAME_LOCAL_NED; + } + /* only accept supported navigation waypoints, skip unknown ones */ do { + /* pick up the last valid navigation waypoint, this will be one we hold on to after the mission */ + if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT || + wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS || + wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME || + wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM) { + + /* this is a navigation waypoint */ + navigation_frame = cur_wp->frame; + navigation_lat = cur_wp->x; + navigation_lon = cur_wp->y; + navigation_alt = cur_wp->z; + } if (wpm->current_active_wp_id == wpm->size - 1 && wpm->size > 1) { /* the last waypoint was reached, if auto continue is * activated keep the system loitering there. */ cur_wp->command = MAV_CMD_NAV_LOITER_UNLIM; - cur_wp->param3 = 15.0f; // XXX magic number 15 m loiter radius + cur_wp->param3 = 20.0f; // XXX magic number 20 m loiter radius + cur_wp->frame = navigation_frame; + cur_wp->x = navigation_lat; + cur_wp->y = navigation_lon; + cur_wp->z = navigation_alt; + cur_wp->autocontinue = false; + + /* we risk an endless loop for missions without navigation waypoints, abort. */ + break; } else { if ((uint16_t)(wpm->current_active_wp_id + 1) < wpm->size) From c130c9a1048a21511b1a38c4ffff0648d82fb038 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Sep 2013 16:19:29 +0200 Subject: [PATCH 607/872] Style / code cleanup --- src/modules/mavlink/waypoints.c | 191 +------------------------------- 1 file changed, 2 insertions(+), 189 deletions(-) diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index 84fe0082ba..97472c233c 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -246,47 +246,6 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq) // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); } -//float mavlink_wpm_distance_to_segment(uint16_t seq, float x, float y, float z) -//{ -// if (seq < wpm->size) -// { -// mavlink_mission_item_t *cur = waypoints->at(seq); -// -// const PxVector3 A(cur->x, cur->y, cur->z); -// const PxVector3 C(x, y, z); -// -// // seq not the second last waypoint -// if ((uint16_t)(seq+1) < wpm->size) -// { -// mavlink_mission_item_t *next = waypoints->at(seq+1); -// const PxVector3 B(next->x, next->y, next->z); -// const float r = (B-A).dot(C-A) / (B-A).lengthSquared(); -// if (r >= 0 && r <= 1) -// { -// const PxVector3 P(A + r*(B-A)); -// return (P-C).length(); -// } -// else if (r < 0.f) -// { -// return (C-A).length(); -// } -// else -// { -// return (C-B).length(); -// } -// } -// else -// { -// return (C-A).length(); -// } -// } -// else -// { -// // if (verbose) // printf("ERROR: index out of bounds\n"); -// } -// return -1.f; -//} - /* * Calculate distance in global frame. * @@ -337,7 +296,7 @@ float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float float dy = (cur->y - y); float dz = (cur->z - z); - return sqrt(dx * dx + dy * dy + dz * dz); + return sqrtf(dx * dx + dy * dy + dz * dz); } else { return -1.0f; @@ -527,11 +486,6 @@ int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_positio } } - // Do NOT continously send the current WP, since we're not loosing messages - // if (now - wpm->timestamp_last_send_setpoint > wpm->delay_setpoint && wpm->current_active_wp_id < wpm->size) { - // mavlink_wpm_send_setpoint(wpm->current_active_wp_id); - // } - check_waypoints_reached(now, global_position, local_position); return OK; @@ -543,115 +497,6 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi uint64_t now = mavlink_missionlib_get_system_timestamp(); switch (msg->msgid) { -// case MAVLINK_MSG_ID_ATTITUDE: -// { -// if(msg->sysid == mavlink_system.sysid && wpm->current_active_wp_id < wpm->size) -// { -// mavlink_mission_item_t *wp = &(wpm->waypoints[wpm->current_active_wp_id]); -// if(wp->frame == MAV_FRAME_LOCAL_ENU || wp->frame == MAV_FRAME_LOCAL_NED) -// { -// mavlink_attitude_t att; -// mavlink_msg_attitude_decode(msg, &att); -// float yaw_tolerance = wpm->accept_range_yaw; -// //compare current yaw -// if (att.yaw - yaw_tolerance >= 0.0f && att.yaw + yaw_tolerance < 2.f*FM_PI) -// { -// if (att.yaw - yaw_tolerance <= wp->param4 && att.yaw + yaw_tolerance >= wp->param4) -// wpm->yaw_reached = true; -// } -// else if(att.yaw - yaw_tolerance < 0.0f) -// { -// float lowerBound = 360.0f + att.yaw - yaw_tolerance; -// if (lowerBound < wp->param4 || wp->param4 < att.yaw + yaw_tolerance) -// wpm->yaw_reached = true; -// } -// else -// { -// float upperBound = att.yaw + yaw_tolerance - 2.f*FM_PI; -// if (att.yaw - yaw_tolerance < wp->param4 || wp->param4 < upperBound) -// wpm->yaw_reached = true; -// } -// } -// } -// break; -// } -// -// case MAVLINK_MSG_ID_LOCAL_POSITION_NED: -// { -// if(msg->sysid == mavlink_system.sysid && wpm->current_active_wp_id < wpm->size) -// { -// mavlink_mission_item_t *wp = &(wpm->waypoints[wpm->current_active_wp_id]); -// -// if(wp->frame == MAV_FRAME_LOCAL_ENU || MAV_FRAME_LOCAL_NED) -// { -// mavlink_local_position_ned_t pos; -// mavlink_msg_local_position_ned_decode(msg, &pos); -// //// if (debug) // printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z); -// -// wpm->pos_reached = false; -// -// // compare current position (given in message) with current waypoint -// float orbit = wp->param1; -// -// float dist; -//// if (wp->param2 == 0) -//// { -//// // FIXME segment distance -//// //dist = mavlink_wpm_distance_to_segment(current_active_wp_id, pos.x, pos.y, pos.z); -//// } -//// else -//// { -// dist = mavlink_wpm_distance_to_point(wpm->current_active_wp_id, pos.x, pos.y, pos.z); -//// } -// -// if (dist >= 0.f && dist <= orbit && wpm->yaw_reached) -// { -// wpm->pos_reached = true; -// } -// } -// } -// break; -// } - -// case MAVLINK_MSG_ID_CMD: // special action from ground station -// { -// mavlink_cmd_t action; -// mavlink_msg_cmd_decode(msg, &action); -// if(action.target == mavlink_system.sysid) -// { -// // if (verbose) std::cerr << "Waypoint: received message with action " << action.action << std::endl; -// switch (action.action) -// { -// // case MAV_ACTION_LAUNCH: -// // // if (verbose) std::cerr << "Launch received" << std::endl; -// // current_active_wp_id = 0; -// // if (wpm->size>0) -// // { -// // setActive(waypoints[current_active_wp_id]); -// // } -// // else -// // // if (verbose) std::cerr << "No launch, waypointList empty" << std::endl; -// // break; -// -// // case MAV_ACTION_CONTINUE: -// // // if (verbose) std::c -// // err << "Continue received" << std::endl; -// // idle = false; -// // setActive(waypoints[current_active_wp_id]); -// // break; -// -// // case MAV_ACTION_HALT: -// // // if (verbose) std::cerr << "Halt received" << std::endl; -// // idle = true; -// // break; -// -// // default: -// // // if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl; -// // break; -// } -// } -// break; -// } case MAVLINK_MSG_ID_MISSION_ACK: { mavlink_mission_ack_t wpa; @@ -662,26 +507,16 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { if (wpm->current_wp_id == wpm->size - 1) { -#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("Got last WP ACK state -> IDLE"); -#else - if (MAVLINK_WPM_VERBOSE) printf("Received ACK after having sent last waypoint, going to state MAVLINK_WPM_STATE_IDLE\n"); - -#endif wpm->current_state = MAVLINK_WPM_STATE_IDLE; wpm->current_wp_id = 0; } } } else { -#ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); - -#endif } break; @@ -709,13 +544,8 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi } } -#ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("NEW WP SET"); -#else - if (MAVLINK_WPM_VERBOSE) printf("New current waypoint %u\n", wpm->current_active_wp_id); - -#endif wpm->yaw_reached = false; wpm->pos_reached = false; mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id); @@ -723,33 +553,16 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi wpm->timestamp_firstinside_orbit = 0; } else { -#ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT: Index out of bounds\n"); - -#endif } } else { -#ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy"); -#else - if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE NOT IN IDLE STATE\n"); - -#endif } } else { -#ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); - -#endif } break; From 21f4ce0ebd0439552b718447ccc250faa221f765 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Sep 2013 16:19:29 +0200 Subject: [PATCH 608/872] Style / code cleanup --- src/lib/ecl/l1/ecl_l1_pos_control.cpp | 12 +- src/modules/mavlink/waypoints.c | 191 +------------------------- 2 files changed, 8 insertions(+), 195 deletions(-) diff --git a/src/lib/ecl/l1/ecl_l1_pos_control.cpp b/src/lib/ecl/l1/ecl_l1_pos_control.cpp index 533409217e..6f50659601 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_control.cpp +++ b/src/lib/ecl/l1/ecl_l1_pos_control.cpp @@ -6,9 +6,9 @@ * modification, are permitted provided that the following conditions * are met: * - * 1. Redistributions of source code must retain the vector_ABove copyright + * 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 vector_ABove copyright + * 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. @@ -18,14 +18,14 @@ * * 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 MERCHANTvector_ABILITY AND FITNESS + * 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 LIvector_ABLE FOR ANY DIRECT, INDIRECT, + * 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 LIvector_ABILITY, WHETHER IN CONTRACT, STRICT - * LIvector_ABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * 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/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index 84fe0082ba..97472c233c 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -246,47 +246,6 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq) // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); } -//float mavlink_wpm_distance_to_segment(uint16_t seq, float x, float y, float z) -//{ -// if (seq < wpm->size) -// { -// mavlink_mission_item_t *cur = waypoints->at(seq); -// -// const PxVector3 A(cur->x, cur->y, cur->z); -// const PxVector3 C(x, y, z); -// -// // seq not the second last waypoint -// if ((uint16_t)(seq+1) < wpm->size) -// { -// mavlink_mission_item_t *next = waypoints->at(seq+1); -// const PxVector3 B(next->x, next->y, next->z); -// const float r = (B-A).dot(C-A) / (B-A).lengthSquared(); -// if (r >= 0 && r <= 1) -// { -// const PxVector3 P(A + r*(B-A)); -// return (P-C).length(); -// } -// else if (r < 0.f) -// { -// return (C-A).length(); -// } -// else -// { -// return (C-B).length(); -// } -// } -// else -// { -// return (C-A).length(); -// } -// } -// else -// { -// // if (verbose) // printf("ERROR: index out of bounds\n"); -// } -// return -1.f; -//} - /* * Calculate distance in global frame. * @@ -337,7 +296,7 @@ float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float float dy = (cur->y - y); float dz = (cur->z - z); - return sqrt(dx * dx + dy * dy + dz * dz); + return sqrtf(dx * dx + dy * dy + dz * dz); } else { return -1.0f; @@ -527,11 +486,6 @@ int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_positio } } - // Do NOT continously send the current WP, since we're not loosing messages - // if (now - wpm->timestamp_last_send_setpoint > wpm->delay_setpoint && wpm->current_active_wp_id < wpm->size) { - // mavlink_wpm_send_setpoint(wpm->current_active_wp_id); - // } - check_waypoints_reached(now, global_position, local_position); return OK; @@ -543,115 +497,6 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi uint64_t now = mavlink_missionlib_get_system_timestamp(); switch (msg->msgid) { -// case MAVLINK_MSG_ID_ATTITUDE: -// { -// if(msg->sysid == mavlink_system.sysid && wpm->current_active_wp_id < wpm->size) -// { -// mavlink_mission_item_t *wp = &(wpm->waypoints[wpm->current_active_wp_id]); -// if(wp->frame == MAV_FRAME_LOCAL_ENU || wp->frame == MAV_FRAME_LOCAL_NED) -// { -// mavlink_attitude_t att; -// mavlink_msg_attitude_decode(msg, &att); -// float yaw_tolerance = wpm->accept_range_yaw; -// //compare current yaw -// if (att.yaw - yaw_tolerance >= 0.0f && att.yaw + yaw_tolerance < 2.f*FM_PI) -// { -// if (att.yaw - yaw_tolerance <= wp->param4 && att.yaw + yaw_tolerance >= wp->param4) -// wpm->yaw_reached = true; -// } -// else if(att.yaw - yaw_tolerance < 0.0f) -// { -// float lowerBound = 360.0f + att.yaw - yaw_tolerance; -// if (lowerBound < wp->param4 || wp->param4 < att.yaw + yaw_tolerance) -// wpm->yaw_reached = true; -// } -// else -// { -// float upperBound = att.yaw + yaw_tolerance - 2.f*FM_PI; -// if (att.yaw - yaw_tolerance < wp->param4 || wp->param4 < upperBound) -// wpm->yaw_reached = true; -// } -// } -// } -// break; -// } -// -// case MAVLINK_MSG_ID_LOCAL_POSITION_NED: -// { -// if(msg->sysid == mavlink_system.sysid && wpm->current_active_wp_id < wpm->size) -// { -// mavlink_mission_item_t *wp = &(wpm->waypoints[wpm->current_active_wp_id]); -// -// if(wp->frame == MAV_FRAME_LOCAL_ENU || MAV_FRAME_LOCAL_NED) -// { -// mavlink_local_position_ned_t pos; -// mavlink_msg_local_position_ned_decode(msg, &pos); -// //// if (debug) // printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z); -// -// wpm->pos_reached = false; -// -// // compare current position (given in message) with current waypoint -// float orbit = wp->param1; -// -// float dist; -//// if (wp->param2 == 0) -//// { -//// // FIXME segment distance -//// //dist = mavlink_wpm_distance_to_segment(current_active_wp_id, pos.x, pos.y, pos.z); -//// } -//// else -//// { -// dist = mavlink_wpm_distance_to_point(wpm->current_active_wp_id, pos.x, pos.y, pos.z); -//// } -// -// if (dist >= 0.f && dist <= orbit && wpm->yaw_reached) -// { -// wpm->pos_reached = true; -// } -// } -// } -// break; -// } - -// case MAVLINK_MSG_ID_CMD: // special action from ground station -// { -// mavlink_cmd_t action; -// mavlink_msg_cmd_decode(msg, &action); -// if(action.target == mavlink_system.sysid) -// { -// // if (verbose) std::cerr << "Waypoint: received message with action " << action.action << std::endl; -// switch (action.action) -// { -// // case MAV_ACTION_LAUNCH: -// // // if (verbose) std::cerr << "Launch received" << std::endl; -// // current_active_wp_id = 0; -// // if (wpm->size>0) -// // { -// // setActive(waypoints[current_active_wp_id]); -// // } -// // else -// // // if (verbose) std::cerr << "No launch, waypointList empty" << std::endl; -// // break; -// -// // case MAV_ACTION_CONTINUE: -// // // if (verbose) std::c -// // err << "Continue received" << std::endl; -// // idle = false; -// // setActive(waypoints[current_active_wp_id]); -// // break; -// -// // case MAV_ACTION_HALT: -// // // if (verbose) std::cerr << "Halt received" << std::endl; -// // idle = true; -// // break; -// -// // default: -// // // if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl; -// // break; -// } -// } -// break; -// } case MAVLINK_MSG_ID_MISSION_ACK: { mavlink_mission_ack_t wpa; @@ -662,26 +507,16 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { if (wpm->current_wp_id == wpm->size - 1) { -#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("Got last WP ACK state -> IDLE"); -#else - if (MAVLINK_WPM_VERBOSE) printf("Received ACK after having sent last waypoint, going to state MAVLINK_WPM_STATE_IDLE\n"); - -#endif wpm->current_state = MAVLINK_WPM_STATE_IDLE; wpm->current_wp_id = 0; } } } else { -#ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); - -#endif } break; @@ -709,13 +544,8 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi } } -#ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("NEW WP SET"); -#else - if (MAVLINK_WPM_VERBOSE) printf("New current waypoint %u\n", wpm->current_active_wp_id); - -#endif wpm->yaw_reached = false; wpm->pos_reached = false; mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id); @@ -723,33 +553,16 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi wpm->timestamp_firstinside_orbit = 0; } else { -#ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT: Index out of bounds\n"); - -#endif } } else { -#ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy"); -#else - if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE NOT IN IDLE STATE\n"); - -#endif } } else { -#ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); - -#endif } break; From 4e0c5f948902cbcd38eb71967e936b2526b8da72 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Sep 2013 20:26:50 +0200 Subject: [PATCH 609/872] Compile fixes, cleanups, better references --- src/lib/ecl/l1/ecl_l1_pos_control.cpp | 12 +++--- src/lib/ecl/l1/ecl_l1_pos_control.h | 42 ++++++++++++------- .../fw_pos_control_l1_main.cpp | 8 ++-- 3 files changed, 39 insertions(+), 23 deletions(-) diff --git a/src/lib/ecl/l1/ecl_l1_pos_control.cpp b/src/lib/ecl/l1/ecl_l1_pos_control.cpp index 6f50659601..533409217e 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_control.cpp +++ b/src/lib/ecl/l1/ecl_l1_pos_control.cpp @@ -6,9 +6,9 @@ * modification, are permitted provided that the following conditions * are met: * - * 1. Redistributions of source code must retain the above copyright + * 1. Redistributions of source code must retain the vector_ABove copyright * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright + * 2. Redistributions in binary form must reproduce the vector_ABove copyright * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. @@ -18,14 +18,14 @@ * * 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 + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTvector_ABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIvector_ABLE 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 + * AND ON ANY THEORY OF LIvector_ABILITY, WHETHER IN CONTRACT, STRICT + * LIvector_ABILITY, 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/src/lib/ecl/l1/ecl_l1_pos_control.h b/src/lib/ecl/l1/ecl_l1_pos_control.h index 661651f086..8bb94d3ebc 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_control.h +++ b/src/lib/ecl/l1/ecl_l1_pos_control.h @@ -39,18 +39,22 @@ * * Acknowledgements and References: * - * Original publication: - * S. Park, J. Deyst, and J. P. How, "A New Nonlinear Guidance Logic for Trajectory Tracking," + * This implementation has been built for PX4 based on the original + * publication from [1] and does include a lot of the ideas (not code) + * from [2]. + * + * + * [1] S. Park, J. Deyst, and J. P. How, "A New Nonlinear Guidance Logic for Trajectory Tracking," * Proceedings of the AIAA Guidance, Navigation and Control * Conference, Aug 2004. AIAA-2004-4900. * - * Original navigation logic modified by Paul Riseborough 2013: - * - Explicit control over frequency and damping - * - Explicit control over track capture angle - * - Ability to use loiter radius smaller than L1 length - * - Modified to use PD control for circle tracking to enable loiter radius less than L1 length - * - Modified to enable period and damping of guidance loop to be set explicitly - * - Modified to provide explicit control over capture angle + * [2] Paul Riseborough and Andrew Tridgell, L1 control for APM. Aug 2013. + * - Explicit control over frequency and damping + * - Explicit control over track capture angle + * - Ability to use loiter radius smaller than L1 length + * - Modified to use PD control for circle tracking to enable loiter radius less than L1 length + * - Modified to enable period and damping of guidance loop to be set explicitly + * - Modified to provide explicit control over capture angle * */ @@ -79,6 +83,7 @@ public: */ float nav_bearing(); + /** * Get lateral acceleration demand. * @@ -86,8 +91,6 @@ public: */ float nav_lateral_acceleration_demand(); - // return the heading error angle +ve to left of track - /** * Heading error. @@ -146,6 +149,7 @@ public: * Calling this function with two waypoints results in the * control outputs to fly to the line segment defined by * the points and once captured following the line segment. + * This follows the logic in [1]. * * @return sets _lateral_accel setpoint */ @@ -156,6 +160,9 @@ public: /** * Navigate on an orbit around a loiter waypoint. * + * This allow orbits smaller than the L1 length, + * this modification was introduced in [2]. + * * @return sets _lateral_accel setpoint */ void navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction, @@ -166,7 +173,8 @@ public: * Navigate on a fixed bearing. * * This only holds a certain direction and does not perform cross - * track correction. + * track correction. Helpful for semi-autonomous modes. Introduced + * by [2]. * * @return sets _lateral_accel setpoint */ @@ -174,7 +182,10 @@ public: /** - * Keep the wings level + * Keep the wings level. + * + * This is typically needed for maximum-lift-demand situations, + * such as takeoff or near stall. Introduced in [2]. */ void navigate_level_flight(float current_heading); @@ -184,6 +195,7 @@ public: */ void set_l1_period(float period) { _L1_period = period; + /* calculate the ratio introduced in [2] */ _L1_ratio = 1.0f / M_PI_F * _L1_damping * _L1_period; /* calculate normalized frequency for heading tracking */ _heading_omega = sqrtf(2.0f) * M_PI_F / _L1_period; @@ -197,14 +209,16 @@ public: */ void set_l1_damping(float damping) { _L1_damping = damping; + /* calculate the ratio introduced in [2] */ _L1_ratio = 1.0f / M_PI_F * _L1_damping * _L1_period; + /* calculate the L1 gain (following [2]) */ _K_L1 = 4.0f * _L1_damping * _L1_damping; } private: float _lateral_accel; ///< Lateral acceleration setpoint in m/s^2 - float _L1_distance; ///< L1 lead distance, defined by period and damping + float _L1_distance; ///< L1 lead distance, defined by period and damping bool _circle_mode; ///< flag for loiter mode float _nav_bearing; ///< bearing to L1 reference point float _bearing_error; ///< bearing error diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 7671cae722..e34c178d52 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -36,14 +36,16 @@ * Implementation of a generic position controller based on the L1 norm. Outputs a bank / roll * angle, equivalent to a lateral motion (for copters and rovers). * - * Original publication for horizontal control: + * Original publication for horizontal control class: * S. Park, J. Deyst, and J. P. How, "A New Nonlinear Guidance Logic for Trajectory Tracking," * Proceedings of the AIAA Guidance, Navigation and Control * Conference, Aug 2004. AIAA-2004-4900. * - * Original implementation for total energy control: + * Original implementation for total energy control class: * Paul Riseborough and Andrew Tridgell, 2013 (code in lib/external_lgpl) * + * More details and acknowledgements in the referenced library headers. + * * @author Lorenz Meier */ @@ -79,7 +81,7 @@ #include #include -#include +#include #include /** From c0b309760a6c9f4c089f8fc8ca7799a189ed6624 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Sep 2013 21:00:59 +0200 Subject: [PATCH 610/872] More cleanup --- src/lib/ecl/l1/ecl_l1_pos_control.cpp | 12 ++++++------ src/lib/external_lgpl/tecs/{TECS.cpp => tecs.cpp} | 4 ++-- src/lib/external_lgpl/tecs/{TECS.h => tecs.h} | 2 +- 3 files changed, 9 insertions(+), 9 deletions(-) rename src/lib/external_lgpl/tecs/{TECS.cpp => tecs.cpp} (99%) rename src/lib/external_lgpl/tecs/{TECS.h => tecs.h} (99%) diff --git a/src/lib/ecl/l1/ecl_l1_pos_control.cpp b/src/lib/ecl/l1/ecl_l1_pos_control.cpp index 533409217e..6f50659601 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_control.cpp +++ b/src/lib/ecl/l1/ecl_l1_pos_control.cpp @@ -6,9 +6,9 @@ * modification, are permitted provided that the following conditions * are met: * - * 1. Redistributions of source code must retain the vector_ABove copyright + * 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 vector_ABove copyright + * 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. @@ -18,14 +18,14 @@ * * 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 MERCHANTvector_ABILITY AND FITNESS + * 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 LIvector_ABLE FOR ANY DIRECT, INDIRECT, + * 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 LIvector_ABILITY, WHETHER IN CONTRACT, STRICT - * LIvector_ABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * 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/src/lib/external_lgpl/tecs/TECS.cpp b/src/lib/external_lgpl/tecs/tecs.cpp similarity index 99% rename from src/lib/external_lgpl/tecs/TECS.cpp rename to src/lib/external_lgpl/tecs/tecs.cpp index 3f53552437..864a9d24d2 100644 --- a/src/lib/external_lgpl/tecs/TECS.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -1,6 +1,6 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- -#include "TECS.h" +#include "tecs.h" #include using namespace math; @@ -10,7 +10,7 @@ using namespace math; #endif /** - * @file TECS.cpp + * @file tecs.cpp * * @author Paul Riseborough * diff --git a/src/lib/external_lgpl/tecs/TECS.h b/src/lib/external_lgpl/tecs/tecs.h similarity index 99% rename from src/lib/external_lgpl/tecs/TECS.h rename to src/lib/external_lgpl/tecs/tecs.h index 3732156986..2ae6b28bb4 100644 --- a/src/lib/external_lgpl/tecs/TECS.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -1,6 +1,6 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- -/// @file TECS.h +/// @file tecs.h /// @brief Combined Total Energy Speed & Height Control. /* From 3047b6ced05cc8b4cfac3e1014144ea3cfef17d5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Sep 2013 21:09:11 +0200 Subject: [PATCH 611/872] Another set of minor style edits --- src/lib/ecl/l1/ecl_l1_pos_control.h | 1 - src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 ++ 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/src/lib/ecl/l1/ecl_l1_pos_control.h b/src/lib/ecl/l1/ecl_l1_pos_control.h index 8bb94d3ebc..7f4a440188 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_control.h +++ b/src/lib/ecl/l1/ecl_l1_pos_control.h @@ -35,7 +35,6 @@ * @file ecl_l1_pos_control.h * Implementation of L1 position control. * - * @author Lorenz Meier * * Acknowledgements and References: * diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index e34c178d52..3e83f11c8f 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -31,6 +31,8 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ + + /** * @file fw_pos_control_l1_main.c * Implementation of a generic position controller based on the L1 norm. Outputs a bank / roll From 8131d28a0faf7d33060cf067f5bd8dee41666fed Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Sep 2013 21:35:50 +0200 Subject: [PATCH 612/872] Exported disarmed PWM values as IOCTLs --- src/drivers/drv_pwm_output.h | 12 +++- src/drivers/px4io/px4io.cpp | 110 ++++++++++++++++++++--------------- 2 files changed, 75 insertions(+), 47 deletions(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index ec9d4ca098..6ed9320cb1 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -79,6 +79,7 @@ typedef uint16_t servo_position_t; struct pwm_output_values { /** desired pulse widths for each of the supported channels */ servo_position_t values[PWM_OUTPUT_MAX_CHANNELS]; + int channel_count; }; /* @@ -118,9 +119,18 @@ ORB_DECLARE(output_pwm); /** start DSM bind */ #define DSM_BIND_START _IOC(_PWM_SERVO_BASE, 7) -/** Power up DSM receiver */ +/** power up DSM receiver */ #define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 8) +/** set the PWM value when disarmed - should be no PWM (zero) by default */ +#define PWM_SERVO_SET_DISARMED_PWM _IOC(_PWM_SERVO_BASE, 9) + +/** set the minimum PWM value the output will send */ +#define PWM_SERVO_SET_MIN_PWM _IOC(_PWM_SERVO_BASE, 10) + +/** set the maximum PWM value the output will send */ +#define PWM_SERVO_SET_MAX_PWM _IOC(_PWM_SERVO_BASE, 11) + /** set a single servo to a specific value */ #define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index c88abe59a8..bd5f330436 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -177,21 +177,6 @@ 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); - - /** - * 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 IO status. * @@ -391,6 +376,21 @@ private: */ int mixer_send(const char *buf, unsigned buflen); + /** + * 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); + + /** + * 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); + /** * Handle a status update from IO. * @@ -1674,6 +1674,24 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) break; } + case PWM_SERVO_SET_DISARMED_PWM: { + struct pwm_output_values* pwm = (struct pwm_output_values*)arg; + set_idle_values(pwm->values, pwm->channel_count); + } + break; + + case PWM_SERVO_SET_MIN_PWM: { + struct pwm_output_values* pwm = (struct pwm_output_values*)arg; + set_min_values(pwm->values, pwm->channel_count); + } + break; + + case PWM_SERVO_SET_MAX_PWM: { + struct pwm_output_values* pwm = (struct pwm_output_values*)arg; + set_max_values(pwm->values, pwm->channel_count); + } + break; + case PWM_SERVO_GET_COUNT: *(unsigned *)arg = _max_actuators; break; @@ -2265,26 +2283,26 @@ px4io_main(int argc, char *argv[]) errx(1, "min command needs at least one channel value (PWM)"); } - if (g_dev != nullptr) { + int iofd = open(PWM_OUTPUT_DEVICE_PATH, 0); + struct pwm_output_values pwm; - /* set values for first 8 channels, fill unassigned channels with 900. */ - uint16_t min[8]; + if (iofd > 0) { - for (unsigned i = 0; i < sizeof(min) / sizeof(min[0]); i++) + pwm.channel_count = 0; + + for (unsigned i = 0; i < sizeof(pwm.values) / sizeof(pwm.values[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) { + pwm.values[i] = atoi(argv[i+2]); + if (pwm.values[i] < 900 || pwm.values[i] > 1200) { errx(1, "value out of range of 900 < value < 1200. Aborting."); } - } else { - /* a zero value will the default */ - min[i] = 0; + pwm.channel_count++; } } - int ret = g_dev->set_min_values(min, sizeof(min) / sizeof(min[0])); + int ret = ioctl(iofd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm); if (ret != OK) errx(ret, "failed setting min values"); @@ -2300,26 +2318,26 @@ px4io_main(int argc, char *argv[]) errx(1, "max command needs at least one channel value (PWM)"); } - if (g_dev != nullptr) { + int iofd = open(PWM_OUTPUT_DEVICE_PATH, 0); + struct pwm_output_values pwm; - /* set values for first 8 channels, fill unassigned channels with 2100. */ - uint16_t max[8]; + if (iofd > 0) { - for (unsigned i = 0; i < sizeof(max) / sizeof(max[0]); i++) + pwm.channel_count = 0; + + for (int i = 0; i < sizeof(pwm.values) / sizeof(pwm.values[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) { + pwm.values[i] = atoi(argv[i+2]); + if (pwm.values[i] < 1800 || pwm.values[i] > 2100) { errx(1, "value out of range of 1800 < value < 2100. Aborting."); } - } else { - /* a zero value will the default */ - max[i] = 0; + pwm.channel_count++; } } - int ret = g_dev->set_max_values(max, sizeof(max) / sizeof(max[0])); + int ret = ioctl(iofd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm); if (ret != OK) errx(ret, "failed setting max values"); @@ -2329,32 +2347,32 @@ px4io_main(int argc, char *argv[]) exit(0); } - if (!strcmp(argv[1], "idle")) { + if (!strcmp(argv[1], "idle") || !strcmp(argv[1], "disarmed")) { if (argc < 3) { errx(1, "max command needs at least one channel value (PWM)"); } - if (g_dev != nullptr) { + int iofd = open(PWM_OUTPUT_DEVICE_PATH, 0); + struct pwm_output_values pwm; - /* set values for first 8 channels, fill unassigned channels with 0. */ - uint16_t idle[8]; + if (iofd > 0) { - for (unsigned i = 0; i < sizeof(idle) / sizeof(idle[0]); i++) + pwm.channel_count = 0; + + for (unsigned i = 0; i < sizeof(pwm.values) / sizeof(pwm.values[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) { + pwm.values[i] = atoi(argv[i+2]); + if (pwm.values[i] < 900 || pwm.values[i] > 2100) { errx(1, "value out of range of 900 < value < 2100. Aborting."); } - } else { - /* a zero value will the default */ - idle[i] = 0; + pwm.channel_count++; } } - int ret = g_dev->set_idle_values(idle, sizeof(idle) / sizeof(idle[0])); + int ret = ioctl(iofd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm); if (ret != OK) errx(ret, "failed setting idle values"); From 90873474a9c52b66696f31c12b35b25ab0f6abd6 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 10 Sep 2013 22:58:44 +0200 Subject: [PATCH 613/872] multirotor_pos_control: setpint reset rewritten --- .../multirotor_pos_control.c | 117 +++++++++++------- src/modules/systemlib/pid/pid.c | 26 ++-- 2 files changed, 85 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 3365230724..1b3ddfdcda 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -212,17 +212,17 @@ 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 global_pos_sp_reproject = false; + bool reset_mission_sp = 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_man_sp_z = true; + bool reset_man_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; - bool reset_auto_pos = true; + bool reset_auto_sp_xy = true; + bool reset_auto_sp_z = true; + bool reset_takeoff_sp = true; hrt_abstime t_prev = 0; const float alt_ctl_dz = 0.2f; @@ -270,11 +270,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* use integral_limit_out = tilt_max / 2 */ float i_limit; - if (params.xy_vel_i == 0.0f) { + if (params.xy_vel_i > 0.0f) { i_limit = params.tilt_max / params.xy_vel_i / 2.0f; } else { - i_limit = 1.0f; // not used really + i_limit = 0.0f; // not used } pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max); @@ -297,7 +297,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) 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; + reset_mission_sp = true; } hrt_abstime t = hrt_absolute_time(); @@ -312,8 +312,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (control_mode.flag_armed && !was_armed) { /* reset setpoints and integrals on arming */ - reset_sp_z = true; - reset_sp_xy = true; + reset_man_sp_z = true; + reset_man_sp_xy = true; + reset_auto_sp_z = true; + reset_auto_sp_xy = true; + reset_takeoff_sp = true; reset_int_z = true; reset_int_xy = true; } @@ -348,8 +351,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* reset setpoints to current position if needed */ if (control_mode.flag_control_altitude_enabled) { - if (reset_sp_z) { - reset_sp_z = false; + if (reset_man_sp_z) { + reset_man_sp_z = false; local_pos_sp.z = local_pos.z; mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - local_pos_sp.z); } @@ -371,8 +374,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } if (control_mode.flag_control_position_enabled) { - if (reset_sp_xy) { - reset_sp_xy = false; + if (reset_man_sp_xy) { + reset_man_sp_xy = false; local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; pid_reset_integral(&xy_vel_pids[0]); @@ -407,39 +410,43 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) 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; + /* local position setpoint is valid and can be used for auto loiter after position controlled mode */ + reset_auto_sp_xy = !control_mode.flag_control_position_enabled; + reset_auto_sp_z = !control_mode.flag_control_altitude_enabled; + reset_takeoff_sp = true; /* force reprojection of global setpoint after manual mode */ - global_pos_sp_reproject = true; + reset_mission_sp = true; } 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; + reset_auto_sp_xy = true; + reset_auto_sp_z = true; } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - if (reset_auto_pos) { + if (reset_takeoff_sp) { + reset_takeoff_sp = false; local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; local_pos_sp.z = - params.takeoff_alt - params.takeoff_gap; 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", (double)local_pos_sp.x, (double)local_pos_sp.y, (double) - local_pos_sp.z); } + reset_auto_sp_xy = false; + reset_auto_sp_z = true; + } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) { // TODO - reset_auto_pos = true; + reset_auto_sp_xy = true; + reset_auto_sp_z = 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; + reset_mission_sp = 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; @@ -447,9 +454,9 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home); } - if (global_pos_sp_reproject) { + if (reset_mission_sp) { + reset_mission_sp = false; /* update global setpoint projection */ - global_pos_sp_reproject = false; if (global_pos_sp_valid) { /* global position setpoint valid, use it */ @@ -471,33 +478,43 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y); } else { - if (!local_pos_sp_valid) { + if (reset_auto_sp_xy) { + reset_auto_sp_xy = false; /* 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; local_pos_sp.yaw = att.yaw; - local_pos_sp_valid = true; + att_sp.yaw_body = global_pos_sp.yaw; + } + + if (reset_auto_sp_z) { + reset_auto_sp_z = false; + local_pos_sp.z = local_pos.z; } mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); } } - reset_auto_pos = true; + reset_auto_sp_xy = true; + reset_auto_sp_z = true; + } + + if (control_mode.auto_state != NAVIGATION_STATE_AUTO_TAKEOFF) { + reset_takeoff_sp = true; } if (control_mode.auto_state != NAVIGATION_STATE_AUTO_MISSION) { - global_pos_sp_reproject = true; + reset_mission_sp = true; } /* reset setpoints after AUTO mode */ - reset_sp_xy = true; - reset_sp_z = true; + reset_man_sp_xy = true; + reset_man_sp_z = true; } else { - /* no control, loiter or stay on ground */ + /* no control (failsafe), loiter or stay on ground */ if (local_pos.landed) { /* landed: move setpoint down */ /* in air: hold altitude */ @@ -508,27 +525,30 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", (double) - local_pos_sp.z); } - reset_sp_z = true; + reset_man_sp_z = true; } else { /* in air: hold altitude */ - if (reset_sp_z) { - reset_sp_z = false; + if (reset_man_sp_z) { + reset_man_sp_z = false; local_pos_sp.z = local_pos.z; mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", (double) - local_pos_sp.z); } + + reset_auto_sp_z = false; } if (control_mode.flag_control_position_enabled) { - if (reset_sp_xy) { - reset_sp_xy = false; + if (reset_man_sp_xy) { + reset_man_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", (double)local_pos_sp.x, (double)local_pos_sp.y); } + + reset_auto_sp_xy = false; } } @@ -540,7 +560,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) 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; + reset_man_sp_z = true; global_vel_sp.vz = 0.0f; } @@ -558,7 +578,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } } else { - reset_sp_xy = true; + reset_man_sp_xy = true; global_vel_sp.vx = 0.0f; global_vel_sp.vy = 0.0f; } @@ -640,12 +660,13 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } else { /* position controller disabled, reset setpoints */ - reset_sp_z = true; - reset_sp_xy = true; + reset_man_sp_z = true; + reset_man_sp_xy = true; reset_int_z = true; reset_int_xy = true; - global_pos_sp_reproject = true; - reset_auto_pos = true; + reset_mission_sp = true; + reset_auto_sp_xy = true; + reset_auto_sp_z = true; } /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 739503ed41..77c952f52c 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -167,20 +167,26 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo d = 0.0f; } - // Calculate the error integral and check for saturation - i = pid->integral + (error * dt); + if (pid->ki > 0.0f) { + // Calculate the error integral and check for saturation + i = pid->integral + (error * dt); - if ((pid->limit > SIGMA && (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; + if ((pid->limit > SIGMA && (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; - } else { - if (!isfinite(i)) { - i = 0.0f; + } else { + if (!isfinite(i)) { + i = 0.0f; + } + + pid->integral = i; + pid->saturated = 0; } - pid->integral = i; + } else { + i = 0.0f; pid->saturated = 0; } From 516481aa2bcffc7e7adacbdd403ded159be6ac5a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Sep 2013 23:53:25 +0200 Subject: [PATCH 614/872] Updated MAVLink version --- .../v1.0/ardupilotmega/ardupilotmega.h | 11 +- .../v1.0/ardupilotmega/mavlink_msg_ahrs.h | 18 +- .../mavlink_msg_airspeed_autocal.h | 419 ++++++++++++++++++ .../v1.0/ardupilotmega/mavlink_msg_ap_adc.h | 18 +- .../v1.0/ardupilotmega/mavlink_msg_data16.h | 18 +- .../v1.0/ardupilotmega/mavlink_msg_data32.h | 18 +- .../v1.0/ardupilotmega/mavlink_msg_data64.h | 18 +- .../v1.0/ardupilotmega/mavlink_msg_data96.h | 18 +- .../mavlink_msg_digicam_configure.h | 18 +- .../mavlink_msg_digicam_control.h | 18 +- .../mavlink_msg_fence_fetch_point.h | 18 +- .../ardupilotmega/mavlink_msg_fence_point.h | 18 +- .../ardupilotmega/mavlink_msg_fence_status.h | 18 +- .../v1.0/ardupilotmega/mavlink_msg_hwstatus.h | 18 +- .../ardupilotmega/mavlink_msg_limits_status.h | 18 +- .../v1.0/ardupilotmega/mavlink_msg_meminfo.h | 18 +- .../mavlink_msg_mount_configure.h | 18 +- .../ardupilotmega/mavlink_msg_mount_control.h | 18 +- .../ardupilotmega/mavlink_msg_mount_status.h | 18 +- .../v1.0/ardupilotmega/mavlink_msg_radio.h | 18 +- .../ardupilotmega/mavlink_msg_rangefinder.h | 18 +- .../mavlink_msg_sensor_offsets.h | 18 +- .../mavlink_msg_set_mag_offsets.h | 18 +- .../v1.0/ardupilotmega/mavlink_msg_simstate.h | 72 +-- .../v1.0/ardupilotmega/mavlink_msg_wind.h | 18 +- .../mavlink/v1.0/ardupilotmega/testsuite.h | 70 ++- .../mavlink/v1.0/ardupilotmega/version.h | 2 +- .../include/mavlink/v1.0/autoquad/autoquad.h | 28 +- .../autoquad/mavlink_msg_aq_telemetry_f.h | 18 +- .../include/mavlink/v1.0/autoquad/version.h | 2 +- mavlink/include/mavlink/v1.0/common/common.h | 4 +- .../v1.0/common/mavlink_msg_attitude.h | 18 +- .../common/mavlink_msg_attitude_quaternion.h | 18 +- .../v1.0/common/mavlink_msg_auth_key.h | 18 +- .../v1.0/common/mavlink_msg_battery_status.h | 18 +- .../mavlink_msg_change_operator_control.h | 18 +- .../mavlink_msg_change_operator_control_ack.h | 18 +- .../v1.0/common/mavlink_msg_command_ack.h | 18 +- .../v1.0/common/mavlink_msg_command_long.h | 18 +- .../v1.0/common/mavlink_msg_data_stream.h | 18 +- .../mavlink/v1.0/common/mavlink_msg_debug.h | 18 +- .../v1.0/common/mavlink_msg_debug_vect.h | 18 +- .../mavlink_msg_file_transfer_dir_list.h | 18 +- .../common/mavlink_msg_file_transfer_res.h | 18 +- .../common/mavlink_msg_file_transfer_start.h | 18 +- .../common/mavlink_msg_global_position_int.h | 28 +- ...mavlink_msg_global_position_setpoint_int.h | 18 +- ...link_msg_global_vision_position_estimate.h | 18 +- .../common/mavlink_msg_gps_global_origin.h | 18 +- .../v1.0/common/mavlink_msg_gps_raw_int.h | 58 ++- .../v1.0/common/mavlink_msg_gps_status.h | 18 +- .../v1.0/common/mavlink_msg_heartbeat.h | 18 +- .../v1.0/common/mavlink_msg_highres_imu.h | 18 +- .../v1.0/common/mavlink_msg_hil_controls.h | 18 +- .../mavlink/v1.0/common/mavlink_msg_hil_gps.h | 18 +- .../common/mavlink_msg_hil_optical_flow.h | 18 +- .../common/mavlink_msg_hil_rc_inputs_raw.h | 18 +- .../v1.0/common/mavlink_msg_hil_sensor.h | 18 +- .../v1.0/common/mavlink_msg_hil_state.h | 18 +- .../common/mavlink_msg_hil_state_quaternion.h | 18 +- .../common/mavlink_msg_local_position_ned.h | 18 +- ..._local_position_ned_system_global_offset.h | 18 +- .../mavlink_msg_local_position_setpoint.h | 18 +- .../v1.0/common/mavlink_msg_manual_control.h | 18 +- .../v1.0/common/mavlink_msg_manual_setpoint.h | 18 +- .../v1.0/common/mavlink_msg_memory_vect.h | 18 +- .../v1.0/common/mavlink_msg_mission_ack.h | 18 +- .../common/mavlink_msg_mission_clear_all.h | 18 +- .../v1.0/common/mavlink_msg_mission_count.h | 18 +- .../v1.0/common/mavlink_msg_mission_current.h | 18 +- .../v1.0/common/mavlink_msg_mission_item.h | 18 +- .../common/mavlink_msg_mission_item_reached.h | 18 +- .../v1.0/common/mavlink_msg_mission_request.h | 18 +- .../common/mavlink_msg_mission_request_list.h | 18 +- ...mavlink_msg_mission_request_partial_list.h | 18 +- .../common/mavlink_msg_mission_set_current.h | 18 +- .../mavlink_msg_mission_write_partial_list.h | 18 +- .../common/mavlink_msg_named_value_float.h | 18 +- .../v1.0/common/mavlink_msg_named_value_int.h | 18 +- .../mavlink_msg_nav_controller_output.h | 18 +- .../common/mavlink_msg_omnidirectional_flow.h | 18 +- .../v1.0/common/mavlink_msg_optical_flow.h | 38 +- .../common/mavlink_msg_param_request_list.h | 18 +- .../common/mavlink_msg_param_request_read.h | 18 +- .../v1.0/common/mavlink_msg_param_set.h | 18 +- .../v1.0/common/mavlink_msg_param_value.h | 18 +- .../mavlink/v1.0/common/mavlink_msg_ping.h | 18 +- .../v1.0/common/mavlink_msg_radio_status.h | 18 +- .../mavlink/v1.0/common/mavlink_msg_raw_imu.h | 18 +- .../v1.0/common/mavlink_msg_raw_pressure.h | 18 +- .../common/mavlink_msg_rc_channels_override.h | 98 ++-- .../v1.0/common/mavlink_msg_rc_channels_raw.h | 98 ++-- .../common/mavlink_msg_rc_channels_scaled.h | 98 ++-- .../common/mavlink_msg_request_data_stream.h | 18 +- ...msg_roll_pitch_yaw_rates_thrust_setpoint.h | 18 +- ...msg_roll_pitch_yaw_speed_thrust_setpoint.h | 18 +- ...vlink_msg_roll_pitch_yaw_thrust_setpoint.h | 18 +- .../common/mavlink_msg_safety_allowed_area.h | 18 +- .../mavlink_msg_safety_set_allowed_area.h | 18 +- .../v1.0/common/mavlink_msg_scaled_imu.h | 18 +- .../v1.0/common/mavlink_msg_scaled_pressure.h | 18 +- .../common/mavlink_msg_servo_output_raw.h | 18 +- ...ink_msg_set_global_position_setpoint_int.h | 18 +- .../mavlink_msg_set_gps_global_origin.h | 18 +- .../mavlink_msg_set_local_position_setpoint.h | 18 +- .../v1.0/common/mavlink_msg_set_mode.h | 18 +- .../mavlink_msg_set_quad_motors_setpoint.h | 18 +- ...set_quad_swarm_led_roll_pitch_yaw_thrust.h | 58 ++- ...msg_set_quad_swarm_roll_pitch_yaw_thrust.h | 58 ++- ...link_msg_set_roll_pitch_yaw_speed_thrust.h | 18 +- .../mavlink_msg_set_roll_pitch_yaw_thrust.h | 18 +- .../v1.0/common/mavlink_msg_setpoint_6dof.h | 18 +- .../v1.0/common/mavlink_msg_setpoint_8dof.h | 18 +- .../v1.0/common/mavlink_msg_sim_state.h | 18 +- .../common/mavlink_msg_state_correction.h | 18 +- .../v1.0/common/mavlink_msg_statustext.h | 18 +- .../v1.0/common/mavlink_msg_sys_status.h | 18 +- .../v1.0/common/mavlink_msg_system_time.h | 18 +- .../mavlink/v1.0/common/mavlink_msg_vfr_hud.h | 18 +- .../mavlink_msg_vicon_position_estimate.h | 18 +- .../mavlink_msg_vision_position_estimate.h | 18 +- .../mavlink_msg_vision_speed_estimate.h | 18 +- mavlink/include/mavlink/v1.0/common/version.h | 2 +- .../mavlink/v1.0/matrixpilot/matrixpilot.h | 4 +- .../v1.0/matrixpilot/mavlink_msg_airspeeds.h | 18 +- .../v1.0/matrixpilot/mavlink_msg_altitudes.h | 18 +- ...avlink_msg_flexifunction_buffer_function.h | 18 +- ...nk_msg_flexifunction_buffer_function_ack.h | 18 +- .../mavlink_msg_flexifunction_command.h | 18 +- .../mavlink_msg_flexifunction_command_ack.h | 18 +- .../mavlink_msg_flexifunction_directory.h | 18 +- .../mavlink_msg_flexifunction_directory_ack.h | 18 +- .../mavlink_msg_flexifunction_read_req.h | 18 +- .../mavlink_msg_flexifunction_set.h | 18 +- .../mavlink_msg_serial_udb_extra_f13.h | 18 +- .../mavlink_msg_serial_udb_extra_f14.h | 18 +- .../mavlink_msg_serial_udb_extra_f15.h | 18 +- .../mavlink_msg_serial_udb_extra_f16.h | 18 +- .../mavlink_msg_serial_udb_extra_f2_a.h | 18 +- .../mavlink_msg_serial_udb_extra_f2_b.h | 18 +- .../mavlink_msg_serial_udb_extra_f4.h | 18 +- .../mavlink_msg_serial_udb_extra_f5.h | 18 +- .../mavlink_msg_serial_udb_extra_f6.h | 18 +- .../mavlink_msg_serial_udb_extra_f7.h | 18 +- .../mavlink_msg_serial_udb_extra_f8.h | 18 +- .../mavlink/v1.0/matrixpilot/version.h | 2 +- .../include/mavlink/v1.0/mavlink_helpers.h | 10 +- .../pixhawk/mavlink_msg_attitude_control.h | 18 +- .../v1.0/pixhawk/mavlink_msg_brief_feature.h | 18 +- .../mavlink_msg_data_transmission_handshake.h | 18 +- .../pixhawk/mavlink_msg_encapsulated_data.h | 18 +- .../pixhawk/mavlink_msg_image_available.h | 18 +- .../mavlink_msg_image_trigger_control.h | 18 +- .../pixhawk/mavlink_msg_image_triggered.h | 18 +- .../mavlink/v1.0/pixhawk/mavlink_msg_marker.h | 18 +- .../pixhawk/mavlink_msg_pattern_detected.h | 18 +- .../pixhawk/mavlink_msg_point_of_interest.h | 18 +- ...mavlink_msg_point_of_interest_connection.h | 18 +- .../mavlink_msg_position_control_setpoint.h | 18 +- .../v1.0/pixhawk/mavlink_msg_raw_aux.h | 18 +- .../pixhawk/mavlink_msg_set_cam_shutter.h | 18 +- .../mavlink_msg_set_position_control_offset.h | 18 +- .../pixhawk/mavlink_msg_watchdog_command.h | 18 +- .../pixhawk/mavlink_msg_watchdog_heartbeat.h | 18 +- .../mavlink_msg_watchdog_process_info.h | 18 +- .../mavlink_msg_watchdog_process_status.h | 18 +- .../include/mavlink/v1.0/pixhawk/pixhawk.h | 4 +- .../include/mavlink/v1.0/pixhawk/version.h | 2 +- mavlink/include/mavlink/v1.0/protocol.h | 2 +- .../sensesoar/mavlink_msg_cmd_airspeed_ack.h | 18 +- .../sensesoar/mavlink_msg_cmd_airspeed_chng.h | 18 +- .../v1.0/sensesoar/mavlink_msg_filt_rot_vel.h | 18 +- .../v1.0/sensesoar/mavlink_msg_llc_out.h | 18 +- .../v1.0/sensesoar/mavlink_msg_obs_air_temp.h | 18 +- .../sensesoar/mavlink_msg_obs_air_velocity.h | 18 +- .../v1.0/sensesoar/mavlink_msg_obs_attitude.h | 18 +- .../v1.0/sensesoar/mavlink_msg_obs_bias.h | 18 +- .../v1.0/sensesoar/mavlink_msg_obs_position.h | 18 +- .../v1.0/sensesoar/mavlink_msg_obs_qff.h | 18 +- .../v1.0/sensesoar/mavlink_msg_obs_velocity.h | 18 +- .../v1.0/sensesoar/mavlink_msg_obs_wind.h | 18 +- .../v1.0/sensesoar/mavlink_msg_pm_elec.h | 18 +- .../v1.0/sensesoar/mavlink_msg_sys_stat.h | 18 +- .../include/mavlink/v1.0/sensesoar/version.h | 2 +- 184 files changed, 3469 insertions(+), 581 deletions(-) create mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_airspeed_autocal.h diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h b/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h index eec9a89c50..3aad8e6785 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h @@ -12,15 +12,15 @@ extern "C" { // MESSAGE LENGTHS AND CRCS #ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 0, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 16, 33, 25, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 12, 3, 8, 28, 44, 3, 9, 22, 12, 18, 34, 66, 98, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} +#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 0, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 16, 33, 25, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 12, 3, 8, 28, 44, 3, 9, 22, 12, 18, 34, 66, 98, 8, 48, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} #endif #ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 0, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 241, 15, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 111, 21, 21, 144, 1, 234, 73, 181, 22, 83, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} +#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 0, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 241, 15, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 154, 21, 21, 144, 1, 234, 73, 181, 22, 83, 167, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} #endif #ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE, MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE, MAVLINK_MESSAGE_INFO_MOUNT_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_STATUS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_FENCE_POINT, MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_AHRS, MAVLINK_MESSAGE_INFO_SIMSTATE, MAVLINK_MESSAGE_INFO_HWSTATUS, MAVLINK_MESSAGE_INFO_RADIO, MAVLINK_MESSAGE_INFO_LIMITS_STATUS, MAVLINK_MESSAGE_INFO_WIND, MAVLINK_MESSAGE_INFO_DATA16, MAVLINK_MESSAGE_INFO_DATA32, MAVLINK_MESSAGE_INFO_DATA64, MAVLINK_MESSAGE_INFO_DATA96, MAVLINK_MESSAGE_INFO_RANGEFINDER, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE, MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE, MAVLINK_MESSAGE_INFO_MOUNT_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_STATUS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_FENCE_POINT, MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_AHRS, MAVLINK_MESSAGE_INFO_SIMSTATE, MAVLINK_MESSAGE_INFO_HWSTATUS, MAVLINK_MESSAGE_INFO_RADIO, MAVLINK_MESSAGE_INFO_LIMITS_STATUS, MAVLINK_MESSAGE_INFO_WIND, MAVLINK_MESSAGE_INFO_DATA16, MAVLINK_MESSAGE_INFO_DATA32, MAVLINK_MESSAGE_INFO_DATA64, MAVLINK_MESSAGE_INFO_DATA96, MAVLINK_MESSAGE_INFO_RANGEFINDER, MAVLINK_MESSAGE_INFO_AIRSPEED_AUTOCAL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} #endif #include "../protocol.h" @@ -74,6 +74,7 @@ enum MAV_CMD MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ MAV_CMD_DO_DIGICAM_CONFIGURE=202, /* Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)| */ MAV_CMD_DO_DIGICAM_CONTROL=203, /* Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty| */ MAV_CMD_DO_MOUNT_CONFIGURE=204, /* Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty| */ @@ -86,7 +87,8 @@ enum MAV_CMD MAV_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ MAV_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ MAV_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ - MAV_CMD_ENUM_END=401, /* | */ + MAV_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ + MAV_CMD_ENUM_END=501, /* | */ }; #endif @@ -179,6 +181,7 @@ enum LIMIT_MODULE #include "./mavlink_msg_data64.h" #include "./mavlink_msg_data96.h" #include "./mavlink_msg_rangefinder.h" +#include "./mavlink_msg_airspeed_autocal.h" #ifdef __cplusplus } diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs.h index d9d2ccb041..c3ead11401 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_ahrs_pack(uint8_t system_id, uint8_t componen * @brief Pack a ahrs message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param omegaIx X gyro drift estimate rad/s * @param omegaIy Y gyro drift estimate rad/s @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_ahrs_pack_chan(uint8_t system_id, uint8_t com } /** - * @brief Encode a ahrs struct into a message + * @brief Encode a ahrs struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -149,6 +149,20 @@ static inline uint16_t mavlink_msg_ahrs_encode(uint8_t system_id, uint8_t compon return mavlink_msg_ahrs_pack(system_id, component_id, msg, ahrs->omegaIx, ahrs->omegaIy, ahrs->omegaIz, ahrs->accel_weight, ahrs->renorm_val, ahrs->error_rp, ahrs->error_yaw); } +/** + * @brief Encode a ahrs struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param ahrs C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ahrs_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_ahrs_t* ahrs) +{ + return mavlink_msg_ahrs_pack_chan(system_id, component_id, chan, msg, ahrs->omegaIx, ahrs->omegaIy, ahrs->omegaIz, ahrs->accel_weight, ahrs->renorm_val, ahrs->error_rp, ahrs->error_yaw); +} + /** * @brief Send a ahrs message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_airspeed_autocal.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_airspeed_autocal.h new file mode 100644 index 0000000000..d046f2ad09 --- /dev/null +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_airspeed_autocal.h @@ -0,0 +1,419 @@ +// MESSAGE AIRSPEED_AUTOCAL PACKING + +#define MAVLINK_MSG_ID_AIRSPEED_AUTOCAL 174 + +typedef struct __mavlink_airspeed_autocal_t +{ + float vx; ///< GPS velocity north m/s + float vy; ///< GPS velocity east m/s + float vz; ///< GPS velocity down m/s + float diff_pressure; ///< Differential pressure pascals + float EAS2TAS; ///< Estimated to true airspeed ratio + float ratio; ///< Airspeed ratio + float state_x; ///< EKF state x + float state_y; ///< EKF state y + float state_z; ///< EKF state z + float Pax; ///< EKF Pax + float Pby; ///< EKF Pby + float Pcz; ///< EKF Pcz +} mavlink_airspeed_autocal_t; + +#define MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN 48 +#define MAVLINK_MSG_ID_174_LEN 48 + +#define MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_CRC 167 +#define MAVLINK_MSG_ID_174_CRC 167 + + + +#define MAVLINK_MESSAGE_INFO_AIRSPEED_AUTOCAL { \ + "AIRSPEED_AUTOCAL", \ + 12, \ + { { "vx", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_airspeed_autocal_t, vx) }, \ + { "vy", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_airspeed_autocal_t, vy) }, \ + { "vz", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_airspeed_autocal_t, vz) }, \ + { "diff_pressure", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_airspeed_autocal_t, diff_pressure) }, \ + { "EAS2TAS", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_airspeed_autocal_t, EAS2TAS) }, \ + { "ratio", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_airspeed_autocal_t, ratio) }, \ + { "state_x", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_airspeed_autocal_t, state_x) }, \ + { "state_y", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_airspeed_autocal_t, state_y) }, \ + { "state_z", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_airspeed_autocal_t, state_z) }, \ + { "Pax", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_airspeed_autocal_t, Pax) }, \ + { "Pby", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_airspeed_autocal_t, Pby) }, \ + { "Pcz", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_airspeed_autocal_t, Pcz) }, \ + } \ +} + + +/** + * @brief Pack a airspeed_autocal message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param vx GPS velocity north m/s + * @param vy GPS velocity east m/s + * @param vz GPS velocity down m/s + * @param diff_pressure Differential pressure pascals + * @param EAS2TAS Estimated to true airspeed ratio + * @param ratio Airspeed ratio + * @param state_x EKF state x + * @param state_y EKF state y + * @param state_z EKF state z + * @param Pax EKF Pax + * @param Pby EKF Pby + * @param Pcz EKF Pcz + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_airspeed_autocal_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + float vx, float vy, float vz, float diff_pressure, float EAS2TAS, float ratio, float state_x, float state_y, float state_z, float Pax, float Pby, float Pcz) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN]; + _mav_put_float(buf, 0, vx); + _mav_put_float(buf, 4, vy); + _mav_put_float(buf, 8, vz); + _mav_put_float(buf, 12, diff_pressure); + _mav_put_float(buf, 16, EAS2TAS); + _mav_put_float(buf, 20, ratio); + _mav_put_float(buf, 24, state_x); + _mav_put_float(buf, 28, state_y); + _mav_put_float(buf, 32, state_z); + _mav_put_float(buf, 36, Pax); + _mav_put_float(buf, 40, Pby); + _mav_put_float(buf, 44, Pcz); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); +#else + mavlink_airspeed_autocal_t packet; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.diff_pressure = diff_pressure; + packet.EAS2TAS = EAS2TAS; + packet.ratio = ratio; + packet.state_x = state_x; + packet.state_y = state_y; + packet.state_z = state_z; + packet.Pax = Pax; + packet.Pby = Pby; + packet.Pcz = Pcz; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AIRSPEED_AUTOCAL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); +#endif +} + +/** + * @brief Pack a airspeed_autocal message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param vx GPS velocity north m/s + * @param vy GPS velocity east m/s + * @param vz GPS velocity down m/s + * @param diff_pressure Differential pressure pascals + * @param EAS2TAS Estimated to true airspeed ratio + * @param ratio Airspeed ratio + * @param state_x EKF state x + * @param state_y EKF state y + * @param state_z EKF state z + * @param Pax EKF Pax + * @param Pby EKF Pby + * @param Pcz EKF Pcz + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_airspeed_autocal_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + float vx,float vy,float vz,float diff_pressure,float EAS2TAS,float ratio,float state_x,float state_y,float state_z,float Pax,float Pby,float Pcz) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN]; + _mav_put_float(buf, 0, vx); + _mav_put_float(buf, 4, vy); + _mav_put_float(buf, 8, vz); + _mav_put_float(buf, 12, diff_pressure); + _mav_put_float(buf, 16, EAS2TAS); + _mav_put_float(buf, 20, ratio); + _mav_put_float(buf, 24, state_x); + _mav_put_float(buf, 28, state_y); + _mav_put_float(buf, 32, state_z); + _mav_put_float(buf, 36, Pax); + _mav_put_float(buf, 40, Pby); + _mav_put_float(buf, 44, Pcz); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); +#else + mavlink_airspeed_autocal_t packet; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.diff_pressure = diff_pressure; + packet.EAS2TAS = EAS2TAS; + packet.ratio = ratio; + packet.state_x = state_x; + packet.state_y = state_y; + packet.state_z = state_z; + packet.Pax = Pax; + packet.Pby = Pby; + packet.Pcz = Pcz; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AIRSPEED_AUTOCAL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); +#endif +} + +/** + * @brief Encode a airspeed_autocal struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param airspeed_autocal C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_airspeed_autocal_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_airspeed_autocal_t* airspeed_autocal) +{ + return mavlink_msg_airspeed_autocal_pack(system_id, component_id, msg, airspeed_autocal->vx, airspeed_autocal->vy, airspeed_autocal->vz, airspeed_autocal->diff_pressure, airspeed_autocal->EAS2TAS, airspeed_autocal->ratio, airspeed_autocal->state_x, airspeed_autocal->state_y, airspeed_autocal->state_z, airspeed_autocal->Pax, airspeed_autocal->Pby, airspeed_autocal->Pcz); +} + +/** + * @brief Encode a airspeed_autocal struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param airspeed_autocal C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_airspeed_autocal_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_airspeed_autocal_t* airspeed_autocal) +{ + return mavlink_msg_airspeed_autocal_pack_chan(system_id, component_id, chan, msg, airspeed_autocal->vx, airspeed_autocal->vy, airspeed_autocal->vz, airspeed_autocal->diff_pressure, airspeed_autocal->EAS2TAS, airspeed_autocal->ratio, airspeed_autocal->state_x, airspeed_autocal->state_y, airspeed_autocal->state_z, airspeed_autocal->Pax, airspeed_autocal->Pby, airspeed_autocal->Pcz); +} + +/** + * @brief Send a airspeed_autocal message + * @param chan MAVLink channel to send the message + * + * @param vx GPS velocity north m/s + * @param vy GPS velocity east m/s + * @param vz GPS velocity down m/s + * @param diff_pressure Differential pressure pascals + * @param EAS2TAS Estimated to true airspeed ratio + * @param ratio Airspeed ratio + * @param state_x EKF state x + * @param state_y EKF state y + * @param state_z EKF state z + * @param Pax EKF Pax + * @param Pby EKF Pby + * @param Pcz EKF Pcz + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_airspeed_autocal_send(mavlink_channel_t chan, float vx, float vy, float vz, float diff_pressure, float EAS2TAS, float ratio, float state_x, float state_y, float state_z, float Pax, float Pby, float Pcz) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN]; + _mav_put_float(buf, 0, vx); + _mav_put_float(buf, 4, vy); + _mav_put_float(buf, 8, vz); + _mav_put_float(buf, 12, diff_pressure); + _mav_put_float(buf, 16, EAS2TAS); + _mav_put_float(buf, 20, ratio); + _mav_put_float(buf, 24, state_x); + _mav_put_float(buf, 28, state_y); + _mav_put_float(buf, 32, state_z); + _mav_put_float(buf, 36, Pax); + _mav_put_float(buf, 40, Pby); + _mav_put_float(buf, 44, Pcz); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL, buf, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL, buf, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); +#endif +#else + mavlink_airspeed_autocal_t packet; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.diff_pressure = diff_pressure; + packet.EAS2TAS = EAS2TAS; + packet.ratio = ratio; + packet.state_x = state_x; + packet.state_y = state_y; + packet.state_z = state_z; + packet.Pax = Pax; + packet.Pby = Pby; + packet.Pcz = Pcz; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL, (const char *)&packet, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL, (const char *)&packet, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); +#endif +#endif +} + +#endif + +// MESSAGE AIRSPEED_AUTOCAL UNPACKING + + +/** + * @brief Get field vx from airspeed_autocal message + * + * @return GPS velocity north m/s + */ +static inline float mavlink_msg_airspeed_autocal_get_vx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field vy from airspeed_autocal message + * + * @return GPS velocity east m/s + */ +static inline float mavlink_msg_airspeed_autocal_get_vy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field vz from airspeed_autocal message + * + * @return GPS velocity down m/s + */ +static inline float mavlink_msg_airspeed_autocal_get_vz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field diff_pressure from airspeed_autocal message + * + * @return Differential pressure pascals + */ +static inline float mavlink_msg_airspeed_autocal_get_diff_pressure(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field EAS2TAS from airspeed_autocal message + * + * @return Estimated to true airspeed ratio + */ +static inline float mavlink_msg_airspeed_autocal_get_EAS2TAS(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field ratio from airspeed_autocal message + * + * @return Airspeed ratio + */ +static inline float mavlink_msg_airspeed_autocal_get_ratio(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field state_x from airspeed_autocal message + * + * @return EKF state x + */ +static inline float mavlink_msg_airspeed_autocal_get_state_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field state_y from airspeed_autocal message + * + * @return EKF state y + */ +static inline float mavlink_msg_airspeed_autocal_get_state_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field state_z from airspeed_autocal message + * + * @return EKF state z + */ +static inline float mavlink_msg_airspeed_autocal_get_state_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field Pax from airspeed_autocal message + * + * @return EKF Pax + */ +static inline float mavlink_msg_airspeed_autocal_get_Pax(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); +} + +/** + * @brief Get field Pby from airspeed_autocal message + * + * @return EKF Pby + */ +static inline float mavlink_msg_airspeed_autocal_get_Pby(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 40); +} + +/** + * @brief Get field Pcz from airspeed_autocal message + * + * @return EKF Pcz + */ +static inline float mavlink_msg_airspeed_autocal_get_Pcz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 44); +} + +/** + * @brief Decode a airspeed_autocal message into a struct + * + * @param msg The message to decode + * @param airspeed_autocal C-struct to decode the message contents into + */ +static inline void mavlink_msg_airspeed_autocal_decode(const mavlink_message_t* msg, mavlink_airspeed_autocal_t* airspeed_autocal) +{ +#if MAVLINK_NEED_BYTE_SWAP + airspeed_autocal->vx = mavlink_msg_airspeed_autocal_get_vx(msg); + airspeed_autocal->vy = mavlink_msg_airspeed_autocal_get_vy(msg); + airspeed_autocal->vz = mavlink_msg_airspeed_autocal_get_vz(msg); + airspeed_autocal->diff_pressure = mavlink_msg_airspeed_autocal_get_diff_pressure(msg); + airspeed_autocal->EAS2TAS = mavlink_msg_airspeed_autocal_get_EAS2TAS(msg); + airspeed_autocal->ratio = mavlink_msg_airspeed_autocal_get_ratio(msg); + airspeed_autocal->state_x = mavlink_msg_airspeed_autocal_get_state_x(msg); + airspeed_autocal->state_y = mavlink_msg_airspeed_autocal_get_state_y(msg); + airspeed_autocal->state_z = mavlink_msg_airspeed_autocal_get_state_z(msg); + airspeed_autocal->Pax = mavlink_msg_airspeed_autocal_get_Pax(msg); + airspeed_autocal->Pby = mavlink_msg_airspeed_autocal_get_Pby(msg); + airspeed_autocal->Pcz = mavlink_msg_airspeed_autocal_get_Pcz(msg); +#else + memcpy(airspeed_autocal, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ap_adc.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ap_adc.h index c6c7624513..821ce73e47 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ap_adc.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ap_adc.h @@ -84,7 +84,7 @@ static inline uint16_t mavlink_msg_ap_adc_pack(uint8_t system_id, uint8_t compon * @brief Pack a ap_adc message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param adc1 ADC output 1 * @param adc2 ADC output 2 @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_ap_adc_pack_chan(uint8_t system_id, uint8_t c } /** - * @brief Encode a ap_adc struct into a message + * @brief Encode a ap_adc struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -141,6 +141,20 @@ static inline uint16_t mavlink_msg_ap_adc_encode(uint8_t system_id, uint8_t comp return mavlink_msg_ap_adc_pack(system_id, component_id, msg, ap_adc->adc1, ap_adc->adc2, ap_adc->adc3, ap_adc->adc4, ap_adc->adc5, ap_adc->adc6); } +/** + * @brief Encode a ap_adc struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param ap_adc C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ap_adc_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_ap_adc_t* ap_adc) +{ + return mavlink_msg_ap_adc_pack_chan(system_id, component_id, chan, msg, ap_adc->adc1, ap_adc->adc2, ap_adc->adc3, ap_adc->adc4, ap_adc->adc5, ap_adc->adc6); +} + /** * @brief Send a ap_adc message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data16.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data16.h index c753585267..9200eefa0d 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data16.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data16.h @@ -67,7 +67,7 @@ static inline uint16_t mavlink_msg_data16_pack(uint8_t system_id, uint8_t compon * @brief Pack a data16 message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param type data type * @param len data length @@ -101,7 +101,7 @@ static inline uint16_t mavlink_msg_data16_pack_chan(uint8_t system_id, uint8_t c } /** - * @brief Encode a data16 struct into a message + * @brief Encode a data16 struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -113,6 +113,20 @@ static inline uint16_t mavlink_msg_data16_encode(uint8_t system_id, uint8_t comp return mavlink_msg_data16_pack(system_id, component_id, msg, data16->type, data16->len, data16->data); } +/** + * @brief Encode a data16 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param data16 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_data16_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_data16_t* data16) +{ + return mavlink_msg_data16_pack_chan(system_id, component_id, chan, msg, data16->type, data16->len, data16->data); +} + /** * @brief Send a data16 message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data32.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data32.h index 46c804a3c2..3afedb7874 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data32.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data32.h @@ -67,7 +67,7 @@ static inline uint16_t mavlink_msg_data32_pack(uint8_t system_id, uint8_t compon * @brief Pack a data32 message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param type data type * @param len data length @@ -101,7 +101,7 @@ static inline uint16_t mavlink_msg_data32_pack_chan(uint8_t system_id, uint8_t c } /** - * @brief Encode a data32 struct into a message + * @brief Encode a data32 struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -113,6 +113,20 @@ static inline uint16_t mavlink_msg_data32_encode(uint8_t system_id, uint8_t comp return mavlink_msg_data32_pack(system_id, component_id, msg, data32->type, data32->len, data32->data); } +/** + * @brief Encode a data32 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param data32 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_data32_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_data32_t* data32) +{ + return mavlink_msg_data32_pack_chan(system_id, component_id, chan, msg, data32->type, data32->len, data32->data); +} + /** * @brief Send a data32 message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data64.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data64.h index 843084dffa..6931ada167 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data64.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data64.h @@ -67,7 +67,7 @@ static inline uint16_t mavlink_msg_data64_pack(uint8_t system_id, uint8_t compon * @brief Pack a data64 message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param type data type * @param len data length @@ -101,7 +101,7 @@ static inline uint16_t mavlink_msg_data64_pack_chan(uint8_t system_id, uint8_t c } /** - * @brief Encode a data64 struct into a message + * @brief Encode a data64 struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -113,6 +113,20 @@ static inline uint16_t mavlink_msg_data64_encode(uint8_t system_id, uint8_t comp return mavlink_msg_data64_pack(system_id, component_id, msg, data64->type, data64->len, data64->data); } +/** + * @brief Encode a data64 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param data64 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_data64_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_data64_t* data64) +{ + return mavlink_msg_data64_pack_chan(system_id, component_id, chan, msg, data64->type, data64->len, data64->data); +} + /** * @brief Send a data64 message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data96.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data96.h index 095628865c..cffc7d7e7f 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data96.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data96.h @@ -67,7 +67,7 @@ static inline uint16_t mavlink_msg_data96_pack(uint8_t system_id, uint8_t compon * @brief Pack a data96 message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param type data type * @param len data length @@ -101,7 +101,7 @@ static inline uint16_t mavlink_msg_data96_pack_chan(uint8_t system_id, uint8_t c } /** - * @brief Encode a data96 struct into a message + * @brief Encode a data96 struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -113,6 +113,20 @@ static inline uint16_t mavlink_msg_data96_encode(uint8_t system_id, uint8_t comp return mavlink_msg_data96_pack(system_id, component_id, msg, data96->type, data96->len, data96->data); } +/** + * @brief Encode a data96 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param data96 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_data96_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_data96_t* data96) +{ + return mavlink_msg_data96_pack_chan(system_id, component_id, chan, msg, data96->type, data96->len, data96->data); +} + /** * @brief Send a data96 message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_configure.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_configure.h index bcc706a887..c6518c4199 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_configure.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_configure.h @@ -109,7 +109,7 @@ static inline uint16_t mavlink_msg_digicam_configure_pack(uint8_t system_id, uin * @brief Pack a digicam_configure message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -169,7 +169,7 @@ static inline uint16_t mavlink_msg_digicam_configure_pack_chan(uint8_t system_id } /** - * @brief Encode a digicam_configure struct into a message + * @brief Encode a digicam_configure struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -181,6 +181,20 @@ static inline uint16_t mavlink_msg_digicam_configure_encode(uint8_t system_id, u return mavlink_msg_digicam_configure_pack(system_id, component_id, msg, digicam_configure->target_system, digicam_configure->target_component, digicam_configure->mode, digicam_configure->shutter_speed, digicam_configure->aperture, digicam_configure->iso, digicam_configure->exposure_type, digicam_configure->command_id, digicam_configure->engine_cut_off, digicam_configure->extra_param, digicam_configure->extra_value); } +/** + * @brief Encode a digicam_configure struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param digicam_configure C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_digicam_configure_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_digicam_configure_t* digicam_configure) +{ + return mavlink_msg_digicam_configure_pack_chan(system_id, component_id, chan, msg, digicam_configure->target_system, digicam_configure->target_component, digicam_configure->mode, digicam_configure->shutter_speed, digicam_configure->aperture, digicam_configure->iso, digicam_configure->exposure_type, digicam_configure->command_id, digicam_configure->engine_cut_off, digicam_configure->extra_param, digicam_configure->extra_value); +} + /** * @brief Send a digicam_configure message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_control.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_control.h index 7fa8cdfef8..bfa5414a39 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_control.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_control.h @@ -104,7 +104,7 @@ static inline uint16_t mavlink_msg_digicam_control_pack(uint8_t system_id, uint8 * @brief Pack a digicam_control message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -161,7 +161,7 @@ static inline uint16_t mavlink_msg_digicam_control_pack_chan(uint8_t system_id, } /** - * @brief Encode a digicam_control struct into a message + * @brief Encode a digicam_control struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -173,6 +173,20 @@ static inline uint16_t mavlink_msg_digicam_control_encode(uint8_t system_id, uin return mavlink_msg_digicam_control_pack(system_id, component_id, msg, digicam_control->target_system, digicam_control->target_component, digicam_control->session, digicam_control->zoom_pos, digicam_control->zoom_step, digicam_control->focus_lock, digicam_control->shot, digicam_control->command_id, digicam_control->extra_param, digicam_control->extra_value); } +/** + * @brief Encode a digicam_control struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param digicam_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_digicam_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_digicam_control_t* digicam_control) +{ + return mavlink_msg_digicam_control_pack_chan(system_id, component_id, chan, msg, digicam_control->target_system, digicam_control->target_component, digicam_control->session, digicam_control->zoom_pos, digicam_control->zoom_step, digicam_control->focus_lock, digicam_control->shot, digicam_control->command_id, digicam_control->extra_param, digicam_control->extra_value); +} + /** * @brief Send a digicam_control message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_fetch_point.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_fetch_point.h index 2cd4fc798c..fe3677d53d 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_fetch_point.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_fetch_point.h @@ -69,7 +69,7 @@ static inline uint16_t mavlink_msg_fence_fetch_point_pack(uint8_t system_id, uin * @brief Pack a fence_fetch_point message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -105,7 +105,7 @@ static inline uint16_t mavlink_msg_fence_fetch_point_pack_chan(uint8_t system_id } /** - * @brief Encode a fence_fetch_point struct into a message + * @brief Encode a fence_fetch_point struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -117,6 +117,20 @@ static inline uint16_t mavlink_msg_fence_fetch_point_encode(uint8_t system_id, u return mavlink_msg_fence_fetch_point_pack(system_id, component_id, msg, fence_fetch_point->target_system, fence_fetch_point->target_component, fence_fetch_point->idx); } +/** + * @brief Encode a fence_fetch_point struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param fence_fetch_point C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_fence_fetch_point_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_fence_fetch_point_t* fence_fetch_point) +{ + return mavlink_msg_fence_fetch_point_pack_chan(system_id, component_id, chan, msg, fence_fetch_point->target_system, fence_fetch_point->target_component, fence_fetch_point->idx); +} + /** * @brief Send a fence_fetch_point message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_point.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_point.h index b3c4706ee0..febda6cdc5 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_point.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_point.h @@ -84,7 +84,7 @@ static inline uint16_t mavlink_msg_fence_point_pack(uint8_t system_id, uint8_t c * @brief Pack a fence_point message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_fence_point_pack_chan(uint8_t system_id, uint } /** - * @brief Encode a fence_point struct into a message + * @brief Encode a fence_point struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -141,6 +141,20 @@ static inline uint16_t mavlink_msg_fence_point_encode(uint8_t system_id, uint8_t return mavlink_msg_fence_point_pack(system_id, component_id, msg, fence_point->target_system, fence_point->target_component, fence_point->idx, fence_point->count, fence_point->lat, fence_point->lng); } +/** + * @brief Encode a fence_point struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param fence_point C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_fence_point_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_fence_point_t* fence_point) +{ + return mavlink_msg_fence_point_pack_chan(system_id, component_id, chan, msg, fence_point->target_system, fence_point->target_component, fence_point->idx, fence_point->count, fence_point->lat, fence_point->lng); +} + /** * @brief Send a fence_point message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_status.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_status.h index 32f2bc03a8..6120904061 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_status.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_status.h @@ -74,7 +74,7 @@ static inline uint16_t mavlink_msg_fence_status_pack(uint8_t system_id, uint8_t * @brief Pack a fence_status message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param breach_status 0 if currently inside fence, 1 if outside * @param breach_count number of fence breaches @@ -113,7 +113,7 @@ static inline uint16_t mavlink_msg_fence_status_pack_chan(uint8_t system_id, uin } /** - * @brief Encode a fence_status struct into a message + * @brief Encode a fence_status struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -125,6 +125,20 @@ static inline uint16_t mavlink_msg_fence_status_encode(uint8_t system_id, uint8_ return mavlink_msg_fence_status_pack(system_id, component_id, msg, fence_status->breach_status, fence_status->breach_count, fence_status->breach_type, fence_status->breach_time); } +/** + * @brief Encode a fence_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param fence_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_fence_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_fence_status_t* fence_status) +{ + return mavlink_msg_fence_status_pack_chan(system_id, component_id, chan, msg, fence_status->breach_status, fence_status->breach_count, fence_status->breach_type, fence_status->breach_time); +} + /** * @brief Send a fence_status message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_hwstatus.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_hwstatus.h index 73870ec0fb..2f5dea513a 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_hwstatus.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_hwstatus.h @@ -64,7 +64,7 @@ static inline uint16_t mavlink_msg_hwstatus_pack(uint8_t system_id, uint8_t comp * @brief Pack a hwstatus message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param Vcc board voltage (mV) * @param I2Cerr I2C error count @@ -97,7 +97,7 @@ static inline uint16_t mavlink_msg_hwstatus_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a hwstatus struct into a message + * @brief Encode a hwstatus struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -109,6 +109,20 @@ static inline uint16_t mavlink_msg_hwstatus_encode(uint8_t system_id, uint8_t co return mavlink_msg_hwstatus_pack(system_id, component_id, msg, hwstatus->Vcc, hwstatus->I2Cerr); } +/** + * @brief Encode a hwstatus struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param hwstatus C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hwstatus_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hwstatus_t* hwstatus) +{ + return mavlink_msg_hwstatus_pack_chan(system_id, component_id, chan, msg, hwstatus->Vcc, hwstatus->I2Cerr); +} + /** * @brief Send a hwstatus message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_limits_status.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_limits_status.h index f7b04fba71..34743fd021 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_limits_status.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_limits_status.h @@ -99,7 +99,7 @@ static inline uint16_t mavlink_msg_limits_status_pack(uint8_t system_id, uint8_t * @brief Pack a limits_status message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param limits_state state of AP_Limits, (see enum LimitState, LIMITS_STATE) * @param last_trigger time of last breach in milliseconds since boot @@ -153,7 +153,7 @@ static inline uint16_t mavlink_msg_limits_status_pack_chan(uint8_t system_id, ui } /** - * @brief Encode a limits_status struct into a message + * @brief Encode a limits_status struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -165,6 +165,20 @@ static inline uint16_t mavlink_msg_limits_status_encode(uint8_t system_id, uint8 return mavlink_msg_limits_status_pack(system_id, component_id, msg, limits_status->limits_state, limits_status->last_trigger, limits_status->last_action, limits_status->last_recovery, limits_status->last_clear, limits_status->breach_count, limits_status->mods_enabled, limits_status->mods_required, limits_status->mods_triggered); } +/** + * @brief Encode a limits_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param limits_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_limits_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_limits_status_t* limits_status) +{ + return mavlink_msg_limits_status_pack_chan(system_id, component_id, chan, msg, limits_status->limits_state, limits_status->last_trigger, limits_status->last_action, limits_status->last_recovery, limits_status->last_clear, limits_status->breach_count, limits_status->mods_enabled, limits_status->mods_required, limits_status->mods_triggered); +} + /** * @brief Send a limits_status message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_meminfo.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_meminfo.h index 437029eed7..55f772bbc5 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_meminfo.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_meminfo.h @@ -64,7 +64,7 @@ static inline uint16_t mavlink_msg_meminfo_pack(uint8_t system_id, uint8_t compo * @brief Pack a meminfo message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param brkval heap top * @param freemem free memory @@ -97,7 +97,7 @@ static inline uint16_t mavlink_msg_meminfo_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a meminfo struct into a message + * @brief Encode a meminfo struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -109,6 +109,20 @@ static inline uint16_t mavlink_msg_meminfo_encode(uint8_t system_id, uint8_t com return mavlink_msg_meminfo_pack(system_id, component_id, msg, meminfo->brkval, meminfo->freemem); } +/** + * @brief Encode a meminfo struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param meminfo C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_meminfo_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_meminfo_t* meminfo) +{ + return mavlink_msg_meminfo_pack_chan(system_id, component_id, chan, msg, meminfo->brkval, meminfo->freemem); +} + /** * @brief Send a meminfo message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_configure.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_configure.h index 450153c6fe..de717dfa4d 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_configure.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_configure.h @@ -84,7 +84,7 @@ static inline uint16_t mavlink_msg_mount_configure_pack(uint8_t system_id, uint8 * @brief Pack a mount_configure message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_mount_configure_pack_chan(uint8_t system_id, } /** - * @brief Encode a mount_configure struct into a message + * @brief Encode a mount_configure struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -141,6 +141,20 @@ static inline uint16_t mavlink_msg_mount_configure_encode(uint8_t system_id, uin return mavlink_msg_mount_configure_pack(system_id, component_id, msg, mount_configure->target_system, mount_configure->target_component, mount_configure->mount_mode, mount_configure->stab_roll, mount_configure->stab_pitch, mount_configure->stab_yaw); } +/** + * @brief Encode a mount_configure struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mount_configure C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mount_configure_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mount_configure_t* mount_configure) +{ + return mavlink_msg_mount_configure_pack_chan(system_id, component_id, chan, msg, mount_configure->target_system, mount_configure->target_component, mount_configure->mount_mode, mount_configure->stab_roll, mount_configure->stab_pitch, mount_configure->stab_yaw); +} + /** * @brief Send a mount_configure message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_control.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_control.h index 5b83d7e970..44416353ed 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_control.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_control.h @@ -84,7 +84,7 @@ static inline uint16_t mavlink_msg_mount_control_pack(uint8_t system_id, uint8_t * @brief Pack a mount_control message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_mount_control_pack_chan(uint8_t system_id, ui } /** - * @brief Encode a mount_control struct into a message + * @brief Encode a mount_control struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -141,6 +141,20 @@ static inline uint16_t mavlink_msg_mount_control_encode(uint8_t system_id, uint8 return mavlink_msg_mount_control_pack(system_id, component_id, msg, mount_control->target_system, mount_control->target_component, mount_control->input_a, mount_control->input_b, mount_control->input_c, mount_control->save_position); } +/** + * @brief Encode a mount_control struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mount_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mount_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mount_control_t* mount_control) +{ + return mavlink_msg_mount_control_pack_chan(system_id, component_id, chan, msg, mount_control->target_system, mount_control->target_component, mount_control->input_a, mount_control->input_b, mount_control->input_c, mount_control->save_position); +} + /** * @brief Send a mount_control message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_status.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_status.h index c031db42b4..4905905dc5 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_status.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_status.h @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_mount_status_pack(uint8_t system_id, uint8_t * @brief Pack a mount_status message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -121,7 +121,7 @@ static inline uint16_t mavlink_msg_mount_status_pack_chan(uint8_t system_id, uin } /** - * @brief Encode a mount_status struct into a message + * @brief Encode a mount_status struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -133,6 +133,20 @@ static inline uint16_t mavlink_msg_mount_status_encode(uint8_t system_id, uint8_ return mavlink_msg_mount_status_pack(system_id, component_id, msg, mount_status->target_system, mount_status->target_component, mount_status->pointing_a, mount_status->pointing_b, mount_status->pointing_c); } +/** + * @brief Encode a mount_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mount_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mount_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mount_status_t* mount_status) +{ + return mavlink_msg_mount_status_pack_chan(system_id, component_id, chan, msg, mount_status->target_system, mount_status->target_component, mount_status->pointing_a, mount_status->pointing_b, mount_status->pointing_c); +} + /** * @brief Send a mount_status message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_radio.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_radio.h index e13776993e..8e9740e828 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_radio.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_radio.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_radio_pack(uint8_t system_id, uint8_t compone * @brief Pack a radio message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param rssi local signal strength * @param remrssi remote signal strength @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_radio_pack_chan(uint8_t system_id, uint8_t co } /** - * @brief Encode a radio struct into a message + * @brief Encode a radio struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -149,6 +149,20 @@ static inline uint16_t mavlink_msg_radio_encode(uint8_t system_id, uint8_t compo return mavlink_msg_radio_pack(system_id, component_id, msg, radio->rssi, radio->remrssi, radio->txbuf, radio->noise, radio->remnoise, radio->rxerrors, radio->fixed); } +/** + * @brief Encode a radio struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param radio C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_radio_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_radio_t* radio) +{ + return mavlink_msg_radio_pack_chan(system_id, component_id, chan, msg, radio->rssi, radio->remrssi, radio->txbuf, radio->noise, radio->remnoise, radio->rxerrors, radio->fixed); +} + /** * @brief Send a radio message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rangefinder.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rangefinder.h index d88abe36a4..c476447a87 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rangefinder.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rangefinder.h @@ -64,7 +64,7 @@ static inline uint16_t mavlink_msg_rangefinder_pack(uint8_t system_id, uint8_t c * @brief Pack a rangefinder message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param distance distance in meters * @param voltage raw voltage if available, zero otherwise @@ -97,7 +97,7 @@ static inline uint16_t mavlink_msg_rangefinder_pack_chan(uint8_t system_id, uint } /** - * @brief Encode a rangefinder struct into a message + * @brief Encode a rangefinder struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -109,6 +109,20 @@ static inline uint16_t mavlink_msg_rangefinder_encode(uint8_t system_id, uint8_t return mavlink_msg_rangefinder_pack(system_id, component_id, msg, rangefinder->distance, rangefinder->voltage); } +/** + * @brief Encode a rangefinder struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param rangefinder C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rangefinder_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rangefinder_t* rangefinder) +{ + return mavlink_msg_rangefinder_pack_chan(system_id, component_id, chan, msg, rangefinder->distance, rangefinder->voltage); +} + /** * @brief Send a rangefinder message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_sensor_offsets.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_sensor_offsets.h index d121e48e66..31b7d989de 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_sensor_offsets.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_sensor_offsets.h @@ -114,7 +114,7 @@ static inline uint16_t mavlink_msg_sensor_offsets_pack(uint8_t system_id, uint8_ * @brief Pack a sensor_offsets message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param mag_ofs_x magnetometer X offset * @param mag_ofs_y magnetometer Y offset @@ -177,7 +177,7 @@ static inline uint16_t mavlink_msg_sensor_offsets_pack_chan(uint8_t system_id, u } /** - * @brief Encode a sensor_offsets struct into a message + * @brief Encode a sensor_offsets struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -189,6 +189,20 @@ static inline uint16_t mavlink_msg_sensor_offsets_encode(uint8_t system_id, uint return mavlink_msg_sensor_offsets_pack(system_id, component_id, msg, sensor_offsets->mag_ofs_x, sensor_offsets->mag_ofs_y, sensor_offsets->mag_ofs_z, sensor_offsets->mag_declination, sensor_offsets->raw_press, sensor_offsets->raw_temp, sensor_offsets->gyro_cal_x, sensor_offsets->gyro_cal_y, sensor_offsets->gyro_cal_z, sensor_offsets->accel_cal_x, sensor_offsets->accel_cal_y, sensor_offsets->accel_cal_z); } +/** + * @brief Encode a sensor_offsets struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param sensor_offsets C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_sensor_offsets_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_sensor_offsets_t* sensor_offsets) +{ + return mavlink_msg_sensor_offsets_pack_chan(system_id, component_id, chan, msg, sensor_offsets->mag_ofs_x, sensor_offsets->mag_ofs_y, sensor_offsets->mag_ofs_z, sensor_offsets->mag_declination, sensor_offsets->raw_press, sensor_offsets->raw_temp, sensor_offsets->gyro_cal_x, sensor_offsets->gyro_cal_y, sensor_offsets->gyro_cal_z, sensor_offsets->accel_cal_x, sensor_offsets->accel_cal_y, sensor_offsets->accel_cal_z); +} + /** * @brief Send a sensor_offsets message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_set_mag_offsets.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_set_mag_offsets.h index a59e906494..b2c629c39a 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_set_mag_offsets.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_set_mag_offsets.h @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_set_mag_offsets_pack(uint8_t system_id, uint8 * @brief Pack a set_mag_offsets message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -121,7 +121,7 @@ static inline uint16_t mavlink_msg_set_mag_offsets_pack_chan(uint8_t system_id, } /** - * @brief Encode a set_mag_offsets struct into a message + * @brief Encode a set_mag_offsets struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -133,6 +133,20 @@ static inline uint16_t mavlink_msg_set_mag_offsets_encode(uint8_t system_id, uin return mavlink_msg_set_mag_offsets_pack(system_id, component_id, msg, set_mag_offsets->target_system, set_mag_offsets->target_component, set_mag_offsets->mag_ofs_x, set_mag_offsets->mag_ofs_y, set_mag_offsets->mag_ofs_z); } +/** + * @brief Encode a set_mag_offsets struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_mag_offsets C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_mag_offsets_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_mag_offsets_t* set_mag_offsets) +{ + return mavlink_msg_set_mag_offsets_pack_chan(system_id, component_id, chan, msg, set_mag_offsets->target_system, set_mag_offsets->target_component, set_mag_offsets->mag_ofs_x, set_mag_offsets->mag_ofs_y, set_mag_offsets->mag_ofs_z); +} + /** * @brief Send a set_mag_offsets message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_simstate.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_simstate.h index 7373c8bffe..8ef44f76d5 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_simstate.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_simstate.h @@ -13,15 +13,15 @@ typedef struct __mavlink_simstate_t float xgyro; ///< Angular speed around X axis rad/s float ygyro; ///< Angular speed around Y axis rad/s float zgyro; ///< Angular speed around Z axis rad/s - float lat; ///< Latitude in degrees - float lng; ///< Longitude in degrees + int32_t lat; ///< Latitude in degrees * 1E7 + int32_t lng; ///< Longitude in degrees * 1E7 } mavlink_simstate_t; #define MAVLINK_MSG_ID_SIMSTATE_LEN 44 #define MAVLINK_MSG_ID_164_LEN 44 -#define MAVLINK_MSG_ID_SIMSTATE_CRC 111 -#define MAVLINK_MSG_ID_164_CRC 111 +#define MAVLINK_MSG_ID_SIMSTATE_CRC 154 +#define MAVLINK_MSG_ID_164_CRC 154 @@ -37,8 +37,8 @@ typedef struct __mavlink_simstate_t { "xgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_simstate_t, xgyro) }, \ { "ygyro", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_simstate_t, ygyro) }, \ { "zgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_simstate_t, zgyro) }, \ - { "lat", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_simstate_t, lat) }, \ - { "lng", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_simstate_t, lng) }, \ + { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 36, offsetof(mavlink_simstate_t, lat) }, \ + { "lng", NULL, MAVLINK_TYPE_INT32_T, 0, 40, offsetof(mavlink_simstate_t, lng) }, \ } \ } @@ -58,12 +58,12 @@ typedef struct __mavlink_simstate_t * @param xgyro Angular speed around X axis rad/s * @param ygyro Angular speed around Y axis rad/s * @param zgyro Angular speed around Z axis rad/s - * @param lat Latitude in degrees - * @param lng Longitude in degrees + * @param lat Latitude in degrees * 1E7 + * @param lng Longitude in degrees * 1E7 * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_simstate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float roll, float pitch, float yaw, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float lat, float lng) + float roll, float pitch, float yaw, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, int32_t lat, int32_t lng) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SIMSTATE_LEN]; @@ -76,8 +76,8 @@ static inline uint16_t mavlink_msg_simstate_pack(uint8_t system_id, uint8_t comp _mav_put_float(buf, 24, xgyro); _mav_put_float(buf, 28, ygyro); _mav_put_float(buf, 32, zgyro); - _mav_put_float(buf, 36, lat); - _mav_put_float(buf, 40, lng); + _mav_put_int32_t(buf, 36, lat); + _mav_put_int32_t(buf, 40, lng); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SIMSTATE_LEN); #else @@ -109,7 +109,7 @@ static inline uint16_t mavlink_msg_simstate_pack(uint8_t system_id, uint8_t comp * @brief Pack a simstate message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param roll Roll angle (rad) * @param pitch Pitch angle (rad) @@ -120,13 +120,13 @@ static inline uint16_t mavlink_msg_simstate_pack(uint8_t system_id, uint8_t comp * @param xgyro Angular speed around X axis rad/s * @param ygyro Angular speed around Y axis rad/s * @param zgyro Angular speed around Z axis rad/s - * @param lat Latitude in degrees - * @param lng Longitude in degrees + * @param lat Latitude in degrees * 1E7 + * @param lng Longitude in degrees * 1E7 * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_simstate_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - float roll,float pitch,float yaw,float xacc,float yacc,float zacc,float xgyro,float ygyro,float zgyro,float lat,float lng) + float roll,float pitch,float yaw,float xacc,float yacc,float zacc,float xgyro,float ygyro,float zgyro,int32_t lat,int32_t lng) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SIMSTATE_LEN]; @@ -139,8 +139,8 @@ static inline uint16_t mavlink_msg_simstate_pack_chan(uint8_t system_id, uint8_t _mav_put_float(buf, 24, xgyro); _mav_put_float(buf, 28, ygyro); _mav_put_float(buf, 32, zgyro); - _mav_put_float(buf, 36, lat); - _mav_put_float(buf, 40, lng); + _mav_put_int32_t(buf, 36, lat); + _mav_put_int32_t(buf, 40, lng); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SIMSTATE_LEN); #else @@ -169,7 +169,7 @@ static inline uint16_t mavlink_msg_simstate_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a simstate struct into a message + * @brief Encode a simstate struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -181,6 +181,20 @@ static inline uint16_t mavlink_msg_simstate_encode(uint8_t system_id, uint8_t co return mavlink_msg_simstate_pack(system_id, component_id, msg, simstate->roll, simstate->pitch, simstate->yaw, simstate->xacc, simstate->yacc, simstate->zacc, simstate->xgyro, simstate->ygyro, simstate->zgyro, simstate->lat, simstate->lng); } +/** + * @brief Encode a simstate struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param simstate C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_simstate_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_simstate_t* simstate) +{ + return mavlink_msg_simstate_pack_chan(system_id, component_id, chan, msg, simstate->roll, simstate->pitch, simstate->yaw, simstate->xacc, simstate->yacc, simstate->zacc, simstate->xgyro, simstate->ygyro, simstate->zgyro, simstate->lat, simstate->lng); +} + /** * @brief Send a simstate message * @param chan MAVLink channel to send the message @@ -194,12 +208,12 @@ static inline uint16_t mavlink_msg_simstate_encode(uint8_t system_id, uint8_t co * @param xgyro Angular speed around X axis rad/s * @param ygyro Angular speed around Y axis rad/s * @param zgyro Angular speed around Z axis rad/s - * @param lat Latitude in degrees - * @param lng Longitude in degrees + * @param lat Latitude in degrees * 1E7 + * @param lng Longitude in degrees * 1E7 */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_simstate_send(mavlink_channel_t chan, float roll, float pitch, float yaw, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float lat, float lng) +static inline void mavlink_msg_simstate_send(mavlink_channel_t chan, float roll, float pitch, float yaw, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, int32_t lat, int32_t lng) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SIMSTATE_LEN]; @@ -212,8 +226,8 @@ static inline void mavlink_msg_simstate_send(mavlink_channel_t chan, float roll, _mav_put_float(buf, 24, xgyro); _mav_put_float(buf, 28, ygyro); _mav_put_float(buf, 32, zgyro); - _mav_put_float(buf, 36, lat); - _mav_put_float(buf, 40, lng); + _mav_put_int32_t(buf, 36, lat); + _mav_put_int32_t(buf, 40, lng); #if MAVLINK_CRC_EXTRA _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIMSTATE, buf, MAVLINK_MSG_ID_SIMSTATE_LEN, MAVLINK_MSG_ID_SIMSTATE_CRC); @@ -340,21 +354,21 @@ static inline float mavlink_msg_simstate_get_zgyro(const mavlink_message_t* msg) /** * @brief Get field lat from simstate message * - * @return Latitude in degrees + * @return Latitude in degrees * 1E7 */ -static inline float mavlink_msg_simstate_get_lat(const mavlink_message_t* msg) +static inline int32_t mavlink_msg_simstate_get_lat(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 36); + return _MAV_RETURN_int32_t(msg, 36); } /** * @brief Get field lng from simstate message * - * @return Longitude in degrees + * @return Longitude in degrees * 1E7 */ -static inline float mavlink_msg_simstate_get_lng(const mavlink_message_t* msg) +static inline int32_t mavlink_msg_simstate_get_lng(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 40); + return _MAV_RETURN_int32_t(msg, 40); } /** diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_wind.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_wind.h index ebdd2949d9..7608a7bd10 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_wind.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_wind.h @@ -69,7 +69,7 @@ static inline uint16_t mavlink_msg_wind_pack(uint8_t system_id, uint8_t componen * @brief Pack a wind message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param direction wind direction that wind is coming from (degrees) * @param speed wind speed in ground plane (m/s) @@ -105,7 +105,7 @@ static inline uint16_t mavlink_msg_wind_pack_chan(uint8_t system_id, uint8_t com } /** - * @brief Encode a wind struct into a message + * @brief Encode a wind struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -117,6 +117,20 @@ static inline uint16_t mavlink_msg_wind_encode(uint8_t system_id, uint8_t compon return mavlink_msg_wind_pack(system_id, component_id, msg, wind->direction, wind->speed, wind->speed_z); } +/** + * @brief Encode a wind struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param wind C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_wind_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_wind_t* wind) +{ + return mavlink_msg_wind_pack_chan(system_id, component_id, chan, msg, wind->direction, wind->speed, wind->speed_z); +} + /** * @brief Send a wind message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h b/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h index 07bf19ee07..2c9cfef3dc 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h @@ -738,8 +738,8 @@ static void mavlink_test_simstate(uint8_t system_id, uint8_t component_id, mavli 185.0, 213.0, 241.0, - 269.0, - 297.0, + 963499336, + 963499544, }; mavlink_simstate_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -1225,6 +1225,71 @@ static void mavlink_test_rangefinder(uint8_t system_id, uint8_t component_id, ma MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } +static void mavlink_test_airspeed_autocal(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_airspeed_autocal_t packet_in = { + 17.0, + 45.0, + 73.0, + 101.0, + 129.0, + 157.0, + 185.0, + 213.0, + 241.0, + 269.0, + 297.0, + 325.0, + }; + mavlink_airspeed_autocal_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.vx = packet_in.vx; + packet1.vy = packet_in.vy; + packet1.vz = packet_in.vz; + packet1.diff_pressure = packet_in.diff_pressure; + packet1.EAS2TAS = packet_in.EAS2TAS; + packet1.ratio = packet_in.ratio; + packet1.state_x = packet_in.state_x; + packet1.state_y = packet_in.state_y; + packet1.state_z = packet_in.state_z; + packet1.Pax = packet_in.Pax; + packet1.Pby = packet_in.Pby; + packet1.Pcz = packet_in.Pcz; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_airspeed_autocal_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_airspeed_autocal_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_airspeed_autocal_pack(system_id, component_id, &msg , packet1.vx , packet1.vy , packet1.vz , packet1.diff_pressure , packet1.EAS2TAS , packet1.ratio , packet1.state_x , packet1.state_y , packet1.state_z , packet1.Pax , packet1.Pby , packet1.Pcz ); + mavlink_msg_airspeed_autocal_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_airspeed_autocal_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.vx , packet1.vy , packet1.vz , packet1.diff_pressure , packet1.EAS2TAS , packet1.ratio , packet1.state_x , packet1.state_y , packet1.state_z , packet1.Pax , packet1.Pby , packet1.Pcz ); + mavlink_msg_airspeed_autocal_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ @@ -71,7 +96,8 @@ enum MAV_CMD MAV_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ MAV_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ MAV_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ - MAV_CMD_ENUM_END=401, /* | */ + MAV_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ + MAV_CMD_ENUM_END=501, /* | */ }; #endif diff --git a/mavlink/include/mavlink/v1.0/autoquad/mavlink_msg_aq_telemetry_f.h b/mavlink/include/mavlink/v1.0/autoquad/mavlink_msg_aq_telemetry_f.h index dabccdf733..ed7c86bcb8 100644 --- a/mavlink/include/mavlink/v1.0/autoquad/mavlink_msg_aq_telemetry_f.h +++ b/mavlink/include/mavlink/v1.0/autoquad/mavlink_msg_aq_telemetry_f.h @@ -159,7 +159,7 @@ static inline uint16_t mavlink_msg_aq_telemetry_f_pack(uint8_t system_id, uint8_ * @brief Pack a aq_telemetry_f message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param Index Index of message * @param value1 value1 @@ -249,7 +249,7 @@ static inline uint16_t mavlink_msg_aq_telemetry_f_pack_chan(uint8_t system_id, u } /** - * @brief Encode a aq_telemetry_f struct into a message + * @brief Encode a aq_telemetry_f struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -261,6 +261,20 @@ static inline uint16_t mavlink_msg_aq_telemetry_f_encode(uint8_t system_id, uint return mavlink_msg_aq_telemetry_f_pack(system_id, component_id, msg, aq_telemetry_f->Index, aq_telemetry_f->value1, aq_telemetry_f->value2, aq_telemetry_f->value3, aq_telemetry_f->value4, aq_telemetry_f->value5, aq_telemetry_f->value6, aq_telemetry_f->value7, aq_telemetry_f->value8, aq_telemetry_f->value9, aq_telemetry_f->value10, aq_telemetry_f->value11, aq_telemetry_f->value12, aq_telemetry_f->value13, aq_telemetry_f->value14, aq_telemetry_f->value15, aq_telemetry_f->value16, aq_telemetry_f->value17, aq_telemetry_f->value18, aq_telemetry_f->value19, aq_telemetry_f->value20); } +/** + * @brief Encode a aq_telemetry_f struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param aq_telemetry_f C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_aq_telemetry_f_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_aq_telemetry_f_t* aq_telemetry_f) +{ + return mavlink_msg_aq_telemetry_f_pack_chan(system_id, component_id, chan, msg, aq_telemetry_f->Index, aq_telemetry_f->value1, aq_telemetry_f->value2, aq_telemetry_f->value3, aq_telemetry_f->value4, aq_telemetry_f->value5, aq_telemetry_f->value6, aq_telemetry_f->value7, aq_telemetry_f->value8, aq_telemetry_f->value9, aq_telemetry_f->value10, aq_telemetry_f->value11, aq_telemetry_f->value12, aq_telemetry_f->value13, aq_telemetry_f->value14, aq_telemetry_f->value15, aq_telemetry_f->value16, aq_telemetry_f->value17, aq_telemetry_f->value18, aq_telemetry_f->value19, aq_telemetry_f->value20); +} + /** * @brief Send a aq_telemetry_f message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/autoquad/version.h b/mavlink/include/mavlink/v1.0/autoquad/version.h index a26ce2e909..d8ba74f03c 100644 --- a/mavlink/include/mavlink/v1.0/autoquad/version.h +++ b/mavlink/include/mavlink/v1.0/autoquad/version.h @@ -5,7 +5,7 @@ #ifndef MAVLINK_VERSION_H #define MAVLINK_VERSION_H -#define MAVLINK_BUILD_DATE "Thu Jul 4 13:11:37 2013" +#define MAVLINK_BUILD_DATE "Tue Sep 10 23:49:36 2013" #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 254 diff --git a/mavlink/include/mavlink/v1.0/common/common.h b/mavlink/include/mavlink/v1.0/common/common.h index e2ab66ed1c..9d3db33265 100644 --- a/mavlink/include/mavlink/v1.0/common/common.h +++ b/mavlink/include/mavlink/v1.0/common/common.h @@ -260,6 +260,7 @@ enum MAV_CMD MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ @@ -268,7 +269,8 @@ enum MAV_CMD MAV_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ MAV_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ MAV_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ - MAV_CMD_ENUM_END=401, /* | */ + MAV_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ + MAV_CMD_ENUM_END=501, /* | */ }; #endif diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude.h index 0ba416ee14..8ddf5bf099 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_attitude_pack(uint8_t system_id, uint8_t comp * @brief Pack a attitude message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp (milliseconds since system boot) * @param roll Roll angle (rad, -pi..+pi) @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_attitude_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a attitude struct into a message + * @brief Encode a attitude struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -149,6 +149,20 @@ static inline uint16_t mavlink_msg_attitude_encode(uint8_t system_id, uint8_t co return mavlink_msg_attitude_pack(system_id, component_id, msg, attitude->time_boot_ms, attitude->roll, attitude->pitch, attitude->yaw, attitude->rollspeed, attitude->pitchspeed, attitude->yawspeed); } +/** + * @brief Encode a attitude struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param attitude C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_attitude_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_attitude_t* attitude) +{ + return mavlink_msg_attitude_pack_chan(system_id, component_id, chan, msg, attitude->time_boot_ms, attitude->roll, attitude->pitch, attitude->yaw, attitude->rollspeed, attitude->pitchspeed, attitude->yawspeed); +} + /** * @brief Send a attitude message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_quaternion.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_quaternion.h index 611803f2b7..9f8d587598 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_quaternion.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_quaternion.h @@ -94,7 +94,7 @@ static inline uint16_t mavlink_msg_attitude_quaternion_pack(uint8_t system_id, u * @brief Pack a attitude_quaternion message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp (milliseconds since system boot) * @param q1 Quaternion component 1 @@ -145,7 +145,7 @@ static inline uint16_t mavlink_msg_attitude_quaternion_pack_chan(uint8_t system_ } /** - * @brief Encode a attitude_quaternion struct into a message + * @brief Encode a attitude_quaternion struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -157,6 +157,20 @@ static inline uint16_t mavlink_msg_attitude_quaternion_encode(uint8_t system_id, return mavlink_msg_attitude_quaternion_pack(system_id, component_id, msg, attitude_quaternion->time_boot_ms, attitude_quaternion->q1, attitude_quaternion->q2, attitude_quaternion->q3, attitude_quaternion->q4, attitude_quaternion->rollspeed, attitude_quaternion->pitchspeed, attitude_quaternion->yawspeed); } +/** + * @brief Encode a attitude_quaternion struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param attitude_quaternion C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_attitude_quaternion_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_attitude_quaternion_t* attitude_quaternion) +{ + return mavlink_msg_attitude_quaternion_pack_chan(system_id, component_id, chan, msg, attitude_quaternion->time_boot_ms, attitude_quaternion->q1, attitude_quaternion->q2, attitude_quaternion->q3, attitude_quaternion->q4, attitude_quaternion->rollspeed, attitude_quaternion->pitchspeed, attitude_quaternion->yawspeed); +} + /** * @brief Send a attitude_quaternion message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_auth_key.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_auth_key.h index 030b564c97..5703a59871 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_auth_key.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_auth_key.h @@ -59,7 +59,7 @@ static inline uint16_t mavlink_msg_auth_key_pack(uint8_t system_id, uint8_t comp * @brief Pack a auth_key message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param key key * @return length of the message in bytes (excluding serial stream start sign) @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_auth_key_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a auth_key struct into a message + * @brief Encode a auth_key struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -101,6 +101,20 @@ static inline uint16_t mavlink_msg_auth_key_encode(uint8_t system_id, uint8_t co return mavlink_msg_auth_key_pack(system_id, component_id, msg, auth_key->key); } +/** + * @brief Encode a auth_key struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param auth_key C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_auth_key_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_auth_key_t* auth_key) +{ + return mavlink_msg_auth_key_pack_chan(system_id, component_id, chan, msg, auth_key->key); +} + /** * @brief Send a auth_key message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_battery_status.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_battery_status.h index 83c8159841..c945aebaea 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_battery_status.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_battery_status.h @@ -99,7 +99,7 @@ static inline uint16_t mavlink_msg_battery_status_pack(uint8_t system_id, uint8_ * @brief Pack a battery_status message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param accu_id Accupack ID * @param voltage_cell_1 Battery voltage of cell 1, in millivolts (1 = 1 millivolt) @@ -153,7 +153,7 @@ static inline uint16_t mavlink_msg_battery_status_pack_chan(uint8_t system_id, u } /** - * @brief Encode a battery_status struct into a message + * @brief Encode a battery_status struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -165,6 +165,20 @@ static inline uint16_t mavlink_msg_battery_status_encode(uint8_t system_id, uint return mavlink_msg_battery_status_pack(system_id, component_id, msg, battery_status->accu_id, battery_status->voltage_cell_1, battery_status->voltage_cell_2, battery_status->voltage_cell_3, battery_status->voltage_cell_4, battery_status->voltage_cell_5, battery_status->voltage_cell_6, battery_status->current_battery, battery_status->battery_remaining); } +/** + * @brief Encode a battery_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param battery_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_battery_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_battery_status_t* battery_status) +{ + return mavlink_msg_battery_status_pack_chan(system_id, component_id, chan, msg, battery_status->accu_id, battery_status->voltage_cell_1, battery_status->voltage_cell_2, battery_status->voltage_cell_3, battery_status->voltage_cell_4, battery_status->voltage_cell_5, battery_status->voltage_cell_6, battery_status->current_battery, battery_status->battery_remaining); +} + /** * @brief Send a battery_status message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control.h index c7195dfcaa..0b6de930d2 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control.h @@ -72,7 +72,7 @@ static inline uint16_t mavlink_msg_change_operator_control_pack(uint8_t system_i * @brief Pack a change_operator_control message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System the GCS requests control for * @param control_request 0: request control of this MAV, 1: Release control of this MAV @@ -109,7 +109,7 @@ static inline uint16_t mavlink_msg_change_operator_control_pack_chan(uint8_t sys } /** - * @brief Encode a change_operator_control struct into a message + * @brief Encode a change_operator_control struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -121,6 +121,20 @@ static inline uint16_t mavlink_msg_change_operator_control_encode(uint8_t system return mavlink_msg_change_operator_control_pack(system_id, component_id, msg, change_operator_control->target_system, change_operator_control->control_request, change_operator_control->version, change_operator_control->passkey); } +/** + * @brief Encode a change_operator_control struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param change_operator_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_change_operator_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_change_operator_control_t* change_operator_control) +{ + return mavlink_msg_change_operator_control_pack_chan(system_id, component_id, chan, msg, change_operator_control->target_system, change_operator_control->control_request, change_operator_control->version, change_operator_control->passkey); +} + /** * @brief Send a change_operator_control message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control_ack.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control_ack.h index 5cf98e77fe..c6f6a28e4b 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control_ack.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control_ack.h @@ -69,7 +69,7 @@ static inline uint16_t mavlink_msg_change_operator_control_ack_pack(uint8_t syst * @brief Pack a change_operator_control_ack message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param gcs_system_id ID of the GCS this message * @param control_request 0: request control of this MAV, 1: Release control of this MAV @@ -105,7 +105,7 @@ static inline uint16_t mavlink_msg_change_operator_control_ack_pack_chan(uint8_t } /** - * @brief Encode a change_operator_control_ack struct into a message + * @brief Encode a change_operator_control_ack struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -117,6 +117,20 @@ static inline uint16_t mavlink_msg_change_operator_control_ack_encode(uint8_t sy return mavlink_msg_change_operator_control_ack_pack(system_id, component_id, msg, change_operator_control_ack->gcs_system_id, change_operator_control_ack->control_request, change_operator_control_ack->ack); } +/** + * @brief Encode a change_operator_control_ack struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param change_operator_control_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_change_operator_control_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_change_operator_control_ack_t* change_operator_control_ack) +{ + return mavlink_msg_change_operator_control_ack_pack_chan(system_id, component_id, chan, msg, change_operator_control_ack->gcs_system_id, change_operator_control_ack->control_request, change_operator_control_ack->ack); +} + /** * @brief Send a change_operator_control_ack message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_ack.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_ack.h index 82c2835de0..dca2fe6819 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_ack.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_ack.h @@ -64,7 +64,7 @@ static inline uint16_t mavlink_msg_command_ack_pack(uint8_t system_id, uint8_t c * @brief Pack a command_ack message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param command Command ID, as defined by MAV_CMD enum. * @param result See MAV_RESULT enum @@ -97,7 +97,7 @@ static inline uint16_t mavlink_msg_command_ack_pack_chan(uint8_t system_id, uint } /** - * @brief Encode a command_ack struct into a message + * @brief Encode a command_ack struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -109,6 +109,20 @@ static inline uint16_t mavlink_msg_command_ack_encode(uint8_t system_id, uint8_t return mavlink_msg_command_ack_pack(system_id, component_id, msg, command_ack->command, command_ack->result); } +/** + * @brief Encode a command_ack struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param command_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_command_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_command_ack_t* command_ack) +{ + return mavlink_msg_command_ack_pack_chan(system_id, component_id, chan, msg, command_ack->command, command_ack->result); +} + /** * @brief Send a command_ack message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_long.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_long.h index 5c44c62844..8f705c0dd2 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_long.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_long.h @@ -109,7 +109,7 @@ static inline uint16_t mavlink_msg_command_long_pack(uint8_t system_id, uint8_t * @brief Pack a command_long message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System which should execute the command * @param target_component Component which should execute the command, 0 for all components @@ -169,7 +169,7 @@ static inline uint16_t mavlink_msg_command_long_pack_chan(uint8_t system_id, uin } /** - * @brief Encode a command_long struct into a message + * @brief Encode a command_long struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -181,6 +181,20 @@ static inline uint16_t mavlink_msg_command_long_encode(uint8_t system_id, uint8_ return mavlink_msg_command_long_pack(system_id, component_id, msg, command_long->target_system, command_long->target_component, command_long->command, command_long->confirmation, command_long->param1, command_long->param2, command_long->param3, command_long->param4, command_long->param5, command_long->param6, command_long->param7); } +/** + * @brief Encode a command_long struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param command_long C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_command_long_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_command_long_t* command_long) +{ + return mavlink_msg_command_long_pack_chan(system_id, component_id, chan, msg, command_long->target_system, command_long->target_component, command_long->command, command_long->confirmation, command_long->param1, command_long->param2, command_long->param3, command_long->param4, command_long->param5, command_long->param6, command_long->param7); +} + /** * @brief Send a command_long message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_stream.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_stream.h index 782ea9f26b..dc0768e128 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_stream.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_stream.h @@ -69,7 +69,7 @@ static inline uint16_t mavlink_msg_data_stream_pack(uint8_t system_id, uint8_t c * @brief Pack a data_stream message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param stream_id The ID of the requested data stream * @param message_rate The requested interval between two messages of this type @@ -105,7 +105,7 @@ static inline uint16_t mavlink_msg_data_stream_pack_chan(uint8_t system_id, uint } /** - * @brief Encode a data_stream struct into a message + * @brief Encode a data_stream struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -117,6 +117,20 @@ static inline uint16_t mavlink_msg_data_stream_encode(uint8_t system_id, uint8_t return mavlink_msg_data_stream_pack(system_id, component_id, msg, data_stream->stream_id, data_stream->message_rate, data_stream->on_off); } +/** + * @brief Encode a data_stream struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param data_stream C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_data_stream_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_data_stream_t* data_stream) +{ + return mavlink_msg_data_stream_pack_chan(system_id, component_id, chan, msg, data_stream->stream_id, data_stream->message_rate, data_stream->on_off); +} + /** * @brief Send a data_stream message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug.h index a111619185..9a6ed87eeb 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug.h @@ -69,7 +69,7 @@ static inline uint16_t mavlink_msg_debug_pack(uint8_t system_id, uint8_t compone * @brief Pack a debug message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp (milliseconds since system boot) * @param ind index of debug variable @@ -105,7 +105,7 @@ static inline uint16_t mavlink_msg_debug_pack_chan(uint8_t system_id, uint8_t co } /** - * @brief Encode a debug struct into a message + * @brief Encode a debug struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -117,6 +117,20 @@ static inline uint16_t mavlink_msg_debug_encode(uint8_t system_id, uint8_t compo return mavlink_msg_debug_pack(system_id, component_id, msg, debug->time_boot_ms, debug->ind, debug->value); } +/** + * @brief Encode a debug struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param debug C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_debug_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_debug_t* debug) +{ + return mavlink_msg_debug_pack_chan(system_id, component_id, chan, msg, debug->time_boot_ms, debug->ind, debug->value); +} + /** * @brief Send a debug message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug_vect.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug_vect.h index 5ee4e323ac..6cfc75212e 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug_vect.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug_vect.h @@ -77,7 +77,7 @@ static inline uint16_t mavlink_msg_debug_vect_pack(uint8_t system_id, uint8_t co * @brief Pack a debug_vect message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param name Name * @param time_usec Timestamp @@ -117,7 +117,7 @@ static inline uint16_t mavlink_msg_debug_vect_pack_chan(uint8_t system_id, uint8 } /** - * @brief Encode a debug_vect struct into a message + * @brief Encode a debug_vect struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -129,6 +129,20 @@ static inline uint16_t mavlink_msg_debug_vect_encode(uint8_t system_id, uint8_t return mavlink_msg_debug_vect_pack(system_id, component_id, msg, debug_vect->name, debug_vect->time_usec, debug_vect->x, debug_vect->y, debug_vect->z); } +/** + * @brief Encode a debug_vect struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param debug_vect C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_debug_vect_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_debug_vect_t* debug_vect) +{ + return mavlink_msg_debug_vect_pack_chan(system_id, component_id, chan, msg, debug_vect->name, debug_vect->time_usec, debug_vect->x, debug_vect->y, debug_vect->z); +} + /** * @brief Send a debug_vect message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_dir_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_dir_list.h index c026419a20..4f31698d50 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_dir_list.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_dir_list.h @@ -67,7 +67,7 @@ static inline uint16_t mavlink_msg_file_transfer_dir_list_pack(uint8_t system_id * @brief Pack a file_transfer_dir_list message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param transfer_uid Unique transfer ID * @param dir_path Directory path to list @@ -101,7 +101,7 @@ static inline uint16_t mavlink_msg_file_transfer_dir_list_pack_chan(uint8_t syst } /** - * @brief Encode a file_transfer_dir_list struct into a message + * @brief Encode a file_transfer_dir_list struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -113,6 +113,20 @@ static inline uint16_t mavlink_msg_file_transfer_dir_list_encode(uint8_t system_ return mavlink_msg_file_transfer_dir_list_pack(system_id, component_id, msg, file_transfer_dir_list->transfer_uid, file_transfer_dir_list->dir_path, file_transfer_dir_list->flags); } +/** + * @brief Encode a file_transfer_dir_list struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param file_transfer_dir_list C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_file_transfer_dir_list_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_file_transfer_dir_list_t* file_transfer_dir_list) +{ + return mavlink_msg_file_transfer_dir_list_pack_chan(system_id, component_id, chan, msg, file_transfer_dir_list->transfer_uid, file_transfer_dir_list->dir_path, file_transfer_dir_list->flags); +} + /** * @brief Send a file_transfer_dir_list message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_res.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_res.h index 86d407d66b..fc6247faca 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_res.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_res.h @@ -64,7 +64,7 @@ static inline uint16_t mavlink_msg_file_transfer_res_pack(uint8_t system_id, uin * @brief Pack a file_transfer_res message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param transfer_uid Unique transfer ID * @param result 0: OK, 1: not permitted, 2: bad path / file name, 3: no space left on device @@ -97,7 +97,7 @@ static inline uint16_t mavlink_msg_file_transfer_res_pack_chan(uint8_t system_id } /** - * @brief Encode a file_transfer_res struct into a message + * @brief Encode a file_transfer_res struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -109,6 +109,20 @@ static inline uint16_t mavlink_msg_file_transfer_res_encode(uint8_t system_id, u return mavlink_msg_file_transfer_res_pack(system_id, component_id, msg, file_transfer_res->transfer_uid, file_transfer_res->result); } +/** + * @brief Encode a file_transfer_res struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param file_transfer_res C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_file_transfer_res_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_file_transfer_res_t* file_transfer_res) +{ + return mavlink_msg_file_transfer_res_pack_chan(system_id, component_id, chan, msg, file_transfer_res->transfer_uid, file_transfer_res->result); +} + /** * @brief Send a file_transfer_res message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_start.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_start.h index 24bf25413f..05be77339b 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_start.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_start.h @@ -77,7 +77,7 @@ static inline uint16_t mavlink_msg_file_transfer_start_pack(uint8_t system_id, u * @brief Pack a file_transfer_start message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param transfer_uid Unique transfer ID * @param dest_path Destination path @@ -117,7 +117,7 @@ static inline uint16_t mavlink_msg_file_transfer_start_pack_chan(uint8_t system_ } /** - * @brief Encode a file_transfer_start struct into a message + * @brief Encode a file_transfer_start struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -129,6 +129,20 @@ static inline uint16_t mavlink_msg_file_transfer_start_encode(uint8_t system_id, return mavlink_msg_file_transfer_start_pack(system_id, component_id, msg, file_transfer_start->transfer_uid, file_transfer_start->dest_path, file_transfer_start->direction, file_transfer_start->file_size, file_transfer_start->flags); } +/** + * @brief Encode a file_transfer_start struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param file_transfer_start C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_file_transfer_start_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_file_transfer_start_t* file_transfer_start) +{ + return mavlink_msg_file_transfer_start_pack_chan(system_id, component_id, chan, msg, file_transfer_start->transfer_uid, file_transfer_start->dest_path, file_transfer_start->direction, file_transfer_start->file_size, file_transfer_start->flags); +} + /** * @brief Send a file_transfer_start message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_int.h index 11ab97ee47..7ed3d2a636 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_int.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_int.h @@ -12,7 +12,7 @@ typedef struct __mavlink_global_position_int_t int16_t vx; ///< Ground X Speed (Latitude), expressed as m/s * 100 int16_t vy; ///< Ground Y Speed (Longitude), expressed as m/s * 100 int16_t vz; ///< Ground Z Speed (Altitude), expressed as m/s * 100 - uint16_t hdg; ///< Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + uint16_t hdg; ///< Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX } mavlink_global_position_int_t; #define MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN 28 @@ -53,7 +53,7 @@ typedef struct __mavlink_global_position_int_t * @param vx Ground X Speed (Latitude), expressed as m/s * 100 * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 - * @param hdg Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + * @param hdg Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_global_position_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, @@ -99,7 +99,7 @@ static inline uint16_t mavlink_msg_global_position_int_pack(uint8_t system_id, u * @brief Pack a global_position_int message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp (milliseconds since system boot) * @param lat Latitude, expressed as * 1E7 @@ -109,7 +109,7 @@ static inline uint16_t mavlink_msg_global_position_int_pack(uint8_t system_id, u * @param vx Ground X Speed (Latitude), expressed as m/s * 100 * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 - * @param hdg Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + * @param hdg Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_global_position_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, @@ -153,7 +153,7 @@ static inline uint16_t mavlink_msg_global_position_int_pack_chan(uint8_t system_ } /** - * @brief Encode a global_position_int struct into a message + * @brief Encode a global_position_int struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -165,6 +165,20 @@ static inline uint16_t mavlink_msg_global_position_int_encode(uint8_t system_id, return mavlink_msg_global_position_int_pack(system_id, component_id, msg, global_position_int->time_boot_ms, global_position_int->lat, global_position_int->lon, global_position_int->alt, global_position_int->relative_alt, global_position_int->vx, global_position_int->vy, global_position_int->vz, global_position_int->hdg); } +/** + * @brief Encode a global_position_int struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param global_position_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_global_position_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_global_position_int_t* global_position_int) +{ + return mavlink_msg_global_position_int_pack_chan(system_id, component_id, chan, msg, global_position_int->time_boot_ms, global_position_int->lat, global_position_int->lon, global_position_int->alt, global_position_int->relative_alt, global_position_int->vx, global_position_int->vy, global_position_int->vz, global_position_int->hdg); +} + /** * @brief Send a global_position_int message * @param chan MAVLink channel to send the message @@ -177,7 +191,7 @@ static inline uint16_t mavlink_msg_global_position_int_encode(uint8_t system_id, * @param vx Ground X Speed (Latitude), expressed as m/s * 100 * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 - * @param hdg Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + * @param hdg Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -308,7 +322,7 @@ static inline int16_t mavlink_msg_global_position_int_get_vz(const mavlink_messa /** * @brief Get field hdg from global_position_int message * - * @return Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + * @return Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX */ static inline uint16_t mavlink_msg_global_position_int_get_hdg(const mavlink_message_t* msg) { diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_setpoint_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_setpoint_int.h index 8153628aa8..1a1c97199e 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_setpoint_int.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_setpoint_int.h @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_global_position_setpoint_int_pack(uint8_t sys * @brief Pack a global_position_setpoint_int message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param coordinate_frame Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT * @param latitude Latitude (WGS84), in degrees * 1E7 @@ -121,7 +121,7 @@ static inline uint16_t mavlink_msg_global_position_setpoint_int_pack_chan(uint8_ } /** - * @brief Encode a global_position_setpoint_int struct into a message + * @brief Encode a global_position_setpoint_int struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -133,6 +133,20 @@ static inline uint16_t mavlink_msg_global_position_setpoint_int_encode(uint8_t s return mavlink_msg_global_position_setpoint_int_pack(system_id, component_id, msg, global_position_setpoint_int->coordinate_frame, global_position_setpoint_int->latitude, global_position_setpoint_int->longitude, global_position_setpoint_int->altitude, global_position_setpoint_int->yaw); } +/** + * @brief Encode a global_position_setpoint_int struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param global_position_setpoint_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_global_position_setpoint_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_global_position_setpoint_int_t* global_position_setpoint_int) +{ + return mavlink_msg_global_position_setpoint_int_pack_chan(system_id, component_id, chan, msg, global_position_setpoint_int->coordinate_frame, global_position_setpoint_int->latitude, global_position_setpoint_int->longitude, global_position_setpoint_int->altitude, global_position_setpoint_int->yaw); +} + /** * @brief Send a global_position_setpoint_int message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_vision_position_estimate.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_vision_position_estimate.h index b388fa24ad..f7be74c917 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_vision_position_estimate.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_vision_position_estimate.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_global_vision_position_estimate_pack(uint8_t * @brief Pack a global_vision_position_estimate message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param usec Timestamp (microseconds, synced to UNIX time or since system boot) * @param x Global X position @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_global_vision_position_estimate_pack_chan(uin } /** - * @brief Encode a global_vision_position_estimate struct into a message + * @brief Encode a global_vision_position_estimate struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -149,6 +149,20 @@ static inline uint16_t mavlink_msg_global_vision_position_estimate_encode(uint8_ return mavlink_msg_global_vision_position_estimate_pack(system_id, component_id, msg, global_vision_position_estimate->usec, global_vision_position_estimate->x, global_vision_position_estimate->y, global_vision_position_estimate->z, global_vision_position_estimate->roll, global_vision_position_estimate->pitch, global_vision_position_estimate->yaw); } +/** + * @brief Encode a global_vision_position_estimate struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param global_vision_position_estimate C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_global_vision_position_estimate_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_global_vision_position_estimate_t* global_vision_position_estimate) +{ + return mavlink_msg_global_vision_position_estimate_pack_chan(system_id, component_id, chan, msg, global_vision_position_estimate->usec, global_vision_position_estimate->x, global_vision_position_estimate->y, global_vision_position_estimate->z, global_vision_position_estimate->roll, global_vision_position_estimate->pitch, global_vision_position_estimate->yaw); +} + /** * @brief Send a global_vision_position_estimate message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_global_origin.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_global_origin.h index bd09cb7534..016e9cb0e6 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_global_origin.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_global_origin.h @@ -69,7 +69,7 @@ static inline uint16_t mavlink_msg_gps_global_origin_pack(uint8_t system_id, uin * @brief Pack a gps_global_origin message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param latitude Latitude (WGS84), in degrees * 1E7 * @param longitude Longitude (WGS84), in degrees * 1E7 @@ -105,7 +105,7 @@ static inline uint16_t mavlink_msg_gps_global_origin_pack_chan(uint8_t system_id } /** - * @brief Encode a gps_global_origin struct into a message + * @brief Encode a gps_global_origin struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -117,6 +117,20 @@ static inline uint16_t mavlink_msg_gps_global_origin_encode(uint8_t system_id, u return mavlink_msg_gps_global_origin_pack(system_id, component_id, msg, gps_global_origin->latitude, gps_global_origin->longitude, gps_global_origin->altitude); } +/** + * @brief Encode a gps_global_origin struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gps_global_origin C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps_global_origin_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps_global_origin_t* gps_global_origin) +{ + return mavlink_msg_gps_global_origin_pack_chan(system_id, component_id, chan, msg, gps_global_origin->latitude, gps_global_origin->longitude, gps_global_origin->altitude); +} + /** * @brief Send a gps_global_origin message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_raw_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_raw_int.h index 2136b65eef..3054d4fdab 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_raw_int.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_raw_int.h @@ -8,10 +8,10 @@ typedef struct __mavlink_gps_raw_int_t int32_t lat; ///< Latitude (WGS84), in degrees * 1E7 int32_t lon; ///< Longitude (WGS84), in degrees * 1E7 int32_t alt; ///< Altitude (WGS84), in meters * 1000 (positive for up) - uint16_t eph; ///< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - uint16_t epv; ///< GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - uint16_t vel; ///< GPS ground speed (m/s * 100). If unknown, set to: 65535 - uint16_t cog; ///< Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + uint16_t eph; ///< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + uint16_t epv; ///< GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + uint16_t vel; ///< GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX + uint16_t cog; ///< Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. uint8_t satellites_visible; ///< Number of satellites visible. If unknown, set to 255 } mavlink_gps_raw_int_t; @@ -52,10 +52,10 @@ typedef struct __mavlink_gps_raw_int_t * @param lat Latitude (WGS84), in degrees * 1E7 * @param lon Longitude (WGS84), in degrees * 1E7 * @param alt Altitude (WGS84), in meters * 1000 (positive for up) - * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - * @param epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - * @param vel GPS ground speed (m/s * 100). If unknown, set to: 65535 - * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @param vel GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX + * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX * @param satellites_visible Number of satellites visible. If unknown, set to 255 * @return length of the message in bytes (excluding serial stream start sign) */ @@ -104,17 +104,17 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack(uint8_t system_id, uint8_t c * @brief Pack a gps_raw_int message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. * @param lat Latitude (WGS84), in degrees * 1E7 * @param lon Longitude (WGS84), in degrees * 1E7 * @param alt Altitude (WGS84), in meters * 1000 (positive for up) - * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - * @param epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - * @param vel GPS ground speed (m/s * 100). If unknown, set to: 65535 - * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @param vel GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX + * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX * @param satellites_visible Number of satellites visible. If unknown, set to 255 * @return length of the message in bytes (excluding serial stream start sign) */ @@ -161,7 +161,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack_chan(uint8_t system_id, uint } /** - * @brief Encode a gps_raw_int struct into a message + * @brief Encode a gps_raw_int struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -173,6 +173,20 @@ static inline uint16_t mavlink_msg_gps_raw_int_encode(uint8_t system_id, uint8_t return mavlink_msg_gps_raw_int_pack(system_id, component_id, msg, gps_raw_int->time_usec, gps_raw_int->fix_type, gps_raw_int->lat, gps_raw_int->lon, gps_raw_int->alt, gps_raw_int->eph, gps_raw_int->epv, gps_raw_int->vel, gps_raw_int->cog, gps_raw_int->satellites_visible); } +/** + * @brief Encode a gps_raw_int struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gps_raw_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps_raw_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps_raw_int_t* gps_raw_int) +{ + return mavlink_msg_gps_raw_int_pack_chan(system_id, component_id, chan, msg, gps_raw_int->time_usec, gps_raw_int->fix_type, gps_raw_int->lat, gps_raw_int->lon, gps_raw_int->alt, gps_raw_int->eph, gps_raw_int->epv, gps_raw_int->vel, gps_raw_int->cog, gps_raw_int->satellites_visible); +} + /** * @brief Send a gps_raw_int message * @param chan MAVLink channel to send the message @@ -182,10 +196,10 @@ static inline uint16_t mavlink_msg_gps_raw_int_encode(uint8_t system_id, uint8_t * @param lat Latitude (WGS84), in degrees * 1E7 * @param lon Longitude (WGS84), in degrees * 1E7 * @param alt Altitude (WGS84), in meters * 1000 (positive for up) - * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - * @param epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - * @param vel GPS ground speed (m/s * 100). If unknown, set to: 65535 - * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @param vel GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX + * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX * @param satellites_visible Number of satellites visible. If unknown, set to 255 */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -289,7 +303,7 @@ static inline int32_t mavlink_msg_gps_raw_int_get_alt(const mavlink_message_t* m /** * @brief Get field eph from gps_raw_int message * - * @return GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + * @return GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX */ static inline uint16_t mavlink_msg_gps_raw_int_get_eph(const mavlink_message_t* msg) { @@ -299,7 +313,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_get_eph(const mavlink_message_t* /** * @brief Get field epv from gps_raw_int message * - * @return GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + * @return GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX */ static inline uint16_t mavlink_msg_gps_raw_int_get_epv(const mavlink_message_t* msg) { @@ -309,7 +323,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_get_epv(const mavlink_message_t* /** * @brief Get field vel from gps_raw_int message * - * @return GPS ground speed (m/s * 100). If unknown, set to: 65535 + * @return GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX */ static inline uint16_t mavlink_msg_gps_raw_int_get_vel(const mavlink_message_t* msg) { @@ -319,7 +333,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_get_vel(const mavlink_message_t* /** * @brief Get field cog from gps_raw_int message * - * @return Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + * @return Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX */ static inline uint16_t mavlink_msg_gps_raw_int_get_cog(const mavlink_message_t* msg) { diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_status.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_status.h index cd6dde700f..28d6b57d19 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_status.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_status.h @@ -86,7 +86,7 @@ static inline uint16_t mavlink_msg_gps_status_pack(uint8_t system_id, uint8_t co * @brief Pack a gps_status message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param satellites_visible Number of satellites visible * @param satellite_prn Global satellite ID @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_gps_status_pack_chan(uint8_t system_id, uint8 } /** - * @brief Encode a gps_status struct into a message + * @brief Encode a gps_status struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -141,6 +141,20 @@ static inline uint16_t mavlink_msg_gps_status_encode(uint8_t system_id, uint8_t return mavlink_msg_gps_status_pack(system_id, component_id, msg, gps_status->satellites_visible, gps_status->satellite_prn, gps_status->satellite_used, gps_status->satellite_elevation, gps_status->satellite_azimuth, gps_status->satellite_snr); } +/** + * @brief Encode a gps_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gps_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps_status_t* gps_status) +{ + return mavlink_msg_gps_status_pack_chan(system_id, component_id, chan, msg, gps_status->satellites_visible, gps_status->satellite_prn, gps_status->satellite_used, gps_status->satellite_elevation, gps_status->satellite_azimuth, gps_status->satellite_snr); +} + /** * @brief Send a gps_status message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h index b2f0b65d06..826138fad1 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h @@ -83,7 +83,7 @@ static inline uint16_t mavlink_msg_heartbeat_pack(uint8_t system_id, uint8_t com * @brief Pack a heartbeat message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) * @param autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM @@ -127,7 +127,7 @@ static inline uint16_t mavlink_msg_heartbeat_pack_chan(uint8_t system_id, uint8_ } /** - * @brief Encode a heartbeat struct into a message + * @brief Encode a heartbeat struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -139,6 +139,20 @@ static inline uint16_t mavlink_msg_heartbeat_encode(uint8_t system_id, uint8_t c return mavlink_msg_heartbeat_pack(system_id, component_id, msg, heartbeat->type, heartbeat->autopilot, heartbeat->base_mode, heartbeat->custom_mode, heartbeat->system_status); } +/** + * @brief Encode a heartbeat struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param heartbeat C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_heartbeat_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_heartbeat_t* heartbeat) +{ + return mavlink_msg_heartbeat_pack_chan(system_id, component_id, chan, msg, heartbeat->type, heartbeat->autopilot, heartbeat->base_mode, heartbeat->custom_mode, heartbeat->system_status); +} + /** * @brief Send a heartbeat message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_highres_imu.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_highres_imu.h index 6e7492ea65..0dcd95ed32 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_highres_imu.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_highres_imu.h @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_highres_imu_pack(uint8_t system_id, uint8_t c * @brief Pack a highres_imu message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot) * @param xacc X acceleration (m/s^2) @@ -201,7 +201,7 @@ static inline uint16_t mavlink_msg_highres_imu_pack_chan(uint8_t system_id, uint } /** - * @brief Encode a highres_imu struct into a message + * @brief Encode a highres_imu struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -213,6 +213,20 @@ static inline uint16_t mavlink_msg_highres_imu_encode(uint8_t system_id, uint8_t return mavlink_msg_highres_imu_pack(system_id, component_id, msg, highres_imu->time_usec, highres_imu->xacc, highres_imu->yacc, highres_imu->zacc, highres_imu->xgyro, highres_imu->ygyro, highres_imu->zgyro, highres_imu->xmag, highres_imu->ymag, highres_imu->zmag, highres_imu->abs_pressure, highres_imu->diff_pressure, highres_imu->pressure_alt, highres_imu->temperature, highres_imu->fields_updated); } +/** + * @brief Encode a highres_imu struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param highres_imu C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_highres_imu_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_highres_imu_t* highres_imu) +{ + return mavlink_msg_highres_imu_pack_chan(system_id, component_id, chan, msg, highres_imu->time_usec, highres_imu->xacc, highres_imu->yacc, highres_imu->zacc, highres_imu->xgyro, highres_imu->ygyro, highres_imu->zgyro, highres_imu->xmag, highres_imu->ymag, highres_imu->zmag, highres_imu->abs_pressure, highres_imu->diff_pressure, highres_imu->pressure_alt, highres_imu->temperature, highres_imu->fields_updated); +} + /** * @brief Send a highres_imu message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_controls.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_controls.h index 3319d3fd2d..aed5108d05 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_controls.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_controls.h @@ -109,7 +109,7 @@ static inline uint16_t mavlink_msg_hil_controls_pack(uint8_t system_id, uint8_t * @brief Pack a hil_controls message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) * @param roll_ailerons Control output -1 .. 1 @@ -169,7 +169,7 @@ static inline uint16_t mavlink_msg_hil_controls_pack_chan(uint8_t system_id, uin } /** - * @brief Encode a hil_controls struct into a message + * @brief Encode a hil_controls struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -181,6 +181,20 @@ static inline uint16_t mavlink_msg_hil_controls_encode(uint8_t system_id, uint8_ return mavlink_msg_hil_controls_pack(system_id, component_id, msg, hil_controls->time_usec, hil_controls->roll_ailerons, hil_controls->pitch_elevator, hil_controls->yaw_rudder, hil_controls->throttle, hil_controls->aux1, hil_controls->aux2, hil_controls->aux3, hil_controls->aux4, hil_controls->mode, hil_controls->nav_mode); } +/** + * @brief Encode a hil_controls struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param hil_controls C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_controls_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_controls_t* hil_controls) +{ + return mavlink_msg_hil_controls_pack_chan(system_id, component_id, chan, msg, hil_controls->time_usec, hil_controls->roll_ailerons, hil_controls->pitch_elevator, hil_controls->yaw_rudder, hil_controls->throttle, hil_controls->aux1, hil_controls->aux2, hil_controls->aux3, hil_controls->aux4, hil_controls->mode, hil_controls->nav_mode); +} + /** * @brief Send a hil_controls message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_gps.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_gps.h index 75ab6835dd..36a551872b 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_gps.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_gps.h @@ -119,7 +119,7 @@ static inline uint16_t mavlink_msg_hil_gps_pack(uint8_t system_id, uint8_t compo * @brief Pack a hil_gps message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. @@ -185,7 +185,7 @@ static inline uint16_t mavlink_msg_hil_gps_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a hil_gps struct into a message + * @brief Encode a hil_gps struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -197,6 +197,20 @@ static inline uint16_t mavlink_msg_hil_gps_encode(uint8_t system_id, uint8_t com return mavlink_msg_hil_gps_pack(system_id, component_id, msg, hil_gps->time_usec, hil_gps->fix_type, hil_gps->lat, hil_gps->lon, hil_gps->alt, hil_gps->eph, hil_gps->epv, hil_gps->vel, hil_gps->vn, hil_gps->ve, hil_gps->vd, hil_gps->cog, hil_gps->satellites_visible); } +/** + * @brief Encode a hil_gps struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param hil_gps C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_gps_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_gps_t* hil_gps) +{ + return mavlink_msg_hil_gps_pack_chan(system_id, component_id, chan, msg, hil_gps->time_usec, hil_gps->fix_type, hil_gps->lat, hil_gps->lon, hil_gps->alt, hil_gps->eph, hil_gps->epv, hil_gps->vel, hil_gps->vn, hil_gps->ve, hil_gps->vd, hil_gps->cog, hil_gps->satellites_visible); +} + /** * @brief Send a hil_gps message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_optical_flow.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_optical_flow.h index 13e13d47cb..acb1392e14 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_optical_flow.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_optical_flow.h @@ -94,7 +94,7 @@ static inline uint16_t mavlink_msg_hil_optical_flow_pack(uint8_t system_id, uint * @brief Pack a hil_optical_flow message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (UNIX) * @param sensor_id Sensor ID @@ -145,7 +145,7 @@ static inline uint16_t mavlink_msg_hil_optical_flow_pack_chan(uint8_t system_id, } /** - * @brief Encode a hil_optical_flow struct into a message + * @brief Encode a hil_optical_flow struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -157,6 +157,20 @@ static inline uint16_t mavlink_msg_hil_optical_flow_encode(uint8_t system_id, ui return mavlink_msg_hil_optical_flow_pack(system_id, component_id, msg, hil_optical_flow->time_usec, hil_optical_flow->sensor_id, hil_optical_flow->flow_x, hil_optical_flow->flow_y, hil_optical_flow->flow_comp_m_x, hil_optical_flow->flow_comp_m_y, hil_optical_flow->quality, hil_optical_flow->ground_distance); } +/** + * @brief Encode a hil_optical_flow struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param hil_optical_flow C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_optical_flow_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_optical_flow_t* hil_optical_flow) +{ + return mavlink_msg_hil_optical_flow_pack_chan(system_id, component_id, chan, msg, hil_optical_flow->time_usec, hil_optical_flow->sensor_id, hil_optical_flow->flow_x, hil_optical_flow->flow_y, hil_optical_flow->flow_comp_m_x, hil_optical_flow->flow_comp_m_y, hil_optical_flow->quality, hil_optical_flow->ground_distance); +} + /** * @brief Send a hil_optical_flow message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_rc_inputs_raw.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_rc_inputs_raw.h index f2435dde87..a42bde50b2 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_rc_inputs_raw.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_rc_inputs_raw.h @@ -124,7 +124,7 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_pack(uint8_t system_id, uin * @brief Pack a hil_rc_inputs_raw message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) * @param chan1_raw RC channel 1 value, in microseconds @@ -193,7 +193,7 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_pack_chan(uint8_t system_id } /** - * @brief Encode a hil_rc_inputs_raw struct into a message + * @brief Encode a hil_rc_inputs_raw struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -205,6 +205,20 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_encode(uint8_t system_id, u return mavlink_msg_hil_rc_inputs_raw_pack(system_id, component_id, msg, hil_rc_inputs_raw->time_usec, hil_rc_inputs_raw->chan1_raw, hil_rc_inputs_raw->chan2_raw, hil_rc_inputs_raw->chan3_raw, hil_rc_inputs_raw->chan4_raw, hil_rc_inputs_raw->chan5_raw, hil_rc_inputs_raw->chan6_raw, hil_rc_inputs_raw->chan7_raw, hil_rc_inputs_raw->chan8_raw, hil_rc_inputs_raw->chan9_raw, hil_rc_inputs_raw->chan10_raw, hil_rc_inputs_raw->chan11_raw, hil_rc_inputs_raw->chan12_raw, hil_rc_inputs_raw->rssi); } +/** + * @brief Encode a hil_rc_inputs_raw struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param hil_rc_inputs_raw C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_rc_inputs_raw_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_rc_inputs_raw_t* hil_rc_inputs_raw) +{ + return mavlink_msg_hil_rc_inputs_raw_pack_chan(system_id, component_id, chan, msg, hil_rc_inputs_raw->time_usec, hil_rc_inputs_raw->chan1_raw, hil_rc_inputs_raw->chan2_raw, hil_rc_inputs_raw->chan3_raw, hil_rc_inputs_raw->chan4_raw, hil_rc_inputs_raw->chan5_raw, hil_rc_inputs_raw->chan6_raw, hil_rc_inputs_raw->chan7_raw, hil_rc_inputs_raw->chan8_raw, hil_rc_inputs_raw->chan9_raw, hil_rc_inputs_raw->chan10_raw, hil_rc_inputs_raw->chan11_raw, hil_rc_inputs_raw->chan12_raw, hil_rc_inputs_raw->rssi); +} + /** * @brief Send a hil_rc_inputs_raw message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_sensor.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_sensor.h index 422e55adc8..6c2667473d 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_sensor.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_sensor.h @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_hil_sensor_pack(uint8_t system_id, uint8_t co * @brief Pack a hil_sensor message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot) * @param xacc X acceleration (m/s^2) @@ -201,7 +201,7 @@ static inline uint16_t mavlink_msg_hil_sensor_pack_chan(uint8_t system_id, uint8 } /** - * @brief Encode a hil_sensor struct into a message + * @brief Encode a hil_sensor struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -213,6 +213,20 @@ static inline uint16_t mavlink_msg_hil_sensor_encode(uint8_t system_id, uint8_t return mavlink_msg_hil_sensor_pack(system_id, component_id, msg, hil_sensor->time_usec, hil_sensor->xacc, hil_sensor->yacc, hil_sensor->zacc, hil_sensor->xgyro, hil_sensor->ygyro, hil_sensor->zgyro, hil_sensor->xmag, hil_sensor->ymag, hil_sensor->zmag, hil_sensor->abs_pressure, hil_sensor->diff_pressure, hil_sensor->pressure_alt, hil_sensor->temperature, hil_sensor->fields_updated); } +/** + * @brief Encode a hil_sensor struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param hil_sensor C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_sensor_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_sensor_t* hil_sensor) +{ + return mavlink_msg_hil_sensor_pack_chan(system_id, component_id, chan, msg, hil_sensor->time_usec, hil_sensor->xacc, hil_sensor->yacc, hil_sensor->zacc, hil_sensor->xgyro, hil_sensor->ygyro, hil_sensor->zgyro, hil_sensor->xmag, hil_sensor->ymag, hil_sensor->zmag, hil_sensor->abs_pressure, hil_sensor->diff_pressure, hil_sensor->pressure_alt, hil_sensor->temperature, hil_sensor->fields_updated); +} + /** * @brief Send a hil_sensor message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state.h index 1d3f286649..bcc8577670 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state.h @@ -134,7 +134,7 @@ static inline uint16_t mavlink_msg_hil_state_pack(uint8_t system_id, uint8_t com * @brief Pack a hil_state message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) * @param roll Roll angle (rad) @@ -209,7 +209,7 @@ static inline uint16_t mavlink_msg_hil_state_pack_chan(uint8_t system_id, uint8_ } /** - * @brief Encode a hil_state struct into a message + * @brief Encode a hil_state struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -221,6 +221,20 @@ static inline uint16_t mavlink_msg_hil_state_encode(uint8_t system_id, uint8_t c return mavlink_msg_hil_state_pack(system_id, component_id, msg, hil_state->time_usec, hil_state->roll, hil_state->pitch, hil_state->yaw, hil_state->rollspeed, hil_state->pitchspeed, hil_state->yawspeed, hil_state->lat, hil_state->lon, hil_state->alt, hil_state->vx, hil_state->vy, hil_state->vz, hil_state->xacc, hil_state->yacc, hil_state->zacc); } +/** + * @brief Encode a hil_state struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param hil_state C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_state_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_state_t* hil_state) +{ + return mavlink_msg_hil_state_pack_chan(system_id, component_id, chan, msg, hil_state->time_usec, hil_state->roll, hil_state->pitch, hil_state->yaw, hil_state->rollspeed, hil_state->pitchspeed, hil_state->yawspeed, hil_state->lat, hil_state->lon, hil_state->alt, hil_state->vx, hil_state->vy, hil_state->vz, hil_state->xacc, hil_state->yacc, hil_state->zacc); +} + /** * @brief Send a hil_state message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state_quaternion.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state_quaternion.h index 0474e64a2b..732176193e 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state_quaternion.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state_quaternion.h @@ -132,7 +132,7 @@ static inline uint16_t mavlink_msg_hil_state_quaternion_pack(uint8_t system_id, * @brief Pack a hil_state_quaternion message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) * @param attitude_quaternion Vehicle attitude expressed as normalized quaternion @@ -205,7 +205,7 @@ static inline uint16_t mavlink_msg_hil_state_quaternion_pack_chan(uint8_t system } /** - * @brief Encode a hil_state_quaternion struct into a message + * @brief Encode a hil_state_quaternion struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -217,6 +217,20 @@ static inline uint16_t mavlink_msg_hil_state_quaternion_encode(uint8_t system_id return mavlink_msg_hil_state_quaternion_pack(system_id, component_id, msg, hil_state_quaternion->time_usec, hil_state_quaternion->attitude_quaternion, hil_state_quaternion->rollspeed, hil_state_quaternion->pitchspeed, hil_state_quaternion->yawspeed, hil_state_quaternion->lat, hil_state_quaternion->lon, hil_state_quaternion->alt, hil_state_quaternion->vx, hil_state_quaternion->vy, hil_state_quaternion->vz, hil_state_quaternion->ind_airspeed, hil_state_quaternion->true_airspeed, hil_state_quaternion->xacc, hil_state_quaternion->yacc, hil_state_quaternion->zacc); } +/** + * @brief Encode a hil_state_quaternion struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param hil_state_quaternion C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_state_quaternion_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_state_quaternion_t* hil_state_quaternion) +{ + return mavlink_msg_hil_state_quaternion_pack_chan(system_id, component_id, chan, msg, hil_state_quaternion->time_usec, hil_state_quaternion->attitude_quaternion, hil_state_quaternion->rollspeed, hil_state_quaternion->pitchspeed, hil_state_quaternion->yawspeed, hil_state_quaternion->lat, hil_state_quaternion->lon, hil_state_quaternion->alt, hil_state_quaternion->vx, hil_state_quaternion->vy, hil_state_quaternion->vz, hil_state_quaternion->ind_airspeed, hil_state_quaternion->true_airspeed, hil_state_quaternion->xacc, hil_state_quaternion->yacc, hil_state_quaternion->zacc); +} + /** * @brief Send a hil_state_quaternion message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned.h index 56723f3d7d..a0b72c0e1e 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_local_position_ned_pack(uint8_t system_id, ui * @brief Pack a local_position_ned message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp (milliseconds since system boot) * @param x X Position @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_local_position_ned_pack_chan(uint8_t system_i } /** - * @brief Encode a local_position_ned struct into a message + * @brief Encode a local_position_ned struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -149,6 +149,20 @@ static inline uint16_t mavlink_msg_local_position_ned_encode(uint8_t system_id, return mavlink_msg_local_position_ned_pack(system_id, component_id, msg, local_position_ned->time_boot_ms, local_position_ned->x, local_position_ned->y, local_position_ned->z, local_position_ned->vx, local_position_ned->vy, local_position_ned->vz); } +/** + * @brief Encode a local_position_ned struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param local_position_ned C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_local_position_ned_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_local_position_ned_t* local_position_ned) +{ + return mavlink_msg_local_position_ned_pack_chan(system_id, component_id, chan, msg, local_position_ned->time_boot_ms, local_position_ned->x, local_position_ned->y, local_position_ned->z, local_position_ned->vx, local_position_ned->vy, local_position_ned->vz); +} + /** * @brief Send a local_position_ned message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned_system_global_offset.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned_system_global_offset.h index c206a2906b..8c46862027 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned_system_global_offset.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned_system_global_offset.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_local_position_ned_system_global_offset_pack( * @brief Pack a local_position_ned_system_global_offset message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp (milliseconds since system boot) * @param x X Position @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_local_position_ned_system_global_offset_pack_ } /** - * @brief Encode a local_position_ned_system_global_offset struct into a message + * @brief Encode a local_position_ned_system_global_offset struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -149,6 +149,20 @@ static inline uint16_t mavlink_msg_local_position_ned_system_global_offset_encod return mavlink_msg_local_position_ned_system_global_offset_pack(system_id, component_id, msg, local_position_ned_system_global_offset->time_boot_ms, local_position_ned_system_global_offset->x, local_position_ned_system_global_offset->y, local_position_ned_system_global_offset->z, local_position_ned_system_global_offset->roll, local_position_ned_system_global_offset->pitch, local_position_ned_system_global_offset->yaw); } +/** + * @brief Encode a local_position_ned_system_global_offset struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param local_position_ned_system_global_offset C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_local_position_ned_system_global_offset_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_local_position_ned_system_global_offset_t* local_position_ned_system_global_offset) +{ + return mavlink_msg_local_position_ned_system_global_offset_pack_chan(system_id, component_id, chan, msg, local_position_ned_system_global_offset->time_boot_ms, local_position_ned_system_global_offset->x, local_position_ned_system_global_offset->y, local_position_ned_system_global_offset->z, local_position_ned_system_global_offset->roll, local_position_ned_system_global_offset->pitch, local_position_ned_system_global_offset->yaw); +} + /** * @brief Send a local_position_ned_system_global_offset message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_setpoint.h index 96f35fe625..1794815f8e 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_setpoint.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_setpoint.h @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_local_position_setpoint_pack(uint8_t system_i * @brief Pack a local_position_setpoint message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param coordinate_frame Coordinate frame - valid values are only MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_ENU * @param x x position @@ -121,7 +121,7 @@ static inline uint16_t mavlink_msg_local_position_setpoint_pack_chan(uint8_t sys } /** - * @brief Encode a local_position_setpoint struct into a message + * @brief Encode a local_position_setpoint struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -133,6 +133,20 @@ static inline uint16_t mavlink_msg_local_position_setpoint_encode(uint8_t system return mavlink_msg_local_position_setpoint_pack(system_id, component_id, msg, local_position_setpoint->coordinate_frame, local_position_setpoint->x, local_position_setpoint->y, local_position_setpoint->z, local_position_setpoint->yaw); } +/** + * @brief Encode a local_position_setpoint struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param local_position_setpoint C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_local_position_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_local_position_setpoint_t* local_position_setpoint) +{ + return mavlink_msg_local_position_setpoint_pack_chan(system_id, component_id, chan, msg, local_position_setpoint->coordinate_frame, local_position_setpoint->x, local_position_setpoint->y, local_position_setpoint->z, local_position_setpoint->yaw); +} + /** * @brief Send a local_position_setpoint message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_control.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_control.h index c9e4a4f8a5..6b6e9e148a 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_control.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_control.h @@ -84,7 +84,7 @@ static inline uint16_t mavlink_msg_manual_control_pack(uint8_t system_id, uint8_ * @brief Pack a manual_control message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target The system to be controlled. * @param x X-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to forward(1000)-backward(-1000) movement on a joystick and the pitch of a vehicle. @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_manual_control_pack_chan(uint8_t system_id, u } /** - * @brief Encode a manual_control struct into a message + * @brief Encode a manual_control struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -141,6 +141,20 @@ static inline uint16_t mavlink_msg_manual_control_encode(uint8_t system_id, uint return mavlink_msg_manual_control_pack(system_id, component_id, msg, manual_control->target, manual_control->x, manual_control->y, manual_control->z, manual_control->r, manual_control->buttons); } +/** + * @brief Encode a manual_control struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param manual_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_manual_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_manual_control_t* manual_control) +{ + return mavlink_msg_manual_control_pack_chan(system_id, component_id, chan, msg, manual_control->target, manual_control->x, manual_control->y, manual_control->z, manual_control->r, manual_control->buttons); +} + /** * @brief Send a manual_control message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_setpoint.h index d59e212921..a694947c12 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_setpoint.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_setpoint.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_manual_setpoint_pack(uint8_t system_id, uint8 * @brief Pack a manual_setpoint message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp in milliseconds since system boot * @param roll Desired roll rate in radians per second @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_manual_setpoint_pack_chan(uint8_t system_id, } /** - * @brief Encode a manual_setpoint struct into a message + * @brief Encode a manual_setpoint struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -149,6 +149,20 @@ static inline uint16_t mavlink_msg_manual_setpoint_encode(uint8_t system_id, uin return mavlink_msg_manual_setpoint_pack(system_id, component_id, msg, manual_setpoint->time_boot_ms, manual_setpoint->roll, manual_setpoint->pitch, manual_setpoint->yaw, manual_setpoint->thrust, manual_setpoint->mode_switch, manual_setpoint->manual_override_switch); } +/** + * @brief Encode a manual_setpoint struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param manual_setpoint C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_manual_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_manual_setpoint_t* manual_setpoint) +{ + return mavlink_msg_manual_setpoint_pack_chan(system_id, component_id, chan, msg, manual_setpoint->time_boot_ms, manual_setpoint->roll, manual_setpoint->pitch, manual_setpoint->yaw, manual_setpoint->thrust, manual_setpoint->mode_switch, manual_setpoint->manual_override_switch); +} + /** * @brief Send a manual_setpoint message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_memory_vect.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_memory_vect.h index f8ae21b05c..5f79329c25 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_memory_vect.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_memory_vect.h @@ -72,7 +72,7 @@ static inline uint16_t mavlink_msg_memory_vect_pack(uint8_t system_id, uint8_t c * @brief Pack a memory_vect message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param address Starting address of the debug variables * @param ver Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below @@ -109,7 +109,7 @@ static inline uint16_t mavlink_msg_memory_vect_pack_chan(uint8_t system_id, uint } /** - * @brief Encode a memory_vect struct into a message + * @brief Encode a memory_vect struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -121,6 +121,20 @@ static inline uint16_t mavlink_msg_memory_vect_encode(uint8_t system_id, uint8_t return mavlink_msg_memory_vect_pack(system_id, component_id, msg, memory_vect->address, memory_vect->ver, memory_vect->type, memory_vect->value); } +/** + * @brief Encode a memory_vect struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param memory_vect C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_memory_vect_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_memory_vect_t* memory_vect) +{ + return mavlink_msg_memory_vect_pack_chan(system_id, component_id, chan, msg, memory_vect->address, memory_vect->ver, memory_vect->type, memory_vect->value); +} + /** * @brief Send a memory_vect message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_ack.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_ack.h index 32825647f9..7421d8394a 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_ack.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_ack.h @@ -69,7 +69,7 @@ static inline uint16_t mavlink_msg_mission_ack_pack(uint8_t system_id, uint8_t c * @brief Pack a mission_ack message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -105,7 +105,7 @@ static inline uint16_t mavlink_msg_mission_ack_pack_chan(uint8_t system_id, uint } /** - * @brief Encode a mission_ack struct into a message + * @brief Encode a mission_ack struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -117,6 +117,20 @@ static inline uint16_t mavlink_msg_mission_ack_encode(uint8_t system_id, uint8_t return mavlink_msg_mission_ack_pack(system_id, component_id, msg, mission_ack->target_system, mission_ack->target_component, mission_ack->type); } +/** + * @brief Encode a mission_ack struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_ack_t* mission_ack) +{ + return mavlink_msg_mission_ack_pack_chan(system_id, component_id, chan, msg, mission_ack->target_system, mission_ack->target_component, mission_ack->type); +} + /** * @brief Send a mission_ack message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_clear_all.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_clear_all.h index 06d2ac2e79..8f441c8e5c 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_clear_all.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_clear_all.h @@ -64,7 +64,7 @@ static inline uint16_t mavlink_msg_mission_clear_all_pack(uint8_t system_id, uin * @brief Pack a mission_clear_all message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -97,7 +97,7 @@ static inline uint16_t mavlink_msg_mission_clear_all_pack_chan(uint8_t system_id } /** - * @brief Encode a mission_clear_all struct into a message + * @brief Encode a mission_clear_all struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -109,6 +109,20 @@ static inline uint16_t mavlink_msg_mission_clear_all_encode(uint8_t system_id, u return mavlink_msg_mission_clear_all_pack(system_id, component_id, msg, mission_clear_all->target_system, mission_clear_all->target_component); } +/** + * @brief Encode a mission_clear_all struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_clear_all C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_clear_all_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_clear_all_t* mission_clear_all) +{ + return mavlink_msg_mission_clear_all_pack_chan(system_id, component_id, chan, msg, mission_clear_all->target_system, mission_clear_all->target_component); +} + /** * @brief Send a mission_clear_all message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_count.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_count.h index b28cec6f6d..eac7793067 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_count.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_count.h @@ -69,7 +69,7 @@ static inline uint16_t mavlink_msg_mission_count_pack(uint8_t system_id, uint8_t * @brief Pack a mission_count message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -105,7 +105,7 @@ static inline uint16_t mavlink_msg_mission_count_pack_chan(uint8_t system_id, ui } /** - * @brief Encode a mission_count struct into a message + * @brief Encode a mission_count struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -117,6 +117,20 @@ static inline uint16_t mavlink_msg_mission_count_encode(uint8_t system_id, uint8 return mavlink_msg_mission_count_pack(system_id, component_id, msg, mission_count->target_system, mission_count->target_component, mission_count->count); } +/** + * @brief Encode a mission_count struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_count C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_count_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_count_t* mission_count) +{ + return mavlink_msg_mission_count_pack_chan(system_id, component_id, chan, msg, mission_count->target_system, mission_count->target_component, mission_count->count); +} + /** * @brief Send a mission_count message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_current.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_current.h index 5bf0899bea..dbcdbd3f9c 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_current.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_current.h @@ -59,7 +59,7 @@ static inline uint16_t mavlink_msg_mission_current_pack(uint8_t system_id, uint8 * @brief Pack a mission_current message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param seq Sequence * @return length of the message in bytes (excluding serial stream start sign) @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_mission_current_pack_chan(uint8_t system_id, } /** - * @brief Encode a mission_current struct into a message + * @brief Encode a mission_current struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -101,6 +101,20 @@ static inline uint16_t mavlink_msg_mission_current_encode(uint8_t system_id, uin return mavlink_msg_mission_current_pack(system_id, component_id, msg, mission_current->seq); } +/** + * @brief Encode a mission_current struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_current C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_current_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_current_t* mission_current) +{ + return mavlink_msg_mission_current_pack_chan(system_id, component_id, chan, msg, mission_current->seq); +} + /** * @brief Send a mission_current message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item.h index ed9d6e4af7..634f80c379 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item.h @@ -124,7 +124,7 @@ static inline uint16_t mavlink_msg_mission_item_pack(uint8_t system_id, uint8_t * @brief Pack a mission_item message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -193,7 +193,7 @@ static inline uint16_t mavlink_msg_mission_item_pack_chan(uint8_t system_id, uin } /** - * @brief Encode a mission_item struct into a message + * @brief Encode a mission_item struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -205,6 +205,20 @@ static inline uint16_t mavlink_msg_mission_item_encode(uint8_t system_id, uint8_ return mavlink_msg_mission_item_pack(system_id, component_id, msg, mission_item->target_system, mission_item->target_component, mission_item->seq, mission_item->frame, mission_item->command, mission_item->current, mission_item->autocontinue, mission_item->param1, mission_item->param2, mission_item->param3, mission_item->param4, mission_item->x, mission_item->y, mission_item->z); } +/** + * @brief Encode a mission_item struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_item C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_item_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_item_t* mission_item) +{ + return mavlink_msg_mission_item_pack_chan(system_id, component_id, chan, msg, mission_item->target_system, mission_item->target_component, mission_item->seq, mission_item->frame, mission_item->command, mission_item->current, mission_item->autocontinue, mission_item->param1, mission_item->param2, mission_item->param3, mission_item->param4, mission_item->x, mission_item->y, mission_item->z); +} + /** * @brief Send a mission_item message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item_reached.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item_reached.h index 3f8a51a134..f3744fde6c 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item_reached.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item_reached.h @@ -59,7 +59,7 @@ static inline uint16_t mavlink_msg_mission_item_reached_pack(uint8_t system_id, * @brief Pack a mission_item_reached message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param seq Sequence * @return length of the message in bytes (excluding serial stream start sign) @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_mission_item_reached_pack_chan(uint8_t system } /** - * @brief Encode a mission_item_reached struct into a message + * @brief Encode a mission_item_reached struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -101,6 +101,20 @@ static inline uint16_t mavlink_msg_mission_item_reached_encode(uint8_t system_id return mavlink_msg_mission_item_reached_pack(system_id, component_id, msg, mission_item_reached->seq); } +/** + * @brief Encode a mission_item_reached struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_item_reached C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_item_reached_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_item_reached_t* mission_item_reached) +{ + return mavlink_msg_mission_item_reached_pack_chan(system_id, component_id, chan, msg, mission_item_reached->seq); +} + /** * @brief Send a mission_item_reached message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request.h index 0ced17614e..ac84e35540 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request.h @@ -69,7 +69,7 @@ static inline uint16_t mavlink_msg_mission_request_pack(uint8_t system_id, uint8 * @brief Pack a mission_request message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -105,7 +105,7 @@ static inline uint16_t mavlink_msg_mission_request_pack_chan(uint8_t system_id, } /** - * @brief Encode a mission_request struct into a message + * @brief Encode a mission_request struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -117,6 +117,20 @@ static inline uint16_t mavlink_msg_mission_request_encode(uint8_t system_id, uin return mavlink_msg_mission_request_pack(system_id, component_id, msg, mission_request->target_system, mission_request->target_component, mission_request->seq); } +/** + * @brief Encode a mission_request struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_request C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_request_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_request_t* mission_request) +{ + return mavlink_msg_mission_request_pack_chan(system_id, component_id, chan, msg, mission_request->target_system, mission_request->target_component, mission_request->seq); +} + /** * @brief Send a mission_request message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_list.h index 391df7f4da..d999babdbd 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_list.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_list.h @@ -64,7 +64,7 @@ static inline uint16_t mavlink_msg_mission_request_list_pack(uint8_t system_id, * @brief Pack a mission_request_list message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -97,7 +97,7 @@ static inline uint16_t mavlink_msg_mission_request_list_pack_chan(uint8_t system } /** - * @brief Encode a mission_request_list struct into a message + * @brief Encode a mission_request_list struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -109,6 +109,20 @@ static inline uint16_t mavlink_msg_mission_request_list_encode(uint8_t system_id return mavlink_msg_mission_request_list_pack(system_id, component_id, msg, mission_request_list->target_system, mission_request_list->target_component); } +/** + * @brief Encode a mission_request_list struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_request_list C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_request_list_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_request_list_t* mission_request_list) +{ + return mavlink_msg_mission_request_list_pack_chan(system_id, component_id, chan, msg, mission_request_list->target_system, mission_request_list->target_component); +} + /** * @brief Send a mission_request_list message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_partial_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_partial_list.h index d5a1c69397..35c7e12856 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_partial_list.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_partial_list.h @@ -74,7 +74,7 @@ static inline uint16_t mavlink_msg_mission_request_partial_list_pack(uint8_t sys * @brief Pack a mission_request_partial_list message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -113,7 +113,7 @@ static inline uint16_t mavlink_msg_mission_request_partial_list_pack_chan(uint8_ } /** - * @brief Encode a mission_request_partial_list struct into a message + * @brief Encode a mission_request_partial_list struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -125,6 +125,20 @@ static inline uint16_t mavlink_msg_mission_request_partial_list_encode(uint8_t s return mavlink_msg_mission_request_partial_list_pack(system_id, component_id, msg, mission_request_partial_list->target_system, mission_request_partial_list->target_component, mission_request_partial_list->start_index, mission_request_partial_list->end_index); } +/** + * @brief Encode a mission_request_partial_list struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_request_partial_list C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_request_partial_list_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_request_partial_list_t* mission_request_partial_list) +{ + return mavlink_msg_mission_request_partial_list_pack_chan(system_id, component_id, chan, msg, mission_request_partial_list->target_system, mission_request_partial_list->target_component, mission_request_partial_list->start_index, mission_request_partial_list->end_index); +} + /** * @brief Send a mission_request_partial_list message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_set_current.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_set_current.h index 2e145aa3e9..63b37cf8c8 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_set_current.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_set_current.h @@ -69,7 +69,7 @@ static inline uint16_t mavlink_msg_mission_set_current_pack(uint8_t system_id, u * @brief Pack a mission_set_current message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -105,7 +105,7 @@ static inline uint16_t mavlink_msg_mission_set_current_pack_chan(uint8_t system_ } /** - * @brief Encode a mission_set_current struct into a message + * @brief Encode a mission_set_current struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -117,6 +117,20 @@ static inline uint16_t mavlink_msg_mission_set_current_encode(uint8_t system_id, return mavlink_msg_mission_set_current_pack(system_id, component_id, msg, mission_set_current->target_system, mission_set_current->target_component, mission_set_current->seq); } +/** + * @brief Encode a mission_set_current struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_set_current C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_set_current_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_set_current_t* mission_set_current) +{ + return mavlink_msg_mission_set_current_pack_chan(system_id, component_id, chan, msg, mission_set_current->target_system, mission_set_current->target_component, mission_set_current->seq); +} + /** * @brief Send a mission_set_current message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_write_partial_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_write_partial_list.h index 6342f60383..7de38bd7b5 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_write_partial_list.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_write_partial_list.h @@ -74,7 +74,7 @@ static inline uint16_t mavlink_msg_mission_write_partial_list_pack(uint8_t syste * @brief Pack a mission_write_partial_list message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -113,7 +113,7 @@ static inline uint16_t mavlink_msg_mission_write_partial_list_pack_chan(uint8_t } /** - * @brief Encode a mission_write_partial_list struct into a message + * @brief Encode a mission_write_partial_list struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -125,6 +125,20 @@ static inline uint16_t mavlink_msg_mission_write_partial_list_encode(uint8_t sys return mavlink_msg_mission_write_partial_list_pack(system_id, component_id, msg, mission_write_partial_list->target_system, mission_write_partial_list->target_component, mission_write_partial_list->start_index, mission_write_partial_list->end_index); } +/** + * @brief Encode a mission_write_partial_list struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_write_partial_list C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_write_partial_list_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_write_partial_list_t* mission_write_partial_list) +{ + return mavlink_msg_mission_write_partial_list_pack_chan(system_id, component_id, chan, msg, mission_write_partial_list->target_system, mission_write_partial_list->target_component, mission_write_partial_list->start_index, mission_write_partial_list->end_index); +} + /** * @brief Send a mission_write_partial_list message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_float.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_float.h index a9d28a3d05..fbf4f75c95 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_float.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_float.h @@ -67,7 +67,7 @@ static inline uint16_t mavlink_msg_named_value_float_pack(uint8_t system_id, uin * @brief Pack a named_value_float message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp (milliseconds since system boot) * @param name Name of the debug variable @@ -101,7 +101,7 @@ static inline uint16_t mavlink_msg_named_value_float_pack_chan(uint8_t system_id } /** - * @brief Encode a named_value_float struct into a message + * @brief Encode a named_value_float struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -113,6 +113,20 @@ static inline uint16_t mavlink_msg_named_value_float_encode(uint8_t system_id, u return mavlink_msg_named_value_float_pack(system_id, component_id, msg, named_value_float->time_boot_ms, named_value_float->name, named_value_float->value); } +/** + * @brief Encode a named_value_float struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param named_value_float C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_named_value_float_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_named_value_float_t* named_value_float) +{ + return mavlink_msg_named_value_float_pack_chan(system_id, component_id, chan, msg, named_value_float->time_boot_ms, named_value_float->name, named_value_float->value); +} + /** * @brief Send a named_value_float message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_int.h index ea53ea888d..052f247935 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_int.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_int.h @@ -67,7 +67,7 @@ static inline uint16_t mavlink_msg_named_value_int_pack(uint8_t system_id, uint8 * @brief Pack a named_value_int message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp (milliseconds since system boot) * @param name Name of the debug variable @@ -101,7 +101,7 @@ static inline uint16_t mavlink_msg_named_value_int_pack_chan(uint8_t system_id, } /** - * @brief Encode a named_value_int struct into a message + * @brief Encode a named_value_int struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -113,6 +113,20 @@ static inline uint16_t mavlink_msg_named_value_int_encode(uint8_t system_id, uin return mavlink_msg_named_value_int_pack(system_id, component_id, msg, named_value_int->time_boot_ms, named_value_int->name, named_value_int->value); } +/** + * @brief Encode a named_value_int struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param named_value_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_named_value_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_named_value_int_t* named_value_int) +{ + return mavlink_msg_named_value_int_pack_chan(system_id, component_id, chan, msg, named_value_int->time_boot_ms, named_value_int->name, named_value_int->value); +} + /** * @brief Send a named_value_int message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_nav_controller_output.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_nav_controller_output.h index e9fa0f5220..e95c144de7 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_nav_controller_output.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_nav_controller_output.h @@ -94,7 +94,7 @@ static inline uint16_t mavlink_msg_nav_controller_output_pack(uint8_t system_id, * @brief Pack a nav_controller_output message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param nav_roll Current desired roll in degrees * @param nav_pitch Current desired pitch in degrees @@ -145,7 +145,7 @@ static inline uint16_t mavlink_msg_nav_controller_output_pack_chan(uint8_t syste } /** - * @brief Encode a nav_controller_output struct into a message + * @brief Encode a nav_controller_output struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -157,6 +157,20 @@ static inline uint16_t mavlink_msg_nav_controller_output_encode(uint8_t system_i return mavlink_msg_nav_controller_output_pack(system_id, component_id, msg, nav_controller_output->nav_roll, nav_controller_output->nav_pitch, nav_controller_output->nav_bearing, nav_controller_output->target_bearing, nav_controller_output->wp_dist, nav_controller_output->alt_error, nav_controller_output->aspd_error, nav_controller_output->xtrack_error); } +/** + * @brief Encode a nav_controller_output struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param nav_controller_output C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_nav_controller_output_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_nav_controller_output_t* nav_controller_output) +{ + return mavlink_msg_nav_controller_output_pack_chan(system_id, component_id, chan, msg, nav_controller_output->nav_roll, nav_controller_output->nav_pitch, nav_controller_output->nav_bearing, nav_controller_output->target_bearing, nav_controller_output->wp_dist, nav_controller_output->alt_error, nav_controller_output->aspd_error, nav_controller_output->xtrack_error); +} + /** * @brief Send a nav_controller_output message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_omnidirectional_flow.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_omnidirectional_flow.h index c0e765a444..4debb6e663 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_omnidirectional_flow.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_omnidirectional_flow.h @@ -83,7 +83,7 @@ static inline uint16_t mavlink_msg_omnidirectional_flow_pack(uint8_t system_id, * @brief Pack a omnidirectional_flow message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot) * @param sensor_id Sensor ID @@ -126,7 +126,7 @@ static inline uint16_t mavlink_msg_omnidirectional_flow_pack_chan(uint8_t system } /** - * @brief Encode a omnidirectional_flow struct into a message + * @brief Encode a omnidirectional_flow struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -138,6 +138,20 @@ static inline uint16_t mavlink_msg_omnidirectional_flow_encode(uint8_t system_id return mavlink_msg_omnidirectional_flow_pack(system_id, component_id, msg, omnidirectional_flow->time_usec, omnidirectional_flow->sensor_id, omnidirectional_flow->left, omnidirectional_flow->right, omnidirectional_flow->quality, omnidirectional_flow->front_distance_m); } +/** + * @brief Encode a omnidirectional_flow struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param omnidirectional_flow C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_omnidirectional_flow_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_omnidirectional_flow_t* omnidirectional_flow) +{ + return mavlink_msg_omnidirectional_flow_pack_chan(system_id, component_id, chan, msg, omnidirectional_flow->time_usec, omnidirectional_flow->sensor_id, omnidirectional_flow->left, omnidirectional_flow->right, omnidirectional_flow->quality, omnidirectional_flow->front_distance_m); +} + /** * @brief Send a omnidirectional_flow message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_optical_flow.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_optical_flow.h index e01dc5e79c..cf6db9147e 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_optical_flow.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_optical_flow.h @@ -8,8 +8,8 @@ typedef struct __mavlink_optical_flow_t float flow_comp_m_x; ///< Flow in meters in x-sensor direction, angular-speed compensated float flow_comp_m_y; ///< Flow in meters in y-sensor direction, angular-speed compensated float ground_distance; ///< Ground distance in meters. Positive value: distance known. Negative value: Unknown distance - int16_t flow_x; ///< Flow in pixels in x-sensor direction - int16_t flow_y; ///< Flow in pixels in y-sensor direction + int16_t flow_x; ///< Flow in pixels * 10 in x-sensor direction (dezi-pixels) + int16_t flow_y; ///< Flow in pixels * 10 in y-sensor direction (dezi-pixels) uint8_t sensor_id; ///< Sensor ID uint8_t quality; ///< Optical flow quality / confidence. 0: bad, 255: maximum quality } mavlink_optical_flow_t; @@ -45,8 +45,8 @@ typedef struct __mavlink_optical_flow_t * * @param time_usec Timestamp (UNIX) * @param sensor_id Sensor ID - * @param flow_x Flow in pixels in x-sensor direction - * @param flow_y Flow in pixels in y-sensor direction + * @param flow_x Flow in pixels * 10 in x-sensor direction (dezi-pixels) + * @param flow_y Flow in pixels * 10 in y-sensor direction (dezi-pixels) * @param flow_comp_m_x Flow in meters in x-sensor direction, angular-speed compensated * @param flow_comp_m_y Flow in meters in y-sensor direction, angular-speed compensated * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality @@ -94,12 +94,12 @@ static inline uint16_t mavlink_msg_optical_flow_pack(uint8_t system_id, uint8_t * @brief Pack a optical_flow message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (UNIX) * @param sensor_id Sensor ID - * @param flow_x Flow in pixels in x-sensor direction - * @param flow_y Flow in pixels in y-sensor direction + * @param flow_x Flow in pixels * 10 in x-sensor direction (dezi-pixels) + * @param flow_y Flow in pixels * 10 in y-sensor direction (dezi-pixels) * @param flow_comp_m_x Flow in meters in x-sensor direction, angular-speed compensated * @param flow_comp_m_y Flow in meters in y-sensor direction, angular-speed compensated * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality @@ -145,7 +145,7 @@ static inline uint16_t mavlink_msg_optical_flow_pack_chan(uint8_t system_id, uin } /** - * @brief Encode a optical_flow struct into a message + * @brief Encode a optical_flow struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -157,14 +157,28 @@ static inline uint16_t mavlink_msg_optical_flow_encode(uint8_t system_id, uint8_ return mavlink_msg_optical_flow_pack(system_id, component_id, msg, optical_flow->time_usec, optical_flow->sensor_id, optical_flow->flow_x, optical_flow->flow_y, optical_flow->flow_comp_m_x, optical_flow->flow_comp_m_y, optical_flow->quality, optical_flow->ground_distance); } +/** + * @brief Encode a optical_flow struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param optical_flow C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_optical_flow_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_optical_flow_t* optical_flow) +{ + return mavlink_msg_optical_flow_pack_chan(system_id, component_id, chan, msg, optical_flow->time_usec, optical_flow->sensor_id, optical_flow->flow_x, optical_flow->flow_y, optical_flow->flow_comp_m_x, optical_flow->flow_comp_m_y, optical_flow->quality, optical_flow->ground_distance); +} + /** * @brief Send a optical_flow message * @param chan MAVLink channel to send the message * * @param time_usec Timestamp (UNIX) * @param sensor_id Sensor ID - * @param flow_x Flow in pixels in x-sensor direction - * @param flow_y Flow in pixels in y-sensor direction + * @param flow_x Flow in pixels * 10 in x-sensor direction (dezi-pixels) + * @param flow_y Flow in pixels * 10 in y-sensor direction (dezi-pixels) * @param flow_comp_m_x Flow in meters in x-sensor direction, angular-speed compensated * @param flow_comp_m_y Flow in meters in y-sensor direction, angular-speed compensated * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality @@ -237,7 +251,7 @@ static inline uint8_t mavlink_msg_optical_flow_get_sensor_id(const mavlink_messa /** * @brief Get field flow_x from optical_flow message * - * @return Flow in pixels in x-sensor direction + * @return Flow in pixels * 10 in x-sensor direction (dezi-pixels) */ static inline int16_t mavlink_msg_optical_flow_get_flow_x(const mavlink_message_t* msg) { @@ -247,7 +261,7 @@ static inline int16_t mavlink_msg_optical_flow_get_flow_x(const mavlink_message_ /** * @brief Get field flow_y from optical_flow message * - * @return Flow in pixels in y-sensor direction + * @return Flow in pixels * 10 in y-sensor direction (dezi-pixels) */ static inline int16_t mavlink_msg_optical_flow_get_flow_y(const mavlink_message_t* msg) { diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_list.h index da61181b2c..39e0072741 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_list.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_list.h @@ -64,7 +64,7 @@ static inline uint16_t mavlink_msg_param_request_list_pack(uint8_t system_id, ui * @brief Pack a param_request_list message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -97,7 +97,7 @@ static inline uint16_t mavlink_msg_param_request_list_pack_chan(uint8_t system_i } /** - * @brief Encode a param_request_list struct into a message + * @brief Encode a param_request_list struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -109,6 +109,20 @@ static inline uint16_t mavlink_msg_param_request_list_encode(uint8_t system_id, return mavlink_msg_param_request_list_pack(system_id, component_id, msg, param_request_list->target_system, param_request_list->target_component); } +/** + * @brief Encode a param_request_list struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param param_request_list C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_request_list_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_param_request_list_t* param_request_list) +{ + return mavlink_msg_param_request_list_pack_chan(system_id, component_id, chan, msg, param_request_list->target_system, param_request_list->target_component); +} + /** * @brief Send a param_request_list message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_read.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_read.h index 6b15680260..5d9113114b 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_read.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_read.h @@ -72,7 +72,7 @@ static inline uint16_t mavlink_msg_param_request_read_pack(uint8_t system_id, ui * @brief Pack a param_request_read message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -109,7 +109,7 @@ static inline uint16_t mavlink_msg_param_request_read_pack_chan(uint8_t system_i } /** - * @brief Encode a param_request_read struct into a message + * @brief Encode a param_request_read struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -121,6 +121,20 @@ static inline uint16_t mavlink_msg_param_request_read_encode(uint8_t system_id, return mavlink_msg_param_request_read_pack(system_id, component_id, msg, param_request_read->target_system, param_request_read->target_component, param_request_read->param_id, param_request_read->param_index); } +/** + * @brief Encode a param_request_read struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param param_request_read C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_request_read_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_param_request_read_t* param_request_read) +{ + return mavlink_msg_param_request_read_pack_chan(system_id, component_id, chan, msg, param_request_read->target_system, param_request_read->target_component, param_request_read->param_id, param_request_read->param_index); +} + /** * @brief Send a param_request_read message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_set.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_set.h index 66b0f6629b..1bd1f00596 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_set.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_set.h @@ -77,7 +77,7 @@ static inline uint16_t mavlink_msg_param_set_pack(uint8_t system_id, uint8_t com * @brief Pack a param_set message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -117,7 +117,7 @@ static inline uint16_t mavlink_msg_param_set_pack_chan(uint8_t system_id, uint8_ } /** - * @brief Encode a param_set struct into a message + * @brief Encode a param_set struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -129,6 +129,20 @@ static inline uint16_t mavlink_msg_param_set_encode(uint8_t system_id, uint8_t c return mavlink_msg_param_set_pack(system_id, component_id, msg, param_set->target_system, param_set->target_component, param_set->param_id, param_set->param_value, param_set->param_type); } +/** + * @brief Encode a param_set struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param param_set C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_set_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_param_set_t* param_set) +{ + return mavlink_msg_param_set_pack_chan(system_id, component_id, chan, msg, param_set->target_system, param_set->target_component, param_set->param_id, param_set->param_value, param_set->param_type); +} + /** * @brief Send a param_set message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_value.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_value.h index 5991393746..17c759811f 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_value.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_value.h @@ -77,7 +77,7 @@ static inline uint16_t mavlink_msg_param_value_pack(uint8_t system_id, uint8_t c * @brief Pack a param_value message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string * @param param_value Onboard parameter value @@ -117,7 +117,7 @@ static inline uint16_t mavlink_msg_param_value_pack_chan(uint8_t system_id, uint } /** - * @brief Encode a param_value struct into a message + * @brief Encode a param_value struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -129,6 +129,20 @@ static inline uint16_t mavlink_msg_param_value_encode(uint8_t system_id, uint8_t return mavlink_msg_param_value_pack(system_id, component_id, msg, param_value->param_id, param_value->param_value, param_value->param_type, param_value->param_count, param_value->param_index); } +/** + * @brief Encode a param_value struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param param_value C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_value_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_param_value_t* param_value) +{ + return mavlink_msg_param_value_pack_chan(system_id, component_id, chan, msg, param_value->param_id, param_value->param_value, param_value->param_type, param_value->param_count, param_value->param_index); +} + /** * @brief Send a param_value message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_ping.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_ping.h index 5a4c50907d..0c68ca1765 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_ping.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_ping.h @@ -74,7 +74,7 @@ static inline uint16_t mavlink_msg_ping_pack(uint8_t system_id, uint8_t componen * @brief Pack a ping message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Unix timestamp in microseconds * @param seq PING sequence @@ -113,7 +113,7 @@ static inline uint16_t mavlink_msg_ping_pack_chan(uint8_t system_id, uint8_t com } /** - * @brief Encode a ping struct into a message + * @brief Encode a ping struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -125,6 +125,20 @@ static inline uint16_t mavlink_msg_ping_encode(uint8_t system_id, uint8_t compon return mavlink_msg_ping_pack(system_id, component_id, msg, ping->time_usec, ping->seq, ping->target_system, ping->target_component); } +/** + * @brief Encode a ping struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param ping C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ping_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_ping_t* ping) +{ + return mavlink_msg_ping_pack_chan(system_id, component_id, chan, msg, ping->time_usec, ping->seq, ping->target_system, ping->target_component); +} + /** * @brief Send a ping message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_radio_status.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_radio_status.h index 06e6a55425..fceb7d1688 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_radio_status.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_radio_status.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_radio_status_pack(uint8_t system_id, uint8_t * @brief Pack a radio_status message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param rssi local signal strength * @param remrssi remote signal strength @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_radio_status_pack_chan(uint8_t system_id, uin } /** - * @brief Encode a radio_status struct into a message + * @brief Encode a radio_status struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -149,6 +149,20 @@ static inline uint16_t mavlink_msg_radio_status_encode(uint8_t system_id, uint8_ return mavlink_msg_radio_status_pack(system_id, component_id, msg, radio_status->rssi, radio_status->remrssi, radio_status->txbuf, radio_status->noise, radio_status->remnoise, radio_status->rxerrors, radio_status->fixed); } +/** + * @brief Encode a radio_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param radio_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_radio_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_radio_status_t* radio_status) +{ + return mavlink_msg_radio_status_pack_chan(system_id, component_id, chan, msg, radio_status->rssi, radio_status->remrssi, radio_status->txbuf, radio_status->noise, radio_status->remnoise, radio_status->rxerrors, radio_status->fixed); +} + /** * @brief Send a radio_status message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_imu.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_imu.h index ce88636473..62a9b6eeae 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_imu.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_imu.h @@ -104,7 +104,7 @@ static inline uint16_t mavlink_msg_raw_imu_pack(uint8_t system_id, uint8_t compo * @brief Pack a raw_imu message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) * @param xacc X acceleration (raw) @@ -161,7 +161,7 @@ static inline uint16_t mavlink_msg_raw_imu_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a raw_imu struct into a message + * @brief Encode a raw_imu struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -173,6 +173,20 @@ static inline uint16_t mavlink_msg_raw_imu_encode(uint8_t system_id, uint8_t com return mavlink_msg_raw_imu_pack(system_id, component_id, msg, raw_imu->time_usec, raw_imu->xacc, raw_imu->yacc, raw_imu->zacc, raw_imu->xgyro, raw_imu->ygyro, raw_imu->zgyro, raw_imu->xmag, raw_imu->ymag, raw_imu->zmag); } +/** + * @brief Encode a raw_imu struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param raw_imu C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_raw_imu_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_raw_imu_t* raw_imu) +{ + return mavlink_msg_raw_imu_pack_chan(system_id, component_id, chan, msg, raw_imu->time_usec, raw_imu->xacc, raw_imu->yacc, raw_imu->zacc, raw_imu->xgyro, raw_imu->ygyro, raw_imu->zgyro, raw_imu->xmag, raw_imu->ymag, raw_imu->zmag); +} + /** * @brief Send a raw_imu message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_pressure.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_pressure.h index dcc2cbe063..82c5fad4ab 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_pressure.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_pressure.h @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_raw_pressure_pack(uint8_t system_id, uint8_t * @brief Pack a raw_pressure message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) * @param press_abs Absolute pressure (raw) @@ -121,7 +121,7 @@ static inline uint16_t mavlink_msg_raw_pressure_pack_chan(uint8_t system_id, uin } /** - * @brief Encode a raw_pressure struct into a message + * @brief Encode a raw_pressure struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -133,6 +133,20 @@ static inline uint16_t mavlink_msg_raw_pressure_encode(uint8_t system_id, uint8_ return mavlink_msg_raw_pressure_pack(system_id, component_id, msg, raw_pressure->time_usec, raw_pressure->press_abs, raw_pressure->press_diff1, raw_pressure->press_diff2, raw_pressure->temperature); } +/** + * @brief Encode a raw_pressure struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param raw_pressure C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_raw_pressure_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_raw_pressure_t* raw_pressure) +{ + return mavlink_msg_raw_pressure_pack_chan(system_id, component_id, chan, msg, raw_pressure->time_usec, raw_pressure->press_abs, raw_pressure->press_diff1, raw_pressure->press_diff2, raw_pressure->temperature); +} + /** * @brief Send a raw_pressure message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_override.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_override.h index 9854a190ce..0d8a1514b1 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_override.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_override.h @@ -4,14 +4,14 @@ typedef struct __mavlink_rc_channels_override_t { - uint16_t chan1_raw; ///< RC channel 1 value, in microseconds - uint16_t chan2_raw; ///< RC channel 2 value, in microseconds - uint16_t chan3_raw; ///< RC channel 3 value, in microseconds - uint16_t chan4_raw; ///< RC channel 4 value, in microseconds - uint16_t chan5_raw; ///< RC channel 5 value, in microseconds - uint16_t chan6_raw; ///< RC channel 6 value, in microseconds - uint16_t chan7_raw; ///< RC channel 7 value, in microseconds - uint16_t chan8_raw; ///< RC channel 8 value, in microseconds + uint16_t chan1_raw; ///< RC channel 1 value, in microseconds. A value of UINT16_MAX means to ignore this field. + uint16_t chan2_raw; ///< RC channel 2 value, in microseconds. A value of UINT16_MAX means to ignore this field. + uint16_t chan3_raw; ///< RC channel 3 value, in microseconds. A value of UINT16_MAX means to ignore this field. + uint16_t chan4_raw; ///< RC channel 4 value, in microseconds. A value of UINT16_MAX means to ignore this field. + uint16_t chan5_raw; ///< RC channel 5 value, in microseconds. A value of UINT16_MAX means to ignore this field. + uint16_t chan6_raw; ///< RC channel 6 value, in microseconds. A value of UINT16_MAX means to ignore this field. + uint16_t chan7_raw; ///< RC channel 7 value, in microseconds. A value of UINT16_MAX means to ignore this field. + uint16_t chan8_raw; ///< RC channel 8 value, in microseconds. A value of UINT16_MAX means to ignore this field. uint8_t target_system; ///< System ID uint8_t target_component; ///< Component ID } mavlink_rc_channels_override_t; @@ -49,14 +49,14 @@ typedef struct __mavlink_rc_channels_override_t * * @param target_system System ID * @param target_component Component ID - * @param chan1_raw RC channel 1 value, in microseconds - * @param chan2_raw RC channel 2 value, in microseconds - * @param chan3_raw RC channel 3 value, in microseconds - * @param chan4_raw RC channel 4 value, in microseconds - * @param chan5_raw RC channel 5 value, in microseconds - * @param chan6_raw RC channel 6 value, in microseconds - * @param chan7_raw RC channel 7 value, in microseconds - * @param chan8_raw RC channel 8 value, in microseconds + * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX means to ignore this field. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_rc_channels_override_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, @@ -104,18 +104,18 @@ static inline uint16_t mavlink_msg_rc_channels_override_pack(uint8_t system_id, * @brief Pack a rc_channels_override message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID - * @param chan1_raw RC channel 1 value, in microseconds - * @param chan2_raw RC channel 2 value, in microseconds - * @param chan3_raw RC channel 3 value, in microseconds - * @param chan4_raw RC channel 4 value, in microseconds - * @param chan5_raw RC channel 5 value, in microseconds - * @param chan6_raw RC channel 6 value, in microseconds - * @param chan7_raw RC channel 7 value, in microseconds - * @param chan8_raw RC channel 8 value, in microseconds + * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX means to ignore this field. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_rc_channels_override_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, @@ -161,7 +161,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_pack_chan(uint8_t system } /** - * @brief Encode a rc_channels_override struct into a message + * @brief Encode a rc_channels_override struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -173,20 +173,34 @@ static inline uint16_t mavlink_msg_rc_channels_override_encode(uint8_t system_id return mavlink_msg_rc_channels_override_pack(system_id, component_id, msg, rc_channels_override->target_system, rc_channels_override->target_component, rc_channels_override->chan1_raw, rc_channels_override->chan2_raw, rc_channels_override->chan3_raw, rc_channels_override->chan4_raw, rc_channels_override->chan5_raw, rc_channels_override->chan6_raw, rc_channels_override->chan7_raw, rc_channels_override->chan8_raw); } +/** + * @brief Encode a rc_channels_override struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param rc_channels_override C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rc_channels_override_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rc_channels_override_t* rc_channels_override) +{ + return mavlink_msg_rc_channels_override_pack_chan(system_id, component_id, chan, msg, rc_channels_override->target_system, rc_channels_override->target_component, rc_channels_override->chan1_raw, rc_channels_override->chan2_raw, rc_channels_override->chan3_raw, rc_channels_override->chan4_raw, rc_channels_override->chan5_raw, rc_channels_override->chan6_raw, rc_channels_override->chan7_raw, rc_channels_override->chan8_raw); +} + /** * @brief Send a rc_channels_override message * @param chan MAVLink channel to send the message * * @param target_system System ID * @param target_component Component ID - * @param chan1_raw RC channel 1 value, in microseconds - * @param chan2_raw RC channel 2 value, in microseconds - * @param chan3_raw RC channel 3 value, in microseconds - * @param chan4_raw RC channel 4 value, in microseconds - * @param chan5_raw RC channel 5 value, in microseconds - * @param chan6_raw RC channel 6 value, in microseconds - * @param chan7_raw RC channel 7 value, in microseconds - * @param chan8_raw RC channel 8 value, in microseconds + * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX means to ignore this field. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -259,7 +273,7 @@ static inline uint8_t mavlink_msg_rc_channels_override_get_target_component(cons /** * @brief Get field chan1_raw from rc_channels_override message * - * @return RC channel 1 value, in microseconds + * @return RC channel 1 value, in microseconds. A value of UINT16_MAX means to ignore this field. */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan1_raw(const mavlink_message_t* msg) { @@ -269,7 +283,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan1_raw(const mavl /** * @brief Get field chan2_raw from rc_channels_override message * - * @return RC channel 2 value, in microseconds + * @return RC channel 2 value, in microseconds. A value of UINT16_MAX means to ignore this field. */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan2_raw(const mavlink_message_t* msg) { @@ -279,7 +293,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan2_raw(const mavl /** * @brief Get field chan3_raw from rc_channels_override message * - * @return RC channel 3 value, in microseconds + * @return RC channel 3 value, in microseconds. A value of UINT16_MAX means to ignore this field. */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan3_raw(const mavlink_message_t* msg) { @@ -289,7 +303,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan3_raw(const mavl /** * @brief Get field chan4_raw from rc_channels_override message * - * @return RC channel 4 value, in microseconds + * @return RC channel 4 value, in microseconds. A value of UINT16_MAX means to ignore this field. */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan4_raw(const mavlink_message_t* msg) { @@ -299,7 +313,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan4_raw(const mavl /** * @brief Get field chan5_raw from rc_channels_override message * - * @return RC channel 5 value, in microseconds + * @return RC channel 5 value, in microseconds. A value of UINT16_MAX means to ignore this field. */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan5_raw(const mavlink_message_t* msg) { @@ -309,7 +323,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan5_raw(const mavl /** * @brief Get field chan6_raw from rc_channels_override message * - * @return RC channel 6 value, in microseconds + * @return RC channel 6 value, in microseconds. A value of UINT16_MAX means to ignore this field. */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan6_raw(const mavlink_message_t* msg) { @@ -319,7 +333,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan6_raw(const mavl /** * @brief Get field chan7_raw from rc_channels_override message * - * @return RC channel 7 value, in microseconds + * @return RC channel 7 value, in microseconds. A value of UINT16_MAX means to ignore this field. */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan7_raw(const mavlink_message_t* msg) { @@ -329,7 +343,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan7_raw(const mavl /** * @brief Get field chan8_raw from rc_channels_override message * - * @return RC channel 8 value, in microseconds + * @return RC channel 8 value, in microseconds. A value of UINT16_MAX means to ignore this field. */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan8_raw(const mavlink_message_t* msg) { diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_raw.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_raw.h index 4c1315bed2..3c0aed0202 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_raw.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_raw.h @@ -5,14 +5,14 @@ typedef struct __mavlink_rc_channels_raw_t { uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) - uint16_t chan1_raw; ///< RC channel 1 value, in microseconds. A value of 65535 implies the channel is unused. - uint16_t chan2_raw; ///< RC channel 2 value, in microseconds. A value of 65535 implies the channel is unused. - uint16_t chan3_raw; ///< RC channel 3 value, in microseconds. A value of 65535 implies the channel is unused. - uint16_t chan4_raw; ///< RC channel 4 value, in microseconds. A value of 65535 implies the channel is unused. - uint16_t chan5_raw; ///< RC channel 5 value, in microseconds. A value of 65535 implies the channel is unused. - uint16_t chan6_raw; ///< RC channel 6 value, in microseconds. A value of 65535 implies the channel is unused. - uint16_t chan7_raw; ///< RC channel 7 value, in microseconds. A value of 65535 implies the channel is unused. - uint16_t chan8_raw; ///< RC channel 8 value, in microseconds. A value of 65535 implies the channel is unused. + uint16_t chan1_raw; ///< RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan2_raw; ///< RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan3_raw; ///< RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan4_raw; ///< RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan5_raw; ///< RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan6_raw; ///< RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan7_raw; ///< RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan8_raw; ///< RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. uint8_t port; ///< Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos. uint8_t rssi; ///< Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. } mavlink_rc_channels_raw_t; @@ -51,14 +51,14 @@ typedef struct __mavlink_rc_channels_raw_t * * @param time_boot_ms Timestamp (milliseconds since system boot) * @param port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos. - * @param chan1_raw RC channel 1 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan2_raw RC channel 2 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan3_raw RC channel 3 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan4_raw RC channel 4 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan5_raw RC channel 5 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan6_raw RC channel 6 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan7_raw RC channel 7 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan8_raw RC channel 8 value, in microseconds. A value of 65535 implies the channel is unused. + * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. * @return length of the message in bytes (excluding serial stream start sign) */ @@ -109,18 +109,18 @@ static inline uint16_t mavlink_msg_rc_channels_raw_pack(uint8_t system_id, uint8 * @brief Pack a rc_channels_raw message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp (milliseconds since system boot) * @param port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos. - * @param chan1_raw RC channel 1 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan2_raw RC channel 2 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan3_raw RC channel 3 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan4_raw RC channel 4 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan5_raw RC channel 5 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan6_raw RC channel 6 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan7_raw RC channel 7 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan8_raw RC channel 8 value, in microseconds. A value of 65535 implies the channel is unused. + * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. * @return length of the message in bytes (excluding serial stream start sign) */ @@ -169,7 +169,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_pack_chan(uint8_t system_id, } /** - * @brief Encode a rc_channels_raw struct into a message + * @brief Encode a rc_channels_raw struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -181,20 +181,34 @@ static inline uint16_t mavlink_msg_rc_channels_raw_encode(uint8_t system_id, uin return mavlink_msg_rc_channels_raw_pack(system_id, component_id, msg, rc_channels_raw->time_boot_ms, rc_channels_raw->port, rc_channels_raw->chan1_raw, rc_channels_raw->chan2_raw, rc_channels_raw->chan3_raw, rc_channels_raw->chan4_raw, rc_channels_raw->chan5_raw, rc_channels_raw->chan6_raw, rc_channels_raw->chan7_raw, rc_channels_raw->chan8_raw, rc_channels_raw->rssi); } +/** + * @brief Encode a rc_channels_raw struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param rc_channels_raw C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rc_channels_raw_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rc_channels_raw_t* rc_channels_raw) +{ + return mavlink_msg_rc_channels_raw_pack_chan(system_id, component_id, chan, msg, rc_channels_raw->time_boot_ms, rc_channels_raw->port, rc_channels_raw->chan1_raw, rc_channels_raw->chan2_raw, rc_channels_raw->chan3_raw, rc_channels_raw->chan4_raw, rc_channels_raw->chan5_raw, rc_channels_raw->chan6_raw, rc_channels_raw->chan7_raw, rc_channels_raw->chan8_raw, rc_channels_raw->rssi); +} + /** * @brief Send a rc_channels_raw message * @param chan MAVLink channel to send the message * * @param time_boot_ms Timestamp (milliseconds since system boot) * @param port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos. - * @param chan1_raw RC channel 1 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan2_raw RC channel 2 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan3_raw RC channel 3 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan4_raw RC channel 4 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan5_raw RC channel 5 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan6_raw RC channel 6 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan7_raw RC channel 7 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan8_raw RC channel 8 value, in microseconds. A value of 65535 implies the channel is unused. + * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -270,7 +284,7 @@ static inline uint8_t mavlink_msg_rc_channels_raw_get_port(const mavlink_message /** * @brief Get field chan1_raw from rc_channels_raw message * - * @return RC channel 1 value, in microseconds. A value of 65535 implies the channel is unused. + * @return RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan1_raw(const mavlink_message_t* msg) { @@ -280,7 +294,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan1_raw(const mavlink_m /** * @brief Get field chan2_raw from rc_channels_raw message * - * @return RC channel 2 value, in microseconds. A value of 65535 implies the channel is unused. + * @return RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan2_raw(const mavlink_message_t* msg) { @@ -290,7 +304,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan2_raw(const mavlink_m /** * @brief Get field chan3_raw from rc_channels_raw message * - * @return RC channel 3 value, in microseconds. A value of 65535 implies the channel is unused. + * @return RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan3_raw(const mavlink_message_t* msg) { @@ -300,7 +314,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan3_raw(const mavlink_m /** * @brief Get field chan4_raw from rc_channels_raw message * - * @return RC channel 4 value, in microseconds. A value of 65535 implies the channel is unused. + * @return RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan4_raw(const mavlink_message_t* msg) { @@ -310,7 +324,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan4_raw(const mavlink_m /** * @brief Get field chan5_raw from rc_channels_raw message * - * @return RC channel 5 value, in microseconds. A value of 65535 implies the channel is unused. + * @return RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan5_raw(const mavlink_message_t* msg) { @@ -320,7 +334,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan5_raw(const mavlink_m /** * @brief Get field chan6_raw from rc_channels_raw message * - * @return RC channel 6 value, in microseconds. A value of 65535 implies the channel is unused. + * @return RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan6_raw(const mavlink_message_t* msg) { @@ -330,7 +344,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan6_raw(const mavlink_m /** * @brief Get field chan7_raw from rc_channels_raw message * - * @return RC channel 7 value, in microseconds. A value of 65535 implies the channel is unused. + * @return RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan7_raw(const mavlink_message_t* msg) { @@ -340,7 +354,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan7_raw(const mavlink_m /** * @brief Get field chan8_raw from rc_channels_raw message * - * @return RC channel 8 value, in microseconds. A value of 65535 implies the channel is unused. + * @return RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan8_raw(const mavlink_message_t* msg) { diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_scaled.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_scaled.h index be6322bcd3..2153ab3928 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_scaled.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_scaled.h @@ -5,14 +5,14 @@ typedef struct __mavlink_rc_channels_scaled_t { uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) - int16_t chan1_scaled; ///< RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - int16_t chan2_scaled; ///< RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - int16_t chan3_scaled; ///< RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - int16_t chan4_scaled; ///< RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - int16_t chan5_scaled; ///< RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - int16_t chan6_scaled; ///< RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - int16_t chan7_scaled; ///< RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - int16_t chan8_scaled; ///< RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. + int16_t chan1_scaled; ///< RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + int16_t chan2_scaled; ///< RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + int16_t chan3_scaled; ///< RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + int16_t chan4_scaled; ///< RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + int16_t chan5_scaled; ///< RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + int16_t chan6_scaled; ///< RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + int16_t chan7_scaled; ///< RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + int16_t chan8_scaled; ///< RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. uint8_t port; ///< Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos. uint8_t rssi; ///< Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. } mavlink_rc_channels_scaled_t; @@ -51,14 +51,14 @@ typedef struct __mavlink_rc_channels_scaled_t * * @param time_boot_ms Timestamp (milliseconds since system boot) * @param port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos. - * @param chan1_scaled RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan2_scaled RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan3_scaled RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan4_scaled RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan5_scaled RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan6_scaled RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan7_scaled RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan8_scaled RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. + * @param chan1_scaled RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan2_scaled RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan3_scaled RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan4_scaled RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan5_scaled RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan6_scaled RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan7_scaled RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan8_scaled RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. * @return length of the message in bytes (excluding serial stream start sign) */ @@ -109,18 +109,18 @@ static inline uint16_t mavlink_msg_rc_channels_scaled_pack(uint8_t system_id, ui * @brief Pack a rc_channels_scaled message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp (milliseconds since system boot) * @param port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos. - * @param chan1_scaled RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan2_scaled RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan3_scaled RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan4_scaled RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan5_scaled RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan6_scaled RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan7_scaled RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan8_scaled RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. + * @param chan1_scaled RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan2_scaled RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan3_scaled RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan4_scaled RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan5_scaled RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan6_scaled RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan7_scaled RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan8_scaled RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. * @return length of the message in bytes (excluding serial stream start sign) */ @@ -169,7 +169,7 @@ static inline uint16_t mavlink_msg_rc_channels_scaled_pack_chan(uint8_t system_i } /** - * @brief Encode a rc_channels_scaled struct into a message + * @brief Encode a rc_channels_scaled struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -181,20 +181,34 @@ static inline uint16_t mavlink_msg_rc_channels_scaled_encode(uint8_t system_id, return mavlink_msg_rc_channels_scaled_pack(system_id, component_id, msg, rc_channels_scaled->time_boot_ms, rc_channels_scaled->port, rc_channels_scaled->chan1_scaled, rc_channels_scaled->chan2_scaled, rc_channels_scaled->chan3_scaled, rc_channels_scaled->chan4_scaled, rc_channels_scaled->chan5_scaled, rc_channels_scaled->chan6_scaled, rc_channels_scaled->chan7_scaled, rc_channels_scaled->chan8_scaled, rc_channels_scaled->rssi); } +/** + * @brief Encode a rc_channels_scaled struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param rc_channels_scaled C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rc_channels_scaled_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rc_channels_scaled_t* rc_channels_scaled) +{ + return mavlink_msg_rc_channels_scaled_pack_chan(system_id, component_id, chan, msg, rc_channels_scaled->time_boot_ms, rc_channels_scaled->port, rc_channels_scaled->chan1_scaled, rc_channels_scaled->chan2_scaled, rc_channels_scaled->chan3_scaled, rc_channels_scaled->chan4_scaled, rc_channels_scaled->chan5_scaled, rc_channels_scaled->chan6_scaled, rc_channels_scaled->chan7_scaled, rc_channels_scaled->chan8_scaled, rc_channels_scaled->rssi); +} + /** * @brief Send a rc_channels_scaled message * @param chan MAVLink channel to send the message * * @param time_boot_ms Timestamp (milliseconds since system boot) * @param port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos. - * @param chan1_scaled RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan2_scaled RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan3_scaled RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan4_scaled RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan5_scaled RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan6_scaled RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan7_scaled RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan8_scaled RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. + * @param chan1_scaled RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan2_scaled RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan3_scaled RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan4_scaled RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan5_scaled RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan6_scaled RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan7_scaled RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan8_scaled RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -270,7 +284,7 @@ static inline uint8_t mavlink_msg_rc_channels_scaled_get_port(const mavlink_mess /** * @brief Get field chan1_scaled from rc_channels_scaled message * - * @return RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. + * @return RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan1_scaled(const mavlink_message_t* msg) { @@ -280,7 +294,7 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan1_scaled(const mavl /** * @brief Get field chan2_scaled from rc_channels_scaled message * - * @return RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. + * @return RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan2_scaled(const mavlink_message_t* msg) { @@ -290,7 +304,7 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan2_scaled(const mavl /** * @brief Get field chan3_scaled from rc_channels_scaled message * - * @return RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. + * @return RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan3_scaled(const mavlink_message_t* msg) { @@ -300,7 +314,7 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan3_scaled(const mavl /** * @brief Get field chan4_scaled from rc_channels_scaled message * - * @return RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. + * @return RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan4_scaled(const mavlink_message_t* msg) { @@ -310,7 +324,7 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan4_scaled(const mavl /** * @brief Get field chan5_scaled from rc_channels_scaled message * - * @return RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. + * @return RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan5_scaled(const mavlink_message_t* msg) { @@ -320,7 +334,7 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan5_scaled(const mavl /** * @brief Get field chan6_scaled from rc_channels_scaled message * - * @return RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. + * @return RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan6_scaled(const mavlink_message_t* msg) { @@ -330,7 +344,7 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan6_scaled(const mavl /** * @brief Get field chan7_scaled from rc_channels_scaled message * - * @return RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. + * @return RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan7_scaled(const mavlink_message_t* msg) { @@ -340,7 +354,7 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan7_scaled(const mavl /** * @brief Get field chan8_scaled from rc_channels_scaled message * - * @return RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. + * @return RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan8_scaled(const mavlink_message_t* msg) { diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_request_data_stream.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_request_data_stream.h index ee21d1fe07..c754ad8761 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_request_data_stream.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_request_data_stream.h @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_request_data_stream_pack(uint8_t system_id, u * @brief Pack a request_data_stream message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system The target requested to send the message stream. * @param target_component The target requested to send the message stream. @@ -121,7 +121,7 @@ static inline uint16_t mavlink_msg_request_data_stream_pack_chan(uint8_t system_ } /** - * @brief Encode a request_data_stream struct into a message + * @brief Encode a request_data_stream struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -133,6 +133,20 @@ static inline uint16_t mavlink_msg_request_data_stream_encode(uint8_t system_id, return mavlink_msg_request_data_stream_pack(system_id, component_id, msg, request_data_stream->target_system, request_data_stream->target_component, request_data_stream->req_stream_id, request_data_stream->req_message_rate, request_data_stream->start_stop); } +/** + * @brief Encode a request_data_stream struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param request_data_stream C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_request_data_stream_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_request_data_stream_t* request_data_stream) +{ + return mavlink_msg_request_data_stream_pack_chan(system_id, component_id, chan, msg, request_data_stream->target_system, request_data_stream->target_component, request_data_stream->req_stream_id, request_data_stream->req_message_rate, request_data_stream->start_stop); +} + /** * @brief Send a request_data_stream message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint.h index a7e9df94be..ac3ef4fa9f 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint.h @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_pack(uin * @brief Pack a roll_pitch_yaw_rates_thrust_setpoint message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp in milliseconds since system boot * @param roll_rate Desired roll rate in radians per second @@ -121,7 +121,7 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_pack_cha } /** - * @brief Encode a roll_pitch_yaw_rates_thrust_setpoint struct into a message + * @brief Encode a roll_pitch_yaw_rates_thrust_setpoint struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -133,6 +133,20 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_encode(u return mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_pack(system_id, component_id, msg, roll_pitch_yaw_rates_thrust_setpoint->time_boot_ms, roll_pitch_yaw_rates_thrust_setpoint->roll_rate, roll_pitch_yaw_rates_thrust_setpoint->pitch_rate, roll_pitch_yaw_rates_thrust_setpoint->yaw_rate, roll_pitch_yaw_rates_thrust_setpoint->thrust); } +/** + * @brief Encode a roll_pitch_yaw_rates_thrust_setpoint struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param roll_pitch_yaw_rates_thrust_setpoint C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_roll_pitch_yaw_rates_thrust_setpoint_t* roll_pitch_yaw_rates_thrust_setpoint) +{ + return mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_pack_chan(system_id, component_id, chan, msg, roll_pitch_yaw_rates_thrust_setpoint->time_boot_ms, roll_pitch_yaw_rates_thrust_setpoint->roll_rate, roll_pitch_yaw_rates_thrust_setpoint->pitch_rate, roll_pitch_yaw_rates_thrust_setpoint->yaw_rate, roll_pitch_yaw_rates_thrust_setpoint->thrust); +} + /** * @brief Send a roll_pitch_yaw_rates_thrust_setpoint message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h index 517797655c..626477322b 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack(uin * @brief Pack a roll_pitch_yaw_speed_thrust_setpoint message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp in milliseconds since system boot * @param roll_speed Desired roll angular speed in rad/s @@ -121,7 +121,7 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack_cha } /** - * @brief Encode a roll_pitch_yaw_speed_thrust_setpoint struct into a message + * @brief Encode a roll_pitch_yaw_speed_thrust_setpoint struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -133,6 +133,20 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_encode(u return mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack(system_id, component_id, msg, roll_pitch_yaw_speed_thrust_setpoint->time_boot_ms, roll_pitch_yaw_speed_thrust_setpoint->roll_speed, roll_pitch_yaw_speed_thrust_setpoint->pitch_speed, roll_pitch_yaw_speed_thrust_setpoint->yaw_speed, roll_pitch_yaw_speed_thrust_setpoint->thrust); } +/** + * @brief Encode a roll_pitch_yaw_speed_thrust_setpoint struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param roll_pitch_yaw_speed_thrust_setpoint C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_roll_pitch_yaw_speed_thrust_setpoint_t* roll_pitch_yaw_speed_thrust_setpoint) +{ + return mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack_chan(system_id, component_id, chan, msg, roll_pitch_yaw_speed_thrust_setpoint->time_boot_ms, roll_pitch_yaw_speed_thrust_setpoint->roll_speed, roll_pitch_yaw_speed_thrust_setpoint->pitch_speed, roll_pitch_yaw_speed_thrust_setpoint->yaw_speed, roll_pitch_yaw_speed_thrust_setpoint->thrust); +} + /** * @brief Send a roll_pitch_yaw_speed_thrust_setpoint message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h index aee036022b..ffcdc547b4 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack(uint8_t s * @brief Pack a roll_pitch_yaw_thrust_setpoint message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp in milliseconds since system boot * @param roll Desired roll angle in radians @@ -121,7 +121,7 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack_chan(uint } /** - * @brief Encode a roll_pitch_yaw_thrust_setpoint struct into a message + * @brief Encode a roll_pitch_yaw_thrust_setpoint struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -133,6 +133,20 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_encode(uint8_t return mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack(system_id, component_id, msg, roll_pitch_yaw_thrust_setpoint->time_boot_ms, roll_pitch_yaw_thrust_setpoint->roll, roll_pitch_yaw_thrust_setpoint->pitch, roll_pitch_yaw_thrust_setpoint->yaw, roll_pitch_yaw_thrust_setpoint->thrust); } +/** + * @brief Encode a roll_pitch_yaw_thrust_setpoint struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param roll_pitch_yaw_thrust_setpoint C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_roll_pitch_yaw_thrust_setpoint_t* roll_pitch_yaw_thrust_setpoint) +{ + return mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack_chan(system_id, component_id, chan, msg, roll_pitch_yaw_thrust_setpoint->time_boot_ms, roll_pitch_yaw_thrust_setpoint->roll, roll_pitch_yaw_thrust_setpoint->pitch, roll_pitch_yaw_thrust_setpoint->yaw, roll_pitch_yaw_thrust_setpoint->thrust); +} + /** * @brief Send a roll_pitch_yaw_thrust_setpoint message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_allowed_area.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_allowed_area.h index 100fabf16e..fcd54cbb78 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_allowed_area.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_allowed_area.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_safety_allowed_area_pack(uint8_t system_id, u * @brief Pack a safety_allowed_area message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param frame Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. * @param p1x x position 1 / Latitude 1 @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_safety_allowed_area_pack_chan(uint8_t system_ } /** - * @brief Encode a safety_allowed_area struct into a message + * @brief Encode a safety_allowed_area struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -149,6 +149,20 @@ static inline uint16_t mavlink_msg_safety_allowed_area_encode(uint8_t system_id, return mavlink_msg_safety_allowed_area_pack(system_id, component_id, msg, safety_allowed_area->frame, safety_allowed_area->p1x, safety_allowed_area->p1y, safety_allowed_area->p1z, safety_allowed_area->p2x, safety_allowed_area->p2y, safety_allowed_area->p2z); } +/** + * @brief Encode a safety_allowed_area struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param safety_allowed_area C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_safety_allowed_area_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_safety_allowed_area_t* safety_allowed_area) +{ + return mavlink_msg_safety_allowed_area_pack_chan(system_id, component_id, chan, msg, safety_allowed_area->frame, safety_allowed_area->p1x, safety_allowed_area->p1y, safety_allowed_area->p1z, safety_allowed_area->p2x, safety_allowed_area->p2y, safety_allowed_area->p2z); +} + /** * @brief Send a safety_allowed_area message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_set_allowed_area.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_set_allowed_area.h index d365b7aedc..61f6af1554 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_set_allowed_area.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_set_allowed_area.h @@ -99,7 +99,7 @@ static inline uint16_t mavlink_msg_safety_set_allowed_area_pack(uint8_t system_i * @brief Pack a safety_set_allowed_area message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -153,7 +153,7 @@ static inline uint16_t mavlink_msg_safety_set_allowed_area_pack_chan(uint8_t sys } /** - * @brief Encode a safety_set_allowed_area struct into a message + * @brief Encode a safety_set_allowed_area struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -165,6 +165,20 @@ static inline uint16_t mavlink_msg_safety_set_allowed_area_encode(uint8_t system return mavlink_msg_safety_set_allowed_area_pack(system_id, component_id, msg, safety_set_allowed_area->target_system, safety_set_allowed_area->target_component, safety_set_allowed_area->frame, safety_set_allowed_area->p1x, safety_set_allowed_area->p1y, safety_set_allowed_area->p1z, safety_set_allowed_area->p2x, safety_set_allowed_area->p2y, safety_set_allowed_area->p2z); } +/** + * @brief Encode a safety_set_allowed_area struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param safety_set_allowed_area C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_safety_set_allowed_area_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_safety_set_allowed_area_t* safety_set_allowed_area) +{ + return mavlink_msg_safety_set_allowed_area_pack_chan(system_id, component_id, chan, msg, safety_set_allowed_area->target_system, safety_set_allowed_area->target_component, safety_set_allowed_area->frame, safety_set_allowed_area->p1x, safety_set_allowed_area->p1y, safety_set_allowed_area->p1z, safety_set_allowed_area->p2x, safety_set_allowed_area->p2y, safety_set_allowed_area->p2z); +} + /** * @brief Send a safety_set_allowed_area message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu.h index 2751ddfe7a..3010d051af 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu.h @@ -104,7 +104,7 @@ static inline uint16_t mavlink_msg_scaled_imu_pack(uint8_t system_id, uint8_t co * @brief Pack a scaled_imu message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp (milliseconds since system boot) * @param xacc X acceleration (mg) @@ -161,7 +161,7 @@ static inline uint16_t mavlink_msg_scaled_imu_pack_chan(uint8_t system_id, uint8 } /** - * @brief Encode a scaled_imu struct into a message + * @brief Encode a scaled_imu struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -173,6 +173,20 @@ static inline uint16_t mavlink_msg_scaled_imu_encode(uint8_t system_id, uint8_t return mavlink_msg_scaled_imu_pack(system_id, component_id, msg, scaled_imu->time_boot_ms, scaled_imu->xacc, scaled_imu->yacc, scaled_imu->zacc, scaled_imu->xgyro, scaled_imu->ygyro, scaled_imu->zgyro, scaled_imu->xmag, scaled_imu->ymag, scaled_imu->zmag); } +/** + * @brief Encode a scaled_imu struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param scaled_imu C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_scaled_imu_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_scaled_imu_t* scaled_imu) +{ + return mavlink_msg_scaled_imu_pack_chan(system_id, component_id, chan, msg, scaled_imu->time_boot_ms, scaled_imu->xacc, scaled_imu->yacc, scaled_imu->zacc, scaled_imu->xgyro, scaled_imu->ygyro, scaled_imu->zgyro, scaled_imu->xmag, scaled_imu->ymag, scaled_imu->zmag); +} + /** * @brief Send a scaled_imu message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_pressure.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_pressure.h index f54e281958..10324bc941 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_pressure.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_pressure.h @@ -74,7 +74,7 @@ static inline uint16_t mavlink_msg_scaled_pressure_pack(uint8_t system_id, uint8 * @brief Pack a scaled_pressure message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp (milliseconds since system boot) * @param press_abs Absolute pressure (hectopascal) @@ -113,7 +113,7 @@ static inline uint16_t mavlink_msg_scaled_pressure_pack_chan(uint8_t system_id, } /** - * @brief Encode a scaled_pressure struct into a message + * @brief Encode a scaled_pressure struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -125,6 +125,20 @@ static inline uint16_t mavlink_msg_scaled_pressure_encode(uint8_t system_id, uin return mavlink_msg_scaled_pressure_pack(system_id, component_id, msg, scaled_pressure->time_boot_ms, scaled_pressure->press_abs, scaled_pressure->press_diff, scaled_pressure->temperature); } +/** + * @brief Encode a scaled_pressure struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param scaled_pressure C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_scaled_pressure_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_scaled_pressure_t* scaled_pressure) +{ + return mavlink_msg_scaled_pressure_pack_chan(system_id, component_id, chan, msg, scaled_pressure->time_boot_ms, scaled_pressure->press_abs, scaled_pressure->press_diff, scaled_pressure->temperature); +} + /** * @brief Send a scaled_pressure message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_servo_output_raw.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_servo_output_raw.h index 10bdcbc8c2..6a14e93ed3 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_servo_output_raw.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_servo_output_raw.h @@ -104,7 +104,7 @@ static inline uint16_t mavlink_msg_servo_output_raw_pack(uint8_t system_id, uint * @brief Pack a servo_output_raw message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (microseconds since system boot) * @param port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. @@ -161,7 +161,7 @@ static inline uint16_t mavlink_msg_servo_output_raw_pack_chan(uint8_t system_id, } /** - * @brief Encode a servo_output_raw struct into a message + * @brief Encode a servo_output_raw struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -173,6 +173,20 @@ static inline uint16_t mavlink_msg_servo_output_raw_encode(uint8_t system_id, ui return mavlink_msg_servo_output_raw_pack(system_id, component_id, msg, servo_output_raw->time_usec, servo_output_raw->port, servo_output_raw->servo1_raw, servo_output_raw->servo2_raw, servo_output_raw->servo3_raw, servo_output_raw->servo4_raw, servo_output_raw->servo5_raw, servo_output_raw->servo6_raw, servo_output_raw->servo7_raw, servo_output_raw->servo8_raw); } +/** + * @brief Encode a servo_output_raw struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param servo_output_raw C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_servo_output_raw_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_servo_output_raw_t* servo_output_raw) +{ + return mavlink_msg_servo_output_raw_pack_chan(system_id, component_id, chan, msg, servo_output_raw->time_usec, servo_output_raw->port, servo_output_raw->servo1_raw, servo_output_raw->servo2_raw, servo_output_raw->servo3_raw, servo_output_raw->servo4_raw, servo_output_raw->servo5_raw, servo_output_raw->servo6_raw, servo_output_raw->servo7_raw, servo_output_raw->servo8_raw); +} + /** * @brief Send a servo_output_raw message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_global_position_setpoint_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_global_position_setpoint_int.h index 0364b42415..6f0d7a69da 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_global_position_setpoint_int.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_global_position_setpoint_int.h @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_set_global_position_setpoint_int_pack(uint8_t * @brief Pack a set_global_position_setpoint_int message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param coordinate_frame Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT * @param latitude Latitude (WGS84), in degrees * 1E7 @@ -121,7 +121,7 @@ static inline uint16_t mavlink_msg_set_global_position_setpoint_int_pack_chan(ui } /** - * @brief Encode a set_global_position_setpoint_int struct into a message + * @brief Encode a set_global_position_setpoint_int struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -133,6 +133,20 @@ static inline uint16_t mavlink_msg_set_global_position_setpoint_int_encode(uint8 return mavlink_msg_set_global_position_setpoint_int_pack(system_id, component_id, msg, set_global_position_setpoint_int->coordinate_frame, set_global_position_setpoint_int->latitude, set_global_position_setpoint_int->longitude, set_global_position_setpoint_int->altitude, set_global_position_setpoint_int->yaw); } +/** + * @brief Encode a set_global_position_setpoint_int struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_global_position_setpoint_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_global_position_setpoint_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_global_position_setpoint_int_t* set_global_position_setpoint_int) +{ + return mavlink_msg_set_global_position_setpoint_int_pack_chan(system_id, component_id, chan, msg, set_global_position_setpoint_int->coordinate_frame, set_global_position_setpoint_int->latitude, set_global_position_setpoint_int->longitude, set_global_position_setpoint_int->altitude, set_global_position_setpoint_int->yaw); +} + /** * @brief Send a set_global_position_setpoint_int message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_gps_global_origin.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_gps_global_origin.h index e3cd4f4419..c444d8d52c 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_gps_global_origin.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_gps_global_origin.h @@ -74,7 +74,7 @@ static inline uint16_t mavlink_msg_set_gps_global_origin_pack(uint8_t system_id, * @brief Pack a set_gps_global_origin message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param latitude Latitude (WGS84), in degrees * 1E7 @@ -113,7 +113,7 @@ static inline uint16_t mavlink_msg_set_gps_global_origin_pack_chan(uint8_t syste } /** - * @brief Encode a set_gps_global_origin struct into a message + * @brief Encode a set_gps_global_origin struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -125,6 +125,20 @@ static inline uint16_t mavlink_msg_set_gps_global_origin_encode(uint8_t system_i return mavlink_msg_set_gps_global_origin_pack(system_id, component_id, msg, set_gps_global_origin->target_system, set_gps_global_origin->latitude, set_gps_global_origin->longitude, set_gps_global_origin->altitude); } +/** + * @brief Encode a set_gps_global_origin struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_gps_global_origin C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_gps_global_origin_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_gps_global_origin_t* set_gps_global_origin) +{ + return mavlink_msg_set_gps_global_origin_pack_chan(system_id, component_id, chan, msg, set_gps_global_origin->target_system, set_gps_global_origin->latitude, set_gps_global_origin->longitude, set_gps_global_origin->altitude); +} + /** * @brief Send a set_gps_global_origin message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_local_position_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_local_position_setpoint.h index b92c0560e9..6f2835e031 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_local_position_setpoint.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_local_position_setpoint.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_set_local_position_setpoint_pack(uint8_t syst * @brief Pack a set_local_position_setpoint message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_set_local_position_setpoint_pack_chan(uint8_t } /** - * @brief Encode a set_local_position_setpoint struct into a message + * @brief Encode a set_local_position_setpoint struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -149,6 +149,20 @@ static inline uint16_t mavlink_msg_set_local_position_setpoint_encode(uint8_t sy return mavlink_msg_set_local_position_setpoint_pack(system_id, component_id, msg, set_local_position_setpoint->target_system, set_local_position_setpoint->target_component, set_local_position_setpoint->coordinate_frame, set_local_position_setpoint->x, set_local_position_setpoint->y, set_local_position_setpoint->z, set_local_position_setpoint->yaw); } +/** + * @brief Encode a set_local_position_setpoint struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_local_position_setpoint C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_local_position_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_local_position_setpoint_t* set_local_position_setpoint) +{ + return mavlink_msg_set_local_position_setpoint_pack_chan(system_id, component_id, chan, msg, set_local_position_setpoint->target_system, set_local_position_setpoint->target_component, set_local_position_setpoint->coordinate_frame, set_local_position_setpoint->x, set_local_position_setpoint->y, set_local_position_setpoint->z, set_local_position_setpoint->yaw); +} + /** * @brief Send a set_local_position_setpoint message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_mode.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_mode.h index 08ec733098..1aff42cce3 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_mode.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_mode.h @@ -69,7 +69,7 @@ static inline uint16_t mavlink_msg_set_mode_pack(uint8_t system_id, uint8_t comp * @brief Pack a set_mode message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system The system setting the mode * @param base_mode The new base mode @@ -105,7 +105,7 @@ static inline uint16_t mavlink_msg_set_mode_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a set_mode struct into a message + * @brief Encode a set_mode struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -117,6 +117,20 @@ static inline uint16_t mavlink_msg_set_mode_encode(uint8_t system_id, uint8_t co return mavlink_msg_set_mode_pack(system_id, component_id, msg, set_mode->target_system, set_mode->base_mode, set_mode->custom_mode); } +/** + * @brief Encode a set_mode struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_mode C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_mode_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_mode_t* set_mode) +{ + return mavlink_msg_set_mode_pack_chan(system_id, component_id, chan, msg, set_mode->target_system, set_mode->base_mode, set_mode->custom_mode); +} + /** * @brief Send a set_mode message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_motors_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_motors_setpoint.h index b79114e1a5..8ceb8888fc 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_motors_setpoint.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_motors_setpoint.h @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_set_quad_motors_setpoint_pack(uint8_t system_ * @brief Pack a set_quad_motors_setpoint message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID of the system that should set these motor commands * @param motor_front_nw Front motor in + configuration, front left motor in x configuration @@ -121,7 +121,7 @@ static inline uint16_t mavlink_msg_set_quad_motors_setpoint_pack_chan(uint8_t sy } /** - * @brief Encode a set_quad_motors_setpoint struct into a message + * @brief Encode a set_quad_motors_setpoint struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -133,6 +133,20 @@ static inline uint16_t mavlink_msg_set_quad_motors_setpoint_encode(uint8_t syste return mavlink_msg_set_quad_motors_setpoint_pack(system_id, component_id, msg, set_quad_motors_setpoint->target_system, set_quad_motors_setpoint->motor_front_nw, set_quad_motors_setpoint->motor_right_ne, set_quad_motors_setpoint->motor_back_se, set_quad_motors_setpoint->motor_left_sw); } +/** + * @brief Encode a set_quad_motors_setpoint struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_quad_motors_setpoint C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_quad_motors_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_quad_motors_setpoint_t* set_quad_motors_setpoint) +{ + return mavlink_msg_set_quad_motors_setpoint_pack_chan(system_id, component_id, chan, msg, set_quad_motors_setpoint->target_system, set_quad_motors_setpoint->motor_front_nw, set_quad_motors_setpoint->motor_right_ne, set_quad_motors_setpoint->motor_back_se, set_quad_motors_setpoint->motor_left_sw); +} + /** * @brief Send a set_quad_motors_setpoint message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust.h index 06223845f4..9ef294cc97 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust.h @@ -4,10 +4,10 @@ typedef struct __mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t { - int16_t roll[4]; ///< Desired roll angle in radians +-PI (+-32767) - int16_t pitch[4]; ///< Desired pitch angle in radians +-PI (+-32767) - int16_t yaw[4]; ///< Desired yaw angle in radians, scaled to int16 +-PI (+-32767) - uint16_t thrust[4]; ///< Collective thrust, scaled to uint16 (0..65535) + int16_t roll[4]; ///< Desired roll angle in radians +-PI (+-INT16_MAX) + int16_t pitch[4]; ///< Desired pitch angle in radians +-PI (+-INT16_MAX) + int16_t yaw[4]; ///< Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) + uint16_t thrust[4]; ///< Collective thrust, scaled to uint16 (0..UINT16_MAX) uint8_t group; ///< ID of the quadrotor group (0 - 255, up to 256 groups supported) uint8_t mode; ///< ID of the flight mode (0 - 255, up to 256 modes supported) uint8_t led_red[4]; ///< RGB red channel (0-255) @@ -56,10 +56,10 @@ typedef struct __mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t * @param led_red RGB red channel (0-255) * @param led_blue RGB green channel (0-255) * @param led_green RGB blue channel (0-255) - * @param roll Desired roll angle in radians +-PI (+-32767) - * @param pitch Desired pitch angle in radians +-PI (+-32767) - * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-32767) - * @param thrust Collective thrust, scaled to uint16 (0..65535) + * @param roll Desired roll angle in radians +-PI (+-INT16_MAX) + * @param pitch Desired pitch angle in radians +-PI (+-INT16_MAX) + * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) + * @param thrust Collective thrust, scaled to uint16 (0..UINT16_MAX) * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, @@ -103,17 +103,17 @@ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_pack * @brief Pack a set_quad_swarm_led_roll_pitch_yaw_thrust message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param group ID of the quadrotor group (0 - 255, up to 256 groups supported) * @param mode ID of the flight mode (0 - 255, up to 256 modes supported) * @param led_red RGB red channel (0-255) * @param led_blue RGB green channel (0-255) * @param led_green RGB blue channel (0-255) - * @param roll Desired roll angle in radians +-PI (+-32767) - * @param pitch Desired pitch angle in radians +-PI (+-32767) - * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-32767) - * @param thrust Collective thrust, scaled to uint16 (0..65535) + * @param roll Desired roll angle in radians +-PI (+-INT16_MAX) + * @param pitch Desired pitch angle in radians +-PI (+-INT16_MAX) + * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) + * @param thrust Collective thrust, scaled to uint16 (0..UINT16_MAX) * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, @@ -155,7 +155,7 @@ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_pack } /** - * @brief Encode a set_quad_swarm_led_roll_pitch_yaw_thrust struct into a message + * @brief Encode a set_quad_swarm_led_roll_pitch_yaw_thrust struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -167,6 +167,20 @@ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_enco return mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_pack(system_id, component_id, msg, set_quad_swarm_led_roll_pitch_yaw_thrust->group, set_quad_swarm_led_roll_pitch_yaw_thrust->mode, set_quad_swarm_led_roll_pitch_yaw_thrust->led_red, set_quad_swarm_led_roll_pitch_yaw_thrust->led_blue, set_quad_swarm_led_roll_pitch_yaw_thrust->led_green, set_quad_swarm_led_roll_pitch_yaw_thrust->roll, set_quad_swarm_led_roll_pitch_yaw_thrust->pitch, set_quad_swarm_led_roll_pitch_yaw_thrust->yaw, set_quad_swarm_led_roll_pitch_yaw_thrust->thrust); } +/** + * @brief Encode a set_quad_swarm_led_roll_pitch_yaw_thrust struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_quad_swarm_led_roll_pitch_yaw_thrust C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t* set_quad_swarm_led_roll_pitch_yaw_thrust) +{ + return mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_pack_chan(system_id, component_id, chan, msg, set_quad_swarm_led_roll_pitch_yaw_thrust->group, set_quad_swarm_led_roll_pitch_yaw_thrust->mode, set_quad_swarm_led_roll_pitch_yaw_thrust->led_red, set_quad_swarm_led_roll_pitch_yaw_thrust->led_blue, set_quad_swarm_led_roll_pitch_yaw_thrust->led_green, set_quad_swarm_led_roll_pitch_yaw_thrust->roll, set_quad_swarm_led_roll_pitch_yaw_thrust->pitch, set_quad_swarm_led_roll_pitch_yaw_thrust->yaw, set_quad_swarm_led_roll_pitch_yaw_thrust->thrust); +} + /** * @brief Send a set_quad_swarm_led_roll_pitch_yaw_thrust message * @param chan MAVLink channel to send the message @@ -176,10 +190,10 @@ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_enco * @param led_red RGB red channel (0-255) * @param led_blue RGB green channel (0-255) * @param led_green RGB blue channel (0-255) - * @param roll Desired roll angle in radians +-PI (+-32767) - * @param pitch Desired pitch angle in radians +-PI (+-32767) - * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-32767) - * @param thrust Collective thrust, scaled to uint16 (0..65535) + * @param roll Desired roll angle in radians +-PI (+-INT16_MAX) + * @param pitch Desired pitch angle in radians +-PI (+-INT16_MAX) + * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) + * @param thrust Collective thrust, scaled to uint16 (0..UINT16_MAX) */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -278,7 +292,7 @@ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_ /** * @brief Get field roll from set_quad_swarm_led_roll_pitch_yaw_thrust message * - * @return Desired roll angle in radians +-PI (+-32767) + * @return Desired roll angle in radians +-PI (+-INT16_MAX) */ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_roll(const mavlink_message_t* msg, int16_t *roll) { @@ -288,7 +302,7 @@ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_ /** * @brief Get field pitch from set_quad_swarm_led_roll_pitch_yaw_thrust message * - * @return Desired pitch angle in radians +-PI (+-32767) + * @return Desired pitch angle in radians +-PI (+-INT16_MAX) */ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_pitch(const mavlink_message_t* msg, int16_t *pitch) { @@ -298,7 +312,7 @@ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_ /** * @brief Get field yaw from set_quad_swarm_led_roll_pitch_yaw_thrust message * - * @return Desired yaw angle in radians, scaled to int16 +-PI (+-32767) + * @return Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) */ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_yaw(const mavlink_message_t* msg, int16_t *yaw) { @@ -308,7 +322,7 @@ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_ /** * @brief Get field thrust from set_quad_swarm_led_roll_pitch_yaw_thrust message * - * @return Collective thrust, scaled to uint16 (0..65535) + * @return Collective thrust, scaled to uint16 (0..UINT16_MAX) */ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_thrust(const mavlink_message_t* msg, uint16_t *thrust) { diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust.h index 6c62b35305..7d8d526f8b 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust.h @@ -4,10 +4,10 @@ typedef struct __mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t { - int16_t roll[4]; ///< Desired roll angle in radians +-PI (+-32767) - int16_t pitch[4]; ///< Desired pitch angle in radians +-PI (+-32767) - int16_t yaw[4]; ///< Desired yaw angle in radians, scaled to int16 +-PI (+-32767) - uint16_t thrust[4]; ///< Collective thrust, scaled to uint16 (0..65535) + int16_t roll[4]; ///< Desired roll angle in radians +-PI (+-INT16_MAX) + int16_t pitch[4]; ///< Desired pitch angle in radians +-PI (+-INT16_MAX) + int16_t yaw[4]; ///< Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) + uint16_t thrust[4]; ///< Collective thrust, scaled to uint16 (0..UINT16_MAX) uint8_t group; ///< ID of the quadrotor group (0 - 255, up to 256 groups supported) uint8_t mode; ///< ID of the flight mode (0 - 255, up to 256 modes supported) } mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t; @@ -44,10 +44,10 @@ typedef struct __mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t * * @param group ID of the quadrotor group (0 - 255, up to 256 groups supported) * @param mode ID of the flight mode (0 - 255, up to 256 modes supported) - * @param roll Desired roll angle in radians +-PI (+-32767) - * @param pitch Desired pitch angle in radians +-PI (+-32767) - * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-32767) - * @param thrust Collective thrust, scaled to uint16 (0..65535) + * @param roll Desired roll angle in radians +-PI (+-INT16_MAX) + * @param pitch Desired pitch angle in radians +-PI (+-INT16_MAX) + * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) + * @param thrust Collective thrust, scaled to uint16 (0..UINT16_MAX) * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, @@ -85,14 +85,14 @@ static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_pack(uin * @brief Pack a set_quad_swarm_roll_pitch_yaw_thrust message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param group ID of the quadrotor group (0 - 255, up to 256 groups supported) * @param mode ID of the flight mode (0 - 255, up to 256 modes supported) - * @param roll Desired roll angle in radians +-PI (+-32767) - * @param pitch Desired pitch angle in radians +-PI (+-32767) - * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-32767) - * @param thrust Collective thrust, scaled to uint16 (0..65535) + * @param roll Desired roll angle in radians +-PI (+-INT16_MAX) + * @param pitch Desired pitch angle in radians +-PI (+-INT16_MAX) + * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) + * @param thrust Collective thrust, scaled to uint16 (0..UINT16_MAX) * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, @@ -128,7 +128,7 @@ static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_pack_cha } /** - * @brief Encode a set_quad_swarm_roll_pitch_yaw_thrust struct into a message + * @brief Encode a set_quad_swarm_roll_pitch_yaw_thrust struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -140,16 +140,30 @@ static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_encode(u return mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_pack(system_id, component_id, msg, set_quad_swarm_roll_pitch_yaw_thrust->group, set_quad_swarm_roll_pitch_yaw_thrust->mode, set_quad_swarm_roll_pitch_yaw_thrust->roll, set_quad_swarm_roll_pitch_yaw_thrust->pitch, set_quad_swarm_roll_pitch_yaw_thrust->yaw, set_quad_swarm_roll_pitch_yaw_thrust->thrust); } +/** + * @brief Encode a set_quad_swarm_roll_pitch_yaw_thrust struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_quad_swarm_roll_pitch_yaw_thrust C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t* set_quad_swarm_roll_pitch_yaw_thrust) +{ + return mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_pack_chan(system_id, component_id, chan, msg, set_quad_swarm_roll_pitch_yaw_thrust->group, set_quad_swarm_roll_pitch_yaw_thrust->mode, set_quad_swarm_roll_pitch_yaw_thrust->roll, set_quad_swarm_roll_pitch_yaw_thrust->pitch, set_quad_swarm_roll_pitch_yaw_thrust->yaw, set_quad_swarm_roll_pitch_yaw_thrust->thrust); +} + /** * @brief Send a set_quad_swarm_roll_pitch_yaw_thrust message * @param chan MAVLink channel to send the message * * @param group ID of the quadrotor group (0 - 255, up to 256 groups supported) * @param mode ID of the flight mode (0 - 255, up to 256 modes supported) - * @param roll Desired roll angle in radians +-PI (+-32767) - * @param pitch Desired pitch angle in radians +-PI (+-32767) - * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-32767) - * @param thrust Collective thrust, scaled to uint16 (0..65535) + * @param roll Desired roll angle in radians +-PI (+-INT16_MAX) + * @param pitch Desired pitch angle in radians +-PI (+-INT16_MAX) + * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) + * @param thrust Collective thrust, scaled to uint16 (0..UINT16_MAX) */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -212,7 +226,7 @@ static inline uint8_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_mode( /** * @brief Get field roll from set_quad_swarm_roll_pitch_yaw_thrust message * - * @return Desired roll angle in radians +-PI (+-32767) + * @return Desired roll angle in radians +-PI (+-INT16_MAX) */ static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_roll(const mavlink_message_t* msg, int16_t *roll) { @@ -222,7 +236,7 @@ static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_roll /** * @brief Get field pitch from set_quad_swarm_roll_pitch_yaw_thrust message * - * @return Desired pitch angle in radians +-PI (+-32767) + * @return Desired pitch angle in radians +-PI (+-INT16_MAX) */ static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_pitch(const mavlink_message_t* msg, int16_t *pitch) { @@ -232,7 +246,7 @@ static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_pitc /** * @brief Get field yaw from set_quad_swarm_roll_pitch_yaw_thrust message * - * @return Desired yaw angle in radians, scaled to int16 +-PI (+-32767) + * @return Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) */ static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_yaw(const mavlink_message_t* msg, int16_t *yaw) { @@ -242,7 +256,7 @@ static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_yaw( /** * @brief Get field thrust from set_quad_swarm_roll_pitch_yaw_thrust message * - * @return Collective thrust, scaled to uint16 (0..65535) + * @return Collective thrust, scaled to uint16 (0..UINT16_MAX) */ static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_thrust(const mavlink_message_t* msg, uint16_t *thrust) { diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h index c379a75d9b..5846ba41f7 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h @@ -84,7 +84,7 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack(uint8_t * @brief Pack a set_roll_pitch_yaw_speed_thrust message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack_chan(uin } /** - * @brief Encode a set_roll_pitch_yaw_speed_thrust struct into a message + * @brief Encode a set_roll_pitch_yaw_speed_thrust struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -141,6 +141,20 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_encode(uint8_ return mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack(system_id, component_id, msg, set_roll_pitch_yaw_speed_thrust->target_system, set_roll_pitch_yaw_speed_thrust->target_component, set_roll_pitch_yaw_speed_thrust->roll_speed, set_roll_pitch_yaw_speed_thrust->pitch_speed, set_roll_pitch_yaw_speed_thrust->yaw_speed, set_roll_pitch_yaw_speed_thrust->thrust); } +/** + * @brief Encode a set_roll_pitch_yaw_speed_thrust struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_roll_pitch_yaw_speed_thrust C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_roll_pitch_yaw_speed_thrust_t* set_roll_pitch_yaw_speed_thrust) +{ + return mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack_chan(system_id, component_id, chan, msg, set_roll_pitch_yaw_speed_thrust->target_system, set_roll_pitch_yaw_speed_thrust->target_component, set_roll_pitch_yaw_speed_thrust->roll_speed, set_roll_pitch_yaw_speed_thrust->pitch_speed, set_roll_pitch_yaw_speed_thrust->yaw_speed, set_roll_pitch_yaw_speed_thrust->thrust); +} + /** * @brief Send a set_roll_pitch_yaw_speed_thrust message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_thrust.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_thrust.h index 146891eafb..334fd39e36 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_thrust.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_thrust.h @@ -84,7 +84,7 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_pack(uint8_t system * @brief Pack a set_roll_pitch_yaw_thrust message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_pack_chan(uint8_t s } /** - * @brief Encode a set_roll_pitch_yaw_thrust struct into a message + * @brief Encode a set_roll_pitch_yaw_thrust struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -141,6 +141,20 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_encode(uint8_t syst return mavlink_msg_set_roll_pitch_yaw_thrust_pack(system_id, component_id, msg, set_roll_pitch_yaw_thrust->target_system, set_roll_pitch_yaw_thrust->target_component, set_roll_pitch_yaw_thrust->roll, set_roll_pitch_yaw_thrust->pitch, set_roll_pitch_yaw_thrust->yaw, set_roll_pitch_yaw_thrust->thrust); } +/** + * @brief Encode a set_roll_pitch_yaw_thrust struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_roll_pitch_yaw_thrust C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_roll_pitch_yaw_thrust_t* set_roll_pitch_yaw_thrust) +{ + return mavlink_msg_set_roll_pitch_yaw_thrust_pack_chan(system_id, component_id, chan, msg, set_roll_pitch_yaw_thrust->target_system, set_roll_pitch_yaw_thrust->target_component, set_roll_pitch_yaw_thrust->roll, set_roll_pitch_yaw_thrust->pitch, set_roll_pitch_yaw_thrust->yaw, set_roll_pitch_yaw_thrust->thrust); +} + /** * @brief Send a set_roll_pitch_yaw_thrust message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_6dof.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_6dof.h index f352617a26..93ce345b65 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_6dof.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_6dof.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_setpoint_6dof_pack(uint8_t system_id, uint8_t * @brief Pack a setpoint_6dof message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param trans_x Translational Component in x @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_setpoint_6dof_pack_chan(uint8_t system_id, ui } /** - * @brief Encode a setpoint_6dof struct into a message + * @brief Encode a setpoint_6dof struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -149,6 +149,20 @@ static inline uint16_t mavlink_msg_setpoint_6dof_encode(uint8_t system_id, uint8 return mavlink_msg_setpoint_6dof_pack(system_id, component_id, msg, setpoint_6dof->target_system, setpoint_6dof->trans_x, setpoint_6dof->trans_y, setpoint_6dof->trans_z, setpoint_6dof->rot_x, setpoint_6dof->rot_y, setpoint_6dof->rot_z); } +/** + * @brief Encode a setpoint_6dof struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param setpoint_6dof C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_setpoint_6dof_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_setpoint_6dof_t* setpoint_6dof) +{ + return mavlink_msg_setpoint_6dof_pack_chan(system_id, component_id, chan, msg, setpoint_6dof->target_system, setpoint_6dof->trans_x, setpoint_6dof->trans_y, setpoint_6dof->trans_z, setpoint_6dof->rot_x, setpoint_6dof->rot_y, setpoint_6dof->rot_z); +} + /** * @brief Send a setpoint_6dof message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_8dof.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_8dof.h index d7622b6965..de80e46459 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_8dof.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_8dof.h @@ -99,7 +99,7 @@ static inline uint16_t mavlink_msg_setpoint_8dof_pack(uint8_t system_id, uint8_t * @brief Pack a setpoint_8dof message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param val1 Value 1 @@ -153,7 +153,7 @@ static inline uint16_t mavlink_msg_setpoint_8dof_pack_chan(uint8_t system_id, ui } /** - * @brief Encode a setpoint_8dof struct into a message + * @brief Encode a setpoint_8dof struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -165,6 +165,20 @@ static inline uint16_t mavlink_msg_setpoint_8dof_encode(uint8_t system_id, uint8 return mavlink_msg_setpoint_8dof_pack(system_id, component_id, msg, setpoint_8dof->target_system, setpoint_8dof->val1, setpoint_8dof->val2, setpoint_8dof->val3, setpoint_8dof->val4, setpoint_8dof->val5, setpoint_8dof->val6, setpoint_8dof->val7, setpoint_8dof->val8); } +/** + * @brief Encode a setpoint_8dof struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param setpoint_8dof C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_setpoint_8dof_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_setpoint_8dof_t* setpoint_8dof) +{ + return mavlink_msg_setpoint_8dof_pack_chan(system_id, component_id, chan, msg, setpoint_8dof->target_system, setpoint_8dof->val1, setpoint_8dof->val2, setpoint_8dof->val3, setpoint_8dof->val4, setpoint_8dof->val5, setpoint_8dof->val6, setpoint_8dof->val7, setpoint_8dof->val8); +} + /** * @brief Send a setpoint_8dof message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_sim_state.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_sim_state.h index 6fd32abd25..ebd657cf38 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_sim_state.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_sim_state.h @@ -159,7 +159,7 @@ static inline uint16_t mavlink_msg_sim_state_pack(uint8_t system_id, uint8_t com * @brief Pack a sim_state message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param q1 True attitude quaternion component 1 * @param q2 True attitude quaternion component 2 @@ -249,7 +249,7 @@ static inline uint16_t mavlink_msg_sim_state_pack_chan(uint8_t system_id, uint8_ } /** - * @brief Encode a sim_state struct into a message + * @brief Encode a sim_state struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -261,6 +261,20 @@ static inline uint16_t mavlink_msg_sim_state_encode(uint8_t system_id, uint8_t c return mavlink_msg_sim_state_pack(system_id, component_id, msg, sim_state->q1, sim_state->q2, sim_state->q3, sim_state->q4, sim_state->roll, sim_state->pitch, sim_state->yaw, sim_state->xacc, sim_state->yacc, sim_state->zacc, sim_state->xgyro, sim_state->ygyro, sim_state->zgyro, sim_state->lat, sim_state->lon, sim_state->alt, sim_state->std_dev_horz, sim_state->std_dev_vert, sim_state->vn, sim_state->ve, sim_state->vd); } +/** + * @brief Encode a sim_state struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param sim_state C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_sim_state_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_sim_state_t* sim_state) +{ + return mavlink_msg_sim_state_pack_chan(system_id, component_id, chan, msg, sim_state->q1, sim_state->q2, sim_state->q3, sim_state->q4, sim_state->roll, sim_state->pitch, sim_state->yaw, sim_state->xacc, sim_state->yacc, sim_state->zacc, sim_state->xgyro, sim_state->ygyro, sim_state->zgyro, sim_state->lat, sim_state->lon, sim_state->alt, sim_state->std_dev_horz, sim_state->std_dev_vert, sim_state->vn, sim_state->ve, sim_state->vd); +} + /** * @brief Send a sim_state message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_state_correction.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_state_correction.h index 8a002fc113..2db627b96a 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_state_correction.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_state_correction.h @@ -99,7 +99,7 @@ static inline uint16_t mavlink_msg_state_correction_pack(uint8_t system_id, uint * @brief Pack a state_correction message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param xErr x position error * @param yErr y position error @@ -153,7 +153,7 @@ static inline uint16_t mavlink_msg_state_correction_pack_chan(uint8_t system_id, } /** - * @brief Encode a state_correction struct into a message + * @brief Encode a state_correction struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -165,6 +165,20 @@ static inline uint16_t mavlink_msg_state_correction_encode(uint8_t system_id, ui return mavlink_msg_state_correction_pack(system_id, component_id, msg, state_correction->xErr, state_correction->yErr, state_correction->zErr, state_correction->rollErr, state_correction->pitchErr, state_correction->yawErr, state_correction->vxErr, state_correction->vyErr, state_correction->vzErr); } +/** + * @brief Encode a state_correction struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param state_correction C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_state_correction_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_state_correction_t* state_correction) +{ + return mavlink_msg_state_correction_pack_chan(system_id, component_id, chan, msg, state_correction->xErr, state_correction->yErr, state_correction->zErr, state_correction->rollErr, state_correction->pitchErr, state_correction->yawErr, state_correction->vxErr, state_correction->vyErr, state_correction->vzErr); +} + /** * @brief Send a state_correction message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_statustext.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_statustext.h index 103486863c..536ca06349 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_statustext.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_statustext.h @@ -62,7 +62,7 @@ static inline uint16_t mavlink_msg_statustext_pack(uint8_t system_id, uint8_t co * @brief Pack a statustext message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param severity Severity of status. Relies on the definitions within RFC-5424. See enum MAV_SEVERITY. * @param text Status text message, without null termination character @@ -93,7 +93,7 @@ static inline uint16_t mavlink_msg_statustext_pack_chan(uint8_t system_id, uint8 } /** - * @brief Encode a statustext struct into a message + * @brief Encode a statustext struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -105,6 +105,20 @@ static inline uint16_t mavlink_msg_statustext_encode(uint8_t system_id, uint8_t return mavlink_msg_statustext_pack(system_id, component_id, msg, statustext->severity, statustext->text); } +/** + * @brief Encode a statustext struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param statustext C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_statustext_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_statustext_t* statustext) +{ + return mavlink_msg_statustext_pack_chan(system_id, component_id, chan, msg, statustext->severity, statustext->text); +} + /** * @brief Send a statustext message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_sys_status.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_sys_status.h index 916bc4f07b..058c630ddd 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_sys_status.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_sys_status.h @@ -119,7 +119,7 @@ static inline uint16_t mavlink_msg_sys_status_pack(uint8_t system_id, uint8_t co * @brief Pack a sys_status message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control * @param onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control @@ -185,7 +185,7 @@ static inline uint16_t mavlink_msg_sys_status_pack_chan(uint8_t system_id, uint8 } /** - * @brief Encode a sys_status struct into a message + * @brief Encode a sys_status struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -197,6 +197,20 @@ static inline uint16_t mavlink_msg_sys_status_encode(uint8_t system_id, uint8_t return mavlink_msg_sys_status_pack(system_id, component_id, msg, sys_status->onboard_control_sensors_present, sys_status->onboard_control_sensors_enabled, sys_status->onboard_control_sensors_health, sys_status->load, sys_status->voltage_battery, sys_status->current_battery, sys_status->battery_remaining, sys_status->drop_rate_comm, sys_status->errors_comm, sys_status->errors_count1, sys_status->errors_count2, sys_status->errors_count3, sys_status->errors_count4); } +/** + * @brief Encode a sys_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param sys_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_sys_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_sys_status_t* sys_status) +{ + return mavlink_msg_sys_status_pack_chan(system_id, component_id, chan, msg, sys_status->onboard_control_sensors_present, sys_status->onboard_control_sensors_enabled, sys_status->onboard_control_sensors_health, sys_status->load, sys_status->voltage_battery, sys_status->current_battery, sys_status->battery_remaining, sys_status->drop_rate_comm, sys_status->errors_comm, sys_status->errors_count1, sys_status->errors_count2, sys_status->errors_count3, sys_status->errors_count4); +} + /** * @brief Send a sys_status message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_system_time.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_system_time.h index b235fe2056..1807567aea 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_system_time.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_system_time.h @@ -64,7 +64,7 @@ static inline uint16_t mavlink_msg_system_time_pack(uint8_t system_id, uint8_t c * @brief Pack a system_time message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_unix_usec Timestamp of the master clock in microseconds since UNIX epoch. * @param time_boot_ms Timestamp of the component clock since boot time in milliseconds. @@ -97,7 +97,7 @@ static inline uint16_t mavlink_msg_system_time_pack_chan(uint8_t system_id, uint } /** - * @brief Encode a system_time struct into a message + * @brief Encode a system_time struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -109,6 +109,20 @@ static inline uint16_t mavlink_msg_system_time_encode(uint8_t system_id, uint8_t return mavlink_msg_system_time_pack(system_id, component_id, msg, system_time->time_unix_usec, system_time->time_boot_ms); } +/** + * @brief Encode a system_time struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param system_time C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_system_time_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_system_time_t* system_time) +{ + return mavlink_msg_system_time_pack_chan(system_id, component_id, chan, msg, system_time->time_unix_usec, system_time->time_boot_ms); +} + /** * @brief Send a system_time message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vfr_hud.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vfr_hud.h index 9d459921fe..5b1093a3d7 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vfr_hud.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vfr_hud.h @@ -84,7 +84,7 @@ static inline uint16_t mavlink_msg_vfr_hud_pack(uint8_t system_id, uint8_t compo * @brief Pack a vfr_hud message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param airspeed Current airspeed in m/s * @param groundspeed Current ground speed in m/s @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_vfr_hud_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a vfr_hud struct into a message + * @brief Encode a vfr_hud struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -141,6 +141,20 @@ static inline uint16_t mavlink_msg_vfr_hud_encode(uint8_t system_id, uint8_t com return mavlink_msg_vfr_hud_pack(system_id, component_id, msg, vfr_hud->airspeed, vfr_hud->groundspeed, vfr_hud->heading, vfr_hud->throttle, vfr_hud->alt, vfr_hud->climb); } +/** + * @brief Encode a vfr_hud struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param vfr_hud C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_vfr_hud_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_vfr_hud_t* vfr_hud) +{ + return mavlink_msg_vfr_hud_pack_chan(system_id, component_id, chan, msg, vfr_hud->airspeed, vfr_hud->groundspeed, vfr_hud->heading, vfr_hud->throttle, vfr_hud->alt, vfr_hud->climb); +} + /** * @brief Send a vfr_hud message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vicon_position_estimate.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vicon_position_estimate.h index 75e4b5b7ab..a254202e4e 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vicon_position_estimate.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vicon_position_estimate.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_pack(uint8_t system_i * @brief Pack a vicon_position_estimate message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param usec Timestamp (microseconds, synced to UNIX time or since system boot) * @param x Global X position @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_pack_chan(uint8_t sys } /** - * @brief Encode a vicon_position_estimate struct into a message + * @brief Encode a vicon_position_estimate struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -149,6 +149,20 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_encode(uint8_t system return mavlink_msg_vicon_position_estimate_pack(system_id, component_id, msg, vicon_position_estimate->usec, vicon_position_estimate->x, vicon_position_estimate->y, vicon_position_estimate->z, vicon_position_estimate->roll, vicon_position_estimate->pitch, vicon_position_estimate->yaw); } +/** + * @brief Encode a vicon_position_estimate struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param vicon_position_estimate C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_vicon_position_estimate_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_vicon_position_estimate_t* vicon_position_estimate) +{ + return mavlink_msg_vicon_position_estimate_pack_chan(system_id, component_id, chan, msg, vicon_position_estimate->usec, vicon_position_estimate->x, vicon_position_estimate->y, vicon_position_estimate->z, vicon_position_estimate->roll, vicon_position_estimate->pitch, vicon_position_estimate->yaw); +} + /** * @brief Send a vicon_position_estimate message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_position_estimate.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_position_estimate.h index 47ccb11ec0..f7a741b09e 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_position_estimate.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_position_estimate.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_vision_position_estimate_pack(uint8_t system_ * @brief Pack a vision_position_estimate message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param usec Timestamp (microseconds, synced to UNIX time or since system boot) * @param x Global X position @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_vision_position_estimate_pack_chan(uint8_t sy } /** - * @brief Encode a vision_position_estimate struct into a message + * @brief Encode a vision_position_estimate struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -149,6 +149,20 @@ static inline uint16_t mavlink_msg_vision_position_estimate_encode(uint8_t syste return mavlink_msg_vision_position_estimate_pack(system_id, component_id, msg, vision_position_estimate->usec, vision_position_estimate->x, vision_position_estimate->y, vision_position_estimate->z, vision_position_estimate->roll, vision_position_estimate->pitch, vision_position_estimate->yaw); } +/** + * @brief Encode a vision_position_estimate struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param vision_position_estimate C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_vision_position_estimate_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_vision_position_estimate_t* vision_position_estimate) +{ + return mavlink_msg_vision_position_estimate_pack_chan(system_id, component_id, chan, msg, vision_position_estimate->usec, vision_position_estimate->x, vision_position_estimate->y, vision_position_estimate->z, vision_position_estimate->roll, vision_position_estimate->pitch, vision_position_estimate->yaw); +} + /** * @brief Send a vision_position_estimate message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_speed_estimate.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_speed_estimate.h index c38eee62c0..6602251283 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_speed_estimate.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_speed_estimate.h @@ -74,7 +74,7 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_pack(uint8_t system_id, * @brief Pack a vision_speed_estimate message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param usec Timestamp (microseconds, synced to UNIX time or since system boot) * @param x Global X speed @@ -113,7 +113,7 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_pack_chan(uint8_t syste } /** - * @brief Encode a vision_speed_estimate struct into a message + * @brief Encode a vision_speed_estimate struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -125,6 +125,20 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_encode(uint8_t system_i return mavlink_msg_vision_speed_estimate_pack(system_id, component_id, msg, vision_speed_estimate->usec, vision_speed_estimate->x, vision_speed_estimate->y, vision_speed_estimate->z); } +/** + * @brief Encode a vision_speed_estimate struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param vision_speed_estimate C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_vision_speed_estimate_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_vision_speed_estimate_t* vision_speed_estimate) +{ + return mavlink_msg_vision_speed_estimate_pack_chan(system_id, component_id, chan, msg, vision_speed_estimate->usec, vision_speed_estimate->x, vision_speed_estimate->y, vision_speed_estimate->z); +} + /** * @brief Send a vision_speed_estimate message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/common/version.h b/mavlink/include/mavlink/v1.0/common/version.h index c91a641de8..3c354f4229 100644 --- a/mavlink/include/mavlink/v1.0/common/version.h +++ b/mavlink/include/mavlink/v1.0/common/version.h @@ -5,7 +5,7 @@ #ifndef MAVLINK_VERSION_H #define MAVLINK_VERSION_H -#define MAVLINK_BUILD_DATE "Thu Jul 4 13:47:40 2013" +#define MAVLINK_BUILD_DATE "Tue Sep 10 23:50:05 2013" #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 254 diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/matrixpilot.h b/mavlink/include/mavlink/v1.0/matrixpilot/matrixpilot.h index f5576255ed..f590cda800 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/matrixpilot.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/matrixpilot.h @@ -77,6 +77,7 @@ enum MAV_CMD MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ @@ -85,7 +86,8 @@ enum MAV_CMD MAV_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ MAV_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ MAV_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ - MAV_CMD_ENUM_END=401, /* | */ + MAV_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ + MAV_CMD_ENUM_END=501, /* | */ }; #endif diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_airspeeds.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_airspeeds.h index bd0be342fa..bc47a547ad 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_airspeeds.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_airspeeds.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_airspeeds_pack(uint8_t system_id, uint8_t com * @brief Pack a airspeeds message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp (milliseconds since system boot) * @param airspeed_imu Airspeed estimate from IMU, cm/s @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_airspeeds_pack_chan(uint8_t system_id, uint8_ } /** - * @brief Encode a airspeeds struct into a message + * @brief Encode a airspeeds struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -149,6 +149,20 @@ static inline uint16_t mavlink_msg_airspeeds_encode(uint8_t system_id, uint8_t c return mavlink_msg_airspeeds_pack(system_id, component_id, msg, airspeeds->time_boot_ms, airspeeds->airspeed_imu, airspeeds->airspeed_pitot, airspeeds->airspeed_hot_wire, airspeeds->airspeed_ultrasonic, airspeeds->aoa, airspeeds->aoy); } +/** + * @brief Encode a airspeeds struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param airspeeds C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_airspeeds_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_airspeeds_t* airspeeds) +{ + return mavlink_msg_airspeeds_pack_chan(system_id, component_id, chan, msg, airspeeds->time_boot_ms, airspeeds->airspeed_imu, airspeeds->airspeed_pitot, airspeeds->airspeed_hot_wire, airspeeds->airspeed_ultrasonic, airspeeds->aoa, airspeeds->aoy); +} + /** * @brief Send a airspeeds message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_altitudes.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_altitudes.h index c1ce66875c..e64787cc73 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_altitudes.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_altitudes.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_altitudes_pack(uint8_t system_id, uint8_t com * @brief Pack a altitudes message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp (milliseconds since system boot) * @param alt_gps GPS altitude in meters, expressed as * 1000 (millimeters), above MSL @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_altitudes_pack_chan(uint8_t system_id, uint8_ } /** - * @brief Encode a altitudes struct into a message + * @brief Encode a altitudes struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -149,6 +149,20 @@ static inline uint16_t mavlink_msg_altitudes_encode(uint8_t system_id, uint8_t c return mavlink_msg_altitudes_pack(system_id, component_id, msg, altitudes->time_boot_ms, altitudes->alt_gps, altitudes->alt_imu, altitudes->alt_barometric, altitudes->alt_optical_flow, altitudes->alt_range_finder, altitudes->alt_extra); } +/** + * @brief Encode a altitudes struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param altitudes C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_altitudes_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_altitudes_t* altitudes) +{ + return mavlink_msg_altitudes_pack_chan(system_id, component_id, chan, msg, altitudes->time_boot_ms, altitudes->alt_gps, altitudes->alt_imu, altitudes->alt_barometric, altitudes->alt_optical_flow, altitudes->alt_range_finder, altitudes->alt_extra); +} + /** * @brief Send a altitudes message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function.h index d72a4dcacb..581bd35dc5 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function.h @@ -87,7 +87,7 @@ static inline uint16_t mavlink_msg_flexifunction_buffer_function_pack(uint8_t sy * @brief Pack a flexifunction_buffer_function message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -133,7 +133,7 @@ static inline uint16_t mavlink_msg_flexifunction_buffer_function_pack_chan(uint8 } /** - * @brief Encode a flexifunction_buffer_function struct into a message + * @brief Encode a flexifunction_buffer_function struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -145,6 +145,20 @@ static inline uint16_t mavlink_msg_flexifunction_buffer_function_encode(uint8_t return mavlink_msg_flexifunction_buffer_function_pack(system_id, component_id, msg, flexifunction_buffer_function->target_system, flexifunction_buffer_function->target_component, flexifunction_buffer_function->func_index, flexifunction_buffer_function->func_count, flexifunction_buffer_function->data_address, flexifunction_buffer_function->data_size, flexifunction_buffer_function->data); } +/** + * @brief Encode a flexifunction_buffer_function struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param flexifunction_buffer_function C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_flexifunction_buffer_function_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_buffer_function_t* flexifunction_buffer_function) +{ + return mavlink_msg_flexifunction_buffer_function_pack_chan(system_id, component_id, chan, msg, flexifunction_buffer_function->target_system, flexifunction_buffer_function->target_component, flexifunction_buffer_function->func_index, flexifunction_buffer_function->func_count, flexifunction_buffer_function->data_address, flexifunction_buffer_function->data_size, flexifunction_buffer_function->data); +} + /** * @brief Send a flexifunction_buffer_function message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function_ack.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function_ack.h index 58f1786ef9..790afd52b3 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function_ack.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function_ack.h @@ -74,7 +74,7 @@ static inline uint16_t mavlink_msg_flexifunction_buffer_function_ack_pack(uint8_ * @brief Pack a flexifunction_buffer_function_ack message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -113,7 +113,7 @@ static inline uint16_t mavlink_msg_flexifunction_buffer_function_ack_pack_chan(u } /** - * @brief Encode a flexifunction_buffer_function_ack struct into a message + * @brief Encode a flexifunction_buffer_function_ack struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -125,6 +125,20 @@ static inline uint16_t mavlink_msg_flexifunction_buffer_function_ack_encode(uint return mavlink_msg_flexifunction_buffer_function_ack_pack(system_id, component_id, msg, flexifunction_buffer_function_ack->target_system, flexifunction_buffer_function_ack->target_component, flexifunction_buffer_function_ack->func_index, flexifunction_buffer_function_ack->result); } +/** + * @brief Encode a flexifunction_buffer_function_ack struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param flexifunction_buffer_function_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_flexifunction_buffer_function_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_buffer_function_ack_t* flexifunction_buffer_function_ack) +{ + return mavlink_msg_flexifunction_buffer_function_ack_pack_chan(system_id, component_id, chan, msg, flexifunction_buffer_function_ack->target_system, flexifunction_buffer_function_ack->target_component, flexifunction_buffer_function_ack->func_index, flexifunction_buffer_function_ack->result); +} + /** * @brief Send a flexifunction_buffer_function_ack message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command.h index 2f6668cf95..ce722c8a4d 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command.h @@ -69,7 +69,7 @@ static inline uint16_t mavlink_msg_flexifunction_command_pack(uint8_t system_id, * @brief Pack a flexifunction_command message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -105,7 +105,7 @@ static inline uint16_t mavlink_msg_flexifunction_command_pack_chan(uint8_t syste } /** - * @brief Encode a flexifunction_command struct into a message + * @brief Encode a flexifunction_command struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -117,6 +117,20 @@ static inline uint16_t mavlink_msg_flexifunction_command_encode(uint8_t system_i return mavlink_msg_flexifunction_command_pack(system_id, component_id, msg, flexifunction_command->target_system, flexifunction_command->target_component, flexifunction_command->command_type); } +/** + * @brief Encode a flexifunction_command struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param flexifunction_command C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_flexifunction_command_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_command_t* flexifunction_command) +{ + return mavlink_msg_flexifunction_command_pack_chan(system_id, component_id, chan, msg, flexifunction_command->target_system, flexifunction_command->target_component, flexifunction_command->command_type); +} + /** * @brief Send a flexifunction_command message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command_ack.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command_ack.h index 4798febb01..070dc4bf8d 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command_ack.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command_ack.h @@ -64,7 +64,7 @@ static inline uint16_t mavlink_msg_flexifunction_command_ack_pack(uint8_t system * @brief Pack a flexifunction_command_ack message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param command_type Command acknowledged * @param result result of acknowledge @@ -97,7 +97,7 @@ static inline uint16_t mavlink_msg_flexifunction_command_ack_pack_chan(uint8_t s } /** - * @brief Encode a flexifunction_command_ack struct into a message + * @brief Encode a flexifunction_command_ack struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -109,6 +109,20 @@ static inline uint16_t mavlink_msg_flexifunction_command_ack_encode(uint8_t syst return mavlink_msg_flexifunction_command_ack_pack(system_id, component_id, msg, flexifunction_command_ack->command_type, flexifunction_command_ack->result); } +/** + * @brief Encode a flexifunction_command_ack struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param flexifunction_command_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_flexifunction_command_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_command_ack_t* flexifunction_command_ack) +{ + return mavlink_msg_flexifunction_command_ack_pack_chan(system_id, component_id, chan, msg, flexifunction_command_ack->command_type, flexifunction_command_ack->result); +} + /** * @brief Send a flexifunction_command_ack message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory.h index 947bfc5911..ef262a6b1d 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory.h @@ -82,7 +82,7 @@ static inline uint16_t mavlink_msg_flexifunction_directory_pack(uint8_t system_i * @brief Pack a flexifunction_directory message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -125,7 +125,7 @@ static inline uint16_t mavlink_msg_flexifunction_directory_pack_chan(uint8_t sys } /** - * @brief Encode a flexifunction_directory struct into a message + * @brief Encode a flexifunction_directory struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -137,6 +137,20 @@ static inline uint16_t mavlink_msg_flexifunction_directory_encode(uint8_t system return mavlink_msg_flexifunction_directory_pack(system_id, component_id, msg, flexifunction_directory->target_system, flexifunction_directory->target_component, flexifunction_directory->directory_type, flexifunction_directory->start_index, flexifunction_directory->count, flexifunction_directory->directory_data); } +/** + * @brief Encode a flexifunction_directory struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param flexifunction_directory C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_flexifunction_directory_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_directory_t* flexifunction_directory) +{ + return mavlink_msg_flexifunction_directory_pack_chan(system_id, component_id, chan, msg, flexifunction_directory->target_system, flexifunction_directory->target_component, flexifunction_directory->directory_type, flexifunction_directory->start_index, flexifunction_directory->count, flexifunction_directory->directory_data); +} + /** * @brief Send a flexifunction_directory message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory_ack.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory_ack.h index 5489dd6b5e..d3a386ce54 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory_ack.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory_ack.h @@ -84,7 +84,7 @@ static inline uint16_t mavlink_msg_flexifunction_directory_ack_pack(uint8_t syst * @brief Pack a flexifunction_directory_ack message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_flexifunction_directory_ack_pack_chan(uint8_t } /** - * @brief Encode a flexifunction_directory_ack struct into a message + * @brief Encode a flexifunction_directory_ack struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -141,6 +141,20 @@ static inline uint16_t mavlink_msg_flexifunction_directory_ack_encode(uint8_t sy return mavlink_msg_flexifunction_directory_ack_pack(system_id, component_id, msg, flexifunction_directory_ack->target_system, flexifunction_directory_ack->target_component, flexifunction_directory_ack->directory_type, flexifunction_directory_ack->start_index, flexifunction_directory_ack->count, flexifunction_directory_ack->result); } +/** + * @brief Encode a flexifunction_directory_ack struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param flexifunction_directory_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_flexifunction_directory_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_directory_ack_t* flexifunction_directory_ack) +{ + return mavlink_msg_flexifunction_directory_ack_pack_chan(system_id, component_id, chan, msg, flexifunction_directory_ack->target_system, flexifunction_directory_ack->target_component, flexifunction_directory_ack->directory_type, flexifunction_directory_ack->start_index, flexifunction_directory_ack->count, flexifunction_directory_ack->result); +} + /** * @brief Send a flexifunction_directory_ack message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_read_req.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_read_req.h index 9ffc2caa5f..e50f77b080 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_read_req.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_read_req.h @@ -74,7 +74,7 @@ static inline uint16_t mavlink_msg_flexifunction_read_req_pack(uint8_t system_id * @brief Pack a flexifunction_read_req message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -113,7 +113,7 @@ static inline uint16_t mavlink_msg_flexifunction_read_req_pack_chan(uint8_t syst } /** - * @brief Encode a flexifunction_read_req struct into a message + * @brief Encode a flexifunction_read_req struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -125,6 +125,20 @@ static inline uint16_t mavlink_msg_flexifunction_read_req_encode(uint8_t system_ return mavlink_msg_flexifunction_read_req_pack(system_id, component_id, msg, flexifunction_read_req->target_system, flexifunction_read_req->target_component, flexifunction_read_req->read_req_type, flexifunction_read_req->data_index); } +/** + * @brief Encode a flexifunction_read_req struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param flexifunction_read_req C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_flexifunction_read_req_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_read_req_t* flexifunction_read_req) +{ + return mavlink_msg_flexifunction_read_req_pack_chan(system_id, component_id, chan, msg, flexifunction_read_req->target_system, flexifunction_read_req->target_component, flexifunction_read_req->read_req_type, flexifunction_read_req->data_index); +} + /** * @brief Send a flexifunction_read_req message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_set.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_set.h index fc7d1d2f8e..41afb3811d 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_set.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_set.h @@ -64,7 +64,7 @@ static inline uint16_t mavlink_msg_flexifunction_set_pack(uint8_t system_id, uin * @brief Pack a flexifunction_set message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -97,7 +97,7 @@ static inline uint16_t mavlink_msg_flexifunction_set_pack_chan(uint8_t system_id } /** - * @brief Encode a flexifunction_set struct into a message + * @brief Encode a flexifunction_set struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -109,6 +109,20 @@ static inline uint16_t mavlink_msg_flexifunction_set_encode(uint8_t system_id, u return mavlink_msg_flexifunction_set_pack(system_id, component_id, msg, flexifunction_set->target_system, flexifunction_set->target_component); } +/** + * @brief Encode a flexifunction_set struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param flexifunction_set C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_flexifunction_set_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_set_t* flexifunction_set) +{ + return mavlink_msg_flexifunction_set_pack_chan(system_id, component_id, chan, msg, flexifunction_set->target_system, flexifunction_set->target_component); +} + /** * @brief Send a flexifunction_set message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f13.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f13.h index df5645e765..d21ab48168 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f13.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f13.h @@ -74,7 +74,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f13_pack(uint8_t system_id, * @brief Pack a serial_udb_extra_f13 message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param sue_week_no Serial UDB Extra GPS Week Number * @param sue_lat_origin Serial UDB Extra MP Origin Latitude @@ -113,7 +113,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f13_pack_chan(uint8_t system } /** - * @brief Encode a serial_udb_extra_f13 struct into a message + * @brief Encode a serial_udb_extra_f13 struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -125,6 +125,20 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f13_encode(uint8_t system_id return mavlink_msg_serial_udb_extra_f13_pack(system_id, component_id, msg, serial_udb_extra_f13->sue_week_no, serial_udb_extra_f13->sue_lat_origin, serial_udb_extra_f13->sue_lon_origin, serial_udb_extra_f13->sue_alt_origin); } +/** + * @brief Encode a serial_udb_extra_f13 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param serial_udb_extra_f13 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_serial_udb_extra_f13_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f13_t* serial_udb_extra_f13) +{ + return mavlink_msg_serial_udb_extra_f13_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f13->sue_week_no, serial_udb_extra_f13->sue_lat_origin, serial_udb_extra_f13->sue_lon_origin, serial_udb_extra_f13->sue_alt_origin); +} + /** * @brief Send a serial_udb_extra_f13 message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f14.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f14.h index 5e38a590a4..6ac12f28a6 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f14.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f14.h @@ -109,7 +109,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f14_pack(uint8_t system_id, * @brief Pack a serial_udb_extra_f14 message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param sue_WIND_ESTIMATION Serial UDB Extra Wind Estimation Enabled * @param sue_GPS_TYPE Serial UDB Extra Type of GPS Unit @@ -169,7 +169,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f14_pack_chan(uint8_t system } /** - * @brief Encode a serial_udb_extra_f14 struct into a message + * @brief Encode a serial_udb_extra_f14 struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -181,6 +181,20 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f14_encode(uint8_t system_id return mavlink_msg_serial_udb_extra_f14_pack(system_id, component_id, msg, serial_udb_extra_f14->sue_WIND_ESTIMATION, serial_udb_extra_f14->sue_GPS_TYPE, serial_udb_extra_f14->sue_DR, serial_udb_extra_f14->sue_BOARD_TYPE, serial_udb_extra_f14->sue_AIRFRAME, serial_udb_extra_f14->sue_RCON, serial_udb_extra_f14->sue_TRAP_FLAGS, serial_udb_extra_f14->sue_TRAP_SOURCE, serial_udb_extra_f14->sue_osc_fail_count, serial_udb_extra_f14->sue_CLOCK_CONFIG, serial_udb_extra_f14->sue_FLIGHT_PLAN_TYPE); } +/** + * @brief Encode a serial_udb_extra_f14 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param serial_udb_extra_f14 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_serial_udb_extra_f14_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f14_t* serial_udb_extra_f14) +{ + return mavlink_msg_serial_udb_extra_f14_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f14->sue_WIND_ESTIMATION, serial_udb_extra_f14->sue_GPS_TYPE, serial_udb_extra_f14->sue_DR, serial_udb_extra_f14->sue_BOARD_TYPE, serial_udb_extra_f14->sue_AIRFRAME, serial_udb_extra_f14->sue_RCON, serial_udb_extra_f14->sue_TRAP_FLAGS, serial_udb_extra_f14->sue_TRAP_SOURCE, serial_udb_extra_f14->sue_osc_fail_count, serial_udb_extra_f14->sue_CLOCK_CONFIG, serial_udb_extra_f14->sue_FLIGHT_PLAN_TYPE); +} + /** * @brief Send a serial_udb_extra_f14 message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f15.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f15.h index 8779b25ff5..10c3f4ca4f 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f15.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f15.h @@ -65,7 +65,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f15_pack(uint8_t system_id, * @brief Pack a serial_udb_extra_f15 message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param sue_ID_VEHICLE_MODEL_NAME Serial UDB Extra Model Name Of Vehicle * @param sue_ID_VEHICLE_REGISTRATION Serial UDB Extra Registraton Number of Vehicle @@ -98,7 +98,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f15_pack_chan(uint8_t system } /** - * @brief Encode a serial_udb_extra_f15 struct into a message + * @brief Encode a serial_udb_extra_f15 struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -110,6 +110,20 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f15_encode(uint8_t system_id return mavlink_msg_serial_udb_extra_f15_pack(system_id, component_id, msg, serial_udb_extra_f15->sue_ID_VEHICLE_MODEL_NAME, serial_udb_extra_f15->sue_ID_VEHICLE_REGISTRATION); } +/** + * @brief Encode a serial_udb_extra_f15 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param serial_udb_extra_f15 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_serial_udb_extra_f15_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f15_t* serial_udb_extra_f15) +{ + return mavlink_msg_serial_udb_extra_f15_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f15->sue_ID_VEHICLE_MODEL_NAME, serial_udb_extra_f15->sue_ID_VEHICLE_REGISTRATION); +} + /** * @brief Send a serial_udb_extra_f15 message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f16.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f16.h index 1a173bfe44..659e6b7c52 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f16.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f16.h @@ -65,7 +65,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f16_pack(uint8_t system_id, * @brief Pack a serial_udb_extra_f16 message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param sue_ID_LEAD_PILOT Serial UDB Extra Name of Expected Lead Pilot * @param sue_ID_DIY_DRONES_URL Serial UDB Extra URL of Lead Pilot or Team @@ -98,7 +98,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f16_pack_chan(uint8_t system } /** - * @brief Encode a serial_udb_extra_f16 struct into a message + * @brief Encode a serial_udb_extra_f16 struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -110,6 +110,20 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f16_encode(uint8_t system_id return mavlink_msg_serial_udb_extra_f16_pack(system_id, component_id, msg, serial_udb_extra_f16->sue_ID_LEAD_PILOT, serial_udb_extra_f16->sue_ID_DIY_DRONES_URL); } +/** + * @brief Encode a serial_udb_extra_f16 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param serial_udb_extra_f16 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_serial_udb_extra_f16_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f16_t* serial_udb_extra_f16) +{ + return mavlink_msg_serial_udb_extra_f16_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f16->sue_ID_LEAD_PILOT, serial_udb_extra_f16->sue_ID_DIY_DRONES_URL); +} + /** * @brief Send a serial_udb_extra_f16 message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_a.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_a.h index ddfc236ba4..15ba68a346 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_a.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_a.h @@ -194,7 +194,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f2_a_pack(uint8_t system_id, * @brief Pack a serial_udb_extra_f2_a message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param sue_time Serial UDB Extra Time * @param sue_status Serial UDB Extra Status @@ -305,7 +305,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f2_a_pack_chan(uint8_t syste } /** - * @brief Encode a serial_udb_extra_f2_a struct into a message + * @brief Encode a serial_udb_extra_f2_a struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -317,6 +317,20 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f2_a_encode(uint8_t system_i return mavlink_msg_serial_udb_extra_f2_a_pack(system_id, component_id, msg, serial_udb_extra_f2_a->sue_time, serial_udb_extra_f2_a->sue_status, serial_udb_extra_f2_a->sue_latitude, serial_udb_extra_f2_a->sue_longitude, serial_udb_extra_f2_a->sue_altitude, serial_udb_extra_f2_a->sue_waypoint_index, serial_udb_extra_f2_a->sue_rmat0, serial_udb_extra_f2_a->sue_rmat1, serial_udb_extra_f2_a->sue_rmat2, serial_udb_extra_f2_a->sue_rmat3, serial_udb_extra_f2_a->sue_rmat4, serial_udb_extra_f2_a->sue_rmat5, serial_udb_extra_f2_a->sue_rmat6, serial_udb_extra_f2_a->sue_rmat7, serial_udb_extra_f2_a->sue_rmat8, serial_udb_extra_f2_a->sue_cog, serial_udb_extra_f2_a->sue_sog, serial_udb_extra_f2_a->sue_cpu_load, serial_udb_extra_f2_a->sue_voltage_milis, serial_udb_extra_f2_a->sue_air_speed_3DIMU, serial_udb_extra_f2_a->sue_estimated_wind_0, serial_udb_extra_f2_a->sue_estimated_wind_1, serial_udb_extra_f2_a->sue_estimated_wind_2, serial_udb_extra_f2_a->sue_magFieldEarth0, serial_udb_extra_f2_a->sue_magFieldEarth1, serial_udb_extra_f2_a->sue_magFieldEarth2, serial_udb_extra_f2_a->sue_svs, serial_udb_extra_f2_a->sue_hdop); } +/** + * @brief Encode a serial_udb_extra_f2_a struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param serial_udb_extra_f2_a C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_serial_udb_extra_f2_a_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f2_a_t* serial_udb_extra_f2_a) +{ + return mavlink_msg_serial_udb_extra_f2_a_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f2_a->sue_time, serial_udb_extra_f2_a->sue_status, serial_udb_extra_f2_a->sue_latitude, serial_udb_extra_f2_a->sue_longitude, serial_udb_extra_f2_a->sue_altitude, serial_udb_extra_f2_a->sue_waypoint_index, serial_udb_extra_f2_a->sue_rmat0, serial_udb_extra_f2_a->sue_rmat1, serial_udb_extra_f2_a->sue_rmat2, serial_udb_extra_f2_a->sue_rmat3, serial_udb_extra_f2_a->sue_rmat4, serial_udb_extra_f2_a->sue_rmat5, serial_udb_extra_f2_a->sue_rmat6, serial_udb_extra_f2_a->sue_rmat7, serial_udb_extra_f2_a->sue_rmat8, serial_udb_extra_f2_a->sue_cog, serial_udb_extra_f2_a->sue_sog, serial_udb_extra_f2_a->sue_cpu_load, serial_udb_extra_f2_a->sue_voltage_milis, serial_udb_extra_f2_a->sue_air_speed_3DIMU, serial_udb_extra_f2_a->sue_estimated_wind_0, serial_udb_extra_f2_a->sue_estimated_wind_1, serial_udb_extra_f2_a->sue_estimated_wind_2, serial_udb_extra_f2_a->sue_magFieldEarth0, serial_udb_extra_f2_a->sue_magFieldEarth1, serial_udb_extra_f2_a->sue_magFieldEarth2, serial_udb_extra_f2_a->sue_svs, serial_udb_extra_f2_a->sue_hdop); +} + /** * @brief Send a serial_udb_extra_f2_a message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_b.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_b.h index 74da8416d8..7cb8c87da5 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_b.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_b.h @@ -219,7 +219,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f2_b_pack(uint8_t system_id, * @brief Pack a serial_udb_extra_f2_b message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param sue_time Serial UDB Extra Time * @param sue_pwm_input_1 Serial UDB Extra PWM Input Channel 1 @@ -345,7 +345,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f2_b_pack_chan(uint8_t syste } /** - * @brief Encode a serial_udb_extra_f2_b struct into a message + * @brief Encode a serial_udb_extra_f2_b struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -357,6 +357,20 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f2_b_encode(uint8_t system_i return mavlink_msg_serial_udb_extra_f2_b_pack(system_id, component_id, msg, serial_udb_extra_f2_b->sue_time, serial_udb_extra_f2_b->sue_pwm_input_1, serial_udb_extra_f2_b->sue_pwm_input_2, serial_udb_extra_f2_b->sue_pwm_input_3, serial_udb_extra_f2_b->sue_pwm_input_4, serial_udb_extra_f2_b->sue_pwm_input_5, serial_udb_extra_f2_b->sue_pwm_input_6, serial_udb_extra_f2_b->sue_pwm_input_7, serial_udb_extra_f2_b->sue_pwm_input_8, serial_udb_extra_f2_b->sue_pwm_input_9, serial_udb_extra_f2_b->sue_pwm_input_10, serial_udb_extra_f2_b->sue_pwm_output_1, serial_udb_extra_f2_b->sue_pwm_output_2, serial_udb_extra_f2_b->sue_pwm_output_3, serial_udb_extra_f2_b->sue_pwm_output_4, serial_udb_extra_f2_b->sue_pwm_output_5, serial_udb_extra_f2_b->sue_pwm_output_6, serial_udb_extra_f2_b->sue_pwm_output_7, serial_udb_extra_f2_b->sue_pwm_output_8, serial_udb_extra_f2_b->sue_pwm_output_9, serial_udb_extra_f2_b->sue_pwm_output_10, serial_udb_extra_f2_b->sue_imu_location_x, serial_udb_extra_f2_b->sue_imu_location_y, serial_udb_extra_f2_b->sue_imu_location_z, serial_udb_extra_f2_b->sue_flags, serial_udb_extra_f2_b->sue_osc_fails, serial_udb_extra_f2_b->sue_imu_velocity_x, serial_udb_extra_f2_b->sue_imu_velocity_y, serial_udb_extra_f2_b->sue_imu_velocity_z, serial_udb_extra_f2_b->sue_waypoint_goal_x, serial_udb_extra_f2_b->sue_waypoint_goal_y, serial_udb_extra_f2_b->sue_waypoint_goal_z, serial_udb_extra_f2_b->sue_memory_stack_free); } +/** + * @brief Encode a serial_udb_extra_f2_b struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param serial_udb_extra_f2_b C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_serial_udb_extra_f2_b_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f2_b_t* serial_udb_extra_f2_b) +{ + return mavlink_msg_serial_udb_extra_f2_b_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f2_b->sue_time, serial_udb_extra_f2_b->sue_pwm_input_1, serial_udb_extra_f2_b->sue_pwm_input_2, serial_udb_extra_f2_b->sue_pwm_input_3, serial_udb_extra_f2_b->sue_pwm_input_4, serial_udb_extra_f2_b->sue_pwm_input_5, serial_udb_extra_f2_b->sue_pwm_input_6, serial_udb_extra_f2_b->sue_pwm_input_7, serial_udb_extra_f2_b->sue_pwm_input_8, serial_udb_extra_f2_b->sue_pwm_input_9, serial_udb_extra_f2_b->sue_pwm_input_10, serial_udb_extra_f2_b->sue_pwm_output_1, serial_udb_extra_f2_b->sue_pwm_output_2, serial_udb_extra_f2_b->sue_pwm_output_3, serial_udb_extra_f2_b->sue_pwm_output_4, serial_udb_extra_f2_b->sue_pwm_output_5, serial_udb_extra_f2_b->sue_pwm_output_6, serial_udb_extra_f2_b->sue_pwm_output_7, serial_udb_extra_f2_b->sue_pwm_output_8, serial_udb_extra_f2_b->sue_pwm_output_9, serial_udb_extra_f2_b->sue_pwm_output_10, serial_udb_extra_f2_b->sue_imu_location_x, serial_udb_extra_f2_b->sue_imu_location_y, serial_udb_extra_f2_b->sue_imu_location_z, serial_udb_extra_f2_b->sue_flags, serial_udb_extra_f2_b->sue_osc_fails, serial_udb_extra_f2_b->sue_imu_velocity_x, serial_udb_extra_f2_b->sue_imu_velocity_y, serial_udb_extra_f2_b->sue_imu_velocity_z, serial_udb_extra_f2_b->sue_waypoint_goal_x, serial_udb_extra_f2_b->sue_waypoint_goal_y, serial_udb_extra_f2_b->sue_waypoint_goal_z, serial_udb_extra_f2_b->sue_memory_stack_free); +} + /** * @brief Send a serial_udb_extra_f2_b message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f4.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f4.h index 83ccbbedb4..77c616cc2e 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f4.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f4.h @@ -104,7 +104,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f4_pack(uint8_t system_id, u * @brief Pack a serial_udb_extra_f4 message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param sue_ROLL_STABILIZATION_AILERONS Serial UDB Extra Roll Stabilization with Ailerons Enabled * @param sue_ROLL_STABILIZATION_RUDDER Serial UDB Extra Roll Stabilization with Rudder Enabled @@ -161,7 +161,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f4_pack_chan(uint8_t system_ } /** - * @brief Encode a serial_udb_extra_f4 struct into a message + * @brief Encode a serial_udb_extra_f4 struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -173,6 +173,20 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f4_encode(uint8_t system_id, return mavlink_msg_serial_udb_extra_f4_pack(system_id, component_id, msg, serial_udb_extra_f4->sue_ROLL_STABILIZATION_AILERONS, serial_udb_extra_f4->sue_ROLL_STABILIZATION_RUDDER, serial_udb_extra_f4->sue_PITCH_STABILIZATION, serial_udb_extra_f4->sue_YAW_STABILIZATION_RUDDER, serial_udb_extra_f4->sue_YAW_STABILIZATION_AILERON, serial_udb_extra_f4->sue_AILERON_NAVIGATION, serial_udb_extra_f4->sue_RUDDER_NAVIGATION, serial_udb_extra_f4->sue_ALTITUDEHOLD_STABILIZED, serial_udb_extra_f4->sue_ALTITUDEHOLD_WAYPOINT, serial_udb_extra_f4->sue_RACING_MODE); } +/** + * @brief Encode a serial_udb_extra_f4 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param serial_udb_extra_f4 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_serial_udb_extra_f4_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f4_t* serial_udb_extra_f4) +{ + return mavlink_msg_serial_udb_extra_f4_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f4->sue_ROLL_STABILIZATION_AILERONS, serial_udb_extra_f4->sue_ROLL_STABILIZATION_RUDDER, serial_udb_extra_f4->sue_PITCH_STABILIZATION, serial_udb_extra_f4->sue_YAW_STABILIZATION_RUDDER, serial_udb_extra_f4->sue_YAW_STABILIZATION_AILERON, serial_udb_extra_f4->sue_AILERON_NAVIGATION, serial_udb_extra_f4->sue_RUDDER_NAVIGATION, serial_udb_extra_f4->sue_ALTITUDEHOLD_STABILIZED, serial_udb_extra_f4->sue_ALTITUDEHOLD_WAYPOINT, serial_udb_extra_f4->sue_RACING_MODE); +} + /** * @brief Send a serial_udb_extra_f4 message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f5.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f5.h index 2b2451f00d..e115a9b445 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f5.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f5.h @@ -84,7 +84,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f5_pack(uint8_t system_id, u * @brief Pack a serial_udb_extra_f5 message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param sue_YAWKP_AILERON Serial UDB YAWKP_AILERON Gain for Proporional control of navigation * @param sue_YAWKD_AILERON Serial UDB YAWKD_AILERON Gain for Rate control of navigation @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f5_pack_chan(uint8_t system_ } /** - * @brief Encode a serial_udb_extra_f5 struct into a message + * @brief Encode a serial_udb_extra_f5 struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -141,6 +141,20 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f5_encode(uint8_t system_id, return mavlink_msg_serial_udb_extra_f5_pack(system_id, component_id, msg, serial_udb_extra_f5->sue_YAWKP_AILERON, serial_udb_extra_f5->sue_YAWKD_AILERON, serial_udb_extra_f5->sue_ROLLKP, serial_udb_extra_f5->sue_ROLLKD, serial_udb_extra_f5->sue_YAW_STABILIZATION_AILERON, serial_udb_extra_f5->sue_AILERON_BOOST); } +/** + * @brief Encode a serial_udb_extra_f5 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param serial_udb_extra_f5 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_serial_udb_extra_f5_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f5_t* serial_udb_extra_f5) +{ + return mavlink_msg_serial_udb_extra_f5_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f5->sue_YAWKP_AILERON, serial_udb_extra_f5->sue_YAWKD_AILERON, serial_udb_extra_f5->sue_ROLLKP, serial_udb_extra_f5->sue_ROLLKD, serial_udb_extra_f5->sue_YAW_STABILIZATION_AILERON, serial_udb_extra_f5->sue_AILERON_BOOST); +} + /** * @brief Send a serial_udb_extra_f5 message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f6.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f6.h index 9d58ca9a8e..656103d090 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f6.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f6.h @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f6_pack(uint8_t system_id, u * @brief Pack a serial_udb_extra_f6 message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param sue_PITCHGAIN Serial UDB Extra PITCHGAIN Proportional Control * @param sue_PITCHKD Serial UDB Extra Pitch Rate Control @@ -121,7 +121,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f6_pack_chan(uint8_t system_ } /** - * @brief Encode a serial_udb_extra_f6 struct into a message + * @brief Encode a serial_udb_extra_f6 struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -133,6 +133,20 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f6_encode(uint8_t system_id, return mavlink_msg_serial_udb_extra_f6_pack(system_id, component_id, msg, serial_udb_extra_f6->sue_PITCHGAIN, serial_udb_extra_f6->sue_PITCHKD, serial_udb_extra_f6->sue_RUDDER_ELEV_MIX, serial_udb_extra_f6->sue_ROLL_ELEV_MIX, serial_udb_extra_f6->sue_ELEVATOR_BOOST); } +/** + * @brief Encode a serial_udb_extra_f6 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param serial_udb_extra_f6 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_serial_udb_extra_f6_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f6_t* serial_udb_extra_f6) +{ + return mavlink_msg_serial_udb_extra_f6_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f6->sue_PITCHGAIN, serial_udb_extra_f6->sue_PITCHKD, serial_udb_extra_f6->sue_RUDDER_ELEV_MIX, serial_udb_extra_f6->sue_ROLL_ELEV_MIX, serial_udb_extra_f6->sue_ELEVATOR_BOOST); +} + /** * @brief Send a serial_udb_extra_f6 message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f7.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f7.h index ab73b967e0..51c98e6b7a 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f7.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f7.h @@ -84,7 +84,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f7_pack(uint8_t system_id, u * @brief Pack a serial_udb_extra_f7 message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param sue_YAWKP_RUDDER Serial UDB YAWKP_RUDDER Gain for Proporional control of navigation * @param sue_YAWKD_RUDDER Serial UDB YAWKD_RUDDER Gain for Rate control of navigation @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f7_pack_chan(uint8_t system_ } /** - * @brief Encode a serial_udb_extra_f7 struct into a message + * @brief Encode a serial_udb_extra_f7 struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -141,6 +141,20 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f7_encode(uint8_t system_id, return mavlink_msg_serial_udb_extra_f7_pack(system_id, component_id, msg, serial_udb_extra_f7->sue_YAWKP_RUDDER, serial_udb_extra_f7->sue_YAWKD_RUDDER, serial_udb_extra_f7->sue_ROLLKP_RUDDER, serial_udb_extra_f7->sue_ROLLKD_RUDDER, serial_udb_extra_f7->sue_RUDDER_BOOST, serial_udb_extra_f7->sue_RTL_PITCH_DOWN); } +/** + * @brief Encode a serial_udb_extra_f7 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param serial_udb_extra_f7 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_serial_udb_extra_f7_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f7_t* serial_udb_extra_f7) +{ + return mavlink_msg_serial_udb_extra_f7_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f7->sue_YAWKP_RUDDER, serial_udb_extra_f7->sue_YAWKD_RUDDER, serial_udb_extra_f7->sue_ROLLKP_RUDDER, serial_udb_extra_f7->sue_ROLLKD_RUDDER, serial_udb_extra_f7->sue_RUDDER_BOOST, serial_udb_extra_f7->sue_RTL_PITCH_DOWN); +} + /** * @brief Send a serial_udb_extra_f7 message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f8.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f8.h index 310246e8e7..8557a95e27 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f8.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f8.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f8_pack(uint8_t system_id, u * @brief Pack a serial_udb_extra_f8 message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param sue_HEIGHT_TARGET_MAX Serial UDB Extra HEIGHT_TARGET_MAX * @param sue_HEIGHT_TARGET_MIN Serial UDB Extra HEIGHT_TARGET_MIN @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f8_pack_chan(uint8_t system_ } /** - * @brief Encode a serial_udb_extra_f8 struct into a message + * @brief Encode a serial_udb_extra_f8 struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -149,6 +149,20 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f8_encode(uint8_t system_id, return mavlink_msg_serial_udb_extra_f8_pack(system_id, component_id, msg, serial_udb_extra_f8->sue_HEIGHT_TARGET_MAX, serial_udb_extra_f8->sue_HEIGHT_TARGET_MIN, serial_udb_extra_f8->sue_ALT_HOLD_THROTTLE_MIN, serial_udb_extra_f8->sue_ALT_HOLD_THROTTLE_MAX, serial_udb_extra_f8->sue_ALT_HOLD_PITCH_MIN, serial_udb_extra_f8->sue_ALT_HOLD_PITCH_MAX, serial_udb_extra_f8->sue_ALT_HOLD_PITCH_HIGH); } +/** + * @brief Encode a serial_udb_extra_f8 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param serial_udb_extra_f8 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_serial_udb_extra_f8_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f8_t* serial_udb_extra_f8) +{ + return mavlink_msg_serial_udb_extra_f8_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f8->sue_HEIGHT_TARGET_MAX, serial_udb_extra_f8->sue_HEIGHT_TARGET_MIN, serial_udb_extra_f8->sue_ALT_HOLD_THROTTLE_MIN, serial_udb_extra_f8->sue_ALT_HOLD_THROTTLE_MAX, serial_udb_extra_f8->sue_ALT_HOLD_PITCH_MIN, serial_udb_extra_f8->sue_ALT_HOLD_PITCH_MAX, serial_udb_extra_f8->sue_ALT_HOLD_PITCH_HIGH); +} + /** * @brief Send a serial_udb_extra_f8 message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/version.h b/mavlink/include/mavlink/v1.0/matrixpilot/version.h index ea0265298b..59f3d2ded2 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/version.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/version.h @@ -5,7 +5,7 @@ #ifndef MAVLINK_VERSION_H #define MAVLINK_VERSION_H -#define MAVLINK_BUILD_DATE "Thu Jul 4 13:11:50 2013" +#define MAVLINK_BUILD_DATE "Tue Sep 10 23:49:51 2013" #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 254 diff --git a/mavlink/include/mavlink/v1.0/mavlink_helpers.h b/mavlink/include/mavlink/v1.0/mavlink_helpers.h index 72cf91fb9f..96672f847f 100644 --- a/mavlink/include/mavlink/v1.0/mavlink_helpers.h +++ b/mavlink/include/mavlink/v1.0/mavlink_helpers.h @@ -16,7 +16,12 @@ #ifndef MAVLINK_GET_CHANNEL_STATUS MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan) { +#if MAVLINK_EXTERNAL_RX_STATUS + // No m_mavlink_status array defined in function, + // has to be defined externally +#else static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; +#endif return &m_mavlink_status[chan]; } #endif @@ -29,11 +34,8 @@ MAVLINK_HELPER mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan) { #if MAVLINK_EXTERNAL_RX_BUFFER - // No m_mavlink_message array defined in function, + // No m_mavlink_buffer array defined in function, // has to be defined externally -#ifndef m_mavlink_message -#error ERROR: IF #define MAVLINK_EXTERNAL_RX_BUFFER IS SET, THE BUFFER HAS TO BE ALLOCATED OUTSIDE OF THIS FUNCTION (mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];) -#endif #else static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS]; #endif diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_attitude_control.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_attitude_control.h index 1aac8395df..ef5354d5e1 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_attitude_control.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_attitude_control.h @@ -99,7 +99,7 @@ static inline uint16_t mavlink_msg_attitude_control_pack(uint8_t system_id, uint * @brief Pack a attitude_control message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target The system to be controlled * @param roll roll @@ -153,7 +153,7 @@ static inline uint16_t mavlink_msg_attitude_control_pack_chan(uint8_t system_id, } /** - * @brief Encode a attitude_control struct into a message + * @brief Encode a attitude_control struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -165,6 +165,20 @@ static inline uint16_t mavlink_msg_attitude_control_encode(uint8_t system_id, ui return mavlink_msg_attitude_control_pack(system_id, component_id, msg, attitude_control->target, attitude_control->roll, attitude_control->pitch, attitude_control->yaw, attitude_control->thrust, attitude_control->roll_manual, attitude_control->pitch_manual, attitude_control->yaw_manual, attitude_control->thrust_manual); } +/** + * @brief Encode a attitude_control struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param attitude_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_attitude_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_attitude_control_t* attitude_control) +{ + return mavlink_msg_attitude_control_pack_chan(system_id, component_id, chan, msg, attitude_control->target, attitude_control->roll, attitude_control->pitch, attitude_control->yaw, attitude_control->thrust, attitude_control->roll_manual, attitude_control->pitch_manual, attitude_control->yaw_manual, attitude_control->thrust_manual); +} + /** * @brief Send a attitude_control message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_brief_feature.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_brief_feature.h index 353e105814..d41093792e 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_brief_feature.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_brief_feature.h @@ -92,7 +92,7 @@ static inline uint16_t mavlink_msg_brief_feature_pack(uint8_t system_id, uint8_t * @brief Pack a brief_feature message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param x x position in m * @param y y position in m @@ -141,7 +141,7 @@ static inline uint16_t mavlink_msg_brief_feature_pack_chan(uint8_t system_id, ui } /** - * @brief Encode a brief_feature struct into a message + * @brief Encode a brief_feature struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -153,6 +153,20 @@ static inline uint16_t mavlink_msg_brief_feature_encode(uint8_t system_id, uint8 return mavlink_msg_brief_feature_pack(system_id, component_id, msg, brief_feature->x, brief_feature->y, brief_feature->z, brief_feature->orientation_assignment, brief_feature->size, brief_feature->orientation, brief_feature->descriptor, brief_feature->response); } +/** + * @brief Encode a brief_feature struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param brief_feature C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_brief_feature_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_brief_feature_t* brief_feature) +{ + return mavlink_msg_brief_feature_pack_chan(system_id, component_id, chan, msg, brief_feature->x, brief_feature->y, brief_feature->z, brief_feature->orientation_assignment, brief_feature->size, brief_feature->orientation, brief_feature->descriptor, brief_feature->response); +} + /** * @brief Send a brief_feature message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_data_transmission_handshake.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_data_transmission_handshake.h index 5251c35c32..a7af8c8ebb 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_data_transmission_handshake.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_data_transmission_handshake.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_data_transmission_handshake_pack(uint8_t syst * @brief Pack a data_transmission_handshake message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param type type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) * @param size total data size in bytes (set on ACK only) @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_data_transmission_handshake_pack_chan(uint8_t } /** - * @brief Encode a data_transmission_handshake struct into a message + * @brief Encode a data_transmission_handshake struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -149,6 +149,20 @@ static inline uint16_t mavlink_msg_data_transmission_handshake_encode(uint8_t sy return mavlink_msg_data_transmission_handshake_pack(system_id, component_id, msg, data_transmission_handshake->type, data_transmission_handshake->size, data_transmission_handshake->width, data_transmission_handshake->height, data_transmission_handshake->packets, data_transmission_handshake->payload, data_transmission_handshake->jpg_quality); } +/** + * @brief Encode a data_transmission_handshake struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param data_transmission_handshake C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_data_transmission_handshake_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_data_transmission_handshake_t* data_transmission_handshake) +{ + return mavlink_msg_data_transmission_handshake_pack_chan(system_id, component_id, chan, msg, data_transmission_handshake->type, data_transmission_handshake->size, data_transmission_handshake->width, data_transmission_handshake->height, data_transmission_handshake->packets, data_transmission_handshake->payload, data_transmission_handshake->jpg_quality); +} + /** * @brief Send a data_transmission_handshake message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_encapsulated_data.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_encapsulated_data.h index feeef6f398..40001dc309 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_encapsulated_data.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_encapsulated_data.h @@ -62,7 +62,7 @@ static inline uint16_t mavlink_msg_encapsulated_data_pack(uint8_t system_id, uin * @brief Pack a encapsulated_data message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param seqnr sequence number (starting with 0 on every transmission) * @param data image data bytes @@ -93,7 +93,7 @@ static inline uint16_t mavlink_msg_encapsulated_data_pack_chan(uint8_t system_id } /** - * @brief Encode a encapsulated_data struct into a message + * @brief Encode a encapsulated_data struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -105,6 +105,20 @@ static inline uint16_t mavlink_msg_encapsulated_data_encode(uint8_t system_id, u return mavlink_msg_encapsulated_data_pack(system_id, component_id, msg, encapsulated_data->seqnr, encapsulated_data->data); } +/** + * @brief Encode a encapsulated_data struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param encapsulated_data C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_encapsulated_data_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_encapsulated_data_t* encapsulated_data) +{ + return mavlink_msg_encapsulated_data_pack_chan(system_id, component_id, chan, msg, encapsulated_data->seqnr, encapsulated_data->data); +} + /** * @brief Send a encapsulated_data message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_available.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_available.h index 6d1d9cca76..ae4db825df 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_available.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_available.h @@ -169,7 +169,7 @@ static inline uint16_t mavlink_msg_image_available_pack(uint8_t system_id, uint8 * @brief Pack a image_available message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param cam_id Camera id * @param cam_no Camera # (starts with 0) @@ -265,7 +265,7 @@ static inline uint16_t mavlink_msg_image_available_pack_chan(uint8_t system_id, } /** - * @brief Encode a image_available struct into a message + * @brief Encode a image_available struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -277,6 +277,20 @@ static inline uint16_t mavlink_msg_image_available_encode(uint8_t system_id, uin return mavlink_msg_image_available_pack(system_id, component_id, msg, image_available->cam_id, image_available->cam_no, image_available->timestamp, image_available->valid_until, image_available->img_seq, image_available->img_buf_index, image_available->width, image_available->height, image_available->depth, image_available->channels, image_available->key, image_available->exposure, image_available->gain, image_available->roll, image_available->pitch, image_available->yaw, image_available->local_z, image_available->lat, image_available->lon, image_available->alt, image_available->ground_x, image_available->ground_y, image_available->ground_z); } +/** + * @brief Encode a image_available struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param image_available C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_image_available_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_image_available_t* image_available) +{ + return mavlink_msg_image_available_pack_chan(system_id, component_id, chan, msg, image_available->cam_id, image_available->cam_no, image_available->timestamp, image_available->valid_until, image_available->img_seq, image_available->img_buf_index, image_available->width, image_available->height, image_available->depth, image_available->channels, image_available->key, image_available->exposure, image_available->gain, image_available->roll, image_available->pitch, image_available->yaw, image_available->local_z, image_available->lat, image_available->lon, image_available->alt, image_available->ground_x, image_available->ground_y, image_available->ground_z); +} + /** * @brief Send a image_available message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_trigger_control.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_trigger_control.h index 784cedf8b5..664574be94 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_trigger_control.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_trigger_control.h @@ -59,7 +59,7 @@ static inline uint16_t mavlink_msg_image_trigger_control_pack(uint8_t system_id, * @brief Pack a image_trigger_control message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param enable 0 to disable, 1 to enable * @return length of the message in bytes (excluding serial stream start sign) @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_image_trigger_control_pack_chan(uint8_t syste } /** - * @brief Encode a image_trigger_control struct into a message + * @brief Encode a image_trigger_control struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -101,6 +101,20 @@ static inline uint16_t mavlink_msg_image_trigger_control_encode(uint8_t system_i return mavlink_msg_image_trigger_control_pack(system_id, component_id, msg, image_trigger_control->enable); } +/** + * @brief Encode a image_trigger_control struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param image_trigger_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_image_trigger_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_image_trigger_control_t* image_trigger_control) +{ + return mavlink_msg_image_trigger_control_pack_chan(system_id, component_id, chan, msg, image_trigger_control->enable); +} + /** * @brief Send a image_trigger_control message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_triggered.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_triggered.h index 05b0d775fd..f3a2243afc 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_triggered.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_triggered.h @@ -114,7 +114,7 @@ static inline uint16_t mavlink_msg_image_triggered_pack(uint8_t system_id, uint8 * @brief Pack a image_triggered message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param timestamp Timestamp * @param seq IMU seq @@ -177,7 +177,7 @@ static inline uint16_t mavlink_msg_image_triggered_pack_chan(uint8_t system_id, } /** - * @brief Encode a image_triggered struct into a message + * @brief Encode a image_triggered struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -189,6 +189,20 @@ static inline uint16_t mavlink_msg_image_triggered_encode(uint8_t system_id, uin return mavlink_msg_image_triggered_pack(system_id, component_id, msg, image_triggered->timestamp, image_triggered->seq, image_triggered->roll, image_triggered->pitch, image_triggered->yaw, image_triggered->local_z, image_triggered->lat, image_triggered->lon, image_triggered->alt, image_triggered->ground_x, image_triggered->ground_y, image_triggered->ground_z); } +/** + * @brief Encode a image_triggered struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param image_triggered C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_image_triggered_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_image_triggered_t* image_triggered) +{ + return mavlink_msg_image_triggered_pack_chan(system_id, component_id, chan, msg, image_triggered->timestamp, image_triggered->seq, image_triggered->roll, image_triggered->pitch, image_triggered->yaw, image_triggered->local_z, image_triggered->lat, image_triggered->lon, image_triggered->alt, image_triggered->ground_x, image_triggered->ground_y, image_triggered->ground_z); +} + /** * @brief Send a image_triggered message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_marker.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_marker.h index 817ec60a20..6bdcceaf9a 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_marker.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_marker.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_marker_pack(uint8_t system_id, uint8_t compon * @brief Pack a marker message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param id ID * @param x x position @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_marker_pack_chan(uint8_t system_id, uint8_t c } /** - * @brief Encode a marker struct into a message + * @brief Encode a marker struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -149,6 +149,20 @@ static inline uint16_t mavlink_msg_marker_encode(uint8_t system_id, uint8_t comp return mavlink_msg_marker_pack(system_id, component_id, msg, marker->id, marker->x, marker->y, marker->z, marker->roll, marker->pitch, marker->yaw); } +/** + * @brief Encode a marker struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param marker C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_marker_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_marker_t* marker) +{ + return mavlink_msg_marker_pack_chan(system_id, component_id, chan, msg, marker->id, marker->x, marker->y, marker->z, marker->roll, marker->pitch, marker->yaw); +} + /** * @brief Send a marker message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_pattern_detected.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_pattern_detected.h index 7e29d71525..aa7b827a36 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_pattern_detected.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_pattern_detected.h @@ -72,7 +72,7 @@ static inline uint16_t mavlink_msg_pattern_detected_pack(uint8_t system_id, uint * @brief Pack a pattern_detected message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param type 0: Pattern, 1: Letter * @param confidence Confidence of detection @@ -109,7 +109,7 @@ static inline uint16_t mavlink_msg_pattern_detected_pack_chan(uint8_t system_id, } /** - * @brief Encode a pattern_detected struct into a message + * @brief Encode a pattern_detected struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -121,6 +121,20 @@ static inline uint16_t mavlink_msg_pattern_detected_encode(uint8_t system_id, ui return mavlink_msg_pattern_detected_pack(system_id, component_id, msg, pattern_detected->type, pattern_detected->confidence, pattern_detected->file, pattern_detected->detected); } +/** + * @brief Encode a pattern_detected struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param pattern_detected C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_pattern_detected_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_pattern_detected_t* pattern_detected) +{ + return mavlink_msg_pattern_detected_pack_chan(system_id, component_id, chan, msg, pattern_detected->type, pattern_detected->confidence, pattern_detected->file, pattern_detected->detected); +} + /** * @brief Send a pattern_detected message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest.h index a6faebbb5a..913a52897e 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest.h @@ -92,7 +92,7 @@ static inline uint16_t mavlink_msg_point_of_interest_pack(uint8_t system_id, uin * @brief Pack a point_of_interest message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param type 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug * @param color 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta @@ -141,7 +141,7 @@ static inline uint16_t mavlink_msg_point_of_interest_pack_chan(uint8_t system_id } /** - * @brief Encode a point_of_interest struct into a message + * @brief Encode a point_of_interest struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -153,6 +153,20 @@ static inline uint16_t mavlink_msg_point_of_interest_encode(uint8_t system_id, u return mavlink_msg_point_of_interest_pack(system_id, component_id, msg, point_of_interest->type, point_of_interest->color, point_of_interest->coordinate_system, point_of_interest->timeout, point_of_interest->x, point_of_interest->y, point_of_interest->z, point_of_interest->name); } +/** + * @brief Encode a point_of_interest struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param point_of_interest C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_point_of_interest_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_point_of_interest_t* point_of_interest) +{ + return mavlink_msg_point_of_interest_pack_chan(system_id, component_id, chan, msg, point_of_interest->type, point_of_interest->color, point_of_interest->coordinate_system, point_of_interest->timeout, point_of_interest->x, point_of_interest->y, point_of_interest->z, point_of_interest->name); +} + /** * @brief Send a point_of_interest message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest_connection.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest_connection.h index 8d02f9e5c4..e244364318 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest_connection.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest_connection.h @@ -107,7 +107,7 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_pack(uint8_t sys * @brief Pack a point_of_interest_connection message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param type 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug * @param color 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta @@ -165,7 +165,7 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_pack_chan(uint8_ } /** - * @brief Encode a point_of_interest_connection struct into a message + * @brief Encode a point_of_interest_connection struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -177,6 +177,20 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_encode(uint8_t s return mavlink_msg_point_of_interest_connection_pack(system_id, component_id, msg, point_of_interest_connection->type, point_of_interest_connection->color, point_of_interest_connection->coordinate_system, point_of_interest_connection->timeout, point_of_interest_connection->xp1, point_of_interest_connection->yp1, point_of_interest_connection->zp1, point_of_interest_connection->xp2, point_of_interest_connection->yp2, point_of_interest_connection->zp2, point_of_interest_connection->name); } +/** + * @brief Encode a point_of_interest_connection struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param point_of_interest_connection C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_point_of_interest_connection_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_point_of_interest_connection_t* point_of_interest_connection) +{ + return mavlink_msg_point_of_interest_connection_pack_chan(system_id, component_id, chan, msg, point_of_interest_connection->type, point_of_interest_connection->color, point_of_interest_connection->coordinate_system, point_of_interest_connection->timeout, point_of_interest_connection->xp1, point_of_interest_connection->yp1, point_of_interest_connection->zp1, point_of_interest_connection->xp2, point_of_interest_connection->yp2, point_of_interest_connection->zp2, point_of_interest_connection->name); +} + /** * @brief Send a point_of_interest_connection message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_position_control_setpoint.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_position_control_setpoint.h index 6c86be3ab5..6f4ca510ae 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_position_control_setpoint.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_position_control_setpoint.h @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_position_control_setpoint_pack(uint8_t system * @brief Pack a position_control_setpoint message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param id ID of waypoint, 0 for plain position * @param x x position @@ -121,7 +121,7 @@ static inline uint16_t mavlink_msg_position_control_setpoint_pack_chan(uint8_t s } /** - * @brief Encode a position_control_setpoint struct into a message + * @brief Encode a position_control_setpoint struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -133,6 +133,20 @@ static inline uint16_t mavlink_msg_position_control_setpoint_encode(uint8_t syst return mavlink_msg_position_control_setpoint_pack(system_id, component_id, msg, position_control_setpoint->id, position_control_setpoint->x, position_control_setpoint->y, position_control_setpoint->z, position_control_setpoint->yaw); } +/** + * @brief Encode a position_control_setpoint struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param position_control_setpoint C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_position_control_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_position_control_setpoint_t* position_control_setpoint) +{ + return mavlink_msg_position_control_setpoint_pack_chan(system_id, component_id, chan, msg, position_control_setpoint->id, position_control_setpoint->x, position_control_setpoint->y, position_control_setpoint->z, position_control_setpoint->yaw); +} + /** * @brief Send a position_control_setpoint message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_raw_aux.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_raw_aux.h index 0a0dbdb378..08cedbb4b2 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_raw_aux.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_raw_aux.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_raw_aux_pack(uint8_t system_id, uint8_t compo * @brief Pack a raw_aux message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param adc1 ADC1 (J405 ADC3, LPC2148 AD0.6) * @param adc2 ADC2 (J405 ADC5, LPC2148 AD0.2) @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_raw_aux_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a raw_aux struct into a message + * @brief Encode a raw_aux struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -149,6 +149,20 @@ static inline uint16_t mavlink_msg_raw_aux_encode(uint8_t system_id, uint8_t com return mavlink_msg_raw_aux_pack(system_id, component_id, msg, raw_aux->adc1, raw_aux->adc2, raw_aux->adc3, raw_aux->adc4, raw_aux->vbat, raw_aux->temp, raw_aux->baro); } +/** + * @brief Encode a raw_aux struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param raw_aux C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_raw_aux_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_raw_aux_t* raw_aux) +{ + return mavlink_msg_raw_aux_pack_chan(system_id, component_id, chan, msg, raw_aux->adc1, raw_aux->adc2, raw_aux->adc3, raw_aux->adc4, raw_aux->vbat, raw_aux->temp, raw_aux->baro); +} + /** * @brief Send a raw_aux message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_cam_shutter.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_cam_shutter.h index 7be6409119..b26d748c25 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_cam_shutter.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_cam_shutter.h @@ -84,7 +84,7 @@ static inline uint16_t mavlink_msg_set_cam_shutter_pack(uint8_t system_id, uint8 * @brief Pack a set_cam_shutter message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param cam_no Camera id * @param cam_mode Camera mode: 0 = auto, 1 = manual @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_set_cam_shutter_pack_chan(uint8_t system_id, } /** - * @brief Encode a set_cam_shutter struct into a message + * @brief Encode a set_cam_shutter struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -141,6 +141,20 @@ static inline uint16_t mavlink_msg_set_cam_shutter_encode(uint8_t system_id, uin return mavlink_msg_set_cam_shutter_pack(system_id, component_id, msg, set_cam_shutter->cam_no, set_cam_shutter->cam_mode, set_cam_shutter->trigger_pin, set_cam_shutter->interval, set_cam_shutter->exposure, set_cam_shutter->gain); } +/** + * @brief Encode a set_cam_shutter struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_cam_shutter C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_cam_shutter_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_cam_shutter_t* set_cam_shutter) +{ + return mavlink_msg_set_cam_shutter_pack_chan(system_id, component_id, chan, msg, set_cam_shutter->cam_no, set_cam_shutter->cam_mode, set_cam_shutter->trigger_pin, set_cam_shutter->interval, set_cam_shutter->exposure, set_cam_shutter->gain); +} + /** * @brief Send a set_cam_shutter message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_position_control_offset.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_position_control_offset.h index 25bff659ed..f1f9e698f8 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_position_control_offset.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_position_control_offset.h @@ -84,7 +84,7 @@ static inline uint16_t mavlink_msg_set_position_control_offset_pack(uint8_t syst * @brief Pack a set_position_control_offset message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_set_position_control_offset_pack_chan(uint8_t } /** - * @brief Encode a set_position_control_offset struct into a message + * @brief Encode a set_position_control_offset struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -141,6 +141,20 @@ static inline uint16_t mavlink_msg_set_position_control_offset_encode(uint8_t sy return mavlink_msg_set_position_control_offset_pack(system_id, component_id, msg, set_position_control_offset->target_system, set_position_control_offset->target_component, set_position_control_offset->x, set_position_control_offset->y, set_position_control_offset->z, set_position_control_offset->yaw); } +/** + * @brief Encode a set_position_control_offset struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_position_control_offset C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_position_control_offset_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_position_control_offset_t* set_position_control_offset) +{ + return mavlink_msg_set_position_control_offset_pack_chan(system_id, component_id, chan, msg, set_position_control_offset->target_system, set_position_control_offset->target_component, set_position_control_offset->x, set_position_control_offset->y, set_position_control_offset->z, set_position_control_offset->yaw); +} + /** * @brief Send a set_position_control_offset message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_command.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_command.h index 4267880186..9458087da9 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_command.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_command.h @@ -74,7 +74,7 @@ static inline uint16_t mavlink_msg_watchdog_command_pack(uint8_t system_id, uint * @brief Pack a watchdog_command message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system_id Target system ID * @param watchdog_id Watchdog ID @@ -113,7 +113,7 @@ static inline uint16_t mavlink_msg_watchdog_command_pack_chan(uint8_t system_id, } /** - * @brief Encode a watchdog_command struct into a message + * @brief Encode a watchdog_command struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -125,6 +125,20 @@ static inline uint16_t mavlink_msg_watchdog_command_encode(uint8_t system_id, ui return mavlink_msg_watchdog_command_pack(system_id, component_id, msg, watchdog_command->target_system_id, watchdog_command->watchdog_id, watchdog_command->process_id, watchdog_command->command_id); } +/** + * @brief Encode a watchdog_command struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param watchdog_command C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_watchdog_command_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_watchdog_command_t* watchdog_command) +{ + return mavlink_msg_watchdog_command_pack_chan(system_id, component_id, chan, msg, watchdog_command->target_system_id, watchdog_command->watchdog_id, watchdog_command->process_id, watchdog_command->command_id); +} + /** * @brief Send a watchdog_command message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_heartbeat.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_heartbeat.h index 49478999ce..3f4295ee5d 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_heartbeat.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_heartbeat.h @@ -64,7 +64,7 @@ static inline uint16_t mavlink_msg_watchdog_heartbeat_pack(uint8_t system_id, ui * @brief Pack a watchdog_heartbeat message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param watchdog_id Watchdog ID * @param process_count Number of processes @@ -97,7 +97,7 @@ static inline uint16_t mavlink_msg_watchdog_heartbeat_pack_chan(uint8_t system_i } /** - * @brief Encode a watchdog_heartbeat struct into a message + * @brief Encode a watchdog_heartbeat struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -109,6 +109,20 @@ static inline uint16_t mavlink_msg_watchdog_heartbeat_encode(uint8_t system_id, return mavlink_msg_watchdog_heartbeat_pack(system_id, component_id, msg, watchdog_heartbeat->watchdog_id, watchdog_heartbeat->process_count); } +/** + * @brief Encode a watchdog_heartbeat struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param watchdog_heartbeat C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_watchdog_heartbeat_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_watchdog_heartbeat_t* watchdog_heartbeat) +{ + return mavlink_msg_watchdog_heartbeat_pack_chan(system_id, component_id, chan, msg, watchdog_heartbeat->watchdog_id, watchdog_heartbeat->process_count); +} + /** * @brief Send a watchdog_heartbeat message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_info.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_info.h index d706ef85e1..55853cdde4 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_info.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_info.h @@ -78,7 +78,7 @@ static inline uint16_t mavlink_msg_watchdog_process_info_pack(uint8_t system_id, * @brief Pack a watchdog_process_info message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param watchdog_id Watchdog ID * @param process_id Process ID @@ -118,7 +118,7 @@ static inline uint16_t mavlink_msg_watchdog_process_info_pack_chan(uint8_t syste } /** - * @brief Encode a watchdog_process_info struct into a message + * @brief Encode a watchdog_process_info struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -130,6 +130,20 @@ static inline uint16_t mavlink_msg_watchdog_process_info_encode(uint8_t system_i return mavlink_msg_watchdog_process_info_pack(system_id, component_id, msg, watchdog_process_info->watchdog_id, watchdog_process_info->process_id, watchdog_process_info->name, watchdog_process_info->arguments, watchdog_process_info->timeout); } +/** + * @brief Encode a watchdog_process_info struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param watchdog_process_info C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_watchdog_process_info_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_watchdog_process_info_t* watchdog_process_info) +{ + return mavlink_msg_watchdog_process_info_pack_chan(system_id, component_id, chan, msg, watchdog_process_info->watchdog_id, watchdog_process_info->process_id, watchdog_process_info->name, watchdog_process_info->arguments, watchdog_process_info->timeout); +} + /** * @brief Send a watchdog_process_info message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_status.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_status.h index b1bbaaae7b..a0410d803e 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_status.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_status.h @@ -84,7 +84,7 @@ static inline uint16_t mavlink_msg_watchdog_process_status_pack(uint8_t system_i * @brief Pack a watchdog_process_status message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param watchdog_id Watchdog ID * @param process_id Process ID @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_watchdog_process_status_pack_chan(uint8_t sys } /** - * @brief Encode a watchdog_process_status struct into a message + * @brief Encode a watchdog_process_status struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -141,6 +141,20 @@ static inline uint16_t mavlink_msg_watchdog_process_status_encode(uint8_t system return mavlink_msg_watchdog_process_status_pack(system_id, component_id, msg, watchdog_process_status->watchdog_id, watchdog_process_status->process_id, watchdog_process_status->state, watchdog_process_status->muted, watchdog_process_status->pid, watchdog_process_status->crashes); } +/** + * @brief Encode a watchdog_process_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param watchdog_process_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_watchdog_process_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_watchdog_process_status_t* watchdog_process_status) +{ + return mavlink_msg_watchdog_process_status_pack_chan(system_id, component_id, chan, msg, watchdog_process_status->watchdog_id, watchdog_process_status->process_id, watchdog_process_status->state, watchdog_process_status->muted, watchdog_process_status->pid, watchdog_process_status->crashes); +} + /** * @brief Send a watchdog_process_status message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h b/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h index a2d0d0e14e..3fc9e64773 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h @@ -72,6 +72,7 @@ enum MAV_CMD MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ @@ -80,7 +81,8 @@ enum MAV_CMD MAV_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ MAV_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ MAV_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ - MAV_CMD_ENUM_END=401, /* | */ + MAV_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ + MAV_CMD_ENUM_END=501, /* | */ MAV_CMD_DO_START_SEARCH=10001, /* Starts a search |1 to arm, 0 to disarm| */ MAV_CMD_DO_FINISH_SEARCH=10002, /* Starts a search |1 to arm, 0 to disarm| */ MAV_CMD_NAV_SWEEP=10003, /* Starts a search |1 to arm, 0 to disarm| */ diff --git a/mavlink/include/mavlink/v1.0/pixhawk/version.h b/mavlink/include/mavlink/v1.0/pixhawk/version.h index 61fe6e930d..07831a4256 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/version.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/version.h @@ -5,7 +5,7 @@ #ifndef MAVLINK_VERSION_H #define MAVLINK_VERSION_H -#define MAVLINK_BUILD_DATE "Thu Jul 4 13:12:05 2013" +#define MAVLINK_BUILD_DATE "Tue Sep 10 23:49:13 2013" #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 diff --git a/mavlink/include/mavlink/v1.0/protocol.h b/mavlink/include/mavlink/v1.0/protocol.h index ddacae59c4..86e7ff588b 100644 --- a/mavlink/include/mavlink/v1.0/protocol.h +++ b/mavlink/include/mavlink/v1.0/protocol.h @@ -162,7 +162,7 @@ static inline void byte_copy_8(char *dst, const char *src) /* like memcpy(), but if src is NULL, do a memset to zero */ -static void mav_array_memcpy(void *dest, const void *src, size_t n) +static inline void mav_array_memcpy(void *dest, const void *src, size_t n) { if (src == NULL) { memset(dest, 0, n); diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_ack.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_ack.h index 5cdd584561..73e9d75dbc 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_ack.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_ack.h @@ -72,7 +72,7 @@ static inline uint16_t mavlink_msg_cmd_airspeed_ack_pack(uint8_t system_id, uint * @brief Pack a cmd_airspeed_ack message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param spCmd @@ -109,7 +109,7 @@ static inline uint16_t mavlink_msg_cmd_airspeed_ack_pack_chan(uint8_t system_id, } /** - * @brief Encode a cmd_airspeed_ack struct into a message + * @brief Encode a cmd_airspeed_ack struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -121,6 +121,20 @@ static inline uint16_t mavlink_msg_cmd_airspeed_ack_encode(uint8_t system_id, ui return mavlink_msg_cmd_airspeed_ack_pack(system_id, component_id, msg, cmd_airspeed_ack->spCmd, cmd_airspeed_ack->ack); } +/** + * @brief Encode a cmd_airspeed_ack struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param cmd_airspeed_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_cmd_airspeed_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_cmd_airspeed_ack_t* cmd_airspeed_ack) +{ + return mavlink_msg_cmd_airspeed_ack_pack_chan(system_id, component_id, chan, msg, cmd_airspeed_ack->spCmd, cmd_airspeed_ack->ack); +} + /** * @brief Send a cmd_airspeed_ack message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_chng.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_chng.h index 2a4894fe72..b6b567f993 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_chng.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_chng.h @@ -72,7 +72,7 @@ static inline uint16_t mavlink_msg_cmd_airspeed_chng_pack(uint8_t system_id, uin * @brief Pack a cmd_airspeed_chng message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target @@ -109,7 +109,7 @@ static inline uint16_t mavlink_msg_cmd_airspeed_chng_pack_chan(uint8_t system_id } /** - * @brief Encode a cmd_airspeed_chng struct into a message + * @brief Encode a cmd_airspeed_chng struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -121,6 +121,20 @@ static inline uint16_t mavlink_msg_cmd_airspeed_chng_encode(uint8_t system_id, u return mavlink_msg_cmd_airspeed_chng_pack(system_id, component_id, msg, cmd_airspeed_chng->target, cmd_airspeed_chng->spCmd); } +/** + * @brief Encode a cmd_airspeed_chng struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param cmd_airspeed_chng C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_cmd_airspeed_chng_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_cmd_airspeed_chng_t* cmd_airspeed_chng) +{ + return mavlink_msg_cmd_airspeed_chng_pack_chan(system_id, component_id, chan, msg, cmd_airspeed_chng->target, cmd_airspeed_chng->spCmd); +} + /** * @brief Send a cmd_airspeed_chng message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_filt_rot_vel.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_filt_rot_vel.h index d600e91749..642f7aab93 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_filt_rot_vel.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_filt_rot_vel.h @@ -63,7 +63,7 @@ static inline uint16_t mavlink_msg_filt_rot_vel_pack(uint8_t system_id, uint8_t * @brief Pack a filt_rot_vel message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param rotVel @@ -95,7 +95,7 @@ static inline uint16_t mavlink_msg_filt_rot_vel_pack_chan(uint8_t system_id, uin } /** - * @brief Encode a filt_rot_vel struct into a message + * @brief Encode a filt_rot_vel struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -107,6 +107,20 @@ static inline uint16_t mavlink_msg_filt_rot_vel_encode(uint8_t system_id, uint8_ return mavlink_msg_filt_rot_vel_pack(system_id, component_id, msg, filt_rot_vel->rotVel); } +/** + * @brief Encode a filt_rot_vel struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param filt_rot_vel C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_filt_rot_vel_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_filt_rot_vel_t* filt_rot_vel) +{ + return mavlink_msg_filt_rot_vel_pack_chan(system_id, component_id, chan, msg, filt_rot_vel->rotVel); +} + /** * @brief Send a filt_rot_vel message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_llc_out.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_llc_out.h index cb1c86f436..0f8369881a 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_llc_out.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_llc_out.h @@ -73,7 +73,7 @@ static inline uint16_t mavlink_msg_llc_out_pack(uint8_t system_id, uint8_t compo * @brief Pack a llc_out message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param servoOut @@ -110,7 +110,7 @@ static inline uint16_t mavlink_msg_llc_out_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a llc_out struct into a message + * @brief Encode a llc_out struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -122,6 +122,20 @@ static inline uint16_t mavlink_msg_llc_out_encode(uint8_t system_id, uint8_t com return mavlink_msg_llc_out_pack(system_id, component_id, msg, llc_out->servoOut, llc_out->MotorOut); } +/** + * @brief Encode a llc_out struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param llc_out C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_llc_out_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_llc_out_t* llc_out) +{ + return mavlink_msg_llc_out_pack_chan(system_id, component_id, chan, msg, llc_out->servoOut, llc_out->MotorOut); +} + /** * @brief Send a llc_out message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_temp.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_temp.h index b6bd0d9e52..5d93823262 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_temp.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_temp.h @@ -63,7 +63,7 @@ static inline uint16_t mavlink_msg_obs_air_temp_pack(uint8_t system_id, uint8_t * @brief Pack a obs_air_temp message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param airT @@ -95,7 +95,7 @@ static inline uint16_t mavlink_msg_obs_air_temp_pack_chan(uint8_t system_id, uin } /** - * @brief Encode a obs_air_temp struct into a message + * @brief Encode a obs_air_temp struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -107,6 +107,20 @@ static inline uint16_t mavlink_msg_obs_air_temp_encode(uint8_t system_id, uint8_ return mavlink_msg_obs_air_temp_pack(system_id, component_id, msg, obs_air_temp->airT); } +/** + * @brief Encode a obs_air_temp struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param obs_air_temp C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_obs_air_temp_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_air_temp_t* obs_air_temp) +{ + return mavlink_msg_obs_air_temp_pack_chan(system_id, component_id, chan, msg, obs_air_temp->airT); +} + /** * @brief Send a obs_air_temp message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_velocity.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_velocity.h index 87a65cfa06..35d813ca15 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_velocity.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_velocity.h @@ -81,7 +81,7 @@ static inline uint16_t mavlink_msg_obs_air_velocity_pack(uint8_t system_id, uint * @brief Pack a obs_air_velocity message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param magnitude @@ -123,7 +123,7 @@ static inline uint16_t mavlink_msg_obs_air_velocity_pack_chan(uint8_t system_id, } /** - * @brief Encode a obs_air_velocity struct into a message + * @brief Encode a obs_air_velocity struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -135,6 +135,20 @@ static inline uint16_t mavlink_msg_obs_air_velocity_encode(uint8_t system_id, ui return mavlink_msg_obs_air_velocity_pack(system_id, component_id, msg, obs_air_velocity->magnitude, obs_air_velocity->aoa, obs_air_velocity->slip); } +/** + * @brief Encode a obs_air_velocity struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param obs_air_velocity C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_obs_air_velocity_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_air_velocity_t* obs_air_velocity) +{ + return mavlink_msg_obs_air_velocity_pack_chan(system_id, component_id, chan, msg, obs_air_velocity->magnitude, obs_air_velocity->aoa, obs_air_velocity->slip); +} + /** * @brief Send a obs_air_velocity message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_attitude.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_attitude.h index 602eafc80b..9c80cd66eb 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_attitude.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_attitude.h @@ -63,7 +63,7 @@ static inline uint16_t mavlink_msg_obs_attitude_pack(uint8_t system_id, uint8_t * @brief Pack a obs_attitude message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param quat @@ -95,7 +95,7 @@ static inline uint16_t mavlink_msg_obs_attitude_pack_chan(uint8_t system_id, uin } /** - * @brief Encode a obs_attitude struct into a message + * @brief Encode a obs_attitude struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -107,6 +107,20 @@ static inline uint16_t mavlink_msg_obs_attitude_encode(uint8_t system_id, uint8_ return mavlink_msg_obs_attitude_pack(system_id, component_id, msg, obs_attitude->quat); } +/** + * @brief Encode a obs_attitude struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param obs_attitude C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_obs_attitude_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_attitude_t* obs_attitude) +{ + return mavlink_msg_obs_attitude_pack_chan(system_id, component_id, chan, msg, obs_attitude->quat); +} + /** * @brief Send a obs_attitude message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_bias.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_bias.h index 3ab855ba81..24dd43b579 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_bias.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_bias.h @@ -73,7 +73,7 @@ static inline uint16_t mavlink_msg_obs_bias_pack(uint8_t system_id, uint8_t comp * @brief Pack a obs_bias message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param accBias @@ -110,7 +110,7 @@ static inline uint16_t mavlink_msg_obs_bias_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a obs_bias struct into a message + * @brief Encode a obs_bias struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -122,6 +122,20 @@ static inline uint16_t mavlink_msg_obs_bias_encode(uint8_t system_id, uint8_t co return mavlink_msg_obs_bias_pack(system_id, component_id, msg, obs_bias->accBias, obs_bias->gyroBias); } +/** + * @brief Encode a obs_bias struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param obs_bias C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_obs_bias_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_bias_t* obs_bias) +{ + return mavlink_msg_obs_bias_pack_chan(system_id, component_id, chan, msg, obs_bias->accBias, obs_bias->gyroBias); +} + /** * @brief Send a obs_bias message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_position.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_position.h index ec494d51a2..cfc2fe7e10 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_position.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_position.h @@ -81,7 +81,7 @@ static inline uint16_t mavlink_msg_obs_position_pack(uint8_t system_id, uint8_t * @brief Pack a obs_position message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param lon @@ -123,7 +123,7 @@ static inline uint16_t mavlink_msg_obs_position_pack_chan(uint8_t system_id, uin } /** - * @brief Encode a obs_position struct into a message + * @brief Encode a obs_position struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -135,6 +135,20 @@ static inline uint16_t mavlink_msg_obs_position_encode(uint8_t system_id, uint8_ return mavlink_msg_obs_position_pack(system_id, component_id, msg, obs_position->lon, obs_position->lat, obs_position->alt); } +/** + * @brief Encode a obs_position struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param obs_position C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_obs_position_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_position_t* obs_position) +{ + return mavlink_msg_obs_position_pack_chan(system_id, component_id, chan, msg, obs_position->lon, obs_position->lat, obs_position->alt); +} + /** * @brief Send a obs_position message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_qff.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_qff.h index e42b0261fa..24e272bf7a 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_qff.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_qff.h @@ -63,7 +63,7 @@ static inline uint16_t mavlink_msg_obs_qff_pack(uint8_t system_id, uint8_t compo * @brief Pack a obs_qff message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param qff @@ -95,7 +95,7 @@ static inline uint16_t mavlink_msg_obs_qff_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a obs_qff struct into a message + * @brief Encode a obs_qff struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -107,6 +107,20 @@ static inline uint16_t mavlink_msg_obs_qff_encode(uint8_t system_id, uint8_t com return mavlink_msg_obs_qff_pack(system_id, component_id, msg, obs_qff->qff); } +/** + * @brief Encode a obs_qff struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param obs_qff C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_obs_qff_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_qff_t* obs_qff) +{ + return mavlink_msg_obs_qff_pack_chan(system_id, component_id, chan, msg, obs_qff->qff); +} + /** * @brief Send a obs_qff message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_velocity.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_velocity.h index 2fa6e9111f..6e3776632b 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_velocity.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_velocity.h @@ -63,7 +63,7 @@ static inline uint16_t mavlink_msg_obs_velocity_pack(uint8_t system_id, uint8_t * @brief Pack a obs_velocity message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param vel @@ -95,7 +95,7 @@ static inline uint16_t mavlink_msg_obs_velocity_pack_chan(uint8_t system_id, uin } /** - * @brief Encode a obs_velocity struct into a message + * @brief Encode a obs_velocity struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -107,6 +107,20 @@ static inline uint16_t mavlink_msg_obs_velocity_encode(uint8_t system_id, uint8_ return mavlink_msg_obs_velocity_pack(system_id, component_id, msg, obs_velocity->vel); } +/** + * @brief Encode a obs_velocity struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param obs_velocity C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_obs_velocity_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_velocity_t* obs_velocity) +{ + return mavlink_msg_obs_velocity_pack_chan(system_id, component_id, chan, msg, obs_velocity->vel); +} + /** * @brief Send a obs_velocity message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_wind.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_wind.h index bc9f6e4eff..55f7cb2ae1 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_wind.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_wind.h @@ -63,7 +63,7 @@ static inline uint16_t mavlink_msg_obs_wind_pack(uint8_t system_id, uint8_t comp * @brief Pack a obs_wind message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param wind @@ -95,7 +95,7 @@ static inline uint16_t mavlink_msg_obs_wind_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a obs_wind struct into a message + * @brief Encode a obs_wind struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -107,6 +107,20 @@ static inline uint16_t mavlink_msg_obs_wind_encode(uint8_t system_id, uint8_t co return mavlink_msg_obs_wind_pack(system_id, component_id, msg, obs_wind->wind); } +/** + * @brief Encode a obs_wind struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param obs_wind C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_obs_wind_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_wind_t* obs_wind) +{ + return mavlink_msg_obs_wind_pack_chan(system_id, component_id, chan, msg, obs_wind->wind); +} + /** * @brief Send a obs_wind message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_pm_elec.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_pm_elec.h index 0799af07f8..e0963ece79 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_pm_elec.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_pm_elec.h @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_pm_elec_pack(uint8_t system_id, uint8_t compo * @brief Pack a pm_elec message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param PwCons @@ -119,7 +119,7 @@ static inline uint16_t mavlink_msg_pm_elec_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a pm_elec struct into a message + * @brief Encode a pm_elec struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -131,6 +131,20 @@ static inline uint16_t mavlink_msg_pm_elec_encode(uint8_t system_id, uint8_t com return mavlink_msg_pm_elec_pack(system_id, component_id, msg, pm_elec->PwCons, pm_elec->BatStat, pm_elec->PwGen); } +/** + * @brief Encode a pm_elec struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param pm_elec C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_pm_elec_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_pm_elec_t* pm_elec) +{ + return mavlink_msg_pm_elec_pack_chan(system_id, component_id, chan, msg, pm_elec->PwCons, pm_elec->BatStat, pm_elec->PwGen); +} + /** * @brief Send a pm_elec message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_sys_stat.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_sys_stat.h index e5e483a73a..94f3e58bb9 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_sys_stat.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_sys_stat.h @@ -90,7 +90,7 @@ static inline uint16_t mavlink_msg_sys_stat_pack(uint8_t system_id, uint8_t comp * @brief Pack a sys_stat message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param gps @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_sys_stat_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a sys_stat struct into a message + * @brief Encode a sys_stat struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -149,6 +149,20 @@ static inline uint16_t mavlink_msg_sys_stat_encode(uint8_t system_id, uint8_t co return mavlink_msg_sys_stat_pack(system_id, component_id, msg, sys_stat->gps, sys_stat->act, sys_stat->mod, sys_stat->commRssi); } +/** + * @brief Encode a sys_stat struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param sys_stat C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_sys_stat_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_sys_stat_t* sys_stat) +{ + return mavlink_msg_sys_stat_pack_chan(system_id, component_id, chan, msg, sys_stat->gps, sys_stat->act, sys_stat->mod, sys_stat->commRssi); +} + /** * @brief Send a sys_stat message * @param chan MAVLink channel to send the message diff --git a/mavlink/include/mavlink/v1.0/sensesoar/version.h b/mavlink/include/mavlink/v1.0/sensesoar/version.h index 4633e6eb5c..ab3f47b9df 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/version.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/version.h @@ -5,7 +5,7 @@ #ifndef MAVLINK_VERSION_H #define MAVLINK_VERSION_H -#define MAVLINK_BUILD_DATE "Thu Jul 4 13:12:22 2013" +#define MAVLINK_BUILD_DATE "Tue Sep 10 23:50:02 2013" #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 254 From 235378d62cf48db1dc90d6938cbf48f843b47879 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Sep 2013 23:53:39 +0200 Subject: [PATCH 615/872] Removed unused files --- .../v1.0/common/mavlink_msg_6dof_setpoint.h | 276 --------------- .../v1.0/common/mavlink_msg_8dof_setpoint.h | 320 ------------------ 2 files changed, 596 deletions(-) delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_6dof_setpoint.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_8dof_setpoint.h diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_6dof_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_6dof_setpoint.h deleted file mode 100644 index 0c4ab8c7df..0000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_6dof_setpoint.h +++ /dev/null @@ -1,276 +0,0 @@ -// MESSAGE 6DOF_SETPOINT PACKING - -#define MAVLINK_MSG_ID_6DOF_SETPOINT 149 - -typedef struct __mavlink_6dof_setpoint_t -{ - float trans_x; ///< Translational Component in x - float trans_y; ///< Translational Component in y - float trans_z; ///< Translational Component in z - float rot_x; ///< Rotational Component in x - float rot_y; ///< Rotational Component in y - float rot_z; ///< Rotational Component in z - uint8_t target_system; ///< System ID -} mavlink_6dof_setpoint_t; - -#define MAVLINK_MSG_ID_6DOF_SETPOINT_LEN 25 -#define MAVLINK_MSG_ID_149_LEN 25 - - - -#define MAVLINK_MESSAGE_INFO_6DOF_SETPOINT { \ - "6DOF_SETPOINT", \ - 7, \ - { { "trans_x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_6dof_setpoint_t, trans_x) }, \ - { "trans_y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_6dof_setpoint_t, trans_y) }, \ - { "trans_z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_6dof_setpoint_t, trans_z) }, \ - { "rot_x", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_6dof_setpoint_t, rot_x) }, \ - { "rot_y", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_6dof_setpoint_t, rot_y) }, \ - { "rot_z", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_6dof_setpoint_t, rot_z) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_6dof_setpoint_t, target_system) }, \ - } \ -} - - -/** - * @brief Pack a 6dof_setpoint message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param trans_x Translational Component in x - * @param trans_y Translational Component in y - * @param trans_z Translational Component in z - * @param rot_x Rotational Component in x - * @param rot_y Rotational Component in y - * @param rot_z Rotational Component in z - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_6dof_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, float trans_x, float trans_y, float trans_z, float rot_x, float rot_y, float rot_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[25]; - _mav_put_float(buf, 0, trans_x); - _mav_put_float(buf, 4, trans_y); - _mav_put_float(buf, 8, trans_z); - _mav_put_float(buf, 12, rot_x); - _mav_put_float(buf, 16, rot_y); - _mav_put_float(buf, 20, rot_z); - _mav_put_uint8_t(buf, 24, target_system); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 25); -#else - mavlink_6dof_setpoint_t packet; - packet.trans_x = trans_x; - packet.trans_y = trans_y; - packet.trans_z = trans_z; - packet.rot_x = rot_x; - packet.rot_y = rot_y; - packet.rot_z = rot_z; - packet.target_system = target_system; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 25); -#endif - - msg->msgid = MAVLINK_MSG_ID_6DOF_SETPOINT; - return mavlink_finalize_message(msg, system_id, component_id, 25, 144); -} - -/** - * @brief Pack a 6dof_setpoint message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param trans_x Translational Component in x - * @param trans_y Translational Component in y - * @param trans_z Translational Component in z - * @param rot_x Rotational Component in x - * @param rot_y Rotational Component in y - * @param rot_z Rotational Component in z - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_6dof_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,float trans_x,float trans_y,float trans_z,float rot_x,float rot_y,float rot_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[25]; - _mav_put_float(buf, 0, trans_x); - _mav_put_float(buf, 4, trans_y); - _mav_put_float(buf, 8, trans_z); - _mav_put_float(buf, 12, rot_x); - _mav_put_float(buf, 16, rot_y); - _mav_put_float(buf, 20, rot_z); - _mav_put_uint8_t(buf, 24, target_system); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 25); -#else - mavlink_6dof_setpoint_t packet; - packet.trans_x = trans_x; - packet.trans_y = trans_y; - packet.trans_z = trans_z; - packet.rot_x = rot_x; - packet.rot_y = rot_y; - packet.rot_z = rot_z; - packet.target_system = target_system; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 25); -#endif - - msg->msgid = MAVLINK_MSG_ID_6DOF_SETPOINT; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 25, 144); -} - -/** - * @brief Encode a 6dof_setpoint struct into a message - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param 6dof_setpoint C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_6dof_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_6dof_setpoint_t* 6dof_setpoint) -{ - return mavlink_msg_6dof_setpoint_pack(system_id, component_id, msg, 6dof_setpoint->target_system, 6dof_setpoint->trans_x, 6dof_setpoint->trans_y, 6dof_setpoint->trans_z, 6dof_setpoint->rot_x, 6dof_setpoint->rot_y, 6dof_setpoint->rot_z); -} - -/** - * @brief Send a 6dof_setpoint message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param trans_x Translational Component in x - * @param trans_y Translational Component in y - * @param trans_z Translational Component in z - * @param rot_x Rotational Component in x - * @param rot_y Rotational Component in y - * @param rot_z Rotational Component in z - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_6dof_setpoint_send(mavlink_channel_t chan, uint8_t target_system, float trans_x, float trans_y, float trans_z, float rot_x, float rot_y, float rot_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[25]; - _mav_put_float(buf, 0, trans_x); - _mav_put_float(buf, 4, trans_y); - _mav_put_float(buf, 8, trans_z); - _mav_put_float(buf, 12, rot_x); - _mav_put_float(buf, 16, rot_y); - _mav_put_float(buf, 20, rot_z); - _mav_put_uint8_t(buf, 24, target_system); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_6DOF_SETPOINT, buf, 25, 144); -#else - mavlink_6dof_setpoint_t packet; - packet.trans_x = trans_x; - packet.trans_y = trans_y; - packet.trans_z = trans_z; - packet.rot_x = rot_x; - packet.rot_y = rot_y; - packet.rot_z = rot_z; - packet.target_system = target_system; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_6DOF_SETPOINT, (const char *)&packet, 25, 144); -#endif -} - -#endif - -// MESSAGE 6DOF_SETPOINT UNPACKING - - -/** - * @brief Get field target_system from 6dof_setpoint message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_6dof_setpoint_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 24); -} - -/** - * @brief Get field trans_x from 6dof_setpoint message - * - * @return Translational Component in x - */ -static inline float mavlink_msg_6dof_setpoint_get_trans_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field trans_y from 6dof_setpoint message - * - * @return Translational Component in y - */ -static inline float mavlink_msg_6dof_setpoint_get_trans_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field trans_z from 6dof_setpoint message - * - * @return Translational Component in z - */ -static inline float mavlink_msg_6dof_setpoint_get_trans_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field rot_x from 6dof_setpoint message - * - * @return Rotational Component in x - */ -static inline float mavlink_msg_6dof_setpoint_get_rot_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field rot_y from 6dof_setpoint message - * - * @return Rotational Component in y - */ -static inline float mavlink_msg_6dof_setpoint_get_rot_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field rot_z from 6dof_setpoint message - * - * @return Rotational Component in z - */ -static inline float mavlink_msg_6dof_setpoint_get_rot_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Decode a 6dof_setpoint message into a struct - * - * @param msg The message to decode - * @param 6dof_setpoint C-struct to decode the message contents into - */ -static inline void mavlink_msg_6dof_setpoint_decode(const mavlink_message_t* msg, mavlink_6dof_setpoint_t* 6dof_setpoint) -{ -#if MAVLINK_NEED_BYTE_SWAP - 6dof_setpoint->trans_x = mavlink_msg_6dof_setpoint_get_trans_x(msg); - 6dof_setpoint->trans_y = mavlink_msg_6dof_setpoint_get_trans_y(msg); - 6dof_setpoint->trans_z = mavlink_msg_6dof_setpoint_get_trans_z(msg); - 6dof_setpoint->rot_x = mavlink_msg_6dof_setpoint_get_rot_x(msg); - 6dof_setpoint->rot_y = mavlink_msg_6dof_setpoint_get_rot_y(msg); - 6dof_setpoint->rot_z = mavlink_msg_6dof_setpoint_get_rot_z(msg); - 6dof_setpoint->target_system = mavlink_msg_6dof_setpoint_get_target_system(msg); -#else - memcpy(6dof_setpoint, _MAV_PAYLOAD(msg), 25); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_8dof_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_8dof_setpoint.h deleted file mode 100644 index 521d2fb668..0000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_8dof_setpoint.h +++ /dev/null @@ -1,320 +0,0 @@ -// MESSAGE 8DOF_SETPOINT PACKING - -#define MAVLINK_MSG_ID_8DOF_SETPOINT 148 - -typedef struct __mavlink_8dof_setpoint_t -{ - float val1; ///< Value 1 - float val2; ///< Value 2 - float val3; ///< Value 3 - float val4; ///< Value 4 - float val5; ///< Value 5 - float val6; ///< Value 6 - float val7; ///< Value 7 - float val8; ///< Value 8 - uint8_t target_system; ///< System ID -} mavlink_8dof_setpoint_t; - -#define MAVLINK_MSG_ID_8DOF_SETPOINT_LEN 33 -#define MAVLINK_MSG_ID_148_LEN 33 - - - -#define MAVLINK_MESSAGE_INFO_8DOF_SETPOINT { \ - "8DOF_SETPOINT", \ - 9, \ - { { "val1", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_8dof_setpoint_t, val1) }, \ - { "val2", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_8dof_setpoint_t, val2) }, \ - { "val3", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_8dof_setpoint_t, val3) }, \ - { "val4", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_8dof_setpoint_t, val4) }, \ - { "val5", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_8dof_setpoint_t, val5) }, \ - { "val6", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_8dof_setpoint_t, val6) }, \ - { "val7", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_8dof_setpoint_t, val7) }, \ - { "val8", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_8dof_setpoint_t, val8) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_8dof_setpoint_t, target_system) }, \ - } \ -} - - -/** - * @brief Pack a 8dof_setpoint message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param val1 Value 1 - * @param val2 Value 2 - * @param val3 Value 3 - * @param val4 Value 4 - * @param val5 Value 5 - * @param val6 Value 6 - * @param val7 Value 7 - * @param val8 Value 8 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_8dof_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, float val1, float val2, float val3, float val4, float val5, float val6, float val7, float val8) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[33]; - _mav_put_float(buf, 0, val1); - _mav_put_float(buf, 4, val2); - _mav_put_float(buf, 8, val3); - _mav_put_float(buf, 12, val4); - _mav_put_float(buf, 16, val5); - _mav_put_float(buf, 20, val6); - _mav_put_float(buf, 24, val7); - _mav_put_float(buf, 28, val8); - _mav_put_uint8_t(buf, 32, target_system); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 33); -#else - mavlink_8dof_setpoint_t packet; - packet.val1 = val1; - packet.val2 = val2; - packet.val3 = val3; - packet.val4 = val4; - packet.val5 = val5; - packet.val6 = val6; - packet.val7 = val7; - packet.val8 = val8; - packet.target_system = target_system; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 33); -#endif - - msg->msgid = MAVLINK_MSG_ID_8DOF_SETPOINT; - return mavlink_finalize_message(msg, system_id, component_id, 33, 42); -} - -/** - * @brief Pack a 8dof_setpoint message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param val1 Value 1 - * @param val2 Value 2 - * @param val3 Value 3 - * @param val4 Value 4 - * @param val5 Value 5 - * @param val6 Value 6 - * @param val7 Value 7 - * @param val8 Value 8 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_8dof_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,float val1,float val2,float val3,float val4,float val5,float val6,float val7,float val8) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[33]; - _mav_put_float(buf, 0, val1); - _mav_put_float(buf, 4, val2); - _mav_put_float(buf, 8, val3); - _mav_put_float(buf, 12, val4); - _mav_put_float(buf, 16, val5); - _mav_put_float(buf, 20, val6); - _mav_put_float(buf, 24, val7); - _mav_put_float(buf, 28, val8); - _mav_put_uint8_t(buf, 32, target_system); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 33); -#else - mavlink_8dof_setpoint_t packet; - packet.val1 = val1; - packet.val2 = val2; - packet.val3 = val3; - packet.val4 = val4; - packet.val5 = val5; - packet.val6 = val6; - packet.val7 = val7; - packet.val8 = val8; - packet.target_system = target_system; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 33); -#endif - - msg->msgid = MAVLINK_MSG_ID_8DOF_SETPOINT; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 33, 42); -} - -/** - * @brief Encode a 8dof_setpoint struct into a message - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param 8dof_setpoint C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_8dof_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_8dof_setpoint_t* 8dof_setpoint) -{ - return mavlink_msg_8dof_setpoint_pack(system_id, component_id, msg, 8dof_setpoint->target_system, 8dof_setpoint->val1, 8dof_setpoint->val2, 8dof_setpoint->val3, 8dof_setpoint->val4, 8dof_setpoint->val5, 8dof_setpoint->val6, 8dof_setpoint->val7, 8dof_setpoint->val8); -} - -/** - * @brief Send a 8dof_setpoint message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param val1 Value 1 - * @param val2 Value 2 - * @param val3 Value 3 - * @param val4 Value 4 - * @param val5 Value 5 - * @param val6 Value 6 - * @param val7 Value 7 - * @param val8 Value 8 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_8dof_setpoint_send(mavlink_channel_t chan, uint8_t target_system, float val1, float val2, float val3, float val4, float val5, float val6, float val7, float val8) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[33]; - _mav_put_float(buf, 0, val1); - _mav_put_float(buf, 4, val2); - _mav_put_float(buf, 8, val3); - _mav_put_float(buf, 12, val4); - _mav_put_float(buf, 16, val5); - _mav_put_float(buf, 20, val6); - _mav_put_float(buf, 24, val7); - _mav_put_float(buf, 28, val8); - _mav_put_uint8_t(buf, 32, target_system); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_8DOF_SETPOINT, buf, 33, 42); -#else - mavlink_8dof_setpoint_t packet; - packet.val1 = val1; - packet.val2 = val2; - packet.val3 = val3; - packet.val4 = val4; - packet.val5 = val5; - packet.val6 = val6; - packet.val7 = val7; - packet.val8 = val8; - packet.target_system = target_system; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_8DOF_SETPOINT, (const char *)&packet, 33, 42); -#endif -} - -#endif - -// MESSAGE 8DOF_SETPOINT UNPACKING - - -/** - * @brief Get field target_system from 8dof_setpoint message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_8dof_setpoint_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 32); -} - -/** - * @brief Get field val1 from 8dof_setpoint message - * - * @return Value 1 - */ -static inline float mavlink_msg_8dof_setpoint_get_val1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field val2 from 8dof_setpoint message - * - * @return Value 2 - */ -static inline float mavlink_msg_8dof_setpoint_get_val2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field val3 from 8dof_setpoint message - * - * @return Value 3 - */ -static inline float mavlink_msg_8dof_setpoint_get_val3(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field val4 from 8dof_setpoint message - * - * @return Value 4 - */ -static inline float mavlink_msg_8dof_setpoint_get_val4(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field val5 from 8dof_setpoint message - * - * @return Value 5 - */ -static inline float mavlink_msg_8dof_setpoint_get_val5(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field val6 from 8dof_setpoint message - * - * @return Value 6 - */ -static inline float mavlink_msg_8dof_setpoint_get_val6(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field val7 from 8dof_setpoint message - * - * @return Value 7 - */ -static inline float mavlink_msg_8dof_setpoint_get_val7(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Get field val8 from 8dof_setpoint message - * - * @return Value 8 - */ -static inline float mavlink_msg_8dof_setpoint_get_val8(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 28); -} - -/** - * @brief Decode a 8dof_setpoint message into a struct - * - * @param msg The message to decode - * @param 8dof_setpoint C-struct to decode the message contents into - */ -static inline void mavlink_msg_8dof_setpoint_decode(const mavlink_message_t* msg, mavlink_8dof_setpoint_t* 8dof_setpoint) -{ -#if MAVLINK_NEED_BYTE_SWAP - 8dof_setpoint->val1 = mavlink_msg_8dof_setpoint_get_val1(msg); - 8dof_setpoint->val2 = mavlink_msg_8dof_setpoint_get_val2(msg); - 8dof_setpoint->val3 = mavlink_msg_8dof_setpoint_get_val3(msg); - 8dof_setpoint->val4 = mavlink_msg_8dof_setpoint_get_val4(msg); - 8dof_setpoint->val5 = mavlink_msg_8dof_setpoint_get_val5(msg); - 8dof_setpoint->val6 = mavlink_msg_8dof_setpoint_get_val6(msg); - 8dof_setpoint->val7 = mavlink_msg_8dof_setpoint_get_val7(msg); - 8dof_setpoint->val8 = mavlink_msg_8dof_setpoint_get_val8(msg); - 8dof_setpoint->target_system = mavlink_msg_8dof_setpoint_get_target_system(msg); -#else - memcpy(8dof_setpoint, _MAV_PAYLOAD(msg), 33); -#endif -} From da3620bd53ebe2276a491c89ca02247bbe97ca73 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 11 Sep 2013 01:15:54 +0200 Subject: [PATCH 616/872] Compile fix --- src/lib/external_lgpl/module.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/external_lgpl/module.mk b/src/lib/external_lgpl/module.mk index 619a1a5df3..53f1629e39 100644 --- a/src/lib/external_lgpl/module.mk +++ b/src/lib/external_lgpl/module.mk @@ -45,4 +45,4 @@ # or any of the license implications. # -SRCS = tecs/TECS.cpp +SRCS = tecs/tecs.cpp From 8755d76d1bdc2c9b2fa5a1366217498e24ad1e67 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 11 Sep 2013 08:56:45 -0700 Subject: [PATCH 617/872] Hotfix - fault decode typo in ARMv7M macros --- Debug/ARMv7M | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Debug/ARMv7M b/Debug/ARMv7M index 803d964534..4c539b345d 100644 --- a/Debug/ARMv7M +++ b/Debug/ARMv7M @@ -56,7 +56,7 @@ define vecstate if $mmfsr & (1<<3) printf " during exception return" end - if $mmfsr & (1<<0) + if $mmfsr & (1<<1) printf " during data access" end if $mmfsr & (1<<0) From 3a326cb467e9ba4892c5fbea978b5146677c9876 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 11 Sep 2013 22:14:56 +0200 Subject: [PATCH 618/872] Guard probe / reset against other SPI drivers --- src/drivers/l3gd20/l3gd20.cpp | 13 +++++++++++-- src/drivers/lsm303d/lsm303d.cpp | 19 +++++++++++++++---- 2 files changed, 26 insertions(+), 6 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index e6d765e13a..970e8cf4b4 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -377,9 +377,12 @@ out: int L3GD20::probe() { + irqstate_t flags = irqsave(); /* read dummy value to void to clear SPI statemachine on sensor */ (void)read_reg(ADDR_WHO_AM_I); + bool success = false; + /* verify that the device is attached and functioning, accept L3GD20 and L3GD20H */ if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) { @@ -390,15 +393,21 @@ L3GD20::probe() #else #error This driver needs a board selection, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2 #endif - return OK; + + success = true; } if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_H) { _orientation = SENSOR_BOARD_ROTATION_180_DEG; - return OK; + success = true; } + irqrestore(flags); + + if (success) + return OK; + return -EIO; } diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 05d6f1881d..35904cc4d6 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -404,7 +404,7 @@ public: LSM303D_mag(LSM303D *parent); ~LSM303D_mag(); - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); protected: @@ -498,8 +498,10 @@ LSM303D::init() int mag_ret; /* do SPI init (and probe) first */ - if (SPI::init() != OK) + if (SPI::init() != OK) { + warnx("SPI init failed"); goto out; + } /* allocate basic report buffers */ _num_accel_reports = 2; @@ -541,6 +543,7 @@ out: void LSM303D::reset() { + irqstate_t flags = irqsave(); /* enable accel*/ write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE); @@ -555,6 +558,7 @@ LSM303D::reset() mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA); mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE); + irqrestore(flags); _accel_read = 0; _mag_read = 0; @@ -563,11 +567,16 @@ LSM303D::reset() int LSM303D::probe() { + irqstate_t flags = irqsave(); /* 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) + bool success = (read_reg(ADDR_WHO_AM_I) == WHO_I_AM); + + irqrestore(flags); + + if (success) return OK; return -EIO; @@ -1470,8 +1479,10 @@ start() /* create the driver */ g_dev = new LSM303D(1 /* XXX magic number */, ACCEL_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); - if (g_dev == nullptr) + if (g_dev == nullptr) { + warnx("failed instantiating LSM303D obj"); goto fail; + } if (OK != g_dev->init()) goto fail; From bbac1445b0ab8efc914399ba0c41116e0854c729 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 1 Sep 2013 13:25:57 -0700 Subject: [PATCH 619/872] Add DMA buffer allocation pool. --- src/drivers/boards/px4fmu-v2/px4fmu2_init.c | 61 ++++++++++++++++++++- 1 file changed, 60 insertions(+), 1 deletion(-) diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c index 135767b26a..c5d0377bc1 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c @@ -58,6 +58,7 @@ #include #include #include +#include #include #include "board_config.h" @@ -69,6 +70,7 @@ #include #include +#include /**************************************************************************** * Pre-Processor Definitions @@ -76,6 +78,10 @@ /* Configuration ************************************************************/ +#if !defined(CONFIG_GRAN) || !defined(CONFIG_FAT_DMAMEMORY) +# error microSD DMA support requires CONFIG_GRAN and CONFIG_FAT_DMAMEMORY +#endif + /* Debug ********************************************************************/ #ifdef CONFIG_CPP_HAVE_VARARGS @@ -96,10 +102,59 @@ * Protected Functions ****************************************************************************/ +static GRAN_HANDLE dma_allocator; + +/* + * The DMA heap size constrains the total number of things that can be + * ready to do DMA at a time. + * + * For example, FAT DMA depends on one sector-sized buffer per filesystem plus + * one sector-sized buffer per file. + * + * We use a fundamental alignment / granule size of 64B; this is sufficient + * to guarantee alignment for the largest STM32 DMA burst (16 beats x 32bits). + */ +static uint8_t g_dma_heap[8192] __attribute__((aligned(64))); +static perf_counter_t g_dma_perf; + +static void +dma_alloc_init(void) +{ + dma_allocator = gran_initialize(g_dma_heap, + sizeof(g_dma_heap), + 7, /* 128B granule - must be > alignment (XXX bug?) */ + 6); /* 64B alignment */ + if (dma_allocator == NULL) { + message("[boot] DMA allocator setup FAILED"); + } else { + g_dma_perf = perf_alloc(PC_COUNT, "DMA allocations"); + } +} + /**************************************************************************** * Public Functions ****************************************************************************/ +/* + * DMA-aware allocator stubs for the FAT filesystem. + */ + +__EXPORT void *fat_dma_alloc(size_t size); +__EXPORT void fat_dma_free(FAR void *memory, size_t size); + +void * +fat_dma_alloc(size_t size) +{ + perf_count(g_dma_perf); + return gran_alloc(dma_allocator, size); +} + +void +fat_dma_free(FAR void *memory, size_t size) +{ + gran_free(dma_allocator, memory, size); +} + /************************************************************************************ * Name: stm32_boardinitialize * @@ -110,7 +165,8 @@ * ************************************************************************************/ -__EXPORT void stm32_boardinitialize(void) +__EXPORT void +stm32_boardinitialize(void) { /* configure SPI interfaces */ stm32_spiinitialize(); @@ -170,6 +226,9 @@ __EXPORT int nsh_archinitialize(void) /* configure the high-resolution time/callout interface */ hrt_init(); + /* configure the DMA allocator */ + dma_alloc_init(); + /* configure CPU load estimation */ #ifdef CONFIG_SCHED_INSTRUMENTATION cpuload_initialize_once(); From f49e444ce3b074919ebd187983be5ff0913c5111 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 3 Sep 2013 21:21:07 -0700 Subject: [PATCH 620/872] Defconfig hacks to get me a console. --- nuttx-configs/px4fmu-v2/nsh/defconfig | 125 +++++++++++++------------- 1 file changed, 61 insertions(+), 64 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 0615950a2d..70adf6d9d9 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -38,7 +38,31 @@ CONFIG_ARCH_MATH_H=y # # Debug Options # -# CONFIG_DEBUG is not set +CONFIG_DEBUG=y +CONFIG_DEBUG_VERBOSE=y + +# +# Subsystem Debug Options +# +# CONFIG_DEBUG_MM is not set +# CONFIG_DEBUG_SCHED is not set +# CONFIG_DEBUG_USB is not set +CONFIG_DEBUG_FS=y +# CONFIG_DEBUG_LIB is not set +# CONFIG_DEBUG_BINFMT is not set +# CONFIG_DEBUG_GRAPHICS is not set + +# +# Driver Debug Options +# +# CONFIG_DEBUG_ANALOG is not set +# CONFIG_DEBUG_I2C is not set +# CONFIG_DEBUG_SPI is not set +# CONFIG_DEBUG_SDIO is not set +# CONFIG_DEBUG_GPIO is not set +CONFIG_DEBUG_DMA=y +# CONFIG_DEBUG_WATCHDOG is not set +# CONFIG_DEBUG_AUDIO is not set CONFIG_DEBUG_SYMBOLS=y # @@ -86,6 +110,7 @@ CONFIG_ARCH_HAVE_FPU=y CONFIG_ARCH_FPU=y CONFIG_ARCH_HAVE_MPU=y # CONFIG_ARMV7M_MPU is not set +# CONFIG_DEBUG_HARDFAULT is not set # # ARMV7M Configuration Options @@ -94,7 +119,9 @@ 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=0x00010000 +# CONFIG_SDIO_WIDTH_D1_ONLY is not set # # STM32 Configuration Options @@ -240,9 +267,6 @@ 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 # @@ -254,7 +278,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 is not set CONFIG_STM32_DMACAPABLE=y # CONFIG_STM32_TIM1_PWM is not set # CONFIG_STM32_TIM3_PWM is not set @@ -271,40 +295,22 @@ CONFIG_STM32_USART=y # U[S]ART Configuration # # CONFIG_USART1_RS485 is not set -CONFIG_USART1_RXDMA=y +# CONFIG_USART1_RXDMA is not set # CONFIG_USART2_RS485 is not set CONFIG_USART2_RXDMA=y # CONFIG_USART3_RS485 is not set # CONFIG_USART3_RXDMA is not set # CONFIG_UART4_RS485 is not set # CONFIG_UART4_RXDMA is not set -# 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_UART7_RS485 is not set +# CONFIG_UART7_RXDMA is not set +# CONFIG_UART8_RS485 is not set +# CONFIG_UART8_RXDMA is not set +CONFIG_SERIAL_DISABLE_REORDERING=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 # @@ -323,25 +329,7 @@ CONFIG_STM32_I2CTIMEOTICKS=500 # # SDIO Configuration # - -# -# 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_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 +CONFIG_SDIO_PRI=128 # # USB Host Configuration @@ -393,15 +381,14 @@ CONFIG_BOOT_RUNFROMFLASH=y # # Board Selection # -CONFIG_ARCH_BOARD_PX4FMU_V2=y -# CONFIG_ARCH_BOARD_CUSTOM is not set -CONFIG_ARCH_BOARD="px4fmu-v2" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD="" # # Common Board Options # -CONFIG_NSH_MMCSDSLOTNO=0 CONFIG_NSH_MMCSDMINOR=0 +CONFIG_NSH_MMCSDSLOTNO=0 # # Board-Specific Options @@ -497,7 +484,16 @@ CONFIG_WATCHDOG=y # CONFIG_BCH is not set # CONFIG_INPUT is not set # CONFIG_LCD is not set -# CONFIG_MMCSD is not set +CONFIG_MMCSD=y +CONFIG_MMCSD_NSLOTS=1 +# CONFIG_MMCSD_READONLY is not set +CONFIG_MMCSD_MULTIBLOCK_DISABLE=y +# CONFIG_MMCSD_MMCSUPPORT is not set +# CONFIG_MMCSD_HAVECARDDETECT is not set +# CONFIG_MMCSD_SPI is not set +CONFIG_MMCSD_SDIO=y +# CONFIG_SDIO_MUXBUS is not set +# CONFIG_SDIO_BLOCKSETUP is not set CONFIG_MTD=y # @@ -523,7 +519,7 @@ CONFIG_PIPES=y # CONFIG_POWER is not set # CONFIG_SENSORS is not set CONFIG_SERIAL=y -CONFIG_DEV_LOWCONSOLE=y +# CONFIG_DEV_LOWCONSOLE is not set CONFIG_SERIAL_REMOVABLE=y # CONFIG_16550_UART is not set CONFIG_ARCH_HAVE_UART4=y @@ -536,6 +532,7 @@ CONFIG_ARCH_HAVE_USART6=y CONFIG_MCU_SERIAL=y CONFIG_STANDARD_SERIAL=y CONFIG_SERIAL_NPOLLWAITERS=2 +# CONFIG_SERIAL_TIOCSERGSTRUCT is not set # CONFIG_USART1_SERIAL_CONSOLE is not set # CONFIG_USART2_SERIAL_CONSOLE is not set # CONFIG_USART3_SERIAL_CONSOLE is not set @@ -640,7 +637,6 @@ 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 @@ -650,7 +646,7 @@ CONFIG_USBDEV_MAXPOWER=500 # CONFIG_USBDEV_COMPOSITE is not set # CONFIG_PL2303 is not set CONFIG_CDCACM=y -CONFIG_CDCACM_CONSOLE=n +# CONFIG_CDCACM_CONSOLE is not set CONFIG_CDCACM_EP0MAXPACKET=64 CONFIG_CDCACM_EPINTIN=1 CONFIG_CDCACM_EPINTIN_FSSIZE=64 @@ -702,7 +698,7 @@ CONFIG_FAT_LCNAMES=y CONFIG_FAT_LFN=y CONFIG_FAT_MAXFNAME=32 CONFIG_FS_FATTIME=y -# CONFIG_FAT_DMAMEMORY is not set +CONFIG_FAT_DMAMEMORY=y CONFIG_FS_NXFFS=y CONFIG_NXFFS_PREALLOCATED=y CONFIG_NXFFS_ERASEDSTATE=0xff @@ -716,10 +712,8 @@ CONFIG_FS_BINFS=y # # System Logging # -CONFIG_SYSLOG_ENABLE=y -CONFIG_SYSLOG=y -CONFIG_SYSLOG_CHAR=y -CONFIG_SYSLOG_DEVPATH="/dev/ttyS6" +# CONFIG_SYSLOG_ENABLE is not set +# CONFIG_SYSLOG is not set # # Graphics Support @@ -733,8 +727,9 @@ CONFIG_SYSLOG_DEVPATH="/dev/ttyS6" # CONFIG_MM_SMALL is not set CONFIG_MM_REGIONS=2 CONFIG_GRAN=y -CONFIG_GRAN_SINGLE=y -CONFIG_GRAN_INTR=y +# CONFIG_GRAN_SINGLE is not set +# CONFIG_GRAN_INTR is not set +# CONFIG_DEBUG_GRAN is not set # # Audio Support @@ -1002,6 +997,7 @@ CONFIG_NSH_CONSOLE=y # # USB Trace Support # +# CONFIG_NSH_USBDEV_TRACE is not set # CONFIG_NSH_CONDEV is not set CONFIG_NSH_ARCHINIT=y @@ -1031,6 +1027,7 @@ CONFIG_NSH_ARCHINIT=y # # FLASH Erase-all Command # +# CONFIG_SYSTEM_FLASH_ERASEALL is not set # # readline() From ed4b34547c1bddeb696b9e3c46bdb15407a845c9 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 3 Sep 2013 21:21:41 -0700 Subject: [PATCH 621/872] Make the init code compile if we don't have the granule allocator / dma allocator required --- src/drivers/boards/px4fmu-v2/px4fmu2_init.c | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c index c5d0377bc1..ae2a645f70 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c @@ -78,10 +78,6 @@ /* Configuration ************************************************************/ -#if !defined(CONFIG_GRAN) || !defined(CONFIG_FAT_DMAMEMORY) -# error microSD DMA support requires CONFIG_GRAN and CONFIG_FAT_DMAMEMORY -#endif - /* Debug ********************************************************************/ #ifdef CONFIG_CPP_HAVE_VARARGS @@ -102,6 +98,11 @@ * Protected Functions ****************************************************************************/ +#if defined(CONFIG_FAT_DMAMEMORY) +# if !defined(CONFIG_GRAN) || !defined(CONFIG_FAT_DMAMEMORY) +# error microSD DMA support requires CONFIG_GRAN +# endif + static GRAN_HANDLE dma_allocator; /* @@ -155,6 +156,12 @@ fat_dma_free(FAR void *memory, size_t size) gran_free(dma_allocator, memory, size); } +#else + +# define dma_alloc_init() + +#endif + /************************************************************************************ * Name: stm32_boardinitialize * From 514d32e961e37f68443871dd93f4ce4c89c4aad9 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 3 Sep 2013 21:22:03 -0700 Subject: [PATCH 622/872] Cut down 'tests file' for debugging --- src/systemcmds/tests/tests_file.c | 43 +++++++++++++++++++++++++++++-- 1 file changed, 41 insertions(+), 2 deletions(-) diff --git a/src/systemcmds/tests/tests_file.c b/src/systemcmds/tests/tests_file.c index 47f480758c..588d648bdf 100644 --- a/src/systemcmds/tests/tests_file.c +++ b/src/systemcmds/tests/tests_file.c @@ -52,6 +52,8 @@ int test_file(int argc, char *argv[]) { + const iterations = 10; + /* check if microSD card is mounted */ struct stat buffer; if (stat("/fs/microsd/", &buffer)) { @@ -67,7 +69,43 @@ test_file(int argc, char *argv[]) memset(buf, 0, sizeof(buf)); start = hrt_absolute_time(); - for (unsigned i = 0; i < 1024; i++) { + for (unsigned i = 0; i < iterations; i++) { + perf_begin(wperf); + write(fd, buf, sizeof(buf)); + perf_end(wperf); + } + end = hrt_absolute_time(); + + close(fd); + + warnx("%dKiB in %llu microseconds", iterations / 2, end - start); + perf_print_counter(wperf); + perf_free(wperf); + + return 0; +} +#if 0 +int +test_file(int argc, char *argv[]) +{ + const iterations = 1024; + + /* check if microSD card is mounted */ + struct stat buffer; + if (stat("/fs/microsd/", &buffer)) { + warnx("no microSD card mounted, aborting file test"); + return 1; + } + + uint8_t buf[512]; + hrt_abstime start, end; + perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes"); + + int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); + memset(buf, 0, sizeof(buf)); + + start = hrt_absolute_time(); + for (unsigned i = 0; i < iterations; i++) { perf_begin(wperf); write(fd, buf, sizeof(buf)); perf_end(wperf); @@ -78,7 +116,7 @@ test_file(int argc, char *argv[]) unlink("/fs/microsd/testfile"); - warnx("512KiB in %llu microseconds", end - start); + warnx("%dKiB in %llu microseconds", iterations / 2, end - start); perf_print_counter(wperf); perf_free(wperf); @@ -112,3 +150,4 @@ test_file(int argc, char *argv[]) return 0; } +#endif From 5e6d3604a377ab56bb0f40384fffb9370dbe0d74 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 11 Sep 2013 22:45:40 +0200 Subject: [PATCH 623/872] Made MS5611 startup exclusive as well --- src/drivers/ms5611/ms5611_spi.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index f6c6243408..21caed2ffc 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -134,6 +134,7 @@ int MS5611_SPI::init() { int ret; + irqstate_t flags; ret = SPI::init(); if (ret != OK) { @@ -141,15 +142,23 @@ MS5611_SPI::init() goto out; } + /* disable interrupts, make this section atomic */ + flags = irqsave(); /* send reset command */ ret = _reset(); + /* re-enable interrupts */ + irqrestore(flags); if (ret != OK) { debug("reset failed"); goto out; } + /* disable interrupts, make this section atomic */ + flags = irqsave(); /* read PROM */ ret = _read_prom(); + /* re-enable interrupts */ + irqrestore(flags); if (ret != OK) { debug("prom readout failed"); goto out; From 1f19a27e3cd5d0686dd65ecad6a171d025058b7c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 12 Sep 2013 08:30:00 +1000 Subject: [PATCH 624/872] make upload on Linux much more reliable Upload on Linux now only tries usb-3D_Robotics boards. This should also make it handle more ports on MacOS --- Tools/px_uploader.py | 14 +++++++++++++- makefiles/upload.mk | 4 ++-- 2 files changed, 15 insertions(+), 3 deletions(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index 52d089360a..64af672a34 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -400,7 +400,19 @@ print("Loaded firmware for %x,%x, waiting for the bootloader..." % (fw.property( # Spin waiting for a device to show up while True: - for port in args.port.split(","): + portlist = [] + patterns = args.port.split(",") + # on unix-like platforms use glob to support wildcard ports. This allows + # the use of /dev/serial/by-id/usb-3D_Robotics on Linux, which prevents the upload from + # causing modem hangups etc + if "linux" in _platform or "darwin" in _platform: + import glob + for pattern in patterns: + portlist += glob.glob(pattern) + else: + portlist = patterns + + for port in portlist: #print("Trying %s" % port) diff --git a/makefiles/upload.mk b/makefiles/upload.mk index 470ddfdf16..bc26d743d0 100644 --- a/makefiles/upload.mk +++ b/makefiles/upload.mk @@ -12,10 +12,10 @@ SYSTYPE := $(shell uname -s) # XXX The uploader should be smarter than this. # ifeq ($(SYSTYPE),Darwin) -SERIAL_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" +SERIAL_PORTS ?= "/dev/tty.usbmodemPX*,/dev/tty.usbmodem*" endif ifeq ($(SYSTYPE),Linux) -SERIAL_PORTS ?= "/dev/ttyACM5,/dev/ttyACM4,/dev/ttyACM3,/dev/ttyACM2,/dev/ttyACM1,/dev/ttyACM0" +SERIAL_PORTS ?= "/dev/serial/by-id/usb-3D_Robotics*" 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" From 04f8e338b682d0f72f00ad12f22b5071a8f6bd18 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 8 Sep 2013 21:42:00 +1000 Subject: [PATCH 625/872] hmc5883: add perf count, and removed unnecessary checks for -32768 we've already checked that the absolute value is <= 2048 --- src/drivers/hmc5883/hmc5883.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 0de82c3042..378f433cd1 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -842,8 +842,10 @@ HMC5883::collect() */ if ((abs(report.x) > 2048) || (abs(report.y) > 2048) || - (abs(report.z) > 2048)) + (abs(report.z) > 2048)) { + perf_count(_comms_errors); goto out; + } /* * RAW outputs @@ -852,7 +854,7 @@ HMC5883::collect() * and y needs to be negated */ _reports[_next_report].x_raw = report.y; - _reports[_next_report].y_raw = ((report.x == -32768) ? 32767 : -report.x); + _reports[_next_report].y_raw = -report.x; /* z remains z */ _reports[_next_report].z_raw = report.z; @@ -878,14 +880,14 @@ HMC5883::collect() /* to align the sensor axes with the board, x and y need to be flipped */ _reports[_next_report].x = ((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; } else { #endif /* the standard external mag by 3DR has x pointing to the right, y pointing backwards, and z down, * 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; + _reports[_next_report].x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; /* flip axes and negate value for y */ _reports[_next_report].y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; /* z remains z */ From 1828b57c581dda473d03c9c00cdbf354c7927f23 Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 9 Sep 2013 17:36:07 +1000 Subject: [PATCH 626/872] ringbuffer: added force() and use lockless methods this adds force() which can be used for drivers wanting consumers to get the latest data when the buffer overflows --- src/drivers/device/ringbuffer.h | 136 ++++++++++++++++++++++++++++---- 1 file changed, 120 insertions(+), 16 deletions(-) diff --git a/src/drivers/device/ringbuffer.h b/src/drivers/device/ringbuffer.h index dc0c84052b..c859be647a 100644 --- a/src/drivers/device/ringbuffer.h +++ b/src/drivers/device/ringbuffer.h @@ -55,13 +55,29 @@ public: bool put(T &val); /** - * Put an item into the buffer. + * Put an item into the buffer if there is space. * * @param val Item to put * @return true if the item was put, false if the buffer is full */ bool put(const T &val); + /** + * Force an item into the buffer, discarding an older item if there is not space. + * + * @param val Item to put + * @return true if an item was discarded to make space + */ + bool force(T &val); + + /** + * Force an item into the buffer, discarding an older item if there is not space. + * + * @param val Item to put + * @return true if an item was discarded to make space + */ + bool force(const T &val); + /** * Get an item from the buffer. * @@ -73,8 +89,8 @@ public: /** * Get an item from the buffer (scalars only). * - * @return The value that was fetched, or zero if the buffer was - * empty. + * @return The value that was fetched. If the buffer is empty, + * returns zero. */ T get(void); @@ -97,23 +113,23 @@ public: /* * Returns true if the buffer is empty. */ - bool empty() { return _tail == _head; } + bool empty(); /* * Returns true if the buffer is full. */ - bool full() { return _next(_head) == _tail; } + bool full(); /* * Returns the capacity of the buffer, or zero if the buffer could * not be allocated. */ - unsigned size() { return (_buf != nullptr) ? _size : 0; } + unsigned size(); /* * Empties the buffer. */ - void flush() { _head = _tail = _size; } + void flush(); private: T *const _buf; @@ -139,6 +155,38 @@ RingBuffer::~RingBuffer() delete[] _buf; } +template +bool RingBuffer::empty() +{ + return _tail == _head; +} + +template +bool RingBuffer::full() +{ + return _next(_head) == _tail; +} + +template +unsigned RingBuffer::size() +{ + return (_buf != nullptr) ? _size : 0; +} + +template +void RingBuffer::flush() +{ + T junk; + while (!empty()) + get(junk); +} + +template +unsigned RingBuffer::_next(unsigned index) +{ + return (0 == index) ? _size : (index - 1); +} + template bool RingBuffer::put(T &val) { @@ -165,12 +213,55 @@ bool RingBuffer::put(const T &val) } } +template +bool RingBuffer::force(T &val) +{ + bool overwrote = false; + + for (;;) { + if (put(val)) + break; + T junk; + get(junk); + overwrote = true; + } + return overwrote; +} + +template +bool RingBuffer::force(const T &val) +{ + bool overwrote = false; + + for (;;) { + if (put(val)) + break; + T junk; + get(junk); + overwrote = true; + } + return overwrote; +} + template bool RingBuffer::get(T &val) { if (_tail != _head) { - val = _buf[_tail]; - _tail = _next(_tail); + unsigned candidate; + unsigned next; + do { + /* decide which element we think we're going to read */ + candidate = _tail; + + /* and what the corresponding next index will be */ + next = _next(candidate); + + /* go ahead and read from this index */ + val = _buf[candidate]; + + /* if the tail pointer didn't change, we got our item */ + } while (!__sync_bool_compare_and_swap(&_tail, candidate, next)); + return true; } else { return false; @@ -187,17 +278,30 @@ T RingBuffer::get(void) template unsigned RingBuffer::space(void) { - return (_tail >= _head) ? (_size - (_tail - _head)) : (_head - _tail - 1); + unsigned tail, head; + + /* + * Make a copy of the head/tail pointers in a fashion that + * may err on the side of under-estimating the free space + * in the buffer in the case that the buffer is being updated + * asynchronously with our check. + * If the head pointer changes (reducing space) while copying, + * re-try the copy. + */ + do { + head = _head; + tail = _tail; + } while (head != _head); + + return (tail >= head) ? (_size - (tail - head)) : (head - tail - 1); } template unsigned RingBuffer::count(void) { + /* + * Note that due to the conservative nature of space(), this may + * over-estimate the number of items in the buffer. + */ return _size - space(); } - -template -unsigned RingBuffer::_next(unsigned index) -{ - return (0 == index) ? _size : (index - 1); -} From 3329e3c38c6c566fd8833d862ecf06c07ce4279e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 9 Sep 2013 17:37:29 +1000 Subject: [PATCH 627/872] ringbuffer: added resize() and print_info() methods this simplifies the drivers --- src/drivers/device/ringbuffer.h | 44 +++++++++++++++++++++++++++++++-- 1 file changed, 42 insertions(+), 2 deletions(-) diff --git a/src/drivers/device/ringbuffer.h b/src/drivers/device/ringbuffer.h index c859be647a..e3c5a20bdd 100644 --- a/src/drivers/device/ringbuffer.h +++ b/src/drivers/device/ringbuffer.h @@ -131,9 +131,25 @@ public: */ void flush(); + /* + * resize the buffer. This is unsafe to be called while + * a producer or consuming is running. Caller is responsible + * for any locking needed + * + * @param new_size new size for buffer + * @return true if the resize succeeds, false if + * not (allocation error) + */ + bool resize(unsigned new_size); + + /* + * printf() some info on the buffer + */ + void print_info(const char *name); + private: - T *const _buf; - const unsigned _size; + T *_buf; + unsigned _size; volatile unsigned _head; /**< insertion point */ volatile unsigned _tail; /**< removal point */ @@ -305,3 +321,27 @@ unsigned RingBuffer::count(void) */ return _size - space(); } + +template +bool RingBuffer::resize(unsigned new_size) +{ + T *old_buffer; + T *new_buffer = new T[new_size + 1]; + if (new_buffer == nullptr) { + return false; + } + old_buffer = _buf; + _buf = new_buffer; + _size = new_size; + _head = new_size; + _tail = new_size; + delete[] old_buffer; + return true; +} + +template +void RingBuffer::print_info(const char *name) +{ + printf("%s %u (%u/%u @ %p)\n", + name, _size, _head, _tail, _buf); +} From 3c4526111731ad6701e054f586ae585c9c81f106 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 8 Sep 2013 21:43:24 +1000 Subject: [PATCH 628/872] hmc5883: use a RingBuffer to hold report queue this simplifies the queue handling, and avoids the need for a start()/stop() on queue resize --- src/drivers/hmc5883/hmc5883.cpp | 107 ++++++++++++-------------------- 1 file changed, 40 insertions(+), 67 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 378f433cd1..b838bf16b8 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -65,6 +65,7 @@ #include #include +#include #include #include @@ -148,10 +149,7 @@ private: work_s _work; unsigned _measure_ticks; - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; - mag_report *_reports; + RingBuffer *_reports; mag_scale _scale; float _range_scale; float _range_ga; @@ -310,9 +308,6 @@ private: }; -/* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) - /* * Driver 'main' command. */ @@ -322,9 +317,6 @@ extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]); HMC5883::HMC5883(int bus) : I2C("HMC5883", MAG_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000), _measure_ticks(0), - _num_reports(0), - _next_report(0), - _oldest_report(0), _reports(nullptr), _range_scale(0), /* default range scale from counts to gauss */ _range_ga(1.3f), @@ -356,9 +348,8 @@ HMC5883::~HMC5883() /* make sure we are truly inactive */ stop(); - /* free any existing reports */ if (_reports != nullptr) - delete[] _reports; + delete _reports; // free perf counters perf_free(_sample_perf); @@ -375,21 +366,18 @@ 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]; - + _reports = new RingBuffer(2); if (_reports == nullptr) goto out; - _oldest_report = _next_report = 0; + /* reset the device configuration */ + reset(); /* get a publish handle on the mag topic */ - memset(&_reports[0], 0, sizeof(_reports[0])); - _mag_topic = orb_advertise(ORB_ID(sensor_mag), &_reports[0]); + struct mag_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report); if (_mag_topic < 0) debug("failed to create sensor_mag object"); @@ -493,6 +481,7 @@ ssize_t HMC5883::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct mag_report); + struct mag_report *mag_buf = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -501,17 +490,15 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen) /* 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 (_reports->get(*mag_buf)) { + ret += sizeof(struct mag_report); + mag_buf++; } } @@ -522,7 +509,7 @@ HMC5883::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 { - _oldest_report = _next_report = 0; + _reports->flush(); /* trigger a measurement */ if (OK != measure()) { @@ -539,10 +526,9 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen) break; } - /* state machine will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); - + if (_reports->get(*mag_buf)) { + ret = sizeof(struct mag_report); + } } while (0); return ret; @@ -615,31 +601,22 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) return 1000000/TICK2USEC(_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)) + if ((arg < 1) || (arg > 100)) return -EINVAL; - /* allocate new buffer */ - struct mag_report *buf = new struct mag_report[arg]; - - if (nullptr == buf) + irqstate_t flags = irqsave(); + if (!_reports->resize(arg)) { + irqrestore(flags); return -ENOMEM; - - /* reset the measurement state machine with the new buffer, free the old */ - stop(); - delete[] _reports; - _num_reports = arg; - _reports = buf; - start(); + } + irqrestore(flags); return OK; } case SENSORIOCGQUEUEDEPTH: - return _num_reports - 1; + return _reports->size(); case SENSORIOCRESET: return reset(); @@ -701,7 +678,7 @@ HMC5883::start() { /* reset the report ring and state machine */ _collect_phase = false; - _oldest_report = _next_report = 0; + _reports->flush(); /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&HMC5883::cycle_trampoline, this, 1); @@ -810,9 +787,10 @@ HMC5883::collect() perf_begin(_sample_perf); + struct mag_report new_report; /* this should be fairly close to the end of the measurement, so the best approximation of the time */ - _reports[_next_report].timestamp = hrt_absolute_time(); + new_report.timestamp = hrt_absolute_time(); /* * @note We could read the status register here, which could tell us that @@ -853,10 +831,10 @@ HMC5883::collect() * to align the sensor axes with the board, x and y need to be flipped * and y needs to be negated */ - _reports[_next_report].x_raw = report.y; - _reports[_next_report].y_raw = -report.x; + new_report.x_raw = report.y; + new_report.y_raw = -report.x; /* z remains z */ - _reports[_next_report].z_raw = report.z; + new_report.z_raw = report.z; /* scale values for output */ @@ -878,34 +856,30 @@ HMC5883::collect() #ifdef PX4_I2C_BUS_ONBOARD if (_bus == PX4_I2C_BUS_ONBOARD) { /* to align the sensor axes with the board, x and y need to be flipped */ - _reports[_next_report].x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; + new_report.x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; /* flip axes and negate value for y */ - _reports[_next_report].y = ((-report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; + new_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; + new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; } else { #endif /* the standard external mag by 3DR has x pointing to the right, y pointing backwards, and z down, * therefore switch x and y and invert y */ - _reports[_next_report].x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; + new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; /* flip axes and negate value for y */ - _reports[_next_report].y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; + new_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; + new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; #ifdef PX4_I2C_BUS_ONBOARD } #endif /* publish it */ - orb_publish(ORB_ID(sensor_mag), _mag_topic, &_reports[_next_report]); + orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_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) { + /* post a report to the ring */ + if (_reports->force(new_report)) { perf_count(_buffer_overflows); - INCREMENT(_oldest_report, _num_reports); } /* notify anyone waiting for data */ @@ -1224,8 +1198,7 @@ HMC5883::print_info() 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); + _reports->print_info("report queue"); } /** From 37d09f09448a274d5f9c55674fd6734709f8a383 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 9 Sep 2013 10:55:22 +1000 Subject: [PATCH 629/872] mpu6000: use a wrapper struct to avoid a linker error the linker doesn't cope with us having multiple modules implementing RingBuffer this also switches to use force() instead of put(), so we discard old entries when the buffer overflows --- src/drivers/mpu6000/mpu6000.cpp | 133 +++++++++++++++----------------- 1 file changed, 64 insertions(+), 69 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 14f8f44b82..81612aee79 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -194,7 +194,14 @@ private: struct hrt_call _call; unsigned _call_interval; - typedef RingBuffer AccelReportBuffer; + /* + these wrapper types are needed to avoid a linker error for + RingBuffer instances which appear in two drivers. + */ + struct _accel_report { + struct accel_report r; + }; + typedef RingBuffer<_accel_report> AccelReportBuffer; AccelReportBuffer *_accel_reports; struct accel_scale _accel_scale; @@ -202,7 +209,10 @@ private: float _accel_range_m_s2; orb_advert_t _accel_topic; - typedef RingBuffer GyroReportBuffer; + struct _gyro_report { + struct gyro_report r; + }; + typedef RingBuffer<_gyro_report> GyroReportBuffer; GyroReportBuffer *_gyro_reports; struct gyro_scale _gyro_scale; @@ -465,16 +475,16 @@ MPU6000::init() if (gyro_ret != OK) { _gyro_topic = -1; } else { - gyro_report gr; + _gyro_report gr; _gyro_reports->get(gr); - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr); + _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr.r); } /* advertise accel topic */ - accel_report ar; + _accel_report ar; _accel_reports->get(ar); - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar); + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar.r); out: return ret; @@ -655,7 +665,7 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen) return -EAGAIN; /* copy reports out of our buffer to the caller */ - accel_report *arp = reinterpret_cast(buffer); + _accel_report *arp = reinterpret_cast<_accel_report *>(buffer); int transferred = 0; while (count--) { if (!_accel_reports->get(*arp++)) @@ -748,7 +758,7 @@ MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) return -EAGAIN; /* copy reports out of our buffer to the caller */ - gyro_report *arp = reinterpret_cast(buffer); + _gyro_report *arp = reinterpret_cast<_gyro_report *>(buffer); int transferred = 0; while (count--) { if (!_gyro_reports->get(*arp++)) @@ -837,28 +847,19 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) return 1000000 / _call_interval; case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; - - /* allocate new buffer */ - AccelReportBuffer *buf = new AccelReportBuffer(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 _accel_reports; - _accel_reports = buf; - start(); - - return OK; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) + return -EINVAL; + + irqstate_t flags = irqsave(); + if (!_accel_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; } + irqrestore(flags); + + return OK; + } case SENSORIOCGQUEUEDEPTH: return _accel_reports->size(); @@ -935,21 +936,12 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) if ((arg < 1) || (arg > 100)) return -EINVAL; - /* allocate new buffer */ - GyroReportBuffer *buf = new GyroReportBuffer(arg); - - if (nullptr == buf) - return -ENOMEM; - if (buf->size() == 0) { - delete buf; + irqstate_t flags = irqsave(); + if (!_gyro_reports->resize(arg)) { + irqrestore(flags); return -ENOMEM; } - - /* reset the measurement state machine with the new buffer, free the old */ - stop(); - delete _gyro_reports; - _gyro_reports = buf; - start(); + irqrestore(flags); return OK; } @@ -1197,13 +1189,13 @@ MPU6000::measure() /* * Report buffers. */ - accel_report arb; - gyro_report grb; + _accel_report arb; + _gyro_report grb; /* * Adjust and scale results to m/s^2. */ - grb.timestamp = arb.timestamp = hrt_absolute_time(); + grb.r.timestamp = arb.r.timestamp = hrt_absolute_time(); /* @@ -1224,53 +1216,53 @@ MPU6000::measure() /* NOTE: Axes have been swapped to match the board a few lines above. */ - arb.x_raw = report.accel_x; - arb.y_raw = report.accel_y; - arb.z_raw = report.accel_z; + arb.r.x_raw = report.accel_x; + arb.r.y_raw = report.accel_y; + arb.r.z_raw = report.accel_z; float x_in_new = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; float y_in_new = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; float z_in_new = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; - arb.x = _accel_filter_x.apply(x_in_new); - arb.y = _accel_filter_y.apply(y_in_new); - arb.z = _accel_filter_z.apply(z_in_new); + arb.r.x = _accel_filter_x.apply(x_in_new); + arb.r.y = _accel_filter_y.apply(y_in_new); + arb.r.z = _accel_filter_z.apply(z_in_new); - arb.scaling = _accel_range_scale; - arb.range_m_s2 = _accel_range_m_s2; + arb.r.scaling = _accel_range_scale; + arb.r.range_m_s2 = _accel_range_m_s2; - arb.temperature_raw = report.temp; - arb.temperature = (report.temp) / 361.0f + 35.0f; + arb.r.temperature_raw = report.temp; + arb.r.temperature = (report.temp) / 361.0f + 35.0f; - grb.x_raw = report.gyro_x; - grb.y_raw = report.gyro_y; - grb.z_raw = report.gyro_z; + grb.r.x_raw = report.gyro_x; + grb.r.y_raw = report.gyro_y; + grb.r.z_raw = report.gyro_z; float x_gyro_in_new = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; float y_gyro_in_new = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; float z_gyro_in_new = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; - grb.x = _gyro_filter_x.apply(x_gyro_in_new); - grb.y = _gyro_filter_y.apply(y_gyro_in_new); - grb.z = _gyro_filter_z.apply(z_gyro_in_new); + grb.r.x = _gyro_filter_x.apply(x_gyro_in_new); + grb.r.y = _gyro_filter_y.apply(y_gyro_in_new); + grb.r.z = _gyro_filter_z.apply(z_gyro_in_new); - grb.scaling = _gyro_range_scale; - grb.range_rad_s = _gyro_range_rad_s; + grb.r.scaling = _gyro_range_scale; + grb.r.range_rad_s = _gyro_range_rad_s; - grb.temperature_raw = report.temp; - grb.temperature = (report.temp) / 361.0f + 35.0f; + grb.r.temperature_raw = report.temp; + grb.r.temperature = (report.temp) / 361.0f + 35.0f; - _accel_reports->put(arb); - _gyro_reports->put(grb); + _accel_reports->force(arb); + _gyro_reports->force(grb); /* notify anyone waiting for data */ poll_notify(POLLIN); _gyro->parent_poll_notify(); /* and publish for subscribers */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); + orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb.r); if (_gyro_topic != -1) { - orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb); + orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb.r); } /* stop measuring */ @@ -1280,7 +1272,10 @@ MPU6000::measure() void MPU6000::print_info() { + perf_print_counter(_sample_perf); printf("reads: %u\n", _reads); + _accel_reports->print_info("accel queue"); + _gyro_reports->print_info("gyro queue"); } MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) : From 815ccee0e7f642b2471084296c893a4dd49e5dfb Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 9 Sep 2013 17:16:12 +1000 Subject: [PATCH 630/872] mpu6000: fixed race condition in buffer increment --- src/drivers/mpu6000/mpu6000.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 81612aee79..66d36826a8 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -668,9 +668,10 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen) _accel_report *arp = reinterpret_cast<_accel_report *>(buffer); int transferred = 0; while (count--) { - if (!_accel_reports->get(*arp++)) + if (!_accel_reports->get(*arp)) break; transferred++; + arp++; } /* return the number of bytes transferred */ @@ -758,12 +759,13 @@ MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) return -EAGAIN; /* copy reports out of our buffer to the caller */ - _gyro_report *arp = reinterpret_cast<_gyro_report *>(buffer); + _gyro_report *grp = reinterpret_cast<_gyro_report *>(buffer); int transferred = 0; while (count--) { - if (!_gyro_reports->get(*arp++)) + if (!_gyro_reports->get(*grp)) break; transferred++; + grp++; } /* return the number of bytes transferred */ From 36b7b7bc5f078f373f67cdce3f5c7855c0dcd58b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 9 Sep 2013 16:35:20 +1000 Subject: [PATCH 631/872] airspeed: convert to using RingBuffer class --- src/drivers/airspeed/airspeed.cpp | 74 +++++++++------------ src/drivers/airspeed/airspeed.h | 19 ++++-- src/drivers/ets_airspeed/ets_airspeed.cpp | 28 ++++---- src/drivers/meas_airspeed/meas_airspeed.cpp | 26 +++----- 4 files changed, 67 insertions(+), 80 deletions(-) diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 1ec61eb60b..2a6b190de3 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -68,6 +68,7 @@ #include #include +#include #include #include @@ -77,10 +78,8 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) : I2C("Airspeed", AIRSPEED_DEVICE_PATH, bus, address, 100000), - _num_reports(0), - _next_report(0), - _oldest_report(0), _reports(nullptr), + _max_differential_pressure_pa(0), _sensor_ok(false), _measure_ticks(0), _collect_phase(false), @@ -105,7 +104,7 @@ Airspeed::~Airspeed() /* free any existing reports */ if (_reports != nullptr) - delete[] _reports; + delete _reports; // free perf counters perf_free(_sample_perf); @@ -123,20 +122,14 @@ Airspeed::init() goto out; /* allocate basic report buffers */ - _num_reports = 2; - _reports = new struct differential_pressure_s[_num_reports]; - - for (unsigned i = 0; i < _num_reports; i++) - _reports[i].max_differential_pressure_pa = 0; - + _reports = new RingBuffer(2); if (_reports == nullptr) goto out; - _oldest_report = _next_report = 0; - /* get a publish handle on the airspeed topic */ - memset(&_reports[0], 0, sizeof(_reports[0])); - _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &_reports[0]); + differential_pressure_s zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &zero_report); if (_airspeed_pub < 0) warnx("failed to create airspeed sensor object. Did you start uOrb?"); @@ -229,31 +222,22 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg) 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)) + if ((arg < 1) || (arg > 100)) return -EINVAL; - /* allocate new buffer */ - struct differential_pressure_s *buf = new struct differential_pressure_s[arg]; - - if (nullptr == buf) + irqstate_t flags = irqsave(); + if (!_reports->resize(arg)) { + irqrestore(flags); return -ENOMEM; - - /* reset the measurement state machine with the new buffer, free the old */ - stop(); - delete[] _reports; - _num_reports = arg; - _reports = buf; - start(); + } + irqrestore(flags); return OK; } case SENSORIOCGQUEUEDEPTH: - return _num_reports - 1; + return _reports->size(); case SENSORIOCRESET: /* XXX implement this */ @@ -281,7 +265,8 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg) ssize_t Airspeed::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(struct differential_pressure_s); + unsigned count = buflen / sizeof(differential_pressure_s); + differential_pressure_s *abuf = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -297,10 +282,9 @@ Airspeed::read(struct file *filp, char *buffer, size_t buflen) * 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 (_reports->get(*abuf)) { + ret += sizeof(*abuf); + abuf++; } } @@ -309,9 +293,8 @@ Airspeed::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 { - _oldest_report = _next_report = 0; + _reports->flush(); /* trigger a measurement */ if (OK != measure()) { @@ -329,8 +312,9 @@ Airspeed::read(struct file *filp, char *buffer, size_t buflen) } /* state machine will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); + if (_reports->get(*abuf)) { + ret = sizeof(*abuf); + } } while (0); @@ -342,7 +326,7 @@ Airspeed::start() { /* reset the report ring and state machine */ _collect_phase = false; - _oldest_report = _next_report = 0; + _reports->flush(); /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&Airspeed::cycle_trampoline, this, 1); @@ -385,6 +369,12 @@ Airspeed::print_info() perf_print_counter(_comms_errors); perf_print_counter(_buffer_overflows); warnx("poll interval: %u ticks", _measure_ticks); - warnx("report queue: %u (%u/%u @ %p)", - _num_reports, _oldest_report, _next_report, _reports); + _reports->print_info("report queue"); +} + +void +Airspeed::new_report(const differential_pressure_s &report) +{ + if (!_reports->force(report)) + perf_count(_buffer_overflows); } diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h index b87494b40f..7850ccc7e9 100644 --- a/src/drivers/airspeed/airspeed.h +++ b/src/drivers/airspeed/airspeed.h @@ -68,6 +68,7 @@ #include #include +#include #include #include @@ -102,6 +103,10 @@ public: */ virtual void print_info(); +private: + RingBuffer *_reports; + perf_counter_t _buffer_overflows; + protected: virtual int probe(); @@ -114,10 +119,7 @@ protected: virtual int collect() = 0; work_s _work; - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; - differential_pressure_s *_reports; + uint16_t _max_differential_pressure_pa; bool _sensor_ok; int _measure_ticks; bool _collect_phase; @@ -129,7 +131,6 @@ protected: perf_counter_t _sample_perf; perf_counter_t _comms_errors; - perf_counter_t _buffer_overflows; /** @@ -162,8 +163,12 @@ protected: */ static void cycle_trampoline(void *arg); + /** + * add a new report to the reports queue + * + * @param report differential_pressure_s report + */ + void new_report(const differential_pressure_s &report); }; -/* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index 257b41935c..dd8436b10e 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -68,6 +68,7 @@ #include #include +#include #include #include @@ -173,27 +174,22 @@ ETSAirspeed::collect() diff_pres_pa -= _diff_pres_offset; } - // XXX we may want to smooth out the readings to remove noise. - - _reports[_next_report].timestamp = hrt_absolute_time(); - _reports[_next_report].differential_pressure_pa = diff_pres_pa; - // Track maximum differential pressure measured (so we can work out top speed). - if (diff_pres_pa > _reports[_next_report].max_differential_pressure_pa) { - _reports[_next_report].max_differential_pressure_pa = diff_pres_pa; + if (diff_pres_pa > _max_differential_pressure_pa) { + _max_differential_pressure_pa = diff_pres_pa; } + // XXX we may want to smooth out the readings to remove noise. + differential_pressure_s report; + report.timestamp = hrt_absolute_time(); + report.differential_pressure_pa = diff_pres_pa; + report.voltage = 0; + report.max_differential_pressure_pa = _max_differential_pressure_pa; + /* announce the airspeed if needed, just publish else */ - orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_reports[_next_report]); + orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &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); - } + new_report(report); /* notify anyone waiting for data */ poll_notify(POLLIN); diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index b1cb2b3d84..03d7bbfb91 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -199,27 +199,23 @@ MEASAirspeed::collect() // Calculate differential pressure. As its centered around 8000 // and can go positive or negative, enforce absolute value uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f)); - - _reports[_next_report].timestamp = hrt_absolute_time(); - _reports[_next_report].temperature = temperature; - _reports[_next_report].differential_pressure_pa = diff_press_pa; + struct differential_pressure_s report; // Track maximum differential pressure measured (so we can work out top speed). - if (diff_press_pa > _reports[_next_report].max_differential_pressure_pa) { - _reports[_next_report].max_differential_pressure_pa = diff_press_pa; + if (diff_press_pa > _max_differential_pressure_pa) { + _max_differential_pressure_pa = diff_press_pa; } + report.timestamp = hrt_absolute_time(); + report.temperature = temperature; + report.differential_pressure_pa = diff_press_pa; + report.voltage = 0; + report.max_differential_pressure_pa = _max_differential_pressure_pa; + /* announce the airspeed if needed, just publish else */ - orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_reports[_next_report]); + orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &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); - } + new_report(report); /* notify anyone waiting for data */ poll_notify(POLLIN); From 63fb702d7fe46619e4315ec5edacb82119ee8c8a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 9 Sep 2013 16:36:07 +1000 Subject: [PATCH 632/872] l3gd20: convert to using RingBuffer class --- src/drivers/l3gd20/l3gd20.cpp | 126 ++++++++++++++-------------------- 1 file changed, 51 insertions(+), 75 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 970e8cf4b4..7cebebeb41 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -61,6 +61,7 @@ #include #include #include +#include #include #include @@ -183,11 +184,8 @@ private: struct hrt_call _call; unsigned _call_interval; - - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; - struct gyro_report *_reports; + + RingBuffer *_reports; struct gyro_scale _gyro_scale; float _gyro_range_scale; @@ -299,16 +297,9 @@ private: int self_test(); }; -/* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) - - L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : SPI("L3GD20", 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), @@ -340,7 +331,7 @@ L3GD20::~L3GD20() /* free any existing reports */ if (_reports != nullptr) - delete[] _reports; + delete _reports; /* delete the perf counter */ perf_free(_sample_perf); @@ -356,16 +347,15 @@ L3GD20::init() goto out; /* allocate basic report buffers */ - _num_reports = 2; - _oldest_report = _next_report = 0; - _reports = new struct gyro_report[_num_reports]; + _reports = new RingBuffer(2); if (_reports == nullptr) goto out; /* advertise sensor topic */ - memset(&_reports[0], 0, sizeof(_reports[0])); - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_reports[0]); + struct gyro_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &zero_report); reset(); @@ -415,6 +405,7 @@ ssize_t L3GD20::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct gyro_report); + struct gyro_report *gbuf = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -430,10 +421,9 @@ L3GD20::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 (_reports->get(*gbuf)) { + ret += sizeof(*gbuf); + gbuf++; } } @@ -442,12 +432,13 @@ L3GD20::read(struct file *filp, char *buffer, size_t buflen) } /* manual measurement */ - _oldest_report = _next_report = 0; + _reports->flush(); measure(); /* measurement will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); + if (_reports->get(*gbuf)) { + ret = sizeof(*gbuf); + } return ret; } @@ -515,31 +506,22 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) 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 < 1) || (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 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; + irqstate_t flags = irqsave(); + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; } + irqrestore(flags); + + return OK; + } case SENSORIOCGQUEUEDEPTH: - return _num_reports - 1; + return _reports->size(); case SENSORIOCRESET: reset(); @@ -708,7 +690,7 @@ L3GD20::start() stop(); /* reset the report ring */ - _oldest_report = _next_report = 0; + _reports->flush(); /* start polling at the specified rate */ hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&L3GD20::measure_trampoline, this); @@ -768,7 +750,7 @@ L3GD20::measure() } raw_report; #pragma pack(pop) - gyro_report *report = &_reports[_next_report]; + gyro_report report; /* start the performance counter */ perf_begin(_sample_perf); @@ -791,61 +773,56 @@ L3GD20::measure() * the offset is 74 from the origin and subtracting * 74 from all measurements centers them around zero. */ - report->timestamp = hrt_absolute_time(); + report.timestamp = hrt_absolute_time(); switch (_orientation) { case SENSOR_BOARD_ROTATION_000_DEG: /* keep axes in place */ - report->x_raw = raw_report.x; - report->y_raw = raw_report.y; + 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; + 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); + 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); + 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.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) - _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.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; + 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); + _reports->force(report); /* notify anyone waiting for data */ poll_notify(POLLIN); /* publish for subscribers */ if (_gyro_topic > 0) - orb_publish(ORB_ID(sensor_gyro), _gyro_topic, report); + orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report); _read++; @@ -858,8 +835,7 @@ 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); + _reports->print_info("report queue"); } int From b8ffb574ca3d2a3bc9b85144bdaa778a47fea809 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 9 Sep 2013 16:36:22 +1000 Subject: [PATCH 633/872] mb12xx: convert to using RingBuffer class --- src/drivers/mb12xx/mb12xx.cpp | 102 +++++++++++++--------------------- 1 file changed, 39 insertions(+), 63 deletions(-) diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index f834169935..fabe10b872 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -64,6 +64,7 @@ #include #include +#include #include #include @@ -119,10 +120,7 @@ private: float _min_distance; float _max_distance; work_s _work; - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; - range_finder_report *_reports; + RingBuffer *_reports; bool _sensor_ok; int _measure_ticks; bool _collect_phase; @@ -183,9 +181,6 @@ private: }; -/* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) - /* * Driver 'main' command. */ @@ -195,9 +190,6 @@ MB12XX::MB12XX(int bus, int address) : I2C("MB12xx", RANGE_FINDER_DEVICE_PATH, bus, address, 100000), _min_distance(MB12XX_MIN_DISTANCE), _max_distance(MB12XX_MAX_DISTANCE), - _num_reports(0), - _next_report(0), - _oldest_report(0), _reports(nullptr), _sensor_ok(false), _measure_ticks(0), @@ -221,7 +213,7 @@ MB12XX::~MB12XX() /* free any existing reports */ if (_reports != nullptr) - delete[] _reports; + delete _reports; } int @@ -234,17 +226,15 @@ MB12XX::init() goto out; /* allocate basic report buffers */ - _num_reports = 2; - _reports = new struct range_finder_report[_num_reports]; + _reports = new RingBuffer(2); if (_reports == nullptr) goto out; - _oldest_report = _next_report = 0; - /* get a publish handle on the range finder topic */ - memset(&_reports[0], 0, sizeof(_reports[0])); - _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &_reports[0]); + struct range_finder_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report); if (_range_finder_topic < 0) debug("failed to create sensor_range_finder object. Did you start uOrb?"); @@ -354,31 +344,22 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg) 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 range_finder_report *buf = new struct range_finder_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; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) + return -EINVAL; + + irqstate_t flags = irqsave(); + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; } + irqrestore(flags); + + return OK; + } case SENSORIOCGQUEUEDEPTH: - return _num_reports - 1; + return _reports->size(); case SENSORIOCRESET: /* XXX implement this */ @@ -406,6 +387,7 @@ ssize_t MB12XX::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct range_finder_report); + struct range_finder_report *rbuf = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -421,10 +403,9 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen) * 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 (_reports->get(*rbuf)) { + ret += sizeof(*rbuf); + rbuf++; } } @@ -433,9 +414,8 @@ MB12XX::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 { - _oldest_report = _next_report = 0; + _reports->flush(); /* trigger a measurement */ if (OK != measure()) { @@ -453,8 +433,9 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen) } /* state machine will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); + if (_reports->get(*rbuf)) { + ret = sizeof(*rbuf); + } } while (0); @@ -498,26 +479,25 @@ MB12XX::collect() if (ret < 0) { log("error reading from sensor: %d", ret); + perf_count(_comms_errors); + perf_end(_sample_perf); return ret; } uint16_t distance = val[0] << 8 | val[1]; float si_units = (distance * 1.0f)/ 100.0f; /* cm to m */ + struct range_finder_report report; + /* this should be fairly close to the end of the measurement, so the best approximation of the time */ - _reports[_next_report].timestamp = hrt_absolute_time(); - _reports[_next_report].distance = si_units; - _reports[_next_report].valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; + report.timestamp = hrt_absolute_time(); + report.distance = si_units; + report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; /* publish it */ - orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &_reports[_next_report]); + orb_publish(ORB_ID(sensor_range_finder), _range_finder_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, toss it */ - if (_next_report == _oldest_report) { + if (_reports->force(report)) { perf_count(_buffer_overflows); - INCREMENT(_oldest_report, _num_reports); } /* notify anyone waiting for data */ @@ -525,11 +505,8 @@ MB12XX::collect() ret = OK; -out: perf_end(_sample_perf); return ret; - - return ret; } void @@ -537,7 +514,7 @@ MB12XX::start() { /* reset the report ring and state machine */ _collect_phase = false; - _oldest_report = _next_report = 0; + _reports->flush(); /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 1); @@ -626,8 +603,7 @@ MB12XX::print_info() 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); + _reports->print_info("report queue"); } /** From 274e3aa2ca073b7fdfd30f270a043fd79954e1d4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 9 Sep 2013 17:15:19 +1000 Subject: [PATCH 634/872] bma180: convert to using RingBuffer --- src/drivers/bma180/bma180.cpp | 128 +++++++++++++++------------------- 1 file changed, 55 insertions(+), 73 deletions(-) diff --git a/src/drivers/bma180/bma180.cpp b/src/drivers/bma180/bma180.cpp index 079b5d21cb..f14c008cea 100644 --- a/src/drivers/bma180/bma180.cpp +++ b/src/drivers/bma180/bma180.cpp @@ -63,6 +63,7 @@ #include #include +#include /* oddly, ERROR is not defined for c++ */ @@ -146,10 +147,14 @@ private: struct hrt_call _call; unsigned _call_interval; - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; - struct accel_report *_reports; + /* + this wrapper type is needed to avoid a linker error for + RingBuffer instances which appear in two drivers. + */ + struct _accel_report { + accel_report r; + }; + RingBuffer *_reports; struct accel_scale _accel_scale; float _accel_range_scale; @@ -233,16 +238,9 @@ private: int set_lowpass(unsigned frequency); }; -/* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) - - BMA180::BMA180(int bus, spi_dev_e device) : SPI("BMA180", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, 8000000), _call_interval(0), - _num_reports(0), - _next_report(0), - _oldest_report(0), _reports(nullptr), _accel_range_scale(0.0f), _accel_range_m_s2(0.0f), @@ -270,7 +268,7 @@ BMA180::~BMA180() /* free any existing reports */ if (_reports != nullptr) - delete[] _reports; + delete _reports; /* delete the perf counter */ perf_free(_sample_perf); @@ -286,16 +284,15 @@ BMA180::init() goto out; /* allocate basic report buffers */ - _num_reports = 2; - _oldest_report = _next_report = 0; - _reports = new struct accel_report[_num_reports]; + _reports = new RingBuffer<_accel_report>(2); 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]); + struct accel_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report); /* perform soft reset (p48) */ write_reg(ADDR_RESET, SOFT_RESET); @@ -352,6 +349,7 @@ ssize_t BMA180::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct accel_report); + struct _accel_report *arp = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -367,10 +365,9 @@ BMA180::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 (_reports->get(*arp)) { + ret += sizeof(*arp); + arp++; } } @@ -379,12 +376,12 @@ BMA180::read(struct file *filp, char *buffer, size_t buflen) } /* manual measurement */ - _oldest_report = _next_report = 0; + _reports->flush(); measure(); /* measurement will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); + if (_reports->get(*arp)) + ret = sizeof(*arp); return ret; } @@ -449,31 +446,22 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg) 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; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 2) || (arg > 100)) + return -EINVAL; + + irqstate_t flags = irqsave(); + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; } + irqrestore(flags); + + return OK; + } case SENSORIOCGQUEUEDEPTH: - return _num_reports - 1; + return _reports->size(); case SENSORIOCRESET: /* XXX implement */ @@ -654,7 +642,7 @@ BMA180::start() stop(); /* reset the report ring */ - _oldest_report = _next_report = 0; + _reports->flush(); /* start polling at the specified rate */ hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&BMA180::measure_trampoline, this); @@ -688,7 +676,7 @@ BMA180::measure() // } raw_report; // #pragma pack(pop) - accel_report *report = &_reports[_next_report]; + struct _accel_report report; /* start the performance counter */ perf_begin(_sample_perf); @@ -708,45 +696,40 @@ BMA180::measure() * them before. There is no good way to synchronise with the internal * measurement flow without using the external interrupt. */ - _reports[_next_report].timestamp = hrt_absolute_time(); + report.r.timestamp = hrt_absolute_time(); /* * y of board is x of sensor and x of board is -y of sensor * perform only the axis assignment here. * Two non-value bits are discarded directly */ - report->y_raw = read_reg(ADDR_ACC_X_LSB + 0); - report->y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8; - report->x_raw = read_reg(ADDR_ACC_X_LSB + 2); - report->x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8; - report->z_raw = read_reg(ADDR_ACC_X_LSB + 4); - report->z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8; + report.r.y_raw = read_reg(ADDR_ACC_X_LSB + 0); + report.r.y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8; + report.r.x_raw = read_reg(ADDR_ACC_X_LSB + 2); + report.r.x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8; + report.r.z_raw = read_reg(ADDR_ACC_X_LSB + 4); + report.r.z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8; /* discard two non-value bits in the 16 bit measurement */ - report->x_raw = (report->x_raw / 4); - report->y_raw = (report->y_raw / 4); - report->z_raw = (report->z_raw / 4); + report.r.x_raw = (report.r.x_raw / 4); + report.r.y_raw = (report.r.y_raw / 4); + report.r.z_raw = (report.r.z_raw / 4); /* invert y axis, due to 14 bit data no overflow can occur in the negation */ - report->y_raw = -report->y_raw; + report.r.y_raw = -report.r.y_raw; - report->x = ((report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - report->y = ((report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - report->z = ((report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; - report->scaling = _accel_range_scale; - report->range_m_s2 = _accel_range_m_s2; + report.r.x = ((report.r.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + report.r.y = ((report.r.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + report.r.z = ((report.r.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + report.r.scaling = _accel_range_scale; + report.r.range_m_s2 = _accel_range_m_s2; - /* 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); + _reports->force(report); /* 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, &report.r); /* stop the perf counter */ perf_end(_sample_perf); @@ -756,8 +739,7 @@ void BMA180::print_info() { perf_print_counter(_sample_perf); - printf("report queue: %u (%u/%u @ %p)\n", - _num_reports, _oldest_report, _next_report, _reports); + _reports->print_info("report queue"); } /** From 4b4f4fee5b901da0c93b7fe981426c469840ee64 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 9 Sep 2013 17:15:39 +1000 Subject: [PATCH 635/872] lsm303d: convert to using RingBuffer --- src/drivers/lsm303d/lsm303d.cpp | 216 +++++++++++++------------------- 1 file changed, 86 insertions(+), 130 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 35904cc4d6..997b80fab2 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -62,6 +62,7 @@ #include #include #include +#include #include #include @@ -218,15 +219,19 @@ private: 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; + /* + these wrapper types are needed to avoid a linker error for + RingBuffer instances which appear in two drivers. + */ + struct _accel_report { + struct accel_report r; + }; + RingBuffer *_accel_reports; - unsigned _num_mag_reports; - volatile unsigned _next_mag_report; - volatile unsigned _oldest_mag_report; - struct mag_report *_mag_reports; + struct _mag_report { + struct mag_report r; + }; + RingBuffer *_mag_reports; struct accel_scale _accel_scale; unsigned _accel_range_m_s2; @@ -420,22 +425,12 @@ private: }; -/* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) - - LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 8000000), _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), - _num_mag_reports(0), - _next_mag_report(0), - _oldest_mag_report(0), _mag_reports(nullptr), _accel_range_m_s2(0.0f), _accel_range_scale(0.0f), @@ -480,9 +475,9 @@ LSM303D::~LSM303D() /* free any existing reports */ if (_accel_reports != nullptr) - delete[] _accel_reports; + delete _accel_reports; if (_mag_reports != nullptr) - delete[] _mag_reports; + delete _mag_reports; delete _mag; @@ -504,20 +499,17 @@ LSM303D::init() } /* allocate basic report buffers */ - _num_accel_reports = 2; - _oldest_accel_report = _next_accel_report = 0; - _accel_reports = new struct accel_report[_num_accel_reports]; + _accel_reports = new RingBuffer(2); if (_accel_reports == nullptr) goto out; /* advertise accel topic */ - memset(&_accel_reports[0], 0, sizeof(_accel_reports[0])); - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &_accel_reports[0]); + struct accel_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report); - _num_mag_reports = 2; - _oldest_mag_report = _next_mag_report = 0; - _mag_reports = new struct mag_report[_num_mag_reports]; + _mag_reports = new RingBuffer(2); if (_mag_reports == nullptr) goto out; @@ -525,8 +517,9 @@ LSM303D::init() reset(); /* advertise mag topic */ - memset(&_mag_reports[0], 0, sizeof(_mag_reports[0])); - _mag_topic = orb_advertise(ORB_ID(sensor_mag), &_mag_reports[0]); + struct mag_report zero_mag_report; + memset(&zero_mag_report, 0, sizeof(zero_mag_report)); + _mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_mag_report); /* do CDev init for the mag device node, keep it optional */ mag_ret = _mag->init(); @@ -586,6 +579,7 @@ ssize_t LSM303D::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct accel_report); + struct _accel_report *arb = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -594,17 +588,13 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen) /* if automatic measurement is enabled */ if (_call_accel_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_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); + if (_accel_reports->get(*arb)) { + ret += sizeof(*arb); + arb++; } } @@ -613,12 +603,11 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen) } /* manual measurement */ - _oldest_accel_report = _next_accel_report = 0; measure(); /* measurement will have generated a report, copy it out */ - memcpy(buffer, _accel_reports, sizeof(*_accel_reports)); - ret = sizeof(*_accel_reports); + if (_accel_reports->get(*arb)) + ret = sizeof(*arb); return ret; } @@ -627,6 +616,7 @@ ssize_t LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct mag_report); + struct _mag_report *mrb = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -638,14 +628,11 @@ LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen) /* * 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 (_mag_reports->get(*mrb)) { + ret += sizeof(*mrb); + mrb++; } } @@ -654,12 +641,12 @@ LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen) } /* manual measurement */ - _oldest_mag_report = _next_mag_report = 0; + _mag_reports->flush(); measure(); /* measurement will have generated a report, copy it out */ - memcpy(buffer, _mag_reports, sizeof(*_mag_reports)); - ret = sizeof(*_mag_reports); + if (_mag_reports->get(*mrb)) + ret = sizeof(*mrb); return ret; } @@ -727,31 +714,22 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) return 1000000 / _call_accel_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)) + if ((arg < 1) || (arg > 100)) return -EINVAL; - /* allocate new buffer */ - struct accel_report *buf = new struct accel_report[arg]; - - if (nullptr == buf) + irqstate_t flags = irqsave(); + if (!_accel_reports->resize(arg)) { + irqrestore(flags); 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(); + } + irqrestore(flags); return OK; } case SENSORIOCGQUEUEDEPTH: - return _num_accel_reports - 1; + return _accel_reports->size(); case SENSORIOCRESET: reset(); @@ -863,31 +841,22 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) return 1000000 / _call_mag_interval; case SENSORIOCSQUEUEDEPTH: { - /* account for sentinel in the ring */ - arg++; + /* 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 < 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; + irqstate_t flags = irqsave(); + if (!_mag_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; } + irqrestore(flags); + + return OK; + } case SENSORIOCGQUEUEDEPTH: - return _num_mag_reports - 1; + return _mag_reports->size(); case SENSORIOCRESET: reset(); @@ -1220,8 +1189,8 @@ LSM303D::start() stop(); /* reset the report ring */ - _oldest_accel_report = _next_accel_report = 0; - _oldest_mag_report = _next_mag_report = 0; + _accel_reports->flush(); + _mag_reports->flush(); /* start polling at the specified rate */ hrt_call_every(&_accel_call, 1000, _call_accel_interval, (hrt_callout)&LSM303D::measure_trampoline, this); @@ -1268,7 +1237,7 @@ LSM303D::measure() } raw_accel_report; #pragma pack(pop) - accel_report *accel_report = &_accel_reports[_next_accel_report]; + struct _accel_report accel_report; /* start the performance counter */ perf_begin(_accel_sample_perf); @@ -1293,35 +1262,30 @@ LSM303D::measure() */ - accel_report->timestamp = hrt_absolute_time(); + accel_report.r.timestamp = hrt_absolute_time(); - accel_report->x_raw = raw_accel_report.x; - accel_report->y_raw = raw_accel_report.y; - accel_report->z_raw = raw_accel_report.z; + accel_report.r.x_raw = raw_accel_report.x; + accel_report.r.y_raw = raw_accel_report.y; + accel_report.r.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; + float x_in_new = ((accel_report.r.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + float y_in_new = ((accel_report.r.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + float z_in_new = ((accel_report.r.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.r.x = _accel_filter_x.apply(x_in_new); + accel_report.r.y = _accel_filter_y.apply(y_in_new); + accel_report.r.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.r.scaling = _accel_range_scale; + accel_report.r.range_m_s2 = _accel_range_m_s2; - /* post a report to the ring - note, not locked */ - INCREMENT(_next_accel_report, _num_accel_reports); - - /* if we are running up against the oldest report, fix it */ - if (_next_accel_report == _oldest_accel_report) - INCREMENT(_oldest_accel_report, _num_accel_reports); + _accel_reports->force(accel_report); /* notify anyone waiting for data */ poll_notify(POLLIN); /* publish for subscribers */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, accel_report); + orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report.r); _accel_read++; @@ -1343,7 +1307,7 @@ LSM303D::mag_measure() } raw_mag_report; #pragma pack(pop) - mag_report *mag_report = &_mag_reports[_next_mag_report]; + struct _mag_report mag_report; /* start the performance counter */ perf_begin(_mag_sample_perf); @@ -1368,30 +1332,25 @@ LSM303D::mag_measure() */ - mag_report->timestamp = hrt_absolute_time(); + mag_report.r.timestamp = hrt_absolute_time(); - 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; - mag_report->scaling = _mag_range_scale; - mag_report->range_ga = (float)_mag_range_ga; + mag_report.r.x_raw = raw_mag_report.x; + mag_report.r.y_raw = raw_mag_report.y; + mag_report.r.z_raw = raw_mag_report.z; + mag_report.r.x = ((mag_report.r.x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; + mag_report.r.y = ((mag_report.r.y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; + mag_report.r.z = ((mag_report.r.z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; + mag_report.r.scaling = _mag_range_scale; + mag_report.r.range_ga = (float)_mag_range_ga; - /* 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); + _mag_reports->force(mag_report); /* XXX please check this poll_notify, is it the right one? */ /* notify anyone waiting for data */ poll_notify(POLLIN); /* publish for subscribers */ - orb_publish(ORB_ID(sensor_mag), _mag_topic, mag_report); + orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report.r); _mag_read++; @@ -1405,11 +1364,8 @@ 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); - 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); + _accel_reports->print_info("accel reports"); + _mag_reports->print_info("mag reports"); } LSM303D_mag::LSM303D_mag(LSM303D *parent) : From a5821d29281243385363745d1725a6b3210f7f96 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 9 Sep 2013 17:16:38 +1000 Subject: [PATCH 636/872] ms5611: converted to using RingBuffer --- src/drivers/ms5611/ms5611.cpp | 91 +++++++++++++---------------------- 1 file changed, 34 insertions(+), 57 deletions(-) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 4e43f19c5d..3f57cd68f3 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -60,6 +60,7 @@ #include #include #include +#include #include #include @@ -114,10 +115,7 @@ protected: struct work_s _work; unsigned _measure_ticks; - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; - struct baro_report *_reports; + RingBuffer *_reports; bool _collect_phase; unsigned _measure_phase; @@ -196,9 +194,6 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) : _interface(interface), _prom(prom_buf.s), _measure_ticks(0), - _num_reports(0), - _next_report(0), - _oldest_report(0), _reports(nullptr), _collect_phase(false), _measure_phase(0), @@ -223,7 +218,7 @@ MS5611::~MS5611() /* free any existing reports */ if (_reports != nullptr) - delete[] _reports; + delete _reports; // free perf counters perf_free(_sample_perf); @@ -246,8 +241,7 @@ MS5611::init() } /* allocate basic report buffers */ - _num_reports = 2; - _reports = new struct baro_report[_num_reports]; + _reports = new RingBuffer(2); if (_reports == nullptr) { debug("can't get memory for reports"); @@ -255,11 +249,10 @@ MS5611::init() 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]); + struct baro_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _baro_topic = orb_advertise(ORB_ID(sensor_baro), &zero_report); if (_baro_topic < 0) { debug("failed to create sensor_baro object"); @@ -276,6 +269,7 @@ ssize_t MS5611::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct baro_report); + struct baro_report *brp = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -291,10 +285,9 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) * 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 (_reports->get(*brp)) { + ret += sizeof(*brp); + brp++; } } @@ -303,10 +296,9 @@ 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; + _reports->flush(); /* do temperature first */ if (OK != measure()) { @@ -335,8 +327,8 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) } /* state machine will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); + if (_reports->get(*brp)) + ret = sizeof(*brp); } while (0); @@ -411,31 +403,21 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) 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 < 1) || (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 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; + irqstate_t flags = irqsave(); + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; } + irqrestore(flags); + return OK; + } case SENSORIOCGQUEUEDEPTH: - return _num_reports - 1; + return _reports->size(); case SENSORIOCRESET: /* XXX implement this */ @@ -469,7 +451,7 @@ MS5611::start_cycle() /* reset the report ring and state machine */ _collect_phase = false; _measure_phase = 0; - _oldest_report = _next_report = 0; + _reports->flush(); /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, 1); @@ -588,8 +570,9 @@ MS5611::collect() perf_begin(_sample_perf); + struct baro_report report; /* 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(); + report.timestamp = hrt_absolute_time(); /* read the most recent measurement - read offset/size are hardcoded in the interface */ ret = _interface->read(0, (void *)&raw, 0); @@ -638,8 +621,8 @@ MS5611::collect() 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 */ + report.temperature = _TEMP / 100.0f; + 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 */ @@ -676,18 +659,13 @@ MS5611::collect() * h = ------------------------------- + h1 * a */ - _reports[_next_report].altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; + report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; /* publish it */ - orb_publish(ORB_ID(sensor_baro), _baro_topic, &_reports[_next_report]); + orb_publish(ORB_ID(sensor_baro), _baro_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, toss it */ - if (_next_report == _oldest_report) { + if (_reports->force(report)) { perf_count(_buffer_overflows); - INCREMENT(_oldest_report, _num_reports); } /* notify anyone waiting for data */ @@ -709,8 +687,7 @@ MS5611::print_info() 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); + _reports->print_info("report queue"); printf("TEMP: %d\n", _TEMP); printf("SENS: %lld\n", _SENS); printf("OFF: %lld\n", _OFF); From cefc7ac00e55ade983562a081c3ccda8030e95ce Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 9 Sep 2013 22:23:48 -0700 Subject: [PATCH 637/872] Rework the ringbuffer class so that it's not templated, and refactor its clients so they aren't dancing around the linker anymore. --- src/drivers/airspeed/airspeed.cpp | 12 +- src/drivers/airspeed/airspeed.h | 2 +- src/drivers/bma180/bma180.cpp | 55 ++--- src/drivers/device/ringbuffer.h | 374 +++++++++++++++++++++--------- src/drivers/hmc5883/hmc5883.cpp | 10 +- src/drivers/l3gd20/l3gd20.cpp | 10 +- src/drivers/lsm303d/lsm303d.cpp | 85 +++---- src/drivers/mb12xx/mb12xx.cpp | 10 +- src/drivers/mpu6000/mpu6000.cpp | 94 ++++---- src/drivers/ms5611/ms5611.cpp | 10 +- 10 files changed, 396 insertions(+), 266 deletions(-) diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 2a6b190de3..5e45cc936c 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -79,6 +79,7 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) : I2C("Airspeed", AIRSPEED_DEVICE_PATH, bus, address, 100000), _reports(nullptr), + _buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")), _max_differential_pressure_pa(0), _sensor_ok(false), _measure_ticks(0), @@ -87,8 +88,7 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) : _airspeed_pub(-1), _conversion_interval(conversion_interval), _sample_perf(perf_alloc(PC_ELAPSED, "airspeed_read")), - _comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors")), - _buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")) + _comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors")) { // enable debug() calls _debug_enabled = true; @@ -122,7 +122,7 @@ Airspeed::init() goto out; /* allocate basic report buffers */ - _reports = new RingBuffer(2); + _reports = new RingBuffer(2, sizeof(differential_pressure_s)); if (_reports == nullptr) goto out; @@ -282,7 +282,7 @@ Airspeed::read(struct file *filp, char *buffer, size_t buflen) * we are careful to avoid racing with them. */ while (count--) { - if (_reports->get(*abuf)) { + if (_reports->get(abuf)) { ret += sizeof(*abuf); abuf++; } @@ -312,7 +312,7 @@ Airspeed::read(struct file *filp, char *buffer, size_t buflen) } /* state machine will have generated a report, copy it out */ - if (_reports->get(*abuf)) { + if (_reports->get(abuf)) { ret = sizeof(*abuf); } @@ -375,6 +375,6 @@ Airspeed::print_info() void Airspeed::new_report(const differential_pressure_s &report) { - if (!_reports->force(report)) + if (!_reports->force(&report)) perf_count(_buffer_overflows); } diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h index 7850ccc7e9..0487848133 100644 --- a/src/drivers/airspeed/airspeed.h +++ b/src/drivers/airspeed/airspeed.h @@ -104,7 +104,7 @@ public: virtual void print_info(); private: - RingBuffer *_reports; + RingBuffer *_reports; perf_counter_t _buffer_overflows; protected: diff --git a/src/drivers/bma180/bma180.cpp b/src/drivers/bma180/bma180.cpp index f14c008cea..f0044d36f1 100644 --- a/src/drivers/bma180/bma180.cpp +++ b/src/drivers/bma180/bma180.cpp @@ -147,14 +147,7 @@ private: struct hrt_call _call; unsigned _call_interval; - /* - this wrapper type is needed to avoid a linker error for - RingBuffer instances which appear in two drivers. - */ - struct _accel_report { - accel_report r; - }; - RingBuffer *_reports; + RingBuffer *_reports; struct accel_scale _accel_scale; float _accel_range_scale; @@ -284,7 +277,7 @@ BMA180::init() goto out; /* allocate basic report buffers */ - _reports = new RingBuffer<_accel_report>(2); + _reports = new RingBuffer(2, sizeof(accel_report)); if (_reports == nullptr) goto out; @@ -349,7 +342,7 @@ ssize_t BMA180::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct accel_report); - struct _accel_report *arp = reinterpret_cast(buffer); + struct accel_report *arp = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -365,7 +358,7 @@ BMA180::read(struct file *filp, char *buffer, size_t buflen) * we are careful to avoid racing with it. */ while (count--) { - if (_reports->get(*arp)) { + if (_reports->get(arp)) { ret += sizeof(*arp); arp++; } @@ -380,7 +373,7 @@ BMA180::read(struct file *filp, char *buffer, size_t buflen) measure(); /* measurement will have generated a report, copy it out */ - if (_reports->get(*arp)) + if (_reports->get(arp)) ret = sizeof(*arp); return ret; @@ -676,7 +669,7 @@ BMA180::measure() // } raw_report; // #pragma pack(pop) - struct _accel_report report; + struct accel_report report; /* start the performance counter */ perf_begin(_sample_perf); @@ -696,40 +689,40 @@ BMA180::measure() * them before. There is no good way to synchronise with the internal * measurement flow without using the external interrupt. */ - report.r.timestamp = hrt_absolute_time(); + report.timestamp = hrt_absolute_time(); /* * y of board is x of sensor and x of board is -y of sensor * perform only the axis assignment here. * Two non-value bits are discarded directly */ - report.r.y_raw = read_reg(ADDR_ACC_X_LSB + 0); - report.r.y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8; - report.r.x_raw = read_reg(ADDR_ACC_X_LSB + 2); - report.r.x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8; - report.r.z_raw = read_reg(ADDR_ACC_X_LSB + 4); - report.r.z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8; + report.y_raw = read_reg(ADDR_ACC_X_LSB + 0); + report.y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8; + report.x_raw = read_reg(ADDR_ACC_X_LSB + 2); + report.x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8; + report.z_raw = read_reg(ADDR_ACC_X_LSB + 4); + report.z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8; /* discard two non-value bits in the 16 bit measurement */ - report.r.x_raw = (report.r.x_raw / 4); - report.r.y_raw = (report.r.y_raw / 4); - report.r.z_raw = (report.r.z_raw / 4); + report.x_raw = (report.x_raw / 4); + report.y_raw = (report.y_raw / 4); + report.z_raw = (report.z_raw / 4); /* invert y axis, due to 14 bit data no overflow can occur in the negation */ - report.r.y_raw = -report.r.y_raw; + report.y_raw = -report.y_raw; - report.r.x = ((report.r.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - report.r.y = ((report.r.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - report.r.z = ((report.r.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; - report.r.scaling = _accel_range_scale; - report.r.range_m_s2 = _accel_range_m_s2; + report.x = ((report.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + report.y = ((report.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + report.z = ((report.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + report.scaling = _accel_range_scale; + report.range_m_s2 = _accel_range_m_s2; - _reports->force(report); + _reports->force(&report); /* notify anyone waiting for data */ poll_notify(POLLIN); /* publish for subscribers */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &report.r); + orb_publish(ORB_ID(sensor_accel), _accel_topic, &report); /* stop the perf counter */ perf_end(_sample_perf); diff --git a/src/drivers/device/ringbuffer.h b/src/drivers/device/ringbuffer.h index e3c5a20bdd..24893b977c 100644 --- a/src/drivers/device/ringbuffer.h +++ b/src/drivers/device/ringbuffer.h @@ -1,7 +1,6 @@ /**************************************************************************** * * 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 @@ -35,15 +34,14 @@ /** * @file ringbuffer.h * - * A simple ringbuffer template. + * A flexible ringbuffer class. */ #pragma once -template class RingBuffer { public: - RingBuffer(unsigned size); + RingBuffer(unsigned ring_size, size_t entry_size); virtual ~RingBuffer(); /** @@ -52,15 +50,18 @@ public: * @param val Item to put * @return true if the item was put, false if the buffer is full */ - bool put(T &val); + bool put(const void *val, size_t val_size = 0); - /** - * Put an item into the buffer if there is space. - * - * @param val Item to put - * @return true if the item was put, false if the buffer is full - */ - bool put(const T &val); + bool put(int8_t val); + bool put(uint8_t val); + bool put(int16_t val); + bool put(uint16_t val); + bool put(int32_t val); + bool put(uint32_t val); + bool put(int64_t val); + bool put(uint64_t val); + bool put(float val); + bool put(double val); /** * Force an item into the buffer, discarding an older item if there is not space. @@ -68,15 +69,18 @@ public: * @param val Item to put * @return true if an item was discarded to make space */ - bool force(T &val); + bool force(const void *val, size_t val_size = 0); - /** - * Force an item into the buffer, discarding an older item if there is not space. - * - * @param val Item to put - * @return true if an item was discarded to make space - */ - bool force(const T &val); + bool force(int8_t val); + bool force(uint8_t val); + bool force(int16_t val); + bool force(uint16_t val); + bool force(int32_t val); + bool force(uint32_t val); + bool force(int64_t val); + bool force(uint64_t val); + bool force(float val); + bool force(double val); /** * Get an item from the buffer. @@ -84,15 +88,18 @@ public: * @param val Item that was gotten * @return true if an item was got, false if the buffer was empty. */ - bool get(T &val); + bool get(void *val, size_t val_size = 0); - /** - * Get an item from the buffer (scalars only). - * - * @return The value that was fetched. If the buffer is empty, - * returns zero. - */ - T get(void); + bool get(int8_t &val); + bool get(uint8_t &val); + bool get(int16_t &val); + bool get(uint16_t &val); + bool get(int32_t &val); + bool get(uint32_t &val); + bool get(int64_t &val); + bool get(uint64_t &val); + bool get(float &val); + bool get(double &val); /* * Get the number of slots free in the buffer. @@ -148,67 +155,68 @@ public: void print_info(const char *name); private: - T *_buf; - unsigned _size; + unsigned _ring_size; + const size_t _item_size; + char *_buf; volatile unsigned _head; /**< insertion point */ volatile unsigned _tail; /**< removal point */ unsigned _next(unsigned index); }; -template -RingBuffer::RingBuffer(unsigned with_size) : - _buf(new T[with_size + 1]), - _size(with_size), - _head(with_size), - _tail(with_size) +RingBuffer::RingBuffer(unsigned ring_size, size_t item_size) : + _ring_size((ring_size + 1) * item_size), + _item_size(item_size), + _buf(new char[_ring_size]), + _head(ring_size), + _tail(ring_size) {} -template -RingBuffer::~RingBuffer() +RingBuffer::~RingBuffer() { if (_buf != nullptr) delete[] _buf; } -template -bool RingBuffer::empty() +unsigned +RingBuffer::_next(unsigned index) +{ + return (0 == index) ? _ring_size : (index - _item_size); +} + +bool +RingBuffer::empty() { return _tail == _head; } -template -bool RingBuffer::full() +bool +RingBuffer::full() { return _next(_head) == _tail; } -template -unsigned RingBuffer::size() +unsigned +RingBuffer::size() { - return (_buf != nullptr) ? _size : 0; + return (_buf != nullptr) ? _ring_size : 0; } -template -void RingBuffer::flush() +void +RingBuffer::flush() { - T junk; while (!empty()) - get(junk); + get(NULL); } -template -unsigned RingBuffer::_next(unsigned index) -{ - return (0 == index) ? _size : (index - 1); -} - -template -bool RingBuffer::put(T &val) +bool +RingBuffer::put(const void *val, size_t val_size) { unsigned next = _next(_head); if (next != _tail) { - _buf[_head] = val; + if ((val_size == 0) || (val_size > _item_size)) + val_size = _item_size; + memcpy(&_buf[_head], val, val_size); _head = next; return true; } else { @@ -216,55 +224,150 @@ bool RingBuffer::put(T &val) } } -template -bool RingBuffer::put(const T &val) +bool +RingBuffer::put(int8_t val) { - unsigned next = _next(_head); - if (next != _tail) { - _buf[_head] = val; - _head = next; - return true; - } else { - return false; - } + return put(&val, sizeof(val)); } -template -bool RingBuffer::force(T &val) +bool +RingBuffer::put(uint8_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(int16_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(uint16_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(int32_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(uint32_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(int64_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(uint64_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(float val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(double val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::force(const void *val, size_t val_size) { bool overwrote = false; for (;;) { - if (put(val)) + if (put(val, val_size)) break; - T junk; - get(junk); + get(NULL); overwrote = true; } return overwrote; } -template -bool RingBuffer::force(const T &val) +bool +RingBuffer::force(int8_t val) { - bool overwrote = false; - - for (;;) { - if (put(val)) - break; - T junk; - get(junk); - overwrote = true; - } - return overwrote; + return force(&val, sizeof(val)); } -template -bool RingBuffer::get(T &val) +bool +RingBuffer::force(uint8_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(int16_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(uint16_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(int32_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(uint32_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(int64_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(uint64_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(float val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(double val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::get(void *val, size_t val_size) { if (_tail != _head) { unsigned candidate; unsigned next; + + if ((val_size == 0) || (val_size > _item_size)) + val_size = _item_size; + do { /* decide which element we think we're going to read */ candidate = _tail; @@ -273,7 +376,8 @@ bool RingBuffer::get(T &val) next = _next(candidate); /* go ahead and read from this index */ - val = _buf[candidate]; + if (val != NULL) + memcpy(val, &_buf[candidate], val_size); /* if the tail pointer didn't change, we got our item */ } while (!__sync_bool_compare_and_swap(&_tail, candidate, next)); @@ -284,15 +388,68 @@ bool RingBuffer::get(T &val) } } -template -T RingBuffer::get(void) +bool +RingBuffer::get(int8_t &val) { - T val; - return get(val) ? val : 0; + return get(&val, sizeof(val)); } -template -unsigned RingBuffer::space(void) +bool +RingBuffer::get(uint8_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(int16_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(uint16_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(int32_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(uint32_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(int64_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(uint64_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(float &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(double &val) +{ + return get(&val, sizeof(val)); +} + +unsigned +RingBuffer::space(void) { unsigned tail, head; @@ -309,39 +466,42 @@ unsigned RingBuffer::space(void) tail = _tail; } while (head != _head); - return (tail >= head) ? (_size - (tail - head)) : (head - tail - 1); + unsigned reported_space = (tail >= head) ? (_ring_size - (tail - head)) : (head - tail - 1); + + return reported_space / _item_size; } -template -unsigned RingBuffer::count(void) +unsigned +RingBuffer::count(void) { /* * Note that due to the conservative nature of space(), this may * over-estimate the number of items in the buffer. */ - return _size - space(); + return (_ring_size / _item_size) - space(); } -template -bool RingBuffer::resize(unsigned new_size) +bool +RingBuffer::resize(unsigned new_size) { - T *old_buffer; - T *new_buffer = new T[new_size + 1]; + unsigned new_ring_size = (new_size + 1) * _item_size; + char *old_buffer; + char *new_buffer = new char [new_ring_size]; if (new_buffer == nullptr) { return false; } old_buffer = _buf; _buf = new_buffer; - _size = new_size; + _ring_size = new_ring_size; _head = new_size; _tail = new_size; delete[] old_buffer; return true; } -template -void RingBuffer::print_info(const char *name) +void +RingBuffer::print_info(const char *name) { - printf("%s %u (%u/%u @ %p)\n", - name, _size, _head, _tail, _buf); + printf("%s %u/%u (%u/%u @ %p)\n", + name, _ring_size/_item_size, _head, _tail, _buf); } diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index b838bf16b8..58a1593eda 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -149,7 +149,7 @@ private: work_s _work; unsigned _measure_ticks; - RingBuffer *_reports; + RingBuffer *_reports; mag_scale _scale; float _range_scale; float _range_ga; @@ -367,7 +367,7 @@ HMC5883::init() goto out; /* allocate basic report buffers */ - _reports = new RingBuffer(2); + _reports = new RingBuffer(2, sizeof(mag_report)); if (_reports == nullptr) goto out; @@ -496,7 +496,7 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen) * we are careful to avoid racing with them. */ while (count--) { - if (_reports->get(*mag_buf)) { + if (_reports->get(mag_buf)) { ret += sizeof(struct mag_report); mag_buf++; } @@ -526,7 +526,7 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen) break; } - if (_reports->get(*mag_buf)) { + if (_reports->get(mag_buf)) { ret = sizeof(struct mag_report); } } while (0); @@ -878,7 +878,7 @@ HMC5883::collect() orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); /* post a report to the ring */ - if (_reports->force(new_report)) { + if (_reports->force(&new_report)) { perf_count(_buffer_overflows); } diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 7cebebeb41..4c3b0ce514 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -185,7 +185,7 @@ private: struct hrt_call _call; unsigned _call_interval; - RingBuffer *_reports; + RingBuffer *_reports; struct gyro_scale _gyro_scale; float _gyro_range_scale; @@ -347,7 +347,7 @@ L3GD20::init() goto out; /* allocate basic report buffers */ - _reports = new RingBuffer(2); + _reports = new RingBuffer(2, sizeof(gyro_report)); if (_reports == nullptr) goto out; @@ -421,7 +421,7 @@ L3GD20::read(struct file *filp, char *buffer, size_t buflen) * we are careful to avoid racing with it. */ while (count--) { - if (_reports->get(*gbuf)) { + if (_reports->get(gbuf)) { ret += sizeof(*gbuf); gbuf++; } @@ -436,7 +436,7 @@ L3GD20::read(struct file *filp, char *buffer, size_t buflen) measure(); /* measurement will have generated a report, copy it out */ - if (_reports->get(*gbuf)) { + if (_reports->get(gbuf)) { ret = sizeof(*gbuf); } @@ -815,7 +815,7 @@ L3GD20::measure() report.scaling = _gyro_range_scale; report.range_rad_s = _gyro_range_rad_s; - _reports->force(report); + _reports->force(&report); /* notify anyone waiting for data */ poll_notify(POLLIN); diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 997b80fab2..a90cd0a3d1 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -219,19 +219,8 @@ private: unsigned _call_accel_interval; unsigned _call_mag_interval; - /* - these wrapper types are needed to avoid a linker error for - RingBuffer instances which appear in two drivers. - */ - struct _accel_report { - struct accel_report r; - }; - RingBuffer *_accel_reports; - - struct _mag_report { - struct mag_report r; - }; - RingBuffer *_mag_reports; + RingBuffer *_accel_reports; + RingBuffer *_mag_reports; struct accel_scale _accel_scale; unsigned _accel_range_m_s2; @@ -499,7 +488,7 @@ LSM303D::init() } /* allocate basic report buffers */ - _accel_reports = new RingBuffer(2); + _accel_reports = new RingBuffer(2, sizeof(accel_report)); if (_accel_reports == nullptr) goto out; @@ -509,7 +498,7 @@ LSM303D::init() memset(&zero_report, 0, sizeof(zero_report)); _accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report); - _mag_reports = new RingBuffer(2); + _mag_reports = new RingBuffer(2, sizeof(mag_report)); if (_mag_reports == nullptr) goto out; @@ -579,7 +568,7 @@ ssize_t LSM303D::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct accel_report); - struct _accel_report *arb = reinterpret_cast(buffer); + accel_report *arb = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -592,7 +581,7 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen) * While there is space in the caller's buffer, and reports, copy them. */ while (count--) { - if (_accel_reports->get(*arb)) { + if (_accel_reports->get(arb)) { ret += sizeof(*arb); arb++; } @@ -606,7 +595,7 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen) measure(); /* measurement will have generated a report, copy it out */ - if (_accel_reports->get(*arb)) + if (_accel_reports->get(arb)) ret = sizeof(*arb); return ret; @@ -616,7 +605,7 @@ ssize_t LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct mag_report); - struct _mag_report *mrb = reinterpret_cast(buffer); + mag_report *mrb = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -630,7 +619,7 @@ LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen) * While there is space in the caller's buffer, and reports, copy them. */ while (count--) { - if (_mag_reports->get(*mrb)) { + if (_mag_reports->get(mrb)) { ret += sizeof(*mrb); mrb++; } @@ -645,7 +634,7 @@ LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen) measure(); /* measurement will have generated a report, copy it out */ - if (_mag_reports->get(*mrb)) + if (_mag_reports->get(mrb)) ret = sizeof(*mrb); return ret; @@ -1237,7 +1226,7 @@ LSM303D::measure() } raw_accel_report; #pragma pack(pop) - struct _accel_report accel_report; + accel_report accel_report; /* start the performance counter */ perf_begin(_accel_sample_perf); @@ -1262,30 +1251,30 @@ LSM303D::measure() */ - accel_report.r.timestamp = hrt_absolute_time(); + accel_report.timestamp = hrt_absolute_time(); - accel_report.r.x_raw = raw_accel_report.x; - accel_report.r.y_raw = raw_accel_report.y; - accel_report.r.z_raw = raw_accel_report.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; - float x_in_new = ((accel_report.r.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - float y_in_new = ((accel_report.r.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - float z_in_new = ((accel_report.r.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.r.x = _accel_filter_x.apply(x_in_new); - accel_report.r.y = _accel_filter_y.apply(y_in_new); - accel_report.r.z = _accel_filter_z.apply(z_in_new); + 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.r.scaling = _accel_range_scale; - accel_report.r.range_m_s2 = _accel_range_m_s2; + accel_report.scaling = _accel_range_scale; + accel_report.range_m_s2 = _accel_range_m_s2; - _accel_reports->force(accel_report); + _accel_reports->force(&accel_report); /* notify anyone waiting for data */ poll_notify(POLLIN); /* publish for subscribers */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report.r); + orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); _accel_read++; @@ -1307,7 +1296,7 @@ LSM303D::mag_measure() } raw_mag_report; #pragma pack(pop) - struct _mag_report mag_report; + mag_report mag_report; /* start the performance counter */ perf_begin(_mag_sample_perf); @@ -1332,25 +1321,25 @@ LSM303D::mag_measure() */ - mag_report.r.timestamp = hrt_absolute_time(); + mag_report.timestamp = hrt_absolute_time(); - mag_report.r.x_raw = raw_mag_report.x; - mag_report.r.y_raw = raw_mag_report.y; - mag_report.r.z_raw = raw_mag_report.z; - mag_report.r.x = ((mag_report.r.x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; - mag_report.r.y = ((mag_report.r.y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; - mag_report.r.z = ((mag_report.r.z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; - mag_report.r.scaling = _mag_range_scale; - mag_report.r.range_ga = (float)_mag_range_ga; + 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; + mag_report.scaling = _mag_range_scale; + mag_report.range_ga = (float)_mag_range_ga; - _mag_reports->force(mag_report); + _mag_reports->force(&mag_report); /* XXX please check this poll_notify, is it the right one? */ /* notify anyone waiting for data */ poll_notify(POLLIN); /* publish for subscribers */ - orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report.r); + orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report); _mag_read++; diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index fabe10b872..ccc5bc15e7 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -120,7 +120,7 @@ private: float _min_distance; float _max_distance; work_s _work; - RingBuffer *_reports; + RingBuffer *_reports; bool _sensor_ok; int _measure_ticks; bool _collect_phase; @@ -226,7 +226,7 @@ MB12XX::init() goto out; /* allocate basic report buffers */ - _reports = new RingBuffer(2); + _reports = new RingBuffer(2, sizeof(range_finder_report)); if (_reports == nullptr) goto out; @@ -403,7 +403,7 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen) * we are careful to avoid racing with them. */ while (count--) { - if (_reports->get(*rbuf)) { + if (_reports->get(rbuf)) { ret += sizeof(*rbuf); rbuf++; } @@ -433,7 +433,7 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen) } /* state machine will have generated a report, copy it out */ - if (_reports->get(*rbuf)) { + if (_reports->get(rbuf)) { ret = sizeof(*rbuf); } @@ -496,7 +496,7 @@ MB12XX::collect() /* publish it */ orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); - if (_reports->force(report)) { + if (_reports->force(&report)) { perf_count(_buffer_overflows); } diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 66d36826a8..14a3571dec 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -194,26 +194,14 @@ private: struct hrt_call _call; unsigned _call_interval; - /* - these wrapper types are needed to avoid a linker error for - RingBuffer instances which appear in two drivers. - */ - struct _accel_report { - struct accel_report r; - }; - typedef RingBuffer<_accel_report> AccelReportBuffer; - AccelReportBuffer *_accel_reports; + RingBuffer *_accel_reports; struct accel_scale _accel_scale; float _accel_range_scale; float _accel_range_m_s2; orb_advert_t _accel_topic; - struct _gyro_report { - struct gyro_report r; - }; - typedef RingBuffer<_gyro_report> GyroReportBuffer; - GyroReportBuffer *_gyro_reports; + RingBuffer *_gyro_reports; struct gyro_scale _gyro_scale; float _gyro_range_scale; @@ -441,11 +429,11 @@ MPU6000::init() } /* allocate basic report buffers */ - _accel_reports = new AccelReportBuffer(2); + _accel_reports = new RingBuffer(2, sizeof(accel_report)); if (_accel_reports == nullptr) goto out; - _gyro_reports = new GyroReportBuffer(2); + _gyro_reports = new RingBuffer(2, sizeof(gyro_report)); if (_gyro_reports == nullptr) goto out; @@ -475,16 +463,16 @@ MPU6000::init() if (gyro_ret != OK) { _gyro_topic = -1; } else { - _gyro_report gr; - _gyro_reports->get(gr); + gyro_report gr; + _gyro_reports->get(&gr); - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr.r); + _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr); } /* advertise accel topic */ - _accel_report ar; - _accel_reports->get(ar); - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar.r); + accel_report ar; + _accel_reports->get(&ar); + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar); out: return ret; @@ -665,10 +653,10 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen) return -EAGAIN; /* copy reports out of our buffer to the caller */ - _accel_report *arp = reinterpret_cast<_accel_report *>(buffer); + accel_report *arp = reinterpret_cast(buffer); int transferred = 0; while (count--) { - if (!_accel_reports->get(*arp)) + if (!_accel_reports->get(arp)) break; transferred++; arp++; @@ -759,10 +747,10 @@ MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) return -EAGAIN; /* copy reports out of our buffer to the caller */ - _gyro_report *grp = reinterpret_cast<_gyro_report *>(buffer); + gyro_report *grp = reinterpret_cast(buffer); int transferred = 0; while (count--) { - if (!_gyro_reports->get(*grp)) + if (!_gyro_reports->get(grp)) break; transferred++; grp++; @@ -1191,13 +1179,13 @@ MPU6000::measure() /* * Report buffers. */ - _accel_report arb; - _gyro_report grb; + accel_report arb; + gyro_report grb; /* * Adjust and scale results to m/s^2. */ - grb.r.timestamp = arb.r.timestamp = hrt_absolute_time(); + grb.timestamp = arb.timestamp = hrt_absolute_time(); /* @@ -1218,53 +1206,53 @@ MPU6000::measure() /* NOTE: Axes have been swapped to match the board a few lines above. */ - arb.r.x_raw = report.accel_x; - arb.r.y_raw = report.accel_y; - arb.r.z_raw = report.accel_z; + arb.x_raw = report.accel_x; + arb.y_raw = report.accel_y; + arb.z_raw = report.accel_z; float x_in_new = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; float y_in_new = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; float z_in_new = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; - arb.r.x = _accel_filter_x.apply(x_in_new); - arb.r.y = _accel_filter_y.apply(y_in_new); - arb.r.z = _accel_filter_z.apply(z_in_new); + arb.x = _accel_filter_x.apply(x_in_new); + arb.y = _accel_filter_y.apply(y_in_new); + arb.z = _accel_filter_z.apply(z_in_new); - arb.r.scaling = _accel_range_scale; - arb.r.range_m_s2 = _accel_range_m_s2; + arb.scaling = _accel_range_scale; + arb.range_m_s2 = _accel_range_m_s2; - arb.r.temperature_raw = report.temp; - arb.r.temperature = (report.temp) / 361.0f + 35.0f; + arb.temperature_raw = report.temp; + arb.temperature = (report.temp) / 361.0f + 35.0f; - grb.r.x_raw = report.gyro_x; - grb.r.y_raw = report.gyro_y; - grb.r.z_raw = report.gyro_z; + grb.x_raw = report.gyro_x; + grb.y_raw = report.gyro_y; + grb.z_raw = report.gyro_z; float x_gyro_in_new = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; float y_gyro_in_new = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; float z_gyro_in_new = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; - grb.r.x = _gyro_filter_x.apply(x_gyro_in_new); - grb.r.y = _gyro_filter_y.apply(y_gyro_in_new); - grb.r.z = _gyro_filter_z.apply(z_gyro_in_new); + grb.x = _gyro_filter_x.apply(x_gyro_in_new); + grb.y = _gyro_filter_y.apply(y_gyro_in_new); + grb.z = _gyro_filter_z.apply(z_gyro_in_new); - grb.r.scaling = _gyro_range_scale; - grb.r.range_rad_s = _gyro_range_rad_s; + grb.scaling = _gyro_range_scale; + grb.range_rad_s = _gyro_range_rad_s; - grb.r.temperature_raw = report.temp; - grb.r.temperature = (report.temp) / 361.0f + 35.0f; + grb.temperature_raw = report.temp; + grb.temperature = (report.temp) / 361.0f + 35.0f; - _accel_reports->force(arb); - _gyro_reports->force(grb); + _accel_reports->force(&arb); + _gyro_reports->force(&grb); /* notify anyone waiting for data */ poll_notify(POLLIN); _gyro->parent_poll_notify(); /* and publish for subscribers */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb.r); + orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); if (_gyro_topic != -1) { - orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb.r); + orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb); } /* stop measuring */ diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 3f57cd68f3..1c8a4d776a 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -115,7 +115,7 @@ protected: struct work_s _work; unsigned _measure_ticks; - RingBuffer *_reports; + RingBuffer *_reports; bool _collect_phase; unsigned _measure_phase; @@ -241,7 +241,7 @@ MS5611::init() } /* allocate basic report buffers */ - _reports = new RingBuffer(2); + _reports = new RingBuffer(2, sizeof(baro_report)); if (_reports == nullptr) { debug("can't get memory for reports"); @@ -285,7 +285,7 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) * we are careful to avoid racing with them. */ while (count--) { - if (_reports->get(*brp)) { + if (_reports->get(brp)) { ret += sizeof(*brp); brp++; } @@ -327,7 +327,7 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) } /* state machine will have generated a report, copy it out */ - if (_reports->get(*brp)) + if (_reports->get(brp)) ret = sizeof(*brp); } while (0); @@ -664,7 +664,7 @@ MS5611::collect() /* publish it */ orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); - if (_reports->force(report)) { + if (_reports->force(&report)) { perf_count(_buffer_overflows); } From 760b3ab2e7050154a8ea09fe9358adfed85480ca Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 11 Sep 2013 14:36:44 +1000 Subject: [PATCH 638/872] ringbuffer: converted to item_size units this fixes a number of indexing bugs --- src/drivers/device/ringbuffer.h | 42 +++++++++++++++++---------------- 1 file changed, 22 insertions(+), 20 deletions(-) diff --git a/src/drivers/device/ringbuffer.h b/src/drivers/device/ringbuffer.h index 24893b977c..a9e22eaa6b 100644 --- a/src/drivers/device/ringbuffer.h +++ b/src/drivers/device/ringbuffer.h @@ -155,21 +155,21 @@ public: void print_info(const char *name); private: - unsigned _ring_size; + unsigned _num_items; const size_t _item_size; char *_buf; - volatile unsigned _head; /**< insertion point */ - volatile unsigned _tail; /**< removal point */ + volatile unsigned _head; /**< insertion point in _item_size units */ + volatile unsigned _tail; /**< removal point in _item_size units */ unsigned _next(unsigned index); }; -RingBuffer::RingBuffer(unsigned ring_size, size_t item_size) : - _ring_size((ring_size + 1) * item_size), +RingBuffer::RingBuffer(unsigned num_items, size_t item_size) : + _num_items(num_items), _item_size(item_size), - _buf(new char[_ring_size]), - _head(ring_size), - _tail(ring_size) + _buf(new char[(_num_items+1) * item_size]), + _head(_num_items), + _tail(_num_items) {} RingBuffer::~RingBuffer() @@ -181,7 +181,7 @@ RingBuffer::~RingBuffer() unsigned RingBuffer::_next(unsigned index) { - return (0 == index) ? _ring_size : (index - _item_size); + return (0 == index) ? _num_items : (index - 1); } bool @@ -199,7 +199,7 @@ RingBuffer::full() unsigned RingBuffer::size() { - return (_buf != nullptr) ? _ring_size : 0; + return (_buf != nullptr) ? _num_items : 0; } void @@ -216,7 +216,7 @@ RingBuffer::put(const void *val, size_t val_size) if (next != _tail) { if ((val_size == 0) || (val_size > _item_size)) val_size = _item_size; - memcpy(&_buf[_head], val, val_size); + memcpy(&_buf[_head * _item_size], val, val_size); _head = next; return true; } else { @@ -377,7 +377,7 @@ RingBuffer::get(void *val, size_t val_size) /* go ahead and read from this index */ if (val != NULL) - memcpy(val, &_buf[candidate], val_size); + memcpy(val, &_buf[candidate * _item_size], val_size); /* if the tail pointer didn't change, we got our item */ } while (!__sync_bool_compare_and_swap(&_tail, candidate, next)); @@ -466,9 +466,7 @@ RingBuffer::space(void) tail = _tail; } while (head != _head); - unsigned reported_space = (tail >= head) ? (_ring_size - (tail - head)) : (head - tail - 1); - - return reported_space / _item_size; + return (tail >= head) ? (_num_items - (tail - head)) : (head - tail - 1); } unsigned @@ -478,21 +476,20 @@ RingBuffer::count(void) * Note that due to the conservative nature of space(), this may * over-estimate the number of items in the buffer. */ - return (_ring_size / _item_size) - space(); + return _num_items - space(); } bool RingBuffer::resize(unsigned new_size) { - unsigned new_ring_size = (new_size + 1) * _item_size; char *old_buffer; - char *new_buffer = new char [new_ring_size]; + char *new_buffer = new char [(new_size+1) * _item_size]; if (new_buffer == nullptr) { return false; } old_buffer = _buf; _buf = new_buffer; - _ring_size = new_ring_size; + _num_items = new_size; _head = new_size; _tail = new_size; delete[] old_buffer; @@ -503,5 +500,10 @@ void RingBuffer::print_info(const char *name) { printf("%s %u/%u (%u/%u @ %p)\n", - name, _ring_size/_item_size, _head, _tail, _buf); + name, + _num_items, + _num_items * _item_size, + _head, + _tail, + _buf); } From 3b8039e4e009868ec7722ad559cd5b62c5f7a325 Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Wed, 11 Sep 2013 22:11:37 -0400 Subject: [PATCH 639/872] Implement message based receiver pairing --- src/drivers/px4io/px4io.cpp | 32 ++++++++++++++++++++--- src/modules/uORB/topics/vehicle_command.h | 3 ++- 2 files changed, 31 insertions(+), 4 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 78d1d3e63f..c93904a3ff 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -254,6 +254,7 @@ private: int _t_actuator_armed; ///< system armed control topic int _t_vehicle_control_mode;///< vehicle control mode topic int _t_param; ///< parameter update topic + int _t_vehicle_command; ///< vehicle command topic /* advertised topics */ orb_advert_t _to_input_rc; ///< rc inputs from io @@ -440,6 +441,7 @@ PX4IO::PX4IO(device::Device *interface) : _t_actuator_armed(-1), _t_vehicle_control_mode(-1), _t_param(-1), + _t_vehicle_command(-1), _to_input_rc(0), _to_actuators_effective(0), _to_outputs(0), @@ -732,16 +734,20 @@ PX4IO::task_main() _t_param = orb_subscribe(ORB_ID(parameter_update)); orb_set_interval(_t_param, 500); /* 2Hz update rate max. */ + _t_vehicle_command = orb_subscribe(ORB_ID(vehicle_command)); + orb_set_interval(_t_param, 1000); /* 1Hz update rate max. */ + if ((_t_actuators < 0) || (_t_actuator_armed < 0) || (_t_vehicle_control_mode < 0) || - (_t_param < 0)) { + (_t_param < 0) || + (_t_vehicle_command < 0)) { log("subscription(s) failed"); goto out; } /* poll descriptor */ - pollfd fds[4]; + pollfd fds[5]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; fds[1].fd = _t_actuator_armed; @@ -750,8 +756,10 @@ PX4IO::task_main() fds[2].events = POLLIN; fds[3].fd = _t_param; fds[3].events = POLLIN; + fds[4].fd = _t_vehicle_command; + fds[4].events = POLLIN; - debug("ready"); + log("ready"); /* lock against the ioctl handler */ lock(); @@ -791,6 +799,24 @@ PX4IO::task_main() if ((fds[1].revents & POLLIN) || (fds[2].revents & POLLIN)) io_set_arming_state(); + /* if we have a vehicle command, handle it */ + if (fds[4].revents & POLLIN) { + struct vehicle_command_s cmd; + orb_copy(ORB_ID(vehicle_command), _t_vehicle_command, &cmd); + if ((cmd.command == VEHICLE_CMD_START_RX_PAIR) && (cmd.param1== 0.0f)) { + if (!(_status & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { + if ((cmd.param2 == 0.0f) || (cmd.param2 == 1.0f)) { + mavlink_log_info(mavlink_fd, "[IO] binding dsm%c rx", (cmd.param2 == 0.0f) == 1 ? '2' : 'x'); + ioctl(nullptr, DSM_BIND_START, (cmd.param2 == 0.0f) ? 3 : 7); + } else { + mavlink_log_info(mavlink_fd, "[IO] invalid bind type, bind request rejected"); + } + } else { + mavlink_log_info(mavlink_fd, "[IO] system armed, bind request rejected"); + } + } + } + /* * If it's time for another tick of the polling status machine, * try it now. diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h index 31ff014de9..a62e38db2f 100644 --- a/src/modules/uORB/topics/vehicle_command.h +++ b/src/modules/uORB/topics/vehicle_command.h @@ -86,7 +86,8 @@ enum VEHICLE_CMD VEHICLE_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ VEHICLE_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ VEHICLE_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ - VEHICLE_CMD_ENUM_END=401, /* | */ + VEHICLE_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ + VEHICLE_CMD_ENUM_END=501, /* | */ }; /** From 41982579b3825bf93dad46ec6eed383ce47f04ff Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Wed, 11 Sep 2013 22:54:23 -0400 Subject: [PATCH 640/872] Refactor dsm binding code in px4io.cpp - Move repeated code into member function --- src/drivers/px4io/px4io.cpp | 54 ++++++++++++++++++++++--------------- 1 file changed, 32 insertions(+), 22 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index c93904a3ff..56a2940982 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -241,7 +241,8 @@ private: volatile int _task; /// Date: Thu, 12 Sep 2013 15:00:34 +1000 Subject: [PATCH 641/872] px4io: split io_handle_battery() out from io_handle_status() ready to add vservo and rssi --- src/drivers/px4io/px4io.cpp | 86 ++++++++++++++++++++++--------------- 1 file changed, 52 insertions(+), 34 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 78d1d3e63f..af20e61cbd 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -409,9 +409,18 @@ private: */ int io_handle_alarms(uint16_t alarms); + /** + * Handle a battery update from IO. + * + * Publish IO battery information if necessary. + * + * @param vbatt vbattery register + * @param status ibatter register + */ + void io_handle_battery(uint16_t vbatt, uint16_t ibatt); + }; - namespace { @@ -1158,6 +1167,45 @@ PX4IO::io_handle_alarms(uint16_t alarms) return 0; } +void +PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt) +{ + /* only publish if battery has a valid minimum voltage */ + if (vbatt <= 3300) { + return; + } + + battery_status_s battery_status; + battery_status.timestamp = hrt_absolute_time(); + + /* voltage is scaled to mV */ + battery_status.voltage_v = vbatt / 1000.0f; + + /* + ibatt contains the raw ADC count, as 12 bit ADC + value, with full range being 3.3v + */ + battery_status.current_a = ibatt * (3.3f/4096.0f) * _battery_amp_per_volt; + battery_status.current_a += _battery_amp_bias; + + /* + integrate battery over time to get total mAh used + */ + if (_battery_last_timestamp != 0) { + _battery_mamphour_total += battery_status.current_a * + (battery_status.timestamp - _battery_last_timestamp) * 1.0e-3f / 3600; + } + battery_status.discharged_mah = _battery_mamphour_total; + _battery_last_timestamp = battery_status.timestamp; + + /* lazily publish the battery voltage */ + if (_to_battery > 0) { + orb_publish(ORB_ID(battery_status), _to_battery, &battery_status); + } else { + _to_battery = orb_advertise(ORB_ID(battery_status), &battery_status); + } +} + int PX4IO::io_get_status() { @@ -1171,40 +1219,10 @@ 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; - battery_status.timestamp = hrt_absolute_time(); - - /* voltage is scaled to mV */ - battery_status.voltage_v = regs[2] / 1000.0f; - - /* - regs[3] contains the raw ADC count, as 12 bit ADC - value, with full range being 3.3v - */ - battery_status.current_a = regs[3] * (3.3f/4096.0f) * _battery_amp_per_volt; - battery_status.current_a += _battery_amp_bias; - - /* - integrate battery over time to get total mAh used - */ - if (_battery_last_timestamp != 0) { - _battery_mamphour_total += battery_status.current_a * - (battery_status.timestamp - _battery_last_timestamp) * 1.0e-3f / 3600; - } - battery_status.discharged_mah = _battery_mamphour_total; - _battery_last_timestamp = battery_status.timestamp; - - /* lazily publish the battery voltage */ - if (_to_battery > 0) { - orb_publish(ORB_ID(battery_status), _to_battery, &battery_status); - } else { - _to_battery = orb_advertise(ORB_ID(battery_status), &battery_status); - } - } +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + io_handle_battery(regs[2], regs[3]); +#endif return ret; } From f12794d30eb04d682c1977911752461e5c8a6eb8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 12 Sep 2013 15:48:26 +1000 Subject: [PATCH 642/872] uORB: added new servorail_status object used for VSERVO and RSSI on FMUv2 --- src/modules/uORB/objects_common.cpp | 3 + src/modules/uORB/topics/servorail_status.h | 67 ++++++++++++++++++++++ 2 files changed, 70 insertions(+) create mode 100644 src/modules/uORB/topics/servorail_status.h diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 1eb63a7994..3514dca24d 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -87,6 +87,9 @@ ORB_DEFINE(safety, struct safety_s); #include "topics/battery_status.h" ORB_DEFINE(battery_status, struct battery_status_s); +#include "topics/servorail_status.h" +ORB_DEFINE(servorail_status, struct servorail_status_s); + #include "topics/vehicle_global_position.h" ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s); diff --git a/src/modules/uORB/topics/servorail_status.h b/src/modules/uORB/topics/servorail_status.h new file mode 100644 index 0000000000..55668790b8 --- /dev/null +++ b/src/modules/uORB/topics/servorail_status.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 servorail_status.h + * + * Definition of the servorail status uORB topic. + */ + +#ifndef SERVORAIL_STATUS_H_ +#define SERVORAIL_STATUS_H_ + +#include "../uORB.h" +#include + +/** + * @addtogroup topics + * @{ + */ + +/** + * Servorail voltages and status + */ +struct servorail_status_s { + uint64_t timestamp; /**< microseconds since system boot */ + float voltage_v; /**< Servo rail voltage in volts */ + float rssi_v; /**< RSSI pin voltage in volts */ +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(servorail_status); + +#endif From e9e46f9c9dc1301b4218903b26dbcd58fb096895 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 12 Sep 2013 15:49:04 +1000 Subject: [PATCH 643/872] px4io: added monitoring of vservo and vrssi publish via servorail_status ORB topic --- src/drivers/px4io/px4io.cpp | 41 +++++++++++++++++++++++++-- src/modules/px4iofirmware/registers.c | 30 +++++++++----------- 2 files changed, 52 insertions(+), 19 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index af20e61cbd..27b6a12da4 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -80,6 +80,7 @@ #include #include #include +#include #include #include @@ -260,6 +261,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_servorail; ///< servorail status orb_advert_t _to_safety; ///< status of safety actuator_outputs_s _outputs; /// 0) { + orb_publish(ORB_ID(servorail_status), _to_servorail, &servorail_status); + } else { + _to_servorail = orb_advertise(ORB_ID(servorail_status), &servorail_status); + } +} + int PX4IO::io_get_status() { - uint16_t regs[4]; + uint16_t regs[6]; int ret; /* get STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT in that order */ @@ -1224,6 +1255,10 @@ PX4IO::io_get_status() io_handle_battery(regs[2], regs[3]); #endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + io_handle_vservo(regs[4], regs[5]); +#endif + return ret; } diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 9c95fd1c52..30ef0cceaf 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -678,27 +678,25 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val #ifdef ADC_VSERVO /* PX4IO_P_STATUS_VSERVO */ { - /* - * Coefficients here derived by measurement of the 5-16V - * range on one unit: - * - * XXX pending measurements - * - * slope = xxx - * intercept = xxx - * - * 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_VSERVO_SCALE]) / 10000; - - r_page_status[PX4IO_P_STATUS_VSERVO] = corrected; + // use 3:1 scaling on 3.3V ADC input + unsigned mV = counts * 9900 / 4096; + r_page_status[PX4IO_P_STATUS_VSERVO] = mV; + } + } +#endif +#ifdef ADC_RSSI + /* PX4IO_P_STATUS_VRSSI */ + { + unsigned counts = adc_measure(ADC_RSSI); + if (counts != 0xffff) { + // use 1:1 scaling on 3.3V ADC input + unsigned mV = counts * 3300 / 4096; + r_page_status[PX4IO_P_STATUS_VRSSI] = mV; } } #endif - /* XXX PX4IO_P_STATUS_VRSSI */ /* XXX PX4IO_P_STATUS_PRSSI */ SELECT_PAGE(r_page_status); From 0b7294a26ec33f707a9b5ddeee4269552b147e8b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 12 Sep 2013 16:20:54 +1000 Subject: [PATCH 644/872] added error_count field to sensor report structures --- src/drivers/drv_accel.h | 1 + src/drivers/drv_baro.h | 1 + src/drivers/drv_gyro.h | 1 + src/drivers/drv_mag.h | 1 + src/drivers/drv_range_finder.h | 1 + src/modules/uORB/topics/differential_pressure.h | 1 + 6 files changed, 6 insertions(+) diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h index 794de584b6..eff5e73495 100644 --- a/src/drivers/drv_accel.h +++ b/src/drivers/drv_accel.h @@ -52,6 +52,7 @@ */ struct accel_report { uint64_t timestamp; + uint64_t error_count; float x; /**< acceleration in the NED X board axis in m/s^2 */ float y; /**< acceleration in the NED Y board axis in m/s^2 */ float z; /**< acceleration in the NED Z board axis in m/s^2 */ diff --git a/src/drivers/drv_baro.h b/src/drivers/drv_baro.h index 176f798c06..aa251889f7 100644 --- a/src/drivers/drv_baro.h +++ b/src/drivers/drv_baro.h @@ -55,6 +55,7 @@ struct baro_report { float altitude; float temperature; uint64_t timestamp; + uint64_t error_count; }; /* diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h index 1d0fef6fcc..fefcae50b9 100644 --- a/src/drivers/drv_gyro.h +++ b/src/drivers/drv_gyro.h @@ -52,6 +52,7 @@ */ struct gyro_report { uint64_t timestamp; + uint64_t error_count; float x; /**< angular velocity in the NED X board axis in rad/s */ float y; /**< angular velocity in the NED Y board axis in rad/s */ float z; /**< angular velocity in the NED Z board axis in rad/s */ diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h index 3de5625fd7..06107bd3d1 100644 --- a/src/drivers/drv_mag.h +++ b/src/drivers/drv_mag.h @@ -54,6 +54,7 @@ */ struct mag_report { uint64_t timestamp; + uint64_t error_count; float x; float y; float z; diff --git a/src/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h index 03a82ec5d0..cf91f70304 100644 --- a/src/drivers/drv_range_finder.h +++ b/src/drivers/drv_range_finder.h @@ -52,6 +52,7 @@ */ struct range_finder_report { uint64_t timestamp; + uint64_t error_count; float distance; /** in meters */ uint8_t valid; /** 1 == within sensor range, 0 = outside sensor range */ }; diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h index 1ffeda764b..e4d2c92ceb 100644 --- a/src/modules/uORB/topics/differential_pressure.h +++ b/src/modules/uORB/topics/differential_pressure.h @@ -53,6 +53,7 @@ */ struct differential_pressure_s { uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ + uint64_t error_count; uint16_t differential_pressure_pa; /**< Differential pressure reading */ uint16_t max_differential_pressure_pa; /**< Maximum differential pressure reading */ float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */ From 7257642371f52a96c8d891795d090119437ea933 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 12 Sep 2013 16:21:24 +1000 Subject: [PATCH 645/872] perf: added perf_event_count() method this allows drivers to get an event_count from a perf counter --- src/modules/systemlib/perf_counter.c | 26 ++++++++++++++++++++++++++ src/modules/systemlib/perf_counter.h | 8 ++++++++ 2 files changed, 34 insertions(+) diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c index 3c1e10287d..bf84b79450 100644 --- a/src/modules/systemlib/perf_counter.c +++ b/src/modules/systemlib/perf_counter.c @@ -321,6 +321,32 @@ perf_print_counter(perf_counter_t handle) } } +uint64_t +perf_event_count(perf_counter_t handle) +{ + if (handle == NULL) + return 0; + + switch (handle->type) { + case PC_COUNT: + return ((struct perf_ctr_count *)handle)->event_count; + + case PC_ELAPSED: { + struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; + return pce->event_count; + } + + case PC_INTERVAL: { + struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle; + return pci->event_count; + } + + default: + break; + } + return 0; +} + void perf_print_all(void) { diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h index 4cd8b67a17..e1e3cbe95e 100644 --- a/src/modules/systemlib/perf_counter.h +++ b/src/modules/systemlib/perf_counter.h @@ -135,6 +135,14 @@ __EXPORT extern void perf_print_all(void); */ __EXPORT extern void perf_reset_all(void); +/** + * Return current event_count + * + * @param handle The counter returned from perf_alloc. + * @return event_count + */ +__EXPORT extern uint64_t perf_event_count(perf_counter_t handle); + __END_DECLS #endif From 4893509344f9304060dabc8a9ecad28fe891dcf8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 12 Sep 2013 16:21:59 +1000 Subject: [PATCH 646/872] drivers: report error_count in drivers where possible --- src/drivers/bma180/bma180.cpp | 1 + src/drivers/ets_airspeed/ets_airspeed.cpp | 21 +++++++++++---------- src/drivers/hmc5883/hmc5883.cpp | 1 + src/drivers/l3gd20/l3gd20.cpp | 1 + src/drivers/lsm303d/lsm303d.cpp | 1 + src/drivers/mb12xx/mb12xx.cpp | 1 + src/drivers/meas_airspeed/meas_airspeed.cpp | 4 +--- src/drivers/mpu6000/mpu6000.cpp | 2 +- src/drivers/ms5611/ms5611.cpp | 1 + 9 files changed, 19 insertions(+), 14 deletions(-) diff --git a/src/drivers/bma180/bma180.cpp b/src/drivers/bma180/bma180.cpp index f0044d36f1..1590cc182b 100644 --- a/src/drivers/bma180/bma180.cpp +++ b/src/drivers/bma180/bma180.cpp @@ -690,6 +690,7 @@ BMA180::measure() * measurement flow without using the external interrupt. */ report.timestamp = hrt_absolute_time(); + report.error_count = 0; /* * y of board is x of sensor and x of board is -y of sensor * perform only the axis assignment here. diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index dd8436b10e..084671ae20 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -153,35 +153,36 @@ ETSAirspeed::collect() ret = transfer(nullptr, 0, &val[0], 2); if (ret < 0) { - log("error reading from sensor: %d", ret); + perf_count(_comms_errors); return ret; } uint16_t diff_pres_pa = val[1] << 8 | val[0]; if (diff_pres_pa == 0) { - // a zero value means the pressure sensor cannot give us a - // value. We need to return, and not report a value or the - // caller could end up using this value as part of an - // average - log("zero value from sensor"); - return -1; + // a zero value means the pressure sensor cannot give us a + // value. We need to return, and not report a value or the + // caller could end up using this value as part of an + // average + perf_count(_comms_errors); + log("zero value from sensor"); + return -1; } if (diff_pres_pa < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) { diff_pres_pa = 0; - } else { diff_pres_pa -= _diff_pres_offset; } // Track maximum differential pressure measured (so we can work out top speed). if (diff_pres_pa > _max_differential_pressure_pa) { - _max_differential_pressure_pa = diff_pres_pa; + _max_differential_pressure_pa = diff_pres_pa; } // XXX we may want to smooth out the readings to remove noise. differential_pressure_s report; report.timestamp = hrt_absolute_time(); + report.error_count = perf_event_count(_comms_errors); report.differential_pressure_pa = diff_pres_pa; report.voltage = 0; report.max_differential_pressure_pa = _max_differential_pressure_pa; @@ -209,7 +210,7 @@ ETSAirspeed::cycle() /* perform collection */ if (OK != collect()) { - log("collection error"); + perf_count(_comms_errors); /* restart the measurement state machine */ start(); return; diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 58a1593eda..de043db643 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -791,6 +791,7 @@ HMC5883::collect() /* this should be fairly close to the end of the measurement, so the best approximation of the time */ new_report.timestamp = hrt_absolute_time(); + new_report.error_count = perf_event_count(_comms_errors); /* * @note We could read the status register here, which could tell us that diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 4c3b0ce514..ad6de0ab1c 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -774,6 +774,7 @@ L3GD20::measure() * 74 from all measurements centers them around zero. */ report.timestamp = hrt_absolute_time(); + report.error_count = 0; // not recorded switch (_orientation) { diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index a90cd0a3d1..7244019b1e 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1252,6 +1252,7 @@ LSM303D::measure() accel_report.timestamp = hrt_absolute_time(); + accel_report.error_count = 0; // not reported accel_report.x_raw = raw_accel_report.x; accel_report.y_raw = raw_accel_report.y; diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index ccc5bc15e7..c910e28908 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -490,6 +490,7 @@ MB12XX::collect() /* this should be fairly close to the end of the measurement, so the best approximation of the time */ report.timestamp = hrt_absolute_time(); + report.error_count = perf_event_count(_comms_errors); report.distance = si_units; report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 03d7bbfb91..ee45d46acb 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -138,7 +138,6 @@ MEASAirspeed::measure() if (OK != ret) { perf_count(_comms_errors); - log("i2c::transfer returned %d", ret); return ret; } @@ -161,7 +160,6 @@ MEASAirspeed::collect() ret = transfer(nullptr, 0, &val[0], 4); if (ret < 0) { - log("error reading from sensor: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); return ret; @@ -207,6 +205,7 @@ MEASAirspeed::collect() } report.timestamp = hrt_absolute_time(); + report.error_count = perf_event_count(_comms_errors); report.temperature = temperature; report.differential_pressure_pa = diff_press_pa; report.voltage = 0; @@ -235,7 +234,6 @@ MEASAirspeed::cycle() /* perform collection */ if (OK != collect()) { - log("collection error"); /* restart the measurement state machine */ start(); return; diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 14a3571dec..70359110ea 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1186,7 +1186,7 @@ MPU6000::measure() * Adjust and scale results to m/s^2. */ grb.timestamp = arb.timestamp = hrt_absolute_time(); - + grb.error_count = arb.error_count = 0; // not reported /* * 1) Scale raw value to SI units using scaling from datasheet. diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 1c8a4d776a..938786d3f1 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -573,6 +573,7 @@ MS5611::collect() struct baro_report report; /* this should be fairly close to the end of the conversion, so the best approximation of the time */ report.timestamp = hrt_absolute_time(); + report.error_count = perf_event_count(_comms_errors); /* read the most recent measurement - read offset/size are hardcoded in the interface */ ret = _interface->read(0, (void *)&raw, 0); From 5d09f48110a1a2ebca6c3240b063963bb8e91bca Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 12 Sep 2013 10:03:54 +0200 Subject: [PATCH 647/872] Disabling debug output for further testing --- 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 70adf6d9d9..8e2ae2d000 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -38,8 +38,8 @@ CONFIG_ARCH_MATH_H=y # # Debug Options # -CONFIG_DEBUG=y -CONFIG_DEBUG_VERBOSE=y +CONFIG_DEBUG=n +CONFIG_DEBUG_VERBOSE=n # # Subsystem Debug Options From a7dddc4dfd3cf8920a162a78db0516c8b8633222 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 12 Sep 2013 10:14:33 +0200 Subject: [PATCH 648/872] Hotfix: Do not start MAVLink as default on telemetry port --- ROMFS/px4fmu_common/init.d/rcS | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 32fb67a454..c8590425e6 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -189,8 +189,9 @@ then if [ $MODE == autostart ] then # Telemetry port is on both FMU boards ttyS1 - mavlink start -b 57600 -d /dev/ttyS1 - usleep 5000 + # but the AR.Drone motors can be get 'flashed' + # if starting MAVLink on them - so do not + # start it as default (default link: USB) # Start commander commander start From a418498f1b040ea57633d02d813112d94c8567b9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 12 Sep 2013 12:51:21 +0200 Subject: [PATCH 649/872] Hotfix: Use sensible default gains for users not being able to read instructions. --- .../multirotor_attitude_control.c | 6 +++--- .../multirotor_att_control/multirotor_rate_control.c | 10 +++++----- .../multirotor_pos_control_params.c | 2 +- 3 files changed, 9 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 c78232f11c..8245aa5609 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -62,15 +62,15 @@ #include #include -PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 0.3f); +PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 2.0f); 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_P, 6.8f); PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f); -PARAM_DEFINE_FLOAT(MC_ATT_D, 0.05f); +PARAM_DEFINE_FLOAT(MC_ATT_D, 0.0f); //PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.05f); //PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.4f); diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index 0a336be47d..adb63186c0 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -59,14 +59,14 @@ #include #include -PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.0f); /* same on Flamewheel */ -PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); -PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); /* same on Flamewheel */ +PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.005f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.2f); //PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.0f); //PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 1.0f); -PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.0f); /* 0.15 F405 Flamewheel */ -PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.0f); +PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.09f); /* 0.15 F405 Flamewheel */ +PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f); PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f); //PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f); //PARAM_DEFINE_FLOAT(MC_ATTRATE_LIM, 1.0f); /**< roughly < 500 deg/s limit */ 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 bf9578ea3d..b7041e4d57 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -54,7 +54,7 @@ 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_XY_VEL_MAX, 5.0f); PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 0.5f); int parameters_init(struct multirotor_position_control_param_handles *h) From 7010674f44feb8037c05965438e6bcbdb8c8d7ee Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 12 Sep 2013 12:56:06 +0200 Subject: [PATCH 650/872] Hotfix: Setting tested defaults for AR.Drone --- ROMFS/px4fmu_common/init.d/08_ardrone | 13 ++++++++++++- ROMFS/px4fmu_common/init.d/09_ardrone_flow | 13 ++++++++++++- 2 files changed, 24 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/08_ardrone b/ROMFS/px4fmu_common/init.d/08_ardrone index f6d82a5ec4..7dbbb82847 100644 --- a/ROMFS/px4fmu_common/init.d/08_ardrone +++ b/ROMFS/px4fmu_common/init.d/08_ardrone @@ -8,7 +8,18 @@ echo "[init] 08_ardrone: PX4FMU on PX4IOAR carrier board" if param compare SYS_AUTOCONFIG 1 then # Set all params here, then disable autoconfig - # TODO + param set MC_ATTRATE_D 0 + param set MC_ATTRATE_I 0 + param set MC_ATTRATE_P 0.13 + param set MC_ATT_D 0 + param set MC_ATT_I 0.3 + param set MC_ATT_P 5 + param set MC_YAWPOS_D 0.1 + param set MC_YAWPOS_I 0.15 + param set MC_YAWPOS_P 1 + param set MC_YAWRATE_D 0 + param set MC_YAWRATE_I 0 + param set MC_YAWRATE_P 0.15 param set SYS_AUTOCONFIG 0 param save diff --git a/ROMFS/px4fmu_common/init.d/09_ardrone_flow b/ROMFS/px4fmu_common/init.d/09_ardrone_flow index 794342a0b1..6333aae564 100644 --- a/ROMFS/px4fmu_common/init.d/09_ardrone_flow +++ b/ROMFS/px4fmu_common/init.d/09_ardrone_flow @@ -8,7 +8,18 @@ echo "[init] 09_ardrone_flow: PX4FMU on PX4IOAR carrier board with PX4FLOW" if param compare SYS_AUTOCONFIG 1 then # Set all params here, then disable autoconfig - # TODO + param set MC_ATTRATE_D 0 + param set MC_ATTRATE_I 0 + param set MC_ATTRATE_P 0.13 + param set MC_ATT_D 0 + param set MC_ATT_I 0.3 + param set MC_ATT_P 5 + param set MC_YAWPOS_D 0.1 + param set MC_YAWPOS_I 0.15 + param set MC_YAWPOS_P 1 + param set MC_YAWRATE_D 0 + param set MC_YAWRATE_I 0 + param set MC_YAWRATE_P 0.15 param set SYS_AUTOCONFIG 0 param save From 379623596715d0dce47192329ae9aceabb9ded11 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 12 Sep 2013 12:58:08 +0200 Subject: [PATCH 651/872] Remove accidentally comitted COM tool --- Tools/com | Bin 13632 -> 0 bytes Tools/com.c | 191 ---------------------------------------------------- 2 files changed, 191 deletions(-) delete mode 100755 Tools/com delete mode 100644 Tools/com.c diff --git a/Tools/com b/Tools/com deleted file mode 100755 index 1d80d4aa5be7643c59635245881cd263a756d9b6..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 13632 zcmeHOdu&_P89$~8q@iFJ$_5*cyNH(d$t1! zT1E^t%EcI2h#;h)iI*5d6AOq!snn1@mLN0@HkgVAn*e32&AO3I8Dw2X_WPZ4uWP$4 z`)B`5x<@+S`CjKc-}%ntCf~h&^VnZLKSK!50wIP@6GHehSAh; zz5ix^^G)QI7EdX8Qi^t#V*tZw^>1%2RgwL*Ql34$fNuish|_JcVI<7{#Btqnd%p89 z8Hf+FBMRvXg(_GfhKfnUh$s3w;{{f^z1)S$UYD)m&`yUc+um{iXwT?2Ls2sZrE+^s z7b$ytZ3Cnk_d+?rzd$?@>k03)70T^ZU##p++j7#yrNlM_hS3+kvnL!hdcs{1Ntdq2 zG20&3oQ3ok6~i#QjjmWnuc@@k{nIZ|{_U}ikj7p+W#K*<#sV5fV?%30k(#FL$PG$1 zXj!;nQMlZ+>#Vh5ghNJrptmy;GNOstDec+arM+UmxSoqqonja}BSqWp1baSB`RCXp zU2gAO#~50}2z7jsf0}J?*zQBR*dFJLtz9UF5r~B2$2}~M->9bi%i29iJFzMlDMF5) z`z@PqyushN**UW{W$OT%rJjcrg?@P?rod9%D^dl(2bK@5v$v?R9ds2sw66vZn)~&9 z3f8E*XZc#tke>-w6Zt5#&r1I}>=M*dmF$&5Ty_%S+2CoYi%?hf#bc{NJ)Nt91EC;j zman4HkLMTu_Rl|ix$ddqCzek>fA2cqrQmTMqN-ZfD;%0Ul;xN&`(?i*$pYXwb*Pld zGr8cWz&yvt#`rA*@r2pC^4gW~qo)&M6#CBb_8>l0XfL0ZUo#2xDjw8;6}iWfExif0{=Y%ZMyYG{obQB`q*uWihAnnnYI~ga4tPK zH!5;(Bfp_%YTv@Y;(GeL!^ie#o>$@}KN#`KP_Rm`OmFXJwbl zr_n`E*Fpat=v)4yY5&YJ4JsQ^MM3*+rF}<1yH#rEsLk9*srGl3wqDS_OlnV2yZ!{* zE0p$E3fi+sc3J)psLj+wsqG4-eRQGY_Zw3CeQFP#p#2^O*3*?^1?|VA_PYqcAtidM zAWBNnJNmtcYIYp-Ua||ugpl!19vy`vuoITbLT8XOm4Y7c%}PCBnT+=N{QRVUh74

hr-lLfRyXjo4PfTy8vTOrY;0?Xc<;%aGDeLW>!%nQ@c&MD3ptO z+7DYkEY)FTnOeMRm%bs4v$KD{7HaY~c8-m=?lyybWu-#oM5)R_)B8B<i zg#%4!(1E^e2P%h{Om*Angn!60;ZF)_y_?n-(Yn;ijIghi1S$C(!$_)~)Y!i;^gd_L zc}K?AdY<)oQwX`pe;?i{8+(Un3I(g5MK|4=%-sh&d+7gUY8Wgj4P=Pw$qz39X}w|n z`t1+&z;4Og|JTa&!@Jpe;Q>n_a;>C^^wudWvx{8=BeF?;BrVAPinIUY%JfSkvj2po z5IrO9>3cXf3(O?<(ji^0J?j7LvMZ=V+l3>-@yU0WRy&OLd&$CrUhHyQZ}>p*Uz z&_PB)fjIVoy=ZORDY<5N=B%RJwAfC!Vh*|W5XeQ`PU`n?1DE^XF&RC5j-K8+tFM2< zO0`1fO)+mQht`vacuDOznEMIzyNd5Gxd?b<;6j?#1ATKkRtsjP(^tQs2e!_YEa=xJ zt=Sz1yH*zW$MZ+C24}6l8LMp?+x!ziZSTYThqoTN-+zP~@D;2QbRiVp)T@BFgaC2H=!9ebPc|wU)6|pxIj(A+Ya&Hl zAa9CU_VnOP?sv1ibmu^c{9^kP>E((yW`#PX^*HSVJ$&g;} zvbWMw#5TwKFE0$5ypb%R!7rg-g z8d~O;J%KjcO=kX2aIM=*?0sUKNd7m())IT2SUs^ZVtm7ze~H+w#GWI@^OJv^SU<5x zi0vZw0I`RN-9zlh#QKT7KrBpboLG?9Tf}ZA_7O4OWZisv=C1>GOG|4*Yg@~X4Vq|f zZPCOgg2t9+fK816t-zW$YvQbGv3gBSjaai5U~L`1`gH)S*OPf=9l5nvNzUq8vdsRD z-e}0&pe>7QtAgeos}hNUmKyDr)l$5pTExvn0xvkVc+@n5(5e=jR&;1)e^26D+NKr# zT5lw1_G+P?uo>6liH<}cG~#B+3?#&zv7Usfb#?TF%%G?aM-tsMl!$0Cvm>}ti^d|I z9i5>8EgFd>v|vvh@5sXFX9ibR%Qr|DV)87-EmlpwHRjT=@{KPGaW3&9^z_Nzuj}?*?SnWQCshOrx@4ieyX^5PyKeXFnyM4%RAG6zM>~_>{ z_uB0tyZx2jzH7H1+3iug{nT#7#f1<{7i(>L)Q-;vn{HpIX7FvB9zRFXjW*3c_gFUD zw75ahH#zhpif(gg-dZTPIP?K0fQs)zHv(=1+z7Z4a3kPGz>R<#0XG6}1l$O?5pW~$ z{}h46OSSgJ4MS&CZBt$^t~dBed`IARBhY=jv9qru78KiJzO3R0%~&idvi1B@R(?5e zbmC)jeCa;)fyzf9ho7aTm?kYjJrWaH5nEnnm^mBrYrqeLB5{*M9SG)qh}Q$}isH-n zL>EaNWKSfJke!-9^qRea=m16A6@@essdiA2s2L{S4IH20+b%>wF)u_(KX8>px^#6- zZ4FsN9`X{gKrbdJV#6d8fv!+ryqk1Z(K}6Kpzu98IFBk0#&tN4lOuCdklV#$;2Y%% zqqiF~GXs4I6Tc1UV8pJGlVhxr(Gu78cJzeB^@&(VINoK(jLw0CDYh^dwLoOAAXq_g zm18!0#n6KBh4tdt9~Ln|dNrz(pjZMP)t1{lrkH5x%%V?>LCR#76r7EoO?XX71N^4k z>+w-8>B}XpzGtSKZ_2ztoZne&bmVsbEubN$sw{1Q#q`cH`mQoMRYpHhMl;n}Z0|>9 z^pj=ub7l04W%Qmhdc2J0n}y=>-Y%oxE2F2%=ugV%V`a2Q83$B_i^&CJI;$F$-@S7+ z&qC$92PToZI!u!@#dR(!bB|m%zH{WOk@Hbe>X`_yLj7OZ)bNW1BiIqcZ%2$S{CXtL z<7S*>TN`pC8zr0DDBaeuqz+}tRU>bN5{|ljBum{p_D!Mgh{Ek)-wV!-z*agZZvp50 etCL@H=AFY{IORZ`;|FtLXv&} diff --git a/Tools/com.c b/Tools/com.c deleted file mode 100644 index fa52dcb61e..0000000000 --- a/Tools/com.c +++ /dev/null @@ -1,191 +0,0 @@ -/* - * Building: cc -o com com.c - * Usage : ./com /dev/device [speed] - * Example : ./com /dev/ttyS0 [115200] - * Keys : Ctrl-A - exit, Ctrl-X - display control lines status - * Darcs : darcs get http://tinyserial.sf.net/ - * Homepage: http://tinyserial.sourceforge.net - * Version : 2009-03-05 - * - * Ivan Tikhonov, http://www.brokestream.com, kefeer@brokestream.com - * Patches by Jim Kou, Henry Nestler, Jon Miner, Alan Horstmann - * - */ - - -/* Copyright (C) 2007 Ivan Tikhonov - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. - - Ivan Tikhonov, kefeer@brokestream.com - -*/ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -int transfer_byte(int from, int to, int is_control); - -typedef struct {char *name; int flag; } speed_spec; - - -void print_status(int fd) { - int status; - unsigned int arg; - status = ioctl(fd, TIOCMGET, &arg); - fprintf(stderr, "[STATUS]: "); - if(arg & TIOCM_RTS) fprintf(stderr, "RTS "); - if(arg & TIOCM_CTS) fprintf(stderr, "CTS "); - if(arg & TIOCM_DSR) fprintf(stderr, "DSR "); - if(arg & TIOCM_CAR) fprintf(stderr, "DCD "); - if(arg & TIOCM_DTR) fprintf(stderr, "DTR "); - if(arg & TIOCM_RNG) fprintf(stderr, "RI "); - fprintf(stderr, "\r\n"); -} - -int main(int argc, char *argv[]) -{ - int comfd; - struct termios oldtio, newtio; //place for old and new port settings for serial port - struct termios oldkey, newkey; //place tor old and new port settings for keyboard teletype - char *devicename = argv[1]; - int need_exit = 0; - speed_spec speeds[] = - { - {"1200", B1200}, - {"2400", B2400}, - {"4800", B4800}, - {"9600", B9600}, - {"19200", B19200}, - {"38400", B38400}, - {"57600", B57600}, - {"115200", B115200}, - {NULL, 0} - }; - int speed = B9600; - - if(argc < 2) { - fprintf(stderr, "example: %s /dev/ttyS0 [115200]\n", argv[0]); - exit(1); - } - - comfd = open(devicename, O_RDWR | O_NOCTTY | O_NONBLOCK); - if (comfd < 0) - { - perror(devicename); - exit(-1); - } - - if(argc > 2) { - speed_spec *s; - for(s = speeds; s->name; s++) { - if(strcmp(s->name, argv[2]) == 0) { - speed = s->flag; - fprintf(stderr, "setting speed %s\n", s->name); - break; - } - } - } - - fprintf(stderr, "C-a exit, C-x modem lines status\n"); - - tcgetattr(STDIN_FILENO,&oldkey); - newkey.c_cflag = B9600 | CRTSCTS | CS8 | CLOCAL | CREAD; - newkey.c_iflag = IGNPAR; - newkey.c_oflag = 0; - newkey.c_lflag = 0; - newkey.c_cc[VMIN]=1; - newkey.c_cc[VTIME]=0; - tcflush(STDIN_FILENO, TCIFLUSH); - tcsetattr(STDIN_FILENO,TCSANOW,&newkey); - - - tcgetattr(comfd,&oldtio); // save current port settings - newtio.c_cflag = speed | CS8 | CLOCAL | CREAD; - newtio.c_iflag = IGNPAR; - newtio.c_oflag = 0; - newtio.c_lflag = 0; - newtio.c_cc[VMIN]=1; - newtio.c_cc[VTIME]=0; - tcflush(comfd, TCIFLUSH); - tcsetattr(comfd,TCSANOW,&newtio); - - print_status(comfd); - - while(!need_exit) { - fd_set fds; - int ret; - - FD_ZERO(&fds); - FD_SET(STDIN_FILENO, &fds); - FD_SET(comfd, &fds); - - - ret = select(comfd+1, &fds, NULL, NULL, NULL); - if(ret == -1) { - perror("select"); - } else if (ret > 0) { - if(FD_ISSET(STDIN_FILENO, &fds)) { - need_exit = transfer_byte(STDIN_FILENO, comfd, 1); - } - if(FD_ISSET(comfd, &fds)) { - need_exit = transfer_byte(comfd, STDIN_FILENO, 0); - } - } - } - - tcsetattr(comfd,TCSANOW,&oldtio); - tcsetattr(STDIN_FILENO,TCSANOW,&oldkey); - close(comfd); - - return 0; -} - - -int transfer_byte(int from, int to, int is_control) { - char c; - int ret; - do { - ret = read(from, &c, 1); - } while (ret < 0 && errno == EINTR); - if(ret == 1) { - if(is_control) { - if(c == '\x01') { // C-a - return -1; - } else if(c == '\x18') { // C-x - print_status(to); - return 0; - } - } - while(write(to, &c, 1) == -1) { - if(errno!=EAGAIN && errno!=EINTR) { perror("write failed"); break; } - } - } else { - fprintf(stderr, "\nnothing to read. probably port disconnected.\n"); - return -2; - } - return 0; -} - From 8fbf698e623bf7aa1f651ded574ec2e557beed32 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 12 Sep 2013 14:02:30 +0200 Subject: [PATCH 652/872] Adding missing author info and acknowledgements --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 1 + src/lib/ecl/attitude_fw/ecl_pitch_controller.h | 6 ++++++ src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 1 + src/lib/ecl/attitude_fw/ecl_roll_controller.h | 6 ++++++ src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 1 + 5 files changed, 15 insertions(+) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 44b339ce52..d876f1a39b 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -35,6 +35,7 @@ * @file ecl_pitch_controller.cpp * Implementation of a simple orthogonal pitch PID controller. * + * Authors and acknowledgements in header. */ #include "ecl_pitch_controller.h" diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h index 7fbfd6fbcb..6217eb9d0a 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h @@ -35,6 +35,12 @@ * @file ecl_pitch_controller.h * Definition of a simple orthogonal pitch PID controller. * + * @author Lorenz Meier + * + * Acknowledgements: + * + * The control design is based on a design + * by Paul Riseborough, 2013. */ #ifndef ECL_PITCH_CONTROLLER_H diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index f3a8585ae2..b9a73fc029 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -35,6 +35,7 @@ * @file ecl_roll_controller.cpp * Implementation of a simple orthogonal roll PID controller. * + * Authors and acknowledgements in header. */ #include "../ecl.h" diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h index 3427b67c20..a6d30d2108 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h @@ -35,6 +35,12 @@ * @file ecl_roll_controller.h * Definition of a simple orthogonal roll PID controller. * + * @author Lorenz Meier + * + * Acknowledgements: + * + * The control design is based on a design + * by Paul Riseborough, 2013. */ #ifndef ECL_ROLL_CONTROLLER_H diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 2b7429996d..b786acf24f 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -35,6 +35,7 @@ * @file ecl_yaw_controller.cpp * Implementation of a simple orthogonal coordinated turn yaw PID controller. * + * Authors and acknowledgements in header. */ #include "ecl_yaw_controller.h" From d84fe2913e40e91ce6d7530b438dbe18db49ec01 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 13 Sep 2013 01:34:49 +0200 Subject: [PATCH 653/872] Move IRQ restore to right position --- src/drivers/lsm303d/lsm303d.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index a90cd0a3d1..0b0906d9ee 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -532,6 +532,7 @@ LSM303D::reset() /* enable mag */ write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); + irqrestore(flags); accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE); @@ -540,7 +541,6 @@ LSM303D::reset() mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA); mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE); - irqrestore(flags); _accel_read = 0; _mag_read = 0; @@ -1013,6 +1013,7 @@ LSM303D::accel_set_range(unsigned max_g) _accel_range_scale = new_scale_g_digit * LSM303D_ONE_G; + modify_reg(ADDR_CTRL_REG2, clearbits, setbits); return OK; From 41610ff7dd6233853270e921828c815797fd6aeb Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Thu, 12 Sep 2013 21:36:20 -0400 Subject: [PATCH 654/872] DSM pairing cleanup in px4io.cpp - Simplify parameter range checking in dsm_bind_ioctl - Replace DSM magic numbers with symbolic constants --- src/drivers/drv_pwm_output.h | 3 +++ src/drivers/px4io/px4io.cpp | 26 +++++++++++++------------- src/modules/sensors/sensor_params.c | 2 +- 3 files changed, 17 insertions(+), 14 deletions(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index ec9d4ca098..94e923d71a 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -118,6 +118,9 @@ ORB_DECLARE(output_pwm); /** start DSM bind */ #define DSM_BIND_START _IOC(_PWM_SERVO_BASE, 7) +#define DSM2_BIND_PULSES 3 /* DSM_BIND_START ioctl parameter, pulses required to start dsm2 pairing */ +#define DSMX_BIND_PULSES 7 /* DSM_BIND_START ioctl parameter, pulses required to start dsmx pairing */ + /** Power up DSM receiver */ #define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 8) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 56a2940982..6e3a39aaed 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -415,10 +415,9 @@ private: /** * Handle issuing dsm bind ioctl to px4io. * - * @param valid true: the dsm mode is in range - * @param isDsm2 true: dsm2m false: dsmx + * @param dsmMode 0:dsm2, 1:dsmx */ - void dsm_bind_ioctl(bool valid, bool isDsm2); + void dsm_bind_ioctl(int dsmMode); }; @@ -816,7 +815,7 @@ PX4IO::task_main() orb_copy(ORB_ID(vehicle_command), _t_vehicle_command, &cmd); // Check for a DSM pairing command if ((cmd.command == VEHICLE_CMD_START_RX_PAIR) && (cmd.param1== 0.0f)) { - dsm_bind_ioctl((cmd.param2 == 0.0f) || (cmd.param2 == 1.0f), cmd.param2 == 0.0f); + dsm_bind_ioctl((int)cmd.param2); } } @@ -857,8 +856,8 @@ PX4IO::task_main() // See if bind parameter has been set, and reset it to 0 param_get(dsm_bind_param = param_find("RC_DSM_BIND"), &dsm_bind_val); if (dsm_bind_val) { - dsm_bind_ioctl((dsm_bind_val == 1) || (dsm_bind_val == 2), dsm_bind_val == 1); - dsm_bind_val = 0; + dsm_bind_ioctl(dsm_bind_val); + dsm_bind_val = -1; param_set(dsm_bind_param, &dsm_bind_val); } @@ -1166,14 +1165,15 @@ PX4IO::io_handle_status(uint16_t status) } void -PX4IO::dsm_bind_ioctl(bool valid, bool isDsm2) +PX4IO::dsm_bind_ioctl(int dsmMode) { if (!(_status & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { - if (valid) { - mavlink_log_info(_thread_mavlink_fd, "[IO] binding dsm%c rx", isDsm2 ? '2' : 'x'); - ioctl(nullptr, DSM_BIND_START, isDsm2 ? 3 : 7); + /* 0: dsm2, 1:dsmx */ + if ((dsmMode >= 0) && (dsmMode <= 1)) { + mavlink_log_info(_thread_mavlink_fd, "[IO] binding dsm%c rx", (dsmMode == 0) ? '2' : 'x'); + ioctl(nullptr, DSM_BIND_START, (dsmMode == 0) ? DSM2_BIND_PULSES : DSMX_BIND_PULSES); } else { - mavlink_log_info(_thread_mavlink_fd, "[IO] invalid bind type, bind request rejected"); + mavlink_log_info(_thread_mavlink_fd, "[IO] invalid dsm bind mode, bind request rejected"); } } else { mavlink_log_info(_thread_mavlink_fd, "[IO] system armed, bind request rejected"); @@ -2035,9 +2035,9 @@ bind(int argc, char *argv[]) errx(0, "needs argument, use dsm2 or dsmx"); if (!strcmp(argv[2], "dsm2")) - pulses = 3; + pulses = DSM2_BIND_PULSES; else if (!strcmp(argv[2], "dsmx")) - pulses = 7; + pulses = DSMX_BIND_PULSES; else errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]); diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 992abf2ccf..950af2eba2 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -166,7 +166,7 @@ PARAM_DEFINE_FLOAT(RC15_DZ, 0.0f); 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 */ +PARAM_DEFINE_INT32(RC_DSM_BIND, -1); /* -1 = Idle, 0 = Start DSM2 bind, 1 = Start DSMX bind */ #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); From 19fdaf2009d41885923b586432cb2506a24ca5b3 Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 12 Sep 2013 23:56:53 -0700 Subject: [PATCH 655/872] Use the generic device::SPI locking strategy. --- src/drivers/device/spi.cpp | 34 ++++++++++++++++++++++++++----- src/drivers/device/spi.h | 11 ++++++++++ src/drivers/l3gd20/l3gd20.cpp | 3 --- src/drivers/lsm303d/lsm303d.cpp | 5 ----- src/drivers/ms5611/ms5611_spi.cpp | 16 +-------------- 5 files changed, 41 insertions(+), 28 deletions(-) diff --git a/src/drivers/device/spi.cpp b/src/drivers/device/spi.cpp index 8fffd60cb4..fa6b78d640 100644 --- a/src/drivers/device/spi.cpp +++ b/src/drivers/device/spi.cpp @@ -67,6 +67,7 @@ SPI::SPI(const char *name, CDev(name, devname, irq), // public // protected + locking_mode(LOCK_PREEMPTION), // private _bus(bus), _device(device), @@ -132,13 +133,25 @@ SPI::probe() int SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len) { + irqstate_t state; if ((send == nullptr) && (recv == nullptr)) return -EINVAL; - /* do common setup */ - if (!up_interrupt_context()) - SPI_LOCK(_dev, true); + /* lock the bus as required */ + if (!up_interrupt_context()) { + switch (locking_mode) { + default: + case LOCK_PREEMPTION: + state = irqsave(); + break; + case LOCK_THREADS: + SPI_LOCK(_dev, true); + break; + case LOCK_NONE: + break; + } + } SPI_SETFREQUENCY(_dev, _frequency); SPI_SETMODE(_dev, _mode); @@ -151,8 +164,19 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len) /* and clean up */ SPI_SELECT(_dev, _device, false); - if (!up_interrupt_context()) - SPI_LOCK(_dev, false); + if (!up_interrupt_context()) { + switch (locking_mode) { + default: + case LOCK_PREEMPTION: + irqrestore(state); + break; + case LOCK_THREADS: + SPI_LOCK(_dev, false); + break; + case LOCK_NONE: + break; + } + } return OK; } diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h index e0122372a0..9103dca2ea 100644 --- a/src/drivers/device/spi.h +++ b/src/drivers/device/spi.h @@ -101,6 +101,17 @@ protected: */ int transfer(uint8_t *send, uint8_t *recv, unsigned len); + /** + * Locking modes supported by the driver. + */ + enum LockMode { + LOCK_PREEMPTION, /**< the default; lock against all forms of preemption. */ + LOCK_THREADS, /**< lock only against other threads, using SPI_LOCK */ + LOCK_NONE /**< perform no locking, only safe if the bus is entirely private */ + }; + + LockMode locking_mode; /**< selected locking mode */ + private: int _bus; enum spi_dev_e _device; diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 4c3b0ce514..748809d3f2 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -367,7 +367,6 @@ out: int L3GD20::probe() { - irqstate_t flags = irqsave(); /* read dummy value to void to clear SPI statemachine on sensor */ (void)read_reg(ADDR_WHO_AM_I); @@ -393,8 +392,6 @@ L3GD20::probe() success = true; } - irqrestore(flags); - if (success) return OK; diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index a90cd0a3d1..fcdf2768bd 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -525,7 +525,6 @@ out: void LSM303D::reset() { - irqstate_t flags = irqsave(); /* enable accel*/ write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE); @@ -540,7 +539,6 @@ LSM303D::reset() mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA); mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE); - irqrestore(flags); _accel_read = 0; _mag_read = 0; @@ -549,15 +547,12 @@ LSM303D::reset() int LSM303D::probe() { - irqstate_t flags = irqsave(); /* 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 */ bool success = (read_reg(ADDR_WHO_AM_I) == WHO_I_AM); - irqrestore(flags); - if (success) return OK; diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 21caed2ffc..e547c913ba 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -142,23 +142,15 @@ MS5611_SPI::init() goto out; } - /* disable interrupts, make this section atomic */ - flags = irqsave(); /* send reset command */ ret = _reset(); - /* re-enable interrupts */ - irqrestore(flags); if (ret != OK) { debug("reset failed"); goto out; } - /* disable interrupts, make this section atomic */ - flags = irqsave(); /* read PROM */ ret = _read_prom(); - /* re-enable interrupts */ - irqrestore(flags); if (ret != OK) { debug("prom readout failed"); goto out; @@ -270,13 +262,7 @@ MS5611_SPI::_reg16(unsigned reg) 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; + return transfer(send, recv, len); } #endif /* PX4_SPIDEV_BARO */ From e3864e7dbbe03a4ae7bdc9eb5ea12123477dd07f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 13 Sep 2013 09:55:06 +0200 Subject: [PATCH 656/872] Trust on the microSD for params for now --- ROMFS/px4fmu_common/init.d/rcS | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 4c4b0129ed..cd4d4487d9 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -73,20 +73,20 @@ then # # Load microSD params # - if ramtron start - then - param select /ramtron/params - if [ -f /ramtron/params ] - then - param load /ramtron/params - fi - else + #if ramtron start + #then + # param select /ramtron/params + # if [ -f /ramtron/params ] + # 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 + #fi # # Start system state indicator From 24648b529402abb665c1b7c0064a9ee3150999fe Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 13 Sep 2013 10:16:32 +0200 Subject: [PATCH 657/872] Minor cleanups in the drivers --- src/drivers/hmc5883/hmc5883.cpp | 4 ---- src/drivers/lsm303d/lsm303d.cpp | 1 - 2 files changed, 5 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 58a1593eda..5e891d7bbc 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1292,10 +1292,6 @@ test() if (fd < 0) err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH); - /* set the queue depth to 10 */ - if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) - errx(1, "failed to set queue depth"); - /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 83919d2631..2c56b60358 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -531,7 +531,6 @@ LSM303D::reset() /* enable mag */ write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); - irqrestore(flags); accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE); From ecbb319153955dac3287548703d40ba866ddf070 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 13 Sep 2013 15:06:59 +0200 Subject: [PATCH 658/872] Disable low console for v2, should not be enabled in parallel with normal console --- 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 0615950a2d..2867f9113f 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -523,7 +523,7 @@ CONFIG_PIPES=y # CONFIG_POWER is not set # CONFIG_SENSORS is not set CONFIG_SERIAL=y -CONFIG_DEV_LOWCONSOLE=y +# CONFIG_DEV_LOWCONSOLE is not set CONFIG_SERIAL_REMOVABLE=y # CONFIG_16550_UART is not set CONFIG_ARCH_HAVE_UART4=y From 318abfabcb1e0d8d23df3df8015d4c7bdc011b2a Mon Sep 17 00:00:00 2001 From: marco Date: Fri, 13 Sep 2013 21:05:41 +0200 Subject: [PATCH 659/872] mkblctrl fix and qgroundcontrol2 startup script for different frametypes --- .../init.d/rc.custom_dji_f330_mkblctrl | 122 ++++++++++++++++++ ROMFS/px4fmu_common/init.d/rcS | 36 ++++++ src/drivers/mkblctrl/mkblctrl.cpp | 7 +- 3 files changed, 164 insertions(+), 1 deletion(-) create mode 100644 ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl new file mode 100644 index 0000000000..51bc61c9e8 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl @@ -0,0 +1,122 @@ +#!nsh + +# +# 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.002 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_P 0.09 + param set MC_ATT_D 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_P 6.8 + param set MC_YAWPOS_D 0.0 + 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 set NAV_TAKEOFF_ALT 3.0 + param set MPC_TILT_MAX 0.5 + param set MPC_THR_MAX 0.7 + param set MPC_THR_MIN 0.3 + param set MPC_XY_D 0 + param set MPC_XY_P 0.5 + param set MPC_XY_VEL_D 0 + param set MPC_XY_VEL_I 0 + param set MPC_XY_VEL_MAX 3 + param set MPC_XY_VEL_P 0.2 + param set MPC_Z_D 0 + param set MPC_Z_P 1 + param set MPC_Z_VEL_D 0 + param set MPC_Z_VEL_I 0.1 + param set MPC_Z_VEL_MAX 2 + param set MPC_Z_VEL_P 0.20 + + param save +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 2 = quadrotor +# +param set MAV_TYPE 2 + +set EXIT_ON_END no + +# +# Start the Mikrokopter ESC driver +# +if [ $MKBLCTRL_MODE == yes ] +then + if [ $MKBLCTRL_FRAME == x ] + then + echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame Mikrokopter-Addressing" + mkblctrl -mkmode x + else + echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame Mikrokopter-Addressing" + mkblctrl -mkmode + + fi +else + if [ $MKBLCTRL_FRAME == x ] + then + echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame" + else + echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame" + fi + mkblctrl +fi + +usleep 10000 + +# +# Start and configure PX4IO or FMU interface +# +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start + usleep 5000 + + sh /etc/init.d/rc.io + # Set PWM values for DJI ESCs + px4io idle 900 900 900 900 + px4io min 1200 1200 1200 1200 + px4io max 1800 1800 1800 1800 +else + fmu mode_pwm + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + usleep 5000 + param set BAT_V_SCALING 0.004593 + set EXIT_ON_END yes +fi + +# +# Load mixer +# +if [ $MKBLCTRL_FRAME == x ] +then + mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix +else + mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix +fi + +# +# Set PWM output frequency +# +#pwm -u 400 -m 0xff + +# +# Start common for all multirotors apps +# +sh /etc/init.d/rc.multirotor + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index cd4d4487d9..b7f1329dd2 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -172,6 +172,42 @@ then sh /etc/init.d/16_3dr_iris set MODE custom fi + + # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame + if param compare SYS_AUTOSTART 17 + then + set MKBLCTRL_MODE no + set MKBLCTRL_FRAME x + sh /etc/init.d/rc.custom_dji_f330_mkblctrl + set MODE custom + fi + + # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame + if param compare SYS_AUTOSTART 18 + then + set MKBLCTRL_MODE no + set MKBLCTRL_FRAME + + sh /etc/init.d/rc.custom_dji_f330_mkblctrl + set MODE custom + fi + + # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame Mikrokopter-Addressing + if param compare SYS_AUTOSTART 19 + then + set MKBLCTRL_MODE yes + set MKBLCTRL_FRAME x + sh /etc/init.d/rc.custom_dji_f330_mkblctrl + set MODE custom + fi + + # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame Mikrokopter-Addressing + if param compare SYS_AUTOSTART 20 + then + set MKBLCTRL_MODE yes + set MKBLCTRL_FRAME + + sh /etc/init.d/rc.custom_dji_f330_mkblctrl + set MODE custom + fi if param compare SYS_AUTOSTART 30 then diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 1bc3e97a41..d0de26a1ad 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -96,9 +96,10 @@ class MK : public device::I2C { public: enum Mode { + MODE_NONE, MODE_2PWM, MODE_4PWM, - MODE_NONE + MODE_6PWM, }; enum MappingMode { @@ -1023,9 +1024,11 @@ MK::ioctl(file *filp, int cmd, unsigned long arg) return ret; /* if we are in valid PWM mode, try it as a PWM ioctl as well */ + /* switch (_mode) { case MODE_2PWM: case MODE_4PWM: + case MODE_6PWM: ret = pwm_ioctl(filp, cmd, arg); break; @@ -1033,6 +1036,8 @@ MK::ioctl(file *filp, int cmd, unsigned long arg) debug("not in a PWM mode"); break; } + */ + ret = pwm_ioctl(filp, cmd, arg); /* if nobody wants it, let CDev have it */ if (ret == -ENOTTY) From 8a70efdf43706b42e552a131e332789c114c7d4b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 14 Sep 2013 08:55:45 +0200 Subject: [PATCH 660/872] Beep and print message when arming is not allowed --- src/modules/commander/commander.cpp | 104 +++++++++++++++++----------- 1 file changed, 65 insertions(+), 39 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5eeb018fd5..a2443b0f65 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -369,7 +369,13 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe transition_result_t arming_res = TRANSITION_NOT_CHANGED; if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed); + if (safety->safety_switch_available && !safety->safety_off) { + print_reject_arm("NOT ARMING: Press safety switch first."); + arming_res = TRANSITION_DENIED; + + } else { + 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"); @@ -561,7 +567,7 @@ int commander_thread_main(int argc, char *argv[]) /* flags for control apps */ struct vehicle_control_mode_s control_mode; - + /* Initialize all flags to false */ memset(&control_mode, 0, sizeof(control_mode)); @@ -1083,10 +1089,18 @@ int commander_thread_main(int argc, char *argv[]) /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ if (status.arming_state == ARMING_STATE_STANDBY && - 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); + if (safety.safety_switch_available && !safety.safety_off) { + print_reject_arm("NOT ARMING: Press safety switch first."); + + } else if (status.main_state != MAIN_STATE_MANUAL) { + print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); + + } else { + res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); + } + stick_on_counter = 0; } else { @@ -1106,15 +1120,8 @@ int commander_thread_main(int argc, char *argv[]) } } 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); - } + 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 */ @@ -1228,6 +1235,7 @@ int commander_thread_main(int argc, char *argv[]) /* wait for threads to complete */ ret = pthread_join(commander_low_prio_thread, NULL); + if (ret) { warn("join failed", ret); } @@ -1295,8 +1303,10 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang if (armed->armed) { rgbled_set_mode(RGBLED_MODE_ON); + } else if (armed->ready_to_arm) { rgbled_set_mode(RGBLED_MODE_BREATHE); + } else { rgbled_set_mode(RGBLED_MODE_BLINK_FAST); } @@ -1304,29 +1314,35 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) { switch (status->battery_warning) { - case VEHICLE_BATTERY_WARNING_LOW: - rgbled_set_color(RGBLED_COLOR_YELLOW); - break; - case VEHICLE_BATTERY_WARNING_CRITICAL: - rgbled_set_color(RGBLED_COLOR_AMBER); - break; - default: - break; + case VEHICLE_BATTERY_WARNING_LOW: + rgbled_set_color(RGBLED_COLOR_YELLOW); + break; + + case VEHICLE_BATTERY_WARNING_CRITICAL: + rgbled_set_color(RGBLED_COLOR_AMBER); + break; + + default: + break; } + } else { switch (status->main_state) { - case MAIN_STATE_MANUAL: - rgbled_set_color(RGBLED_COLOR_WHITE); - break; - case MAIN_STATE_SEATBELT: - case MAIN_STATE_EASY: - rgbled_set_color(RGBLED_COLOR_GREEN); - break; - case MAIN_STATE_AUTO: - rgbled_set_color(RGBLED_COLOR_BLUE); - break; - default: - break; + case MAIN_STATE_MANUAL: + rgbled_set_color(RGBLED_COLOR_WHITE); + break; + + case MAIN_STATE_SEATBELT: + case MAIN_STATE_EASY: + rgbled_set_color(RGBLED_COLOR_GREEN); + break; + + case MAIN_STATE_AUTO: + rgbled_set_color(RGBLED_COLOR_BLUE); + break; + + default: + break; } } @@ -1497,16 +1513,18 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c return TRANSITION_NOT_CHANGED; } } + if (status->navigation_state != NAVIGATION_STATE_AUTO_TAKEOFF && - 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_LOITER && + status->navigation_state != NAVIGATION_STATE_AUTO_MISSION && + status->navigation_state != NAVIGATION_STATE_AUTO_RTL) { /* possibly on ground, switch to TAKEOFF if needed */ if (local_pos->z > -takeoff_alt || status->condition_landed) { res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); return res; } } + /* switch to AUTO mode */ if (status->rc_signal_found_once && !status->rc_signal_lost) { /* act depending on switches when manual control enabled */ @@ -1524,18 +1542,20 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_LOITER, control_mode); } } + } else { /* switch to MISSION when no RC control and first time in some AUTO mode */ 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) { + 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 { /* disarmed, always switch to AUTO_READY */ res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode); @@ -1762,35 +1782,41 @@ void *commander_low_prio_loop(void *arg) if (((int)(cmd.param1)) == 0) { int ret = param_load_default(); + if (ret == OK) { 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"); + /* convenience as many parts of NuttX use negative errno */ if (ret < 0) ret = -ret; if (ret < 1000) mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret)); + answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); } } else if (((int)(cmd.param1)) == 1) { int ret = param_save_default(); + if (ret == OK) { 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"); + /* convenience as many parts of NuttX use negative errno */ if (ret < 0) ret = -ret; if (ret < 1000) mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret)); + answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); } } From a0d26cb282145f553aaf2f65d5feadd5c89941e3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Sep 2013 16:25:38 +0200 Subject: [PATCH 661/872] Make mixer upload not depending on single serial transfer error --- src/drivers/px4io/px4io.cpp | 85 ++++++++++++++++++++----------------- 1 file changed, 47 insertions(+), 38 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 6e3a39aaed..3e83fcad85 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -392,7 +392,7 @@ private: /** * Send mixer definition text to IO */ - int mixer_send(const char *buf, unsigned buflen); + int mixer_send(const char *buf, unsigned buflen, unsigned retries=3); /** * Handle a status update from IO. @@ -1468,61 +1468,70 @@ PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t } int -PX4IO::mixer_send(const char *buf, unsigned buflen) +PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) { - uint8_t frame[_max_transfer]; - px4io_mixdata *msg = (px4io_mixdata *)&frame[0]; - unsigned max_len = _max_transfer - sizeof(px4io_mixdata); - msg->f2i_mixer_magic = F2I_MIXER_MAGIC; - msg->action = F2I_MIXER_ACTION_RESET; + uint8_t frame[_max_transfer]; do { - unsigned count = buflen; - if (count > max_len) - count = max_len; + px4io_mixdata *msg = (px4io_mixdata *)&frame[0]; + unsigned max_len = _max_transfer - sizeof(px4io_mixdata); - if (count > 0) { - memcpy(&msg->text[0], buf, count); - buf += count; - buflen -= count; - } + msg->f2i_mixer_magic = F2I_MIXER_MAGIC; + msg->action = F2I_MIXER_ACTION_RESET; - /* - * We have to send an even number of bytes. This - * will only happen on the very last transfer of a - * mixer, and we are guaranteed that there will be - * space left to round up as _max_transfer will be - * even. - */ - unsigned total_len = sizeof(px4io_mixdata) + count; - if (total_len % 1) { - msg->text[count] = '\0'; - total_len++; - } + do { + unsigned count = buflen; - int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2); + if (count > max_len) + count = max_len; - if (ret) { - log("mixer send error %d", ret); - return ret; - } + if (count > 0) { + memcpy(&msg->text[0], buf, count); + buf += count; + buflen -= count; + } - msg->action = F2I_MIXER_ACTION_APPEND; + /* + * We have to send an even number of bytes. This + * will only happen on the very last transfer of a + * mixer, and we are guaranteed that there will be + * space left to round up as _max_transfer will be + * even. + */ + unsigned total_len = sizeof(px4io_mixdata) + count; + if (total_len % 1) { + msg->text[count] = '\0'; + total_len++; + } - } while (buflen > 0); + int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2); + + if (ret) { + log("mixer send error %d", ret); + return ret; + } + + msg->action = F2I_MIXER_ACTION_APPEND; + + } while (buflen > 0); + + retries--; + + log("mixer sent"); + + } while (retries > 0 && (!(io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK))); /* check for the mixer-OK flag */ if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK) { - debug("mixer upload OK"); mavlink_log_info(_mavlink_fd, "[IO] mixer upload ok"); return 0; - } else { - debug("mixer rejected by IO"); - mavlink_log_info(_mavlink_fd, "[IO] mixer upload fail"); } + log("mixer rejected by IO"); + mavlink_log_info(_mavlink_fd, "[IO] mixer upload fail"); + /* load must have failed for some reason */ return -EINVAL; } From 2fbd23cf6ea535adccdeacfd12af731570524fd4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 14 Sep 2013 17:31:58 +0200 Subject: [PATCH 662/872] sdlog2: position & velocity valid, postion global and landed flags added to LPOS, some refactoring --- .../position_estimator_inav_main.c | 10 +++++----- src/modules/sdlog2/sdlog2.c | 3 +++ src/modules/sdlog2/sdlog2_messages.h | 5 ++++- src/modules/uORB/topics/vehicle_local_position.h | 4 ++-- 4 files changed, 14 insertions(+), 8 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 932f610886..10007bf960 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -259,7 +259,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.ref_timestamp = hrt_absolute_time(); local_pos.z_valid = true; local_pos.v_z_valid = true; - local_pos.global_z = true; + local_pos.z_global = true; } } } @@ -597,7 +597,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.timestamp = t; 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.xy_global = 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]; @@ -610,9 +610,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); /* publish global position */ - global_pos.valid = local_pos.global_xy; + global_pos.valid = local_pos.xy_global; - if (local_pos.global_xy) { + if (local_pos.xy_global) { 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); @@ -630,7 +630,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) global_pos.relative_alt = -local_pos.z; } - if (local_pos.global_z) { + if (local_pos.z_global) { global_pos.alt = local_pos.ref_alt - local_pos.z; } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index e83fb7dd3a..ec8033202d 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1046,6 +1046,9 @@ int sdlog2_thread_main(int argc, char *argv[]) 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; + log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0); + log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0); + log_msg.body.log_LPOS.landed = buf.local_pos.landed; LOGBUFFER_WRITE_AND_COUNT(LPOS); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 1343bb3d0e..0e88da0547 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -109,6 +109,9 @@ struct log_LPOS_s { int32_t ref_lat; int32_t ref_lon; float ref_alt; + uint8_t xy_flags; + uint8_t z_flags; + uint8_t landed; }; /* --- LPSP - LOCAL POSITION SETPOINT --- */ @@ -278,7 +281,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, "ffffffLLf", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt"), + LOG_FORMAT(LPOS, "ffffffLLfBBB", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt,XYFlags,ZFlags,Landed"), 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 31a0e632b2..4271537829 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -70,8 +70,8 @@ struct vehicle_local_position_s /* 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) */ + bool xy_global; /**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */ + bool z_global; /**< 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 */ From 2678aba9fe2a7f713dc48e8a362cd28d891b3675 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 14 Sep 2013 21:41:35 -0700 Subject: [PATCH 663/872] A bit more NuttX gdb/python tooling to recover an interrupted context. Needs more fleshing out. --- Debug/Nuttx.py | 173 +++++++++++++++++++++++++++++++++++-------------- 1 file changed, 123 insertions(+), 50 deletions(-) diff --git a/Debug/Nuttx.py b/Debug/Nuttx.py index b0864a2293..093edd0e09 100644 --- a/Debug/Nuttx.py +++ b/Debug/Nuttx.py @@ -2,6 +2,106 @@ import gdb, gdb.types +class NX_register_set(object): + """Copy of the registers for a given context""" + + v7_regmap = { + 'R13': 0, + 'SP': 0, + 'PRIORITY': 1, + 'R4': 2, + 'R5': 3, + 'R6': 4, + 'R7': 5, + 'R8': 6, + 'R9': 7, + 'R10': 8, + 'R11': 9, + 'EXC_RETURN': 10, + 'R0': 11, + 'R1': 12, + 'R2': 13, + 'R3': 14, + 'R12': 15, + 'R14': 16, + 'LR': 16, + 'R15': 17, + 'PC': 17, + 'XPSR': 18, + } + + v7em_regmap = { + 'R13': 0, + 'SP': 0, + 'PRIORITY': 1, + 'R4': 2, + 'R5': 3, + 'R6': 4, + 'R7': 5, + 'R8': 6, + 'R9': 7, + 'R10': 8, + 'R11': 9, + 'EXC_RETURN': 10, + 'R0': 27, + 'R1': 28, + 'R2': 29, + 'R3': 30, + 'R12': 31, + 'R14': 32, + 'LR': 32, + 'R15': 33, + 'PC': 33, + 'XPSR': 34, + } + + regs = dict() + + def __init__(self, xcpt_regs): + if xcpt_regs is None: + self.regs['R0'] = long(gdb.parse_and_eval('$r0')) + self.regs['R1'] = long(gdb.parse_and_eval('$r1')) + self.regs['R2'] = long(gdb.parse_and_eval('$r2')) + self.regs['R3'] = long(gdb.parse_and_eval('$r3')) + self.regs['R4'] = long(gdb.parse_and_eval('$r4')) + self.regs['R5'] = long(gdb.parse_and_eval('$r5')) + self.regs['R6'] = long(gdb.parse_and_eval('$r6')) + self.regs['R7'] = long(gdb.parse_and_eval('$r7')) + self.regs['R8'] = long(gdb.parse_and_eval('$r8')) + self.regs['R9'] = long(gdb.parse_and_eval('$r9')) + self.regs['R10'] = long(gdb.parse_and_eval('$r10')) + self.regs['R11'] = long(gdb.parse_and_eval('$r11')) + self.regs['R12'] = long(gdb.parse_and_eval('$r12')) + self.regs['R13'] = long(gdb.parse_and_eval('$r13')) + self.regs['SP'] = long(gdb.parse_and_eval('$sp')) + self.regs['R14'] = long(gdb.parse_and_eval('$r14')) + self.regs['LR'] = long(gdb.parse_and_eval('$lr')) + self.regs['R15'] = long(gdb.parse_and_eval('$r15')) + self.regs['PC'] = long(gdb.parse_and_eval('$pc')) + self.regs['XPSR'] = long(gdb.parse_and_eval('$xpsr')) + else: + for key in self.v7em_regmap.keys(): + self.regs[key] = int(xcpt_regs[self.v7em_regmap[key]]) + + + @classmethod + def with_xcpt_regs(cls, xcpt_regs): + return cls(xcpt_regs) + + @classmethod + def for_current(cls): + return cls(None) + + def __format__(self, format_spec): + return format_spec.format( + registers = self.registers + ) + + @property + def registers(self): + return self.regs + + class NX_task(object): """Reference to a NuttX task and methods for introspecting it""" @@ -139,56 +239,12 @@ class NX_task(object): if 'registers' not in self.__dict__: registers = dict() if self._state_is('TSTATE_TASK_RUNNING'): - # XXX need to fix this to handle interrupt context - registers['R0'] = long(gdb.parse_and_eval('$r0')) - registers['R1'] = long(gdb.parse_and_eval('$r1')) - registers['R2'] = long(gdb.parse_and_eval('$r2')) - registers['R3'] = long(gdb.parse_and_eval('$r3')) - registers['R4'] = long(gdb.parse_and_eval('$r4')) - registers['R5'] = long(gdb.parse_and_eval('$r5')) - registers['R6'] = long(gdb.parse_and_eval('$r6')) - registers['R7'] = long(gdb.parse_and_eval('$r7')) - registers['R8'] = long(gdb.parse_and_eval('$r8')) - registers['R9'] = long(gdb.parse_and_eval('$r9')) - registers['R10'] = long(gdb.parse_and_eval('$r10')) - registers['R11'] = long(gdb.parse_and_eval('$r11')) - registers['R12'] = long(gdb.parse_and_eval('$r12')) - registers['R13'] = long(gdb.parse_and_eval('$r13')) - registers['SP'] = long(gdb.parse_and_eval('$sp')) - registers['R14'] = long(gdb.parse_and_eval('$r14')) - registers['LR'] = long(gdb.parse_and_eval('$lr')) - registers['R15'] = long(gdb.parse_and_eval('$r15')) - registers['PC'] = long(gdb.parse_and_eval('$pc')) - registers['XPSR'] = long(gdb.parse_and_eval('$xpsr')) - # this would only be valid if we were in an interrupt - registers['EXC_RETURN'] = 0 - # we should be able to get this... - registers['PRIMASK'] = 0 + registers = NX_register_set.for_current().registers else: context = self._tcb['xcp'] regs = context['regs'] - registers['R0'] = long(regs[27]) - registers['R1'] = long(regs[28]) - registers['R2'] = long(regs[29]) - registers['R3'] = long(regs[30]) - registers['R4'] = long(regs[2]) - registers['R5'] = long(regs[3]) - registers['R6'] = long(regs[4]) - registers['R7'] = long(regs[5]) - registers['R8'] = long(regs[6]) - registers['R9'] = long(regs[7]) - registers['R10'] = long(regs[8]) - registers['R11'] = long(regs[9]) - registers['R12'] = long(regs[31]) - registers['R13'] = long(regs[0]) - registers['SP'] = long(regs[0]) - registers['R14'] = long(regs[32]) - registers['LR'] = long(regs[32]) - registers['R15'] = long(regs[33]) - registers['PC'] = long(regs[33]) - registers['XPSR'] = long(regs[34]) - registers['EXC_RETURN'] = long(regs[10]) - registers['PRIMASK'] = long(regs[1]) + registers = NX_register_set.with_xcpt_regs(regs).registers + self.__dict__['registers'] = registers return self.__dict__['registers'] @@ -267,8 +323,7 @@ class NX_show_heap (gdb.Command): def _print_allocations(self, region_start, region_end): if region_start >= region_end: - print 'heap region {} corrupt'.format(hex(region_start)) - return + raise gdb.GdbError('heap region {} corrupt'.format(hex(region_start))) nodecount = region_end - region_start print 'heap {} - {}'.format(region_start, region_end) cursor = 1 @@ -286,13 +341,31 @@ class NX_show_heap (gdb.Command): nregions = heap['mm_nregions'] region_starts = heap['mm_heapstart'] region_ends = heap['mm_heapend'] - print "{} heap(s)".format(nregions) + print '{} heap(s)'.format(nregions) # walk the heaps for i in range(0, nregions): self._print_allocations(region_starts[i], region_ends[i]) NX_show_heap() +class NX_show_interrupted_thread (gdb.Command): + """(NuttX) prints the register state of an interrupted thread when in interrupt/exception context""" + def __init__(self): + super(NX_show_interrupted_thread, self).__init__('show interrupted-thread', gdb.COMMAND_USER) + def invoke(self, args, from_tty): + regs = gdb.lookup_global_symbol('current_regs').value() + if regs is 0: + raise gdb.GdbError('not in interrupt context') + else: + registers = NX_register_set.with_xcpt_regs(regs) + my_fmt = '' + my_fmt += ' R0 {registers[R0]:#010x} {registers[R1]:#010x} {registers[R2]:#010x} {registers[R3]:#010x}\n' + my_fmt += ' R4 {registers[R4]:#010x} {registers[R5]:#010x} {registers[R6]:#010x} {registers[R7]:#010x}\n' + my_fmt += ' R8 {registers[R8]:#010x} {registers[R9]:#010x} {registers[R10]:#010x} {registers[R11]:#010x}\n' + my_fmt += ' R12 {registers[PC]:#010x}\n' + my_fmt += ' SP {registers[SP]:#010x} LR {registers[LR]:#010x} PC {registers[PC]:#010x} XPSR {registers[XPSR]:#010x}\n' + print format(registers, my_fmt) +NX_show_interrupted_thread() From ac3f1c55c7604f45cc6cad228566e95806fae900 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 15 Sep 2013 09:08:14 +0200 Subject: [PATCH 664/872] Adding more references, adding inline references to make sure a reader of the source file will not miss them --- src/lib/ecl/attitude_fw/ecl_roll_controller.h | 2 +- src/lib/ecl/l1/ecl_l1_pos_control.cpp | 9 +++++++++ src/lib/ecl/l1/ecl_l1_pos_control.h | 2 +- 3 files changed, 11 insertions(+), 2 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h index a6d30d2108..3c0362ecc0 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h @@ -40,7 +40,7 @@ * Acknowledgements: * * The control design is based on a design - * by Paul Riseborough, 2013. + * by Paul Riseborough and Andrew Tridgell, 2013. */ #ifndef ECL_ROLL_CONTROLLER_H diff --git a/src/lib/ecl/l1/ecl_l1_pos_control.cpp b/src/lib/ecl/l1/ecl_l1_pos_control.cpp index 6f50659601..4b4e38b0bb 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_control.cpp +++ b/src/lib/ecl/l1/ecl_l1_pos_control.cpp @@ -69,6 +69,7 @@ float ECL_L1_Pos_Control::target_bearing() float ECL_L1_Pos_Control::switch_distance(float wp_radius) { + /* following [2], switching on L1 distance */ return math::min(wp_radius, _L1_distance); } @@ -85,6 +86,7 @@ float ECL_L1_Pos_Control::crosstrack_error(void) void ECL_L1_Pos_Control::navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position, const math::Vector2f &ground_speed_vector) { + /* this follows the logic presented in [1] */ float eta; float xtrack_vel; @@ -126,6 +128,7 @@ void ECL_L1_Pos_Control::navigate_waypoints(const math::Vector2f &vector_A, cons float distance_A_to_airplane = vector_A_to_airplane.length(); float alongTrackDist = vector_A_to_airplane * vector_AB; + /* extension from [2] */ if (distance_A_to_airplane > _L1_distance && alongTrackDist / math::max(distance_A_to_airplane , 1.0f) < -0.7071f) { /* calculate eta to fly to waypoint A */ @@ -175,6 +178,7 @@ void ECL_L1_Pos_Control::navigate_waypoints(const math::Vector2f &vector_A, cons void ECL_L1_Pos_Control::navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction, const math::Vector2f &ground_speed_vector) { + /* the complete guidance logic in this section was proposed by [2] */ /* calculate the gains for the PD loop (circle tracking) */ float omega = (2.0f * M_PI_F / _L1_period); @@ -263,6 +267,7 @@ void ECL_L1_Pos_Control::navigate_loiter(const math::Vector2f &vector_A, const m void ECL_L1_Pos_Control::navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed_vector) { + /* the complete guidance logic in this section was proposed by [2] */ float eta; @@ -298,6 +303,8 @@ void ECL_L1_Pos_Control::navigate_heading(float navigation_heading, float curren void ECL_L1_Pos_Control::navigate_level_flight(float current_heading) { + /* the logic in this section is trivial, but originally proposed by [2] */ + /* reset all heading / error measures resulting in zero roll */ _target_bearing = current_heading; _nav_bearing = current_heading; @@ -313,6 +320,8 @@ void ECL_L1_Pos_Control::navigate_level_flight(float current_heading) math::Vector2f ECL_L1_Pos_Control::get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const { + /* this is an approximation for small angles, proposed by [2] */ + math::Vector2f out; out.setX(math::radians((target.getX() - origin.getX()))); diff --git a/src/lib/ecl/l1/ecl_l1_pos_control.h b/src/lib/ecl/l1/ecl_l1_pos_control.h index 7f4a440188..3ddb691fab 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_control.h +++ b/src/lib/ecl/l1/ecl_l1_pos_control.h @@ -47,7 +47,7 @@ * Proceedings of the AIAA Guidance, Navigation and Control * Conference, Aug 2004. AIAA-2004-4900. * - * [2] Paul Riseborough and Andrew Tridgell, L1 control for APM. Aug 2013. + * [2] Paul Riseborough, Brandon Jones and Andrew Tridgell, L1 control for APM. Aug 2013. * - Explicit control over frequency and damping * - Explicit control over track capture angle * - Ability to use loiter radius smaller than L1 length From 3c59877b502d2fda0d5a00c0f4ac7d9787e72eaf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 15 Sep 2013 09:13:13 +0200 Subject: [PATCH 665/872] Naming consistency improved --- .../ecl/attitude_fw/ecl_pitch_controller.h | 2 +- ..._control.cpp => ecl_l1_pos_controller.cpp} | 30 +++++++++---------- ..._pos_control.h => ecl_l1_pos_controller.h} | 10 +++---- src/lib/ecl/module.mk | 2 +- .../fw_pos_control_l1_main.cpp | 4 +-- 5 files changed, 24 insertions(+), 24 deletions(-) rename src/lib/ecl/l1/{ecl_l1_pos_control.cpp => ecl_l1_pos_controller.cpp} (90%) rename src/lib/ecl/l1/{ecl_l1_pos_control.h => ecl_l1_pos_controller.h} (97%) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h index 6217eb9d0a..3221bd4849 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h @@ -40,7 +40,7 @@ * Acknowledgements: * * The control design is based on a design - * by Paul Riseborough, 2013. + * by Paul Riseborough and Andrew Tridgell, 2013. */ #ifndef ECL_PITCH_CONTROLLER_H diff --git a/src/lib/ecl/l1/ecl_l1_pos_control.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp similarity index 90% rename from src/lib/ecl/l1/ecl_l1_pos_control.cpp rename to src/lib/ecl/l1/ecl_l1_pos_controller.cpp index 4b4e38b0bb..87eb99f16d 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_control.cpp +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp @@ -32,58 +32,58 @@ ****************************************************************************/ /** - * @file ecl_l1_pos_control.h + * @file ecl_l1_pos_controller.h * Implementation of L1 position control. * Authors and acknowledgements in header. * */ -#include "ecl_l1_pos_control.h" +#include "ecl_l1_pos_controller.h" -float ECL_L1_Pos_Control::nav_roll() +float ECL_L1_Pos_Controller::nav_roll() { float ret = atanf(_lateral_accel * 1.0f / CONSTANTS_ONE_G); ret = math::constrain(ret, (-M_PI_F) / 2.0f, M_PI_F / 2.0f); return ret; } -float ECL_L1_Pos_Control::nav_lateral_acceleration_demand() +float ECL_L1_Pos_Controller::nav_lateral_acceleration_demand() { return _lateral_accel; } -float ECL_L1_Pos_Control::nav_bearing() +float ECL_L1_Pos_Controller::nav_bearing() { return _wrap_pi(_nav_bearing); } -float ECL_L1_Pos_Control::bearing_error() +float ECL_L1_Pos_Controller::bearing_error() { return _bearing_error; } -float ECL_L1_Pos_Control::target_bearing() +float ECL_L1_Pos_Controller::target_bearing() { return _target_bearing; } -float ECL_L1_Pos_Control::switch_distance(float wp_radius) +float ECL_L1_Pos_Controller::switch_distance(float wp_radius) { /* following [2], switching on L1 distance */ return math::min(wp_radius, _L1_distance); } -bool ECL_L1_Pos_Control::reached_loiter_target(void) +bool ECL_L1_Pos_Controller::reached_loiter_target(void) { return _circle_mode; } -float ECL_L1_Pos_Control::crosstrack_error(void) +float ECL_L1_Pos_Controller::crosstrack_error(void) { return _crosstrack_error; } -void ECL_L1_Pos_Control::navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position, +void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position, const math::Vector2f &ground_speed_vector) { /* this follows the logic presented in [1] */ @@ -175,7 +175,7 @@ void ECL_L1_Pos_Control::navigate_waypoints(const math::Vector2f &vector_A, cons _bearing_error = eta; } -void ECL_L1_Pos_Control::navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction, +void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction, const math::Vector2f &ground_speed_vector) { /* the complete guidance logic in this section was proposed by [2] */ @@ -265,7 +265,7 @@ void ECL_L1_Pos_Control::navigate_loiter(const math::Vector2f &vector_A, const m } -void ECL_L1_Pos_Control::navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed_vector) +void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed_vector) { /* the complete guidance logic in this section was proposed by [2] */ @@ -301,7 +301,7 @@ void ECL_L1_Pos_Control::navigate_heading(float navigation_heading, float curren _lateral_accel = 2.0f * sinf(eta) * omega_vel; } -void ECL_L1_Pos_Control::navigate_level_flight(float current_heading) +void ECL_L1_Pos_Controller::navigate_level_flight(float current_heading) { /* the logic in this section is trivial, but originally proposed by [2] */ @@ -318,7 +318,7 @@ void ECL_L1_Pos_Control::navigate_level_flight(float current_heading) } -math::Vector2f ECL_L1_Pos_Control::get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const +math::Vector2f ECL_L1_Pos_Controller::get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const { /* this is an approximation for small angles, proposed by [2] */ diff --git a/src/lib/ecl/l1/ecl_l1_pos_control.h b/src/lib/ecl/l1/ecl_l1_pos_controller.h similarity index 97% rename from src/lib/ecl/l1/ecl_l1_pos_control.h rename to src/lib/ecl/l1/ecl_l1_pos_controller.h index 3ddb691fab..a6a2c01563 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_control.h +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.h @@ -57,8 +57,8 @@ * */ -#ifndef ECL_L1_POS_CONTROL_H -#define ECL_L1_POS_CONTROL_H +#ifndef ECL_L1_POS_CONTROLLER_H +#define ECL_L1_POS_CONTROLLER_H #include #include @@ -67,10 +67,10 @@ /** * L1 Nonlinear Guidance Logic */ -class __EXPORT ECL_L1_Pos_Control +class __EXPORT ECL_L1_Pos_Controller { public: - ECL_L1_Pos_Control() { + ECL_L1_Pos_Controller() { _L1_period = 25; _L1_damping = 0.75f; } @@ -246,4 +246,4 @@ private: }; -#endif /* ECL_L1_POS_CONTROL_H */ +#endif /* ECL_L1_POS_CONTROLLER_H */ diff --git a/src/lib/ecl/module.mk b/src/lib/ecl/module.mk index e8bca3c76a..f2aa3db6a5 100644 --- a/src/lib/ecl/module.mk +++ b/src/lib/ecl/module.mk @@ -38,4 +38,4 @@ SRCS = attitude_fw/ecl_pitch_controller.cpp \ attitude_fw/ecl_roll_controller.cpp \ attitude_fw/ecl_yaw_controller.cpp \ - l1/ecl_l1_pos_control.cpp + l1/ecl_l1_pos_controller.cpp diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 3e83f11c8f..d6d135f9f8 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -83,7 +83,7 @@ #include #include -#include +#include #include /** @@ -163,7 +163,7 @@ private: bool _global_pos_valid; ///< global position is valid math::Dcm _R_nb; ///< current attitude - ECL_L1_Pos_Control _l1_control; + ECL_L1_Pos_Controller _l1_control; TECS _tecs; struct { From 03727974f1249adce8f55faf4978910699e5a47e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 15 Sep 2013 18:48:28 +0200 Subject: [PATCH 666/872] Fix binding states for DSM --- 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 3e83fcad85..5fff1feac5 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -853,9 +853,9 @@ PX4IO::task_main() int32_t dsm_bind_val; param_t dsm_bind_param; - // See if bind parameter has been set, and reset it to 0 + // See if bind parameter has been set, and reset it to -1 param_get(dsm_bind_param = param_find("RC_DSM_BIND"), &dsm_bind_val); - if (dsm_bind_val) { + if (dsm_bind_val >= 0) { dsm_bind_ioctl(dsm_bind_val); dsm_bind_val = -1; param_set(dsm_bind_param, &dsm_bind_val); @@ -1167,7 +1167,7 @@ PX4IO::io_handle_status(uint16_t status) void PX4IO::dsm_bind_ioctl(int dsmMode) { - if (!(_status & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { + if (!(_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) { /* 0: dsm2, 1:dsmx */ if ((dsmMode >= 0) && (dsmMode <= 1)) { mavlink_log_info(_thread_mavlink_fd, "[IO] binding dsm%c rx", (dsmMode == 0) ? '2' : 'x'); From 1a641b791dd3802571e56521b7d13585c07061ef Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 16 Sep 2013 11:33:29 +0900 Subject: [PATCH 667/872] tone_alarm: more device paths replaced with #define --- src/modules/commander/commander_helper.cpp | 2 +- src/systemcmds/preflight_check/preflight_check.c | 2 +- src/systemcmds/tests/test_hrt.c | 4 ++-- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 7feace2b4b..017499cb51 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -82,7 +82,7 @@ static int buzzer; int buzzer_init() { - buzzer = open("/dev/tone_alarm", O_WRONLY); + buzzer = open(TONEALARM_DEVICE_PATH, O_WRONLY); if (buzzer < 0) { warnx("Buzzer: open fail\n"); diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index f5bd01ce8c..e9c5f1a2cf 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -163,7 +163,7 @@ system_eval: warnx("PREFLIGHT CHECK ERROR! TRIGGERING ALARM"); fflush(stderr); - int buzzer = open("/dev/tone_alarm", O_WRONLY); + int buzzer = open(TONEALARM_DEVICE_PATH, O_WRONLY); int leds = open(LED_DEVICE_PATH, 0); if (leds < 0) { diff --git a/src/systemcmds/tests/test_hrt.c b/src/systemcmds/tests/test_hrt.c index ba6b86adba..5690997a95 100644 --- a/src/systemcmds/tests/test_hrt.c +++ b/src/systemcmds/tests/test_hrt.c @@ -124,10 +124,10 @@ int test_tone(int argc, char *argv[]) int fd, result; unsigned long tone; - fd = open("/dev/tone_alarm", O_WRONLY); + fd = open(TONEALARM_DEVICE_PATH, O_WRONLY); if (fd < 0) { - printf("failed opening /dev/tone_alarm\n"); + printf("failed opening " TONEALARM_DEVICE_PATH "\n"); goto out; } From f4abcb51a1a40cccf8f929fe1598297ea1d07c4b Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sun, 15 Sep 2013 17:16:47 +0900 Subject: [PATCH 668/872] tone_alarm: add #define for device path --- src/drivers/drv_tone_alarm.h | 2 ++ src/drivers/stm32/tone_alarm/tone_alarm.cpp | 10 +++++----- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h index f0b8606200..caf2b0f6ee 100644 --- a/src/drivers/drv_tone_alarm.h +++ b/src/drivers/drv_tone_alarm.h @@ -60,6 +60,8 @@ #include +#define TONEALARM_DEVICE_PATH "/dev/tone_alarm" + #define _TONE_ALARM_BASE 0x7400 #define TONE_SET_ALARM _IOC(_TONE_ALARM_BASE, 1) diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index a582ece174..9308090366 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -317,7 +317,7 @@ extern "C" __EXPORT int tone_alarm_main(int argc, char *argv[]); ToneAlarm::ToneAlarm() : - CDev("tone_alarm", "/dev/tone_alarm"), + CDev("tone_alarm", TONEALARM_DEVICE_PATH), _default_tune_number(0), _user_tune(nullptr), _tune(nullptr), @@ -820,10 +820,10 @@ play_tune(unsigned tune) { int fd, ret; - fd = open("/dev/tone_alarm", 0); + fd = open(TONEALARM_DEVICE_PATH, 0); if (fd < 0) - err(1, "/dev/tone_alarm"); + err(1, TONEALARM_DEVICE_PATH); ret = ioctl(fd, TONE_SET_ALARM, tune); close(fd); @@ -839,10 +839,10 @@ play_string(const char *str, bool free_buffer) { int fd, ret; - fd = open("/dev/tone_alarm", O_WRONLY); + fd = open(TONEALARM_DEVICE_PATH, O_WRONLY); if (fd < 0) - err(1, "/dev/tone_alarm"); + err(1, TONEALARM_DEVICE_PATH); ret = write(fd, str, strlen(str) + 1); close(fd); From 9e7077fdf9c62ae08c9915e752353671839b5e0b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 16 Sep 2013 13:07:28 +0200 Subject: [PATCH 669/872] Added more acknowledgements after another author sweep --- src/lib/ecl/attitude_fw/ecl_pitch_controller.h | 4 +++- src/lib/ecl/attitude_fw/ecl_roll_controller.h | 4 +++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h index 3221bd4849..1e6cec6a18 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h @@ -40,7 +40,9 @@ * Acknowledgements: * * The control design is based on a design - * by Paul Riseborough and Andrew Tridgell, 2013. + * by Paul Riseborough and Andrew Tridgell, 2013, + * which in turn is based on initial work of + * Jonathan Challinger, 2012. */ #ifndef ECL_PITCH_CONTROLLER_H diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h index 3c0362ecc0..0d4ea9333a 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h @@ -40,7 +40,9 @@ * Acknowledgements: * * The control design is based on a design - * by Paul Riseborough and Andrew Tridgell, 2013. + * by Paul Riseborough and Andrew Tridgell, 2013, + * which in turn is based on initial work of + * Jonathan Challinger, 2012. */ #ifndef ECL_ROLL_CONTROLLER_H From cb1621005c8e04a72f9d1ecefc069af9718fd9cf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 16 Sep 2013 14:36:43 +0200 Subject: [PATCH 670/872] Hotfix: Bumping up interrupt stack size, which fixes a number of evil symptoms seen in some test cases. Needs more inspection, but this fix holds for the test cases --- 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 a1e128a400..3fd4f52e08 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -327,7 +327,7 @@ CONFIG_BOARD_LOOPSPERMSEC=16717 CONFIG_DRAM_START=0x20000000 CONFIG_DRAM_SIZE=196608 CONFIG_ARCH_HAVE_INTERRUPTSTACK=y -CONFIG_ARCH_INTERRUPTSTACK=2048 +CONFIG_ARCH_INTERRUPTSTACK=4096 # # Boot options diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 8e2ae2d000..849c901b50 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -367,7 +367,7 @@ CONFIG_BOARD_LOOPSPERMSEC=16717 CONFIG_DRAM_START=0x20000000 CONFIG_DRAM_SIZE=262144 CONFIG_ARCH_HAVE_INTERRUPTSTACK=y -CONFIG_ARCH_INTERRUPTSTACK=2048 +CONFIG_ARCH_INTERRUPTSTACK=4096 # # Boot options From bd39d101f58aa5e1ca5f8f7feb72ed46e0e19939 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 16 Sep 2013 22:32:54 +0200 Subject: [PATCH 671/872] Fixed HIL rc script --- .../init.d/{rc.hil => 1000_rc.hil} | 37 ++++++++++++++++++- ROMFS/px4fmu_common/init.d/rcS | 2 +- 2 files changed, 36 insertions(+), 3 deletions(-) rename ROMFS/px4fmu_common/init.d/{rc.hil => 1000_rc.hil} (54%) diff --git a/ROMFS/px4fmu_common/init.d/rc.hil b/ROMFS/px4fmu_common/init.d/1000_rc.hil similarity index 54% rename from ROMFS/px4fmu_common/init.d/rc.hil rename to ROMFS/px4fmu_common/init.d/1000_rc.hil index 6c9bfe6dc3..11318023c7 100644 --- a/ROMFS/px4fmu_common/init.d/rc.hil +++ b/ROMFS/px4fmu_common/init.d/1000_rc.hil @@ -3,10 +3,43 @@ # USB HIL start # -echo "[HIL] starting.." +echo "[HIL] HILStar starting.." +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 15 + param set FW_P_LIM_MAX 50 + param set FW_P_LIM_MIN -50 + param set FW_P_P 60 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 1.1 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 100 + param set FW_R_RMAX 100 + param set FW_THR_CRUISE 0.65 + param set FW_THR_MAX 1 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5.0 + param set FW_T_SINK_MIN 4.0 + param set FW_Y_ROLLFF 1.1 + + param set SYS_AUTOCONFIG 0 + param save +fi + +# Allow USB some time to come up +sleep 1 # Tell MAVLink that this link is "fast" -sleep 2 mavlink start -b 230400 -d /dev/ttyACM0 # Create a fake HIL /dev/pwm_output interface diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index cd4d4487d9..040b5d594a 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -103,7 +103,7 @@ then if param compare SYS_AUTOSTART 1000 then - sh /etc/init.d/rc.hil + sh /etc/init.d/1000_rc.hil set MODE custom else # Try to get an USB console From 0810b264e5679795f100df3a7363ba3ad9d7765e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 16 Sep 2013 22:34:37 +0200 Subject: [PATCH 672/872] Hotfix: Increase work stack sizes --- 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 3fd4f52e08..e43b9c18e8 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -701,11 +701,11 @@ CONFIG_SCHED_WORKQUEUE=y CONFIG_SCHED_HPWORK=y CONFIG_SCHED_WORKPRIORITY=192 CONFIG_SCHED_WORKPERIOD=5000 -CONFIG_SCHED_WORKSTACKSIZE=2048 +CONFIG_SCHED_WORKSTACKSIZE=4000 CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORKPRIORITY=50 CONFIG_SCHED_LPWORKPERIOD=50000 -CONFIG_SCHED_LPWORKSTACKSIZE=2048 +CONFIG_SCHED_LPWORKSTACKSIZE=4000 # CONFIG_LIB_KBDCODEC is not set # CONFIG_LIB_SLCDCODEC is not set diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 849c901b50..8f99143102 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -795,11 +795,11 @@ CONFIG_SCHED_WORKQUEUE=y CONFIG_SCHED_HPWORK=y CONFIG_SCHED_WORKPRIORITY=192 CONFIG_SCHED_WORKPERIOD=5000 -CONFIG_SCHED_WORKSTACKSIZE=2048 +CONFIG_SCHED_WORKSTACKSIZE=4000 CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORKPRIORITY=50 CONFIG_SCHED_LPWORKPERIOD=50000 -CONFIG_SCHED_LPWORKSTACKSIZE=2048 +CONFIG_SCHED_LPWORKSTACKSIZE=4000 # CONFIG_LIB_KBDCODEC is not set # CONFIG_LIB_SLCDCODEC is not set From a4ecdc95828484370e0e399d2f3c8b0e28f5c585 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 17 Sep 2013 08:53:39 +0200 Subject: [PATCH 673/872] Removed unneeded flush --- src/modules/sensors/sensors.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index e98c4d5489..085da905c6 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1516,8 +1516,7 @@ Sensors::task_main() { /* inform about start */ - printf("[sensors] Initializing..\n"); - fflush(stdout); + warnx("Initializing.."); /* start individual sensors */ accel_init(); From ac00100cb800423347ad81967a7731ab766be629 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 18 Sep 2013 08:43:38 +0200 Subject: [PATCH 674/872] Hotfix: Disable gyro scale calibration to prevent people from wrongly using it --- src/modules/commander/gyro_calibration.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 82a0349c9e..369e6da624 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -163,7 +163,9 @@ int do_gyro_calibration(int mavlink_fd) return ERROR; } + mavlink_log_info(mavlink_fd, "offset calibration done."); +#if 0 /*** --- SCALING --- ***/ mavlink_log_info(mavlink_fd, "offset done. Rotate for scale 30x or wait 5s to skip."); @@ -241,9 +243,14 @@ int 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", (double)gyro_scale); mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", (double)gyro_scale); +#else + float gyro_scales[] = { 1.0f, 1.0f, 1.0f }; +#endif + + if (isfinite(gyro_scales[0]) && isfinite(gyro_scales[1]) && isfinite(gyro_scales[2])) { @@ -277,9 +284,6 @@ int do_gyro_calibration(int mavlink_fd) 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"); /* third beep by cal end routine */ From 210e7f92454e2b99a448c1563e22fed6f0220ea7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 18 Sep 2013 08:44:02 +0200 Subject: [PATCH 675/872] Make param save command tolerant of FS timing --- src/modules/systemlib/param/param.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index c69de52b70..e7c96fe54f 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -519,6 +519,10 @@ param_save_default(void) int fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL); if (fd < 0) { + /* do another attempt in case the unlink call is not synced yet */ + usleep(5000); + fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL); + warn("opening '%s' for writing failed", param_get_default_file()); return fd; } @@ -528,7 +532,7 @@ param_save_default(void) if (result != 0) { warn("error exporting parameters to '%s'", param_get_default_file()); - unlink(param_get_default_file()); + (void)unlink(param_get_default_file()); return result; } From 6624c8f1c40d08d594fd2f180bff1088eaff82d4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 18 Sep 2013 11:49:22 +0200 Subject: [PATCH 676/872] Hotfix: Make voltage scaling for standalone default --- ROMFS/px4fmu_common/init.d/08_ardrone | 1 + ROMFS/px4fmu_common/init.d/09_ardrone_flow | 1 + src/modules/sensors/sensor_params.c | 3 ++- 3 files changed, 4 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/08_ardrone b/ROMFS/px4fmu_common/init.d/08_ardrone index 7dbbb82847..f6f09ac220 100644 --- a/ROMFS/px4fmu_common/init.d/08_ardrone +++ b/ROMFS/px4fmu_common/init.d/08_ardrone @@ -30,6 +30,7 @@ fi # MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 +param set BAT_V_SCALING 0.008381 # # Start MAVLink diff --git a/ROMFS/px4fmu_common/init.d/09_ardrone_flow b/ROMFS/px4fmu_common/init.d/09_ardrone_flow index 6333aae564..9b739f2455 100644 --- a/ROMFS/px4fmu_common/init.d/09_ardrone_flow +++ b/ROMFS/px4fmu_common/init.d/09_ardrone_flow @@ -30,6 +30,7 @@ fi # MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 +param set BAT_V_SCALING 0.008381 # # Start MAVLink and MAVLink Onboard (PX4FLOW Sensor) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 950af2eba2..ed4fcdd462 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -174,7 +174,8 @@ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); /* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */ /* PX4IOAR: 0.00838095238 */ /* FMU standalone: 1/(10 / (47+10)) * (3.3 / 4095) = 0.00459340659 */ -PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f)); +/* FMU with PX4IOAR: (3.3f * 52.0f / 5.0f / 4095.0f) */ +PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f); #endif PARAM_DEFINE_INT32(RC_MAP_ROLL, 1); From 36ffe2d69404ff62c4899b6f9f20709243a88b43 Mon Sep 17 00:00:00 2001 From: "Hyon Lim (Retina)" Date: Thu, 19 Sep 2013 04:29:46 +0900 Subject: [PATCH 677/872] Quadrotor HILS related RC script has been added --- ROMFS/px4fmu_common/init.d/1001_rc_quad.hil | 110 ++++++++++++++++++++ ROMFS/px4fmu_common/init.d/rcS | 9 +- 2 files changed, 117 insertions(+), 2 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/1001_rc_quad.hil diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil new file mode 100644 index 0000000000..82efeb2e12 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil @@ -0,0 +1,110 @@ +#!nsh +# +# USB HIL start +# + +echo "[HIL] HILS quadrotor starting.." + +# +# 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.002 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_P 0.09 + param set MC_ATT_D 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_P 6.8 + param set MC_YAWPOS_D 0.0 + 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 set NAV_TAKEOFF_ALT 3.0 + param set MPC_TILT_MAX 0.5 + param set MPC_THR_MAX 0.7 + param set MPC_THR_MIN 0.3 + param set MPC_XY_D 0 + param set MPC_XY_P 0.5 + param set MPC_XY_VEL_D 0 + param set MPC_XY_VEL_I 0 + param set MPC_XY_VEL_MAX 3 + param set MPC_XY_VEL_P 0.2 + param set MPC_Z_D 0 + param set MPC_Z_P 1 + param set MPC_Z_VEL_D 0 + param set MPC_Z_VEL_I 0.1 + param set MPC_Z_VEL_MAX 2 + param set MPC_Z_VEL_P 0.20 + + param save +fi + +# Allow USB some time to come up +sleep 1 +# Tell MAVLink that this link is "fast" +mavlink start -b 230400 -d /dev/ttyACM0 + +# Create a fake HIL /dev/pwm_output interface +hil mode_pwm + +# +# 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 commander (depends on orb, mavlink) +# +commander start + +# +# Check if we got an IO +# +if px4io start +then + echo "IO started" +else + fmu mode_serial + echo "FMU started" +fi + +# +# Start the sensors (depends on orb, px4io) +# +sh /etc/init.d/rc.sensors + +# +# Start the attitude estimator (depends on orb) +# +att_pos_estimator_ekf start + +# +# Load mixer and start controllers (depends on px4io) +# +mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix + +# +# Start position estimator +# +position_estimator_inav start + +# +# Start attitude control +# +multirotor_att_control start + +# +# Start position control +# +multirotor_pos_control start + +echo "[HIL] setup done, running" + diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 040b5d594a..89cba5cdfe 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -106,8 +106,13 @@ then sh /etc/init.d/1000_rc.hil set MODE custom else - # Try to get an USB console - nshterm /dev/ttyACM0 & + if param compare SYS_AUTOSTART 1001 + then + sh /etc/init.d/1001_rc_quad.hil + else + # Try to get an USB console + nshterm /dev/ttyACM0 & + fi fi # From a32b25eb52fd9575873fc3b9c78e24736749a004 Mon Sep 17 00:00:00 2001 From: Hyon Lim Date: Thu, 19 Sep 2013 05:33:57 +1000 Subject: [PATCH 678/872] Update 1001_rc_quad.hil Default gain set --- ROMFS/px4fmu_common/init.d/1001_rc_quad.hil | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil index 82efeb2e12..bbe3b7e288 100644 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil @@ -13,22 +13,22 @@ then # Set all params here, then disable autoconfig param set SYS_AUTOCONFIG 0 - param set MC_ATTRATE_D 0.002 + param set MC_ATTRATE_D 0.0 param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.09 + param set MC_ATTRATE_P 0.05 param set MC_ATT_D 0.0 param set MC_ATT_I 0.0 - param set MC_ATT_P 6.8 + param set MC_ATT_P 3.0 param set MC_YAWPOS_D 0.0 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 set MC_YAWPOS_P 2.1 + param set MC_YAWRATE_D 0.0 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_P 0.05 param set NAV_TAKEOFF_ALT 3.0 param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.7 - param set MPC_THR_MIN 0.3 + param set MPC_THR_MAX 0.5 + param set MPC_THR_MIN 0.1 param set MPC_XY_D 0 param set MPC_XY_P 0.5 param set MPC_XY_VEL_D 0 From 89d3e1db281414571fb55b87fb87385a97263cf1 Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Wed, 18 Sep 2013 20:04:22 -0400 Subject: [PATCH 679/872] Implement Spektrum DSM pairing in V2 - Bind control for V2 - Relays and accessory power not supported on V2 hardware --- src/drivers/boards/px4io-v2/board_config.h | 2 +- src/drivers/boards/px4io-v2/px4iov2_init.c | 4 +- src/drivers/px4io/px4io.cpp | 67 ++++++++++++++++++---- src/modules/px4iofirmware/dsm.c | 21 +++++-- src/modules/px4iofirmware/protocol.h | 2 + src/modules/px4iofirmware/px4io.h | 2 + src/modules/px4iofirmware/registers.c | 12 ++-- src/modules/sensors/sensor_params.c | 3 +- 8 files changed, 83 insertions(+), 30 deletions(-) diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h index 818b644362..4d41d0d07e 100644 --- a/src/drivers/boards/px4io-v2/board_config.h +++ b/src/drivers/boards/px4io-v2/board_config.h @@ -84,7 +84,7 @@ /* 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_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) #define GPIO_SERVO_FAULT_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN15) diff --git a/src/drivers/boards/px4io-v2/px4iov2_init.c b/src/drivers/boards/px4io-v2/px4iov2_init.c index 0ea95bded9..ccd01edf56 100644 --- a/src/drivers/boards/px4io-v2/px4iov2_init.c +++ b/src/drivers/boards/px4io-v2/px4iov2_init.c @@ -111,9 +111,7 @@ __EXPORT void stm32_boardinitialize(void) 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); + /* spektrum power enable is active high - enable it by default */ stm32_configgpio(GPIO_SPEKTRUM_PWR_EN); stm32_configgpio(GPIO_SERVO_FAULT_DETECT); diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 5fff1feac5..f9fb9eea96 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -204,6 +204,7 @@ public: */ int disable_rc_handling(); +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 /** * Set the DSM VCC is controlled by relay one flag * @@ -223,6 +224,9 @@ public: { return _dsm_vcc_ctl; }; +#endif + + inline uint16_t system_status() const {return _status;} private: device::Device *_interface; @@ -274,8 +278,9 @@ private: float _battery_mamphour_total;///= 0) { + if (dsm_bind_val > -1) { dsm_bind_ioctl(dsm_bind_val); dsm_bind_val = -1; param_set(dsm_bind_param, &dsm_bind_val); @@ -1169,7 +1177,7 @@ PX4IO::dsm_bind_ioctl(int dsmMode) { if (!(_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) { /* 0: dsm2, 1:dsmx */ - if ((dsmMode >= 0) && (dsmMode <= 1)) { + if ((dsmMode == 0) || (dsmMode == 1)) { mavlink_log_info(_thread_mavlink_fd, "[IO] binding dsm%c rx", (dsmMode == 0) ? '2' : 'x'); ioctl(nullptr, DSM_BIND_START, (dsmMode == 0) ? DSM2_BIND_PULSES : DSMX_BIND_PULSES); } else { @@ -1638,11 +1646,19 @@ PX4IO::print_status() ((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" : "")); +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 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), io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE), io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS)); +#endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + printf("rates 0x%04x default %u alt %u\n", + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE)); +#endif printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG)); printf("controls"); for (unsigned i = 0; i < _max_controls; i++) @@ -1783,36 +1799,58 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) } case GPIO_RESET: { +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 uint32_t bits = (1 << _max_relays) - 1; /* don't touch relay1 if it's controlling RX vcc */ if (_dsm_vcc_ctl) bits &= ~PX4IO_P_SETUP_RELAYS_POWER1; ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, bits, 0); +#endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + ret = -EINVAL; +#endif break; } case GPIO_SET: +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 arg &= ((1 << _max_relays) - 1); /* don't touch relay1 if it's controlling RX vcc */ - if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1)) + 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); + break; + } + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg); +#endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + ret = -EINVAL; +#endif break; case GPIO_CLEAR: +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 arg &= ((1 << _max_relays) - 1); /* don't touch relay1 if it's controlling RX vcc */ - if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1)) + if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1)) { ret = -EINVAL; - else + break; + } ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0); +#endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + ret = -EINVAL; +#endif break; case GPIO_GET: +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 *(uint32_t *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS); if (*(uint32_t *)arg == _io_reg_get_error) ret = -EIO; +#endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + ret = -EINVAL; +#endif break; case MIXERIOCGETOUTPUTCOUNT: @@ -1990,6 +2028,7 @@ start(int argc, char *argv[]) } } +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 int dsm_vcc_ctl; if (param_get(param_find("RC_RL1_DSM_VCC"), &dsm_vcc_ctl) == OK) { @@ -1998,6 +2037,7 @@ start(int argc, char *argv[]) g_dev->ioctl(nullptr, DSM_BIND_POWER_UP, 0); } } +#endif exit(0); } @@ -2037,8 +2077,10 @@ bind(int argc, char *argv[]) if (g_dev == nullptr) errx(1, "px4io must be started first"); +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 if (!g_dev->get_dsm_vcc_ctl()) errx(1, "DSM bind feature not enabled"); +#endif if (argc < 3) errx(0, "needs argument, use dsm2 or dsmx"); @@ -2049,9 +2091,12 @@ bind(int argc, char *argv[]) pulses = DSMX_BIND_PULSES; else errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]); + if (g_dev->system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + errx(1, "system must not be armed"); +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 warnx("This command will only bind DSM if satellite VCC (red wire) is controlled by relay 1."); - +#endif g_dev->ioctl(nullptr, DSM_BIND_START, pulses); exit(0); diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index 206e27db5a..fd6bca62a2 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -243,28 +243,35 @@ dsm_init(const char *device) void dsm_bind(uint16_t cmd, int pulses) { +#if !(defined(CONFIG_ARCH_BOARD_PX4IO_V1) || defined(CONFIG_ARCH_BOARD_PX4IO_V2)) + #warning DSM BIND NOT IMPLEMENTED ON UNKNOWN PLATFORM +#else const uint32_t usart1RxAsOutp = GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10; 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*/ +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 POWER_RELAY1(0); +#else /* CONFIG_ARCH_BOARD_PX4IO_V2 */ + POWER_SPEKTRUM(0); +#endif break; case dsm_bind_power_up: /*power up DSM satellite*/ +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 POWER_RELAY1(1); +#else /* CONFIG_ARCH_BOARD_PX4IO_V2 */ + POWER_SPEKTRUM(1); +#endif dsm_guess_format(true); break; @@ -387,8 +394,10 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) values[channel] = value; } - if (dsm_channel_shift == 11) + if (dsm_channel_shift == 11) { + /* Set the 11-bit data indicator */ *num_values |= 0x8000; + } /* * XXX Note that we may be in failsafe here; we need to work out how to detect that. @@ -412,7 +421,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) * Upon receiving a full dsm frame we attempt to decode it. * * @param[out] values pointer to per channel array of decoded values - * @param[out] num_values pointer to number of raw channel values returned + * @param[out] num_values pointer to number of raw channel values returned, high order bit 0:10 bit data, 1:11 bit data * @return true=decoded raw channel values updated, false=no update */ bool diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index f5fa0ba751..0e2cd16898 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -165,11 +165,13 @@ #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 */ +#if defined(CONFIG_ARCH_BOARD_PX4IO_V1) || defined(CONFIG_ARCH_BOARD_PX4FMU_V1) #define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */ #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 */ +#endif #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) */ diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 11f4d053d5..66c4ca9066 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -100,7 +100,9 @@ extern uint16_t r_page_servo_idle[]; /* PX4IO_PAGE_IDLE_PWM */ #define r_setup_pwm_rates r_page_setup[PX4IO_P_SETUP_PWM_RATES] #define r_setup_pwm_defaultrate r_page_setup[PX4IO_P_SETUP_PWM_DEFAULTRATE] #define r_setup_pwm_altrate r_page_setup[PX4IO_P_SETUP_PWM_ALTRATE] +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 #define r_setup_relays r_page_setup[PX4IO_P_SETUP_RELAYS] +#endif #define r_control_values (&r_page_controls[0]) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 9c95fd1c52..8cb21e54fb 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -145,7 +145,9 @@ volatile uint16_t r_page_setup[] = [PX4IO_P_SETUP_PWM_RATES] = 0, [PX4IO_P_SETUP_PWM_DEFAULTRATE] = 50, [PX4IO_P_SETUP_PWM_ALTRATE] = 200, +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 [PX4IO_P_SETUP_RELAYS] = 0, +#endif #ifdef ADC_VSERVO [PX4IO_P_SETUP_VSERVO_SCALE] = 10000, #else @@ -462,22 +464,16 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value); break; +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 case PX4IO_P_SETUP_RELAYS: value &= PX4IO_P_SETUP_RELAYS_VALID; r_setup_relays = value; -#ifdef POWER_RELAY1 POWER_RELAY1((value & PX4IO_P_SETUP_RELAYS_POWER1) ? 1 : 0); -#endif -#ifdef POWER_RELAY2 POWER_RELAY2((value & PX4IO_P_SETUP_RELAYS_POWER2) ? 1 : 0); -#endif -#ifdef POWER_ACC1 POWER_ACC1((value & PX4IO_P_SETUP_RELAYS_ACC1) ? 1 : 0); -#endif -#ifdef POWER_ACC2 POWER_ACC2((value & PX4IO_P_SETUP_RELAYS_ACC2) ? 1 : 0); -#endif break; +#endif case PX4IO_P_SETUP_VBATT_SCALE: r_page_setup[PX4IO_P_SETUP_VBATT_SCALE] = value; diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index ed4fcdd462..4d719e0e15 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -164,8 +164,9 @@ PARAM_DEFINE_FLOAT(RC15_MAX, 2000); PARAM_DEFINE_FLOAT(RC15_REV, 1.0f); PARAM_DEFINE_FLOAT(RC15_DZ, 0.0f); - +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */ +#endif PARAM_DEFINE_INT32(RC_DSM_BIND, -1); /* -1 = Idle, 0 = Start DSM2 bind, 1 = Start DSMX bind */ #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 From 390d5b34de7dc595340d71c1554fec78c0d9423e Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Wed, 18 Sep 2013 20:14:53 -0400 Subject: [PATCH 680/872] Fix backwards ifdef in dsm.c --- src/modules/px4iofirmware/dsm.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index fd6bca62a2..dc544d991e 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -243,7 +243,7 @@ dsm_init(const char *device) void dsm_bind(uint16_t cmd, int pulses) { -#if !(defined(CONFIG_ARCH_BOARD_PX4IO_V1) || defined(CONFIG_ARCH_BOARD_PX4IO_V2)) +#if !(defined(CONFIG_ARCH_BOARD_PX4IO_V1) && !defined(CONFIG_ARCH_BOARD_PX4IO_V2)) #warning DSM BIND NOT IMPLEMENTED ON UNKNOWN PLATFORM #else const uint32_t usart1RxAsOutp = From 72df378577a7d4dbe590f6ef2e1a119924786fa2 Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Wed, 18 Sep 2013 20:24:33 -0400 Subject: [PATCH 681/872] Finally get the #if right!!! --- src/modules/px4iofirmware/dsm.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index dc544d991e..4339766568 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -243,7 +243,7 @@ dsm_init(const char *device) void dsm_bind(uint16_t cmd, int pulses) { -#if !(defined(CONFIG_ARCH_BOARD_PX4IO_V1) && !defined(CONFIG_ARCH_BOARD_PX4IO_V2)) +#if !defined(CONFIG_ARCH_BOARD_PX4IO_V1) && !defined(CONFIG_ARCH_BOARD_PX4IO_V2) #warning DSM BIND NOT IMPLEMENTED ON UNKNOWN PLATFORM #else const uint32_t usart1RxAsOutp = From 7e0da345f06babd2b99670b559c5c7faf96b6997 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 18 Sep 2013 21:47:10 -0700 Subject: [PATCH 682/872] The parameter system is supposed to have a lock; implement one. --- src/modules/systemlib/param/param.c | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index e7c96fe54f..2832d87ef1 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -48,6 +48,7 @@ #include #include #include +#include #include @@ -95,18 +96,20 @@ ORB_DEFINE(parameter_update, struct parameter_update_s); /** parameter update topic handle */ static orb_advert_t param_topic = -1; +static sem_t param_lock = { .semcount = 1 }; + /** lock the parameter store */ static void param_lock(void) { - /* XXX */ + do {} while (sem_wait(¶m_lock) != 0); } /** unlock the parameter store */ static void param_unlock(void) { - /* XXX */ + sem_post(¶m_lock); } /** assert that the parameter store is locked */ From 978a234d2903b34df7d13fcf0980db283a6f1166 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 18 Sep 2013 22:40:07 -0700 Subject: [PATCH 683/872] Lock name should not equal locking function name. Urr. --- src/modules/systemlib/param/param.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 2832d87ef1..97c547848d 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -96,20 +96,20 @@ ORB_DEFINE(parameter_update, struct parameter_update_s); /** parameter update topic handle */ static orb_advert_t param_topic = -1; -static sem_t param_lock = { .semcount = 1 }; +static sem_t param_sem = { .semcount = 1 }; /** lock the parameter store */ static void param_lock(void) { - do {} while (sem_wait(¶m_lock) != 0); + do {} while (sem_wait(¶m_sem) != 0); } /** unlock the parameter store */ static void param_unlock(void) { - sem_post(¶m_lock); + sem_post(¶m_sem); } /** assert that the parameter store is locked */ From fdc45219493560487d95cdf88a66117ba43fa854 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 19 Sep 2013 07:51:37 +0200 Subject: [PATCH 684/872] Hotfix: Disabling param lock, not operational yet --- src/modules/systemlib/param/param.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 97c547848d..59cbcf5fb0 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -102,14 +102,14 @@ static sem_t param_sem = { .semcount = 1 }; static void param_lock(void) { - do {} while (sem_wait(¶m_sem) != 0); + //do {} while (sem_wait(¶m_sem) != 0); } /** unlock the parameter store */ static void param_unlock(void) { - sem_post(¶m_sem); + //sem_post(¶m_sem); } /** assert that the parameter store is locked */ From 3a49ec9eb1c5daa8b2c9cd3e9f88d18f19ef66a8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 19 Sep 2013 07:55:34 +0200 Subject: [PATCH 685/872] Hotfix: Guard against corrupted param files, still boot the system if they occur --- ROMFS/px4fmu_common/init.d/rcS | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 040b5d594a..07ccbb3f48 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -84,7 +84,12 @@ then param select /fs/microsd/params if [ -f /fs/microsd/params ] then - param load /fs/microsd/params + if param load /fs/microsd/params + then + echo "Parameters loaded" + else + echo "Parameter file corrupt - ignoring" + fi fi #fi From f2d5d008a6d3268412a567b699437cfde111696a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 19 Sep 2013 11:33:04 +0200 Subject: [PATCH 686/872] rgbled: major cleanup and bugfixes --- src/drivers/rgbled/rgbled.cpp | 473 ++++++++++++++++++++-------------- 1 file changed, 273 insertions(+), 200 deletions(-) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index ee1d472a2e..74ea6d9e15 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -92,16 +92,14 @@ public: private: work_s _work; - rgbled_color_t _color; rgbled_mode_t _mode; rgbled_pattern_t _pattern; - float _brightness; uint8_t _r; uint8_t _g; uint8_t _b; + float _brightness; - bool _should_run; bool _running; int _led_interval; int _counter; @@ -109,35 +107,33 @@ private: void set_color(rgbled_color_t ledcolor); void set_mode(rgbled_mode_t mode); void set_pattern(rgbled_pattern_t *pattern); - void set_brightness(float brightness); 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 send_led_enable(bool enable); + int send_led_rgb(); + 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; +RGBLED *g_rgbled; } +void rgbled_usage(); extern "C" __EXPORT int rgbled_main(int argc, char *argv[]); RGBLED::RGBLED(int bus, int rgbled) : I2C("rgbled", RGBLED_DEVICE_PATH, bus, rgbled, 100000), - _color(RGBLED_COLOR_OFF), _mode(RGBLED_MODE_OFF), - _running(false), - _brightness(1.0f), _r(0), _g(0), _b(0), + _brightness(1.0f), + _running(false), _led_interval(0), _counter(0) { @@ -159,8 +155,9 @@ RGBLED::init() return ret; } - /* start off */ - set(false, 0, 0, 0); + /* switch off LED on start */ + send_led_enable(false); + send_led_rgb(); return OK; } @@ -169,10 +166,10 @@ int RGBLED::probe() { int ret; - bool on, not_powersave; + bool on, powersave; uint8_t r, g, b; - ret = get(on, not_powersave, r, g, b); + ret = get(on, powersave, r, g, b); return ret; } @@ -181,15 +178,16 @@ int RGBLED::info() { int ret; - bool on, not_powersave; + bool on, powersave; uint8_t r, g, b; - ret = get(on, not_powersave, r, g, b); + ret = get(on, powersave, 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"); } @@ -201,28 +199,30 @@ int RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg) { int ret = ENOTTY; + switch (cmd) { case RGBLED_SET_RGB: - /* set the specified RGB values */ - 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); + /* set the specified color */ + _r = ((rgbled_rgbset_t *) arg)->red; + _g = ((rgbled_rgbset_t *) arg)->green; + _b = ((rgbled_rgbset_t *) arg)->blue; + send_led_rgb(); return OK; case RGBLED_SET_COLOR: /* set the specified color name */ set_color((rgbled_color_t)arg); + send_led_rgb(); return OK; case RGBLED_SET_MODE: - /* set the specified blink speed */ + /* set the specified mode */ set_mode((rgbled_mode_t)arg); return OK; case RGBLED_SET_PATTERN: /* set a special pattern */ - set_pattern((rgbled_pattern_t*)arg); + set_pattern((rgbled_pattern_t *)arg); return OK; default: @@ -247,33 +247,47 @@ void RGBLED::led() { switch (_mode) { - 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_BREATHE: - if (_counter >= 30) - _counter = 0; - if (_counter <= 15) { - set_brightness(((float)_counter)*((float)_counter)/(15.0f*15.0f)); - } else { - set_brightness(((float)(30-_counter))*((float)(30-_counter))/(15.0f*15.0f)); - } - 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; + case RGBLED_MODE_BLINK_SLOW: + case RGBLED_MODE_BLINK_NORMAL: + case RGBLED_MODE_BLINK_FAST: + if (_counter >= 2) + _counter = 0; - set_color(_pattern.color[_counter]); - _led_interval = _pattern.duration[_counter]; - break; - default: - break; + send_led_enable(_counter == 0); + + break; + + case RGBLED_MODE_BREATHE: + + if (_counter >= 62) + _counter = 0; + + int n; + + if (_counter < 32) { + n = _counter; + + } else { + n = 62 - _counter; + } + + _brightness = n * n / (31.0f * 31.0f); + send_led_rgb(); + 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]); + send_led_rgb(); + _led_interval = _pattern.duration[_counter]; + break; + + default: + break; } _counter++; @@ -282,60 +296,106 @@ RGBLED::led() work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, _led_interval); } +/** + * Parse color constant and set _r _g _b values + */ void -RGBLED::set_color(rgbled_color_t color) { - - _color = color; - +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 - set_rgb(255,0,0); - break; - case RGBLED_COLOR_YELLOW: // yellow - set_rgb(255,70,0); - break; - case RGBLED_COLOR_PURPLE: // purple - set_rgb(255,0,255); - break; - case RGBLED_COLOR_GREEN: // green - set_rgb(0,255,0); - break; - case RGBLED_COLOR_BLUE: // blue - set_rgb(0,0,255); - break; - case RGBLED_COLOR_WHITE: // white - set_rgb(255,255,255); - break; - 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; + case RGBLED_COLOR_OFF: + _r = 0; + _g = 0; + _b = 0; + break; + + case RGBLED_COLOR_RED: + _r = 255; + _g = 0; + _b = 0; + break; + + case RGBLED_COLOR_YELLOW: + _r = 255; + _g = 70; + _b = 0; + break; + + case RGBLED_COLOR_PURPLE: + _r = 255; + _g = 0; + _b = 255; + break; + + case RGBLED_COLOR_GREEN: + _r = 0; + _g = 255; + _b = 0; + break; + + case RGBLED_COLOR_BLUE: + _r = 0; + _g = 0; + _b = 255; + break; + + case RGBLED_COLOR_WHITE: + _r = 255; + _g = 255; + _b = 255; + break; + + case RGBLED_COLOR_AMBER: + _r = 255; + _g = 20; + _b = 0; + break; + + case RGBLED_COLOR_DIM_RED: + _r = 90; + _g = 0; + _b = 0; + break; + + case RGBLED_COLOR_DIM_YELLOW: + _r = 80; + _g = 30; + _b = 0; + break; + + case RGBLED_COLOR_DIM_PURPLE: + _r = 45; + _g = 0; + _b = 45; + break; + + case RGBLED_COLOR_DIM_GREEN: + _r = 0; + _g = 90; + _b = 0; + break; + + case RGBLED_COLOR_DIM_BLUE: + _r = 0; + _g = 0; + _b = 90; + break; + + case RGBLED_COLOR_DIM_WHITE: + _r = 30; + _g = 30; + _b = 30; + break; + + case RGBLED_COLOR_DIM_AMBER: + _r = 80; + _g = 20; + _b = 0; + break; + + default: + warnx("color unknown"); + break; } } @@ -343,51 +403,70 @@ void RGBLED::set_mode(rgbled_mode_t mode) { _mode = mode; + bool should_run = false; 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 = 500; - break; - case RGBLED_MODE_BLINK_FAST: - _should_run = true; - _led_interval = 100; - break; - case RGBLED_MODE_BREATHE: - _should_run = true; - set_on(true); - _counter = 0; - _led_interval = 1000/15; - break; - case RGBLED_MODE_PATTERN: - _should_run = true; - set_on(true); - _counter = 0; - break; - default: - warnx("mode unknown"); - break; + case RGBLED_MODE_OFF: + send_led_enable(false); + break; + + case RGBLED_MODE_ON: + _brightness = 1.0f; + send_led_rgb(); + send_led_enable(true); + break; + + case RGBLED_MODE_BLINK_SLOW: + should_run = true; + _counter = 0; + _led_interval = 2000; + _brightness = 1.0f; + send_led_rgb(); + break; + + case RGBLED_MODE_BLINK_NORMAL: + should_run = true; + _counter = 0; + _led_interval = 500; + _brightness = 1.0f; + send_led_rgb(); + break; + + case RGBLED_MODE_BLINK_FAST: + should_run = true; + _counter = 0; + _led_interval = 100; + _brightness = 1.0f; + send_led_rgb(); + break; + + case RGBLED_MODE_BREATHE: + should_run = true; + _counter = 0; + _led_interval = 25; + send_led_enable(true); + break; + + case RGBLED_MODE_PATTERN: + should_run = true; + _counter = 0; + _brightness = 1.0f; + send_led_enable(true); + break; + + default: + warnx("mode unknown"); + break; } /* if it should run now, start the workq */ - if (_should_run && !_running) { + 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) { + if (!should_run && _running) { _running = false; work_cancel(LPWORK, &_work); } @@ -397,66 +476,44 @@ void RGBLED::set_pattern(rgbled_pattern_t *pattern) { memcpy(&_pattern, pattern, sizeof(rgbled_pattern_t)); - set_mode(RGBLED_MODE_PATTERN); } -void -RGBLED::set_brightness(float brightness) { - - _brightness = brightness; - set_rgb(_r, _g, _b); -} - +/** + * Sent ENABLE flag to LED driver + */ int -RGBLED::set(bool on, uint8_t r, uint8_t g, uint8_t b) +RGBLED::send_led_enable(bool enable) { 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) + if (enable) settings_byte |= SETTING_ENABLE; -/* powersave not used */ -// if (not_powersave) - settings_byte |= SETTING_NOT_POWERSAVE; + settings_byte |= SETTING_NOT_POWERSAVE; const uint8_t msg[2] = { SUB_ADDR_SETTINGS, settings_byte}; return transfer(msg, sizeof(msg), nullptr, 0); } +/** + * Send RGB PWM settings to LED driver according to current color and brightness + */ int -RGBLED::set_rgb(uint8_t r, uint8_t g, uint8_t b) +RGBLED::send_led_rgb() { - /* save the RGB values in case we want to change the brightness later */ - _r = r; - _g = g; - _b = b; - - const uint8_t msg[6] = { SUB_ADDR_PWM0, (uint8_t)((float)b/255.0f*15.0f*_brightness), SUB_ADDR_PWM1, (uint8_t)((float)g/255.0f*15.0f*_brightness), SUB_ADDR_PWM2, (uint8_t)((float)r/255.0f*15.0f*_brightness)}; - + /* To scale from 0..255 -> 0..15 shift right by 4 bits */ + const uint8_t msg[6] = { + SUB_ADDR_PWM0, (uint8_t)((int)(_b * _brightness) >> 4), + SUB_ADDR_PWM1, (uint8_t)((int)(_g * _brightness) >> 4), + SUB_ADDR_PWM2, (uint8_t)((int)(_r * _brightness) >> 4) + }; return transfer(msg, sizeof(msg), nullptr, 0); } - int -RGBLED::get(bool &on, bool ¬_powersave, uint8_t &r, uint8_t &g, uint8_t &b) +RGBLED::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b) { uint8_t result[2]; int ret; @@ -465,24 +522,23 @@ 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; + 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; + r = (result[0] & 0x0f) << 4; + g = (result[1] & 0xf0); + b = (result[1] & 0x0f) << 4; } return ret; } -void rgbled_usage(); - - -void rgbled_usage() { - warnx("missing command: try 'start', 'test', 'info', 'stop'/'off', 'rgb 30 40 50'"); +void +rgbled_usage() +{ + warnx("missing command: try 'start', 'test', 'info', 'off', 'rgb 30 40 50'"); warnx("options:"); warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED); - errx(0, " -a addr (0x%x)", ADDR); + warnx(" -a addr (0x%x)", ADDR); } int @@ -492,17 +548,21 @@ rgbled_main(int argc, char *argv[]) int rgbledadr = ADDR; /* 7bit */ int ch; + /* jump over start/off/etc and look at options first */ - while ((ch = getopt(argc-1, &argv[1], "a:b:")) != EOF) { + while ((ch = getopt(argc - 1, &argv[1], "a:b:")) != EOF) { switch (ch) { case 'a': rgbledadr = strtol(optarg, NULL, 0); break; + case 'b': i2cdevice = strtol(optarg, NULL, 0); break; + default: rgbled_usage(); + exit(0); } } @@ -519,17 +579,21 @@ rgbled_main(int argc, char *argv[]) // 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"); @@ -545,21 +609,24 @@ rgbled_main(int argc, char *argv[]) /* need the driver past this point */ if (g_rgbled == nullptr) { - warnx("not started"); - rgbled_usage(); - exit(0); + warnx("not started"); + rgbled_usage(); + exit(0); } if (!strcmp(verb, "test")) { fd = open(RGBLED_DEVICE_PATH, 0); + if (fd == -1) { errx(1, "Unable to open " RGBLED_DEVICE_PATH); } - rgbled_pattern_t pattern = { {RGBLED_COLOR_RED, RGBLED_COLOR_GREEN, RGBLED_COLOR_BLUE, RGBLED_COLOR_OFF}, - {200, 200, 200, 400 } }; + rgbled_pattern_t pattern = { {RGBLED_COLOR_RED, RGBLED_COLOR_GREEN, RGBLED_COLOR_BLUE, RGBLED_COLOR_WHITE, RGBLED_COLOR_OFF, RGBLED_COLOR_OFF}, + {500, 500, 500, 500, 1000, 0 } + }; ret = ioctl(fd, RGBLED_SET_PATTERN, (unsigned long)&pattern); + ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_PATTERN); close(fd); exit(ret); @@ -570,33 +637,39 @@ rgbled_main(int argc, char *argv[]) exit(0); } - if (!strcmp(verb, "stop") || !strcmp(verb, "off")) { - /* although technically it doesn't stop, this is the excepted syntax */ + if (!strcmp(verb, "off")) { 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")) { - fd = open(RGBLED_DEVICE_PATH, 0); - if (fd == -1) { - errx(1, "Unable to open " RGBLED_DEVICE_PATH); - } if (argc < 5) { errx(1, "Usage: rgbled rgb "); } + + fd = open(RGBLED_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " RGBLED_DEVICE_PATH); + } + rgbled_rgbset_t v; v.red = strtol(argv[2], NULL, 0); v.green = strtol(argv[3], NULL, 0); v.blue = strtol(argv[4], NULL, 0); ret = ioctl(fd, RGBLED_SET_RGB, (unsigned long)&v); + ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_ON); close(fd); exit(ret); } rgbled_usage(); + exit(0); } From a012d5be828a826918b40733130d9222a3be23b2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 19 Sep 2013 18:13:11 +0200 Subject: [PATCH 687/872] rgbled: more cleanup --- src/drivers/rgbled/rgbled.cpp | 134 ++++++++++++++++++---------------- 1 file changed, 71 insertions(+), 63 deletions(-) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 74ea6d9e15..47dbb631cb 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -241,8 +241,9 @@ RGBLED::led_trampoline(void *arg) rgbl->led(); } - - +/** + * Main loop function + */ void RGBLED::led() { @@ -317,7 +318,7 @@ RGBLED::set_color(rgbled_color_t color) case RGBLED_COLOR_YELLOW: _r = 255; - _g = 70; + _g = 200; _b = 0; break; @@ -347,7 +348,7 @@ RGBLED::set_color(rgbled_color_t color) case RGBLED_COLOR_AMBER: _r = 255; - _g = 20; + _g = 80; _b = 0; break; @@ -399,84 +400,91 @@ RGBLED::set_color(rgbled_color_t color) } } +/** + * Set mode, if mode not changed has no any effect (doesn't reset blinks phase) + */ void RGBLED::set_mode(rgbled_mode_t mode) { - _mode = mode; - bool should_run = false; + if (mode != _mode) { + _mode = mode; + bool should_run = false; - switch (mode) { - case RGBLED_MODE_OFF: - send_led_enable(false); - break; + switch (mode) { + case RGBLED_MODE_OFF: + send_led_enable(false); + break; - case RGBLED_MODE_ON: - _brightness = 1.0f; - send_led_rgb(); - send_led_enable(true); - break; + case RGBLED_MODE_ON: + _brightness = 1.0f; + send_led_rgb(); + send_led_enable(true); + break; - case RGBLED_MODE_BLINK_SLOW: - should_run = true; - _counter = 0; - _led_interval = 2000; - _brightness = 1.0f; - send_led_rgb(); - break; + case RGBLED_MODE_BLINK_SLOW: + should_run = true; + _counter = 0; + _led_interval = 2000; + _brightness = 1.0f; + send_led_rgb(); + break; - case RGBLED_MODE_BLINK_NORMAL: - should_run = true; - _counter = 0; - _led_interval = 500; - _brightness = 1.0f; - send_led_rgb(); - break; + case RGBLED_MODE_BLINK_NORMAL: + should_run = true; + _counter = 0; + _led_interval = 500; + _brightness = 1.0f; + send_led_rgb(); + break; - case RGBLED_MODE_BLINK_FAST: - should_run = true; - _counter = 0; - _led_interval = 100; - _brightness = 1.0f; - send_led_rgb(); - break; + case RGBLED_MODE_BLINK_FAST: + should_run = true; + _counter = 0; + _led_interval = 100; + _brightness = 1.0f; + send_led_rgb(); + break; - case RGBLED_MODE_BREATHE: - should_run = true; - _counter = 0; - _led_interval = 25; - send_led_enable(true); - break; + case RGBLED_MODE_BREATHE: + should_run = true; + _counter = 0; + _led_interval = 25; + send_led_enable(true); + break; - case RGBLED_MODE_PATTERN: - should_run = true; - _counter = 0; - _brightness = 1.0f; - send_led_enable(true); - break; + case RGBLED_MODE_PATTERN: + should_run = true; + _counter = 0; + _brightness = 1.0f; + send_led_enable(true); + break; - default: - warnx("mode unknown"); - 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 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); + /* if it should stop, then cancel the workq */ + if (!should_run && _running) { + _running = false; + work_cancel(LPWORK, &_work); + } } } +/** + * Set pattern for PATTERN mode, but don't change current mode + */ void RGBLED::set_pattern(rgbled_pattern_t *pattern) { memcpy(&_pattern, pattern, sizeof(rgbled_pattern_t)); - set_mode(RGBLED_MODE_PATTERN); } /** @@ -622,7 +630,7 @@ rgbled_main(int argc, char *argv[]) } rgbled_pattern_t pattern = { {RGBLED_COLOR_RED, RGBLED_COLOR_GREEN, RGBLED_COLOR_BLUE, RGBLED_COLOR_WHITE, RGBLED_COLOR_OFF, RGBLED_COLOR_OFF}, - {500, 500, 500, 500, 1000, 0 } + {500, 500, 500, 500, 1000, 0 } // "0" indicates end of pattern }; ret = ioctl(fd, RGBLED_SET_PATTERN, (unsigned long)&pattern); From 528666c91275311035d1ffcc96ca7f052a7ad081 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 19 Sep 2013 18:26:33 +0200 Subject: [PATCH 688/872] multirotor_pos_control: yaw setpoint bug fixed --- .../multirotor_pos_control/multirotor_pos_control.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 1b3ddfdcda..36dd370fb2 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -193,7 +193,7 @@ 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(&global_pos_sp, 0, sizeof(local_pos_sp)); + memset(&global_pos_sp, 0, sizeof(global_pos_sp)); struct vehicle_global_velocity_setpoint_s global_vel_sp; memset(&global_vel_sp, 0, sizeof(global_vel_sp)); @@ -408,6 +408,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } } + /* copy yaw setpoint to vehicle_local_position_setpoint topic */ local_pos_sp.yaw = att_sp.yaw_body; /* local position setpoint is valid and can be used for auto loiter after position controlled mode */ @@ -430,7 +431,6 @@ 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 = - params.takeoff_alt - params.takeoff_gap; - local_pos_sp.yaw = att.yaw; att_sp.yaw_body = att.yaw; mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double) - local_pos_sp.z); } @@ -471,8 +471,6 @@ 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)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y); @@ -485,7 +483,6 @@ 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.yaw = att.yaw; - att_sp.yaw_body = global_pos_sp.yaw; } if (reset_auto_sp_z) { @@ -509,6 +506,9 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) reset_mission_sp = true; } + /* copy yaw setpoint to vehicle_local_position_setpoint topic */ + local_pos_sp.yaw = att_sp.yaw_body; + /* reset setpoints after AUTO mode */ reset_man_sp_xy = true; reset_man_sp_z = true; From 09452805b13103172ae8d43eea2a419a761249f2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 19 Sep 2013 18:46:35 +0200 Subject: [PATCH 689/872] rgbled: copyright updated --- src/drivers/rgbled/rgbled.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 47dbb631cb..fedff769b8 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -1,6 +1,8 @@ /**************************************************************************** * * Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: Julian Oes + * Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,7 +38,6 @@ * * Driver for the onboard RGB LED controller (TCA62724FMG) connected via I2C. * - * */ #include From 2362041dc127620ac1fb823b8aab8416ba17f0c7 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 19 Sep 2013 19:58:41 +0200 Subject: [PATCH 690/872] commander: state indication changed, 3 green blinks = succsess, 3 white blinks = neutral, 3 red blinks = reject --- src/modules/commander/commander.cpp | 119 ++++++++++----------- src/modules/commander/commander_helper.cpp | 50 +++++++-- src/modules/commander/commander_helper.h | 3 +- 3 files changed, 100 insertions(+), 72 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index a2443b0f65..1e86e8e247 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -5,6 +5,7 @@ * Lorenz Meier * Thomas Gubler * Julian Oes + * Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -198,7 +199,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 control_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); @@ -650,7 +651,6 @@ int commander_thread_main(int argc, char *argv[]) bool critical_battery_voltage_actions_done = false; uint64_t last_idle_time = 0; - uint64_t start_time = 0; bool status_changed = true; @@ -728,7 +728,7 @@ int commander_thread_main(int argc, char *argv[]) struct subsystem_info_s info; memset(&info, 0, sizeof(info)); - toggle_status_leds(&status, &armed, true); + control_status_leds(&status, &armed, true); /* now initialized */ commander_initialized = true; @@ -950,11 +950,9 @@ int commander_thread_main(int argc, char *argv[]) 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); + arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); } else { - // XXX should we still allow to arm with critical battery? - //arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); } status_changed = true; @@ -1166,9 +1164,6 @@ 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(); @@ -1228,7 +1223,19 @@ int commander_thread_main(int argc, char *argv[]) fflush(stdout); counter++; - toggle_status_leds(&status, &armed, arming_state_changed || status_changed); + int blink_state = blink_msg_state(); + if (blink_state > 0) { + /* blinking LED message, don't touch LEDs */ + if (blink_state == 2) { + /* blinking LED message completed, restore normal state */ + control_status_leds(&status, &armed, true); + } + } else { + /* normal state */ + control_status_leds(&status, &armed, status_changed); + } + + status_changed = false; usleep(COMMANDER_MONITORING_INTERVAL); } @@ -1276,8 +1283,48 @@ check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *val } void -toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed) +control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed) { + /* driving rgbled */ + if (changed) { + bool set_normal_color = false; + /* set mode */ + if (status->arming_state == ARMING_STATE_ARMED) { + rgbled_set_mode(RGBLED_MODE_ON); + set_normal_color = true; + + } else if (status->arming_state == ARMING_STATE_ARMED_ERROR) { + rgbled_set_mode(RGBLED_MODE_BLINK_FAST); + rgbled_set_color(RGBLED_COLOR_RED); + + } else if (status->arming_state == ARMING_STATE_STANDBY) { + rgbled_set_mode(RGBLED_MODE_BREATHE); + set_normal_color = true; + + } else { // STANDBY_ERROR and other states + rgbled_set_mode(RGBLED_MODE_BLINK_NORMAL); + rgbled_set_color(RGBLED_COLOR_RED); + } + + if (set_normal_color) { + /* set color */ + if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) { + if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) { + rgbled_set_color(RGBLED_COLOR_AMBER); + } + /* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */ + + } else { + if (status->condition_local_position_valid) { + rgbled_set_color(RGBLED_COLOR_GREEN); + + } else { + rgbled_set_color(RGBLED_COLOR_BLUE); + } + } + } + } + #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 /* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */ @@ -1298,54 +1345,6 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang #endif - if (changed) { - /* XXX TODO blink fast when armed and serious error occurs */ - - if (armed->armed) { - rgbled_set_mode(RGBLED_MODE_ON); - - } else if (armed->ready_to_arm) { - rgbled_set_mode(RGBLED_MODE_BREATHE); - - } else { - rgbled_set_mode(RGBLED_MODE_BLINK_FAST); - } - } - - if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) { - switch (status->battery_warning) { - case VEHICLE_BATTERY_WARNING_LOW: - rgbled_set_color(RGBLED_COLOR_YELLOW); - break; - - case VEHICLE_BATTERY_WARNING_CRITICAL: - rgbled_set_color(RGBLED_COLOR_AMBER); - break; - - default: - break; - } - - } else { - switch (status->main_state) { - case MAIN_STATE_MANUAL: - rgbled_set_color(RGBLED_COLOR_WHITE); - break; - - case MAIN_STATE_SEATBELT: - case MAIN_STATE_EASY: - rgbled_set_color(RGBLED_COLOR_GREEN); - break; - - case MAIN_STATE_AUTO: - rgbled_set_color(RGBLED_COLOR_BLUE); - break; - - default: - break; - } - } - /* give system warnings on error LED, XXX maybe add memory usage warning too */ if (status->load > 0.95f) { if (leds_counter % 2 == 0) diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 017499cb51..565b4b66ab 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -3,6 +3,7 @@ * Copyright (C) 2013 PX4 Development Team. All rights reserved. * Author: Thomas Gubler * Julian Oes + * Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -55,7 +56,6 @@ #include #include - #include "commander_helper.h" /* oddly, ERROR is not defined for c++ */ @@ -64,21 +64,24 @@ #endif static const int ERROR = -1; +#define BLINK_MSG_TIME 700000 // 3 fast blinks + 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)); + (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); + || (current_status->system_type == VEHICLE_TYPE_COAXIAL); } static int buzzer; +static hrt_abstime blink_msg_end; int buzzer_init() { @@ -104,16 +107,25 @@ void tune_error() void tune_positive() { + blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; + rgbled_set_color(RGBLED_COLOR_GREEN); + rgbled_set_mode(RGBLED_MODE_BLINK_FAST); ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_POSITIVE_TUNE); } void tune_neutral() { + blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; + rgbled_set_color(RGBLED_COLOR_WHITE); + rgbled_set_mode(RGBLED_MODE_BLINK_FAST); ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEUTRAL_TUNE); } void tune_negative() { + blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; + rgbled_set_color(RGBLED_COLOR_RED); + rgbled_set_mode(RGBLED_MODE_BLINK_FAST); ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE); } @@ -132,18 +144,31 @@ int tune_critical_bat() return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_FAST_TUNE); } - - void tune_stop() { ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE); } +int blink_msg_state() +{ + if (blink_msg_end == 0) { + return 0; + + } else if (hrt_absolute_time() > blink_msg_end) { + return 2; + + } else { + return 1; + } +} + static int leds; static int rgbleds; int led_init() { + blink_msg_end = 0; + /* first open normal LEDs */ leds = open(LED_DEVICE_PATH, 0); @@ -159,6 +184,7 @@ int led_init() warnx("Blue LED: ioctl fail\n"); return ERROR; } + #endif if (ioctl(leds, LED_ON, LED_AMBER)) { @@ -168,6 +194,7 @@ int led_init() /* 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); @@ -203,19 +230,22 @@ int led_off(int led) return ioctl(leds, LED_OFF, led); } -void rgbled_set_color(rgbled_color_t color) { +void rgbled_set_color(rgbled_color_t color) +{ if (rgbleds != -1) ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color); } -void rgbled_set_mode(rgbled_mode_t mode) { +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) { +void rgbled_set_pattern(rgbled_pattern_t *pattern) +{ if (rgbleds != -1) ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern); diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index 027d0535f0..e9514446c1 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -62,6 +62,7 @@ int tune_arm(void); int tune_low_bat(void); int tune_critical_bat(void); void tune_stop(void); +int blink_msg_state(); int led_init(void); void led_deinit(void); @@ -70,9 +71,7 @@ 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); /** From d542735b2a945ed874fab8b004f9c31e87bc1346 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 19 Sep 2013 11:10:37 +0200 Subject: [PATCH 691/872] re-enable state hil --- .../init.d/{1000_rc.hil => 1000_rc_fw.hil} | 0 .../px4fmu_common/init.d/1002_rc_fw_state.hil | 84 +++++++++++++++++++ ROMFS/px4fmu_common/init.d/rcS | 13 ++- src/modules/mavlink/mavlink_receiver.cpp | 1 + 4 files changed, 95 insertions(+), 3 deletions(-) rename ROMFS/px4fmu_common/init.d/{1000_rc.hil => 1000_rc_fw.hil} (100%) create mode 100644 ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil diff --git a/ROMFS/px4fmu_common/init.d/1000_rc.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw.hil similarity index 100% rename from ROMFS/px4fmu_common/init.d/1000_rc.hil rename to ROMFS/px4fmu_common/init.d/1000_rc_fw.hil diff --git a/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil b/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil new file mode 100644 index 0000000000..d5782a89b8 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil @@ -0,0 +1,84 @@ +#!nsh +# +# USB HIL start +# + +echo "[HIL] HILStar starting in state-HIL mode.." + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 15 + param set FW_P_LIM_MAX 50 + param set FW_P_LIM_MIN -50 + param set FW_P_P 60 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 1.1 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 100 + param set FW_R_RMAX 100 + param set FW_THR_CRUISE 0.65 + param set FW_THR_MAX 1 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5.0 + param set FW_T_SINK_MIN 4.0 + param set FW_Y_ROLLFF 1.1 + + param set SYS_AUTOCONFIG 0 + param save +fi + +# Allow USB some time to come up +sleep 1 +# Tell MAVLink that this link is "fast" +mavlink start -b 230400 -d /dev/ttyACM0 + +# Create a fake HIL /dev/pwm_output interface +hil mode_pwm + +# +# 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 commander (depends on orb, mavlink) +# +commander start + +# +# Check if we got an IO +# +if px4io start +then + echo "IO started" +else + fmu mode_serial + echo "FMU started" +fi + +# +# Start the sensors (depends on orb, px4io) +# +sh /etc/init.d/rc.sensors + +# +# Load mixer and start controllers (depends on px4io) +# +mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix +fw_pos_control_l1 start +fw_att_control start + +echo "[HIL] setup done, running" + diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index c2a8c0c6ae..9ec346465a 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -108,15 +108,22 @@ then if param compare SYS_AUTOSTART 1000 then - sh /etc/init.d/1000_rc.hil + sh /etc/init.d/1000_rc_fw.hil set MODE custom else if param compare SYS_AUTOSTART 1001 then sh /etc/init.d/1001_rc_quad.hil + set MODE custom else - # Try to get an USB console - nshterm /dev/ttyACM0 & + if param compare SYS_AUTOSTART 1002 + then + sh /etc/init.d/1002_rc_fw_state.hil + set MODE custom + else + # Try to get an USB console + nshterm /dev/ttyACM0 & + fi fi fi diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 222d1f45f2..a3ef1d63b9 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -574,6 +574,7 @@ handle_message(mavlink_message_t *msg) orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed); } + hil_global_pos.valid = true; hil_global_pos.lat = hil_state.lat; hil_global_pos.lon = hil_state.lon; hil_global_pos.alt = hil_state.alt / 1000.0f; From 3851bf5c10181fe0f56af40fc7e35a3b72bbb845 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 20 Sep 2013 19:40:50 +0200 Subject: [PATCH 692/872] Hotfix: Improve UART1 receive performance --- nuttx-configs/px4fmu-v1/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index e43b9c18e8..ba211ccb23 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -262,7 +262,7 @@ CONFIG_STM32_USART=y # U[S]ART Configuration # # CONFIG_USART1_RS485 is not set -# CONFIG_USART1_RXDMA is not set +CONFIG_USART1_RXDMA=y # CONFIG_USART2_RS485 is not set CONFIG_USART2_RXDMA=y # CONFIG_USART3_RXDMA is not set From cd8854e622793b3f3e7104d29f06e614d4dfac42 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 20 Sep 2013 19:59:48 +0200 Subject: [PATCH 693/872] Hotfix: Make param saving relatively robust --- src/modules/systemlib/param/param.c | 42 +++++++++++++++++++++++------ 1 file changed, 34 insertions(+), 8 deletions(-) diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 59cbcf5fb0..ccdb2ea38b 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -509,31 +509,57 @@ int param_save_default(void) { int result; + unsigned retries = 0; /* delete the file in case it exists */ struct stat buffer; if (stat(param_get_default_file(), &buffer) == 0) { - result = unlink(param_get_default_file()); + + do { + result = unlink(param_get_default_file()); + if (result != 0) { + retries++; + usleep(1000 * retries); + } + } while (result != OK && retries < 10); + if (result != OK) warnx("unlinking file %s failed.", param_get_default_file()); } /* create the file */ - int fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL); + int fd; + + do { + /* do another attempt in case the unlink call is not synced yet */ + fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL); + if (fd < 0) { + retries++; + usleep(1000 * retries); + } + + } while (fd < 0 && retries < 10); if (fd < 0) { - /* do another attempt in case the unlink call is not synced yet */ - usleep(5000); - fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL); - + warn("opening '%s' for writing failed", param_get_default_file()); return fd; } - result = param_export(fd, false); + do { + result = param_export(fd, false); + + if (result != OK) { + retries++; + usleep(1000 * retries); + } + + } while (result != 0 && retries < 10); + + close(fd); - if (result != 0) { + if (result != OK) { warn("error exporting parameters to '%s'", param_get_default_file()); (void)unlink(param_get_default_file()); return result; From 8d6a47546a32e6911a0f769070511baa6549ed03 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 21 Sep 2013 08:59:04 +0200 Subject: [PATCH 694/872] gps: fixed code style, more informative and clean messages --- src/drivers/gps/gps.cpp | 116 ++++++++++++++++++++++----------- src/drivers/gps/gps_helper.cpp | 7 +- src/drivers/gps/gps_helper.h | 2 +- src/drivers/gps/mtk.cpp | 26 ++++++-- src/drivers/gps/ubx.cpp | 58 ++++++++++------- src/drivers/gps/ubx.h | 6 +- 6 files changed, 142 insertions(+), 73 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 38835418b0..dd21fe61d4 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -85,7 +85,7 @@ static const int ERROR = -1; class GPS : public device::CDev { public: - GPS(const char* uart_path); + GPS(const char *uart_path); virtual ~GPS(); virtual int init(); @@ -156,7 +156,7 @@ GPS *g_dev; } -GPS::GPS(const char* uart_path) : +GPS::GPS(const char *uart_path) : CDev("gps", GPS_DEVICE_PATH), _task_should_exit(false), _healthy(false), @@ -192,6 +192,7 @@ GPS::~GPS() /* well, kill it anyway, though this will probably crash */ if (_task != -1) task_delete(_task); + g_dev = nullptr; } @@ -270,19 +271,24 @@ GPS::task_main() } switch (_mode) { - case GPS_DRIVER_MODE_UBX: - _Helper = new UBX(_serial_fd, &_report); - break; - case GPS_DRIVER_MODE_MTK: - _Helper = new MTK(_serial_fd, &_report); - break; - case GPS_DRIVER_MODE_NMEA: - //_Helper = new NMEA(); //TODO: add NMEA - break; - default: - break; + case GPS_DRIVER_MODE_UBX: + _Helper = new UBX(_serial_fd, &_report); + break; + + case GPS_DRIVER_MODE_MTK: + _Helper = new MTK(_serial_fd, &_report); + break; + + case GPS_DRIVER_MODE_NMEA: + //_Helper = new NMEA(); //TODO: add NMEA + break; + + default: + break; } + unlock(); + if (_Helper->configure(_baudrate) == 0) { unlock(); @@ -294,6 +300,7 @@ GPS::task_main() /* opportunistic publishing - else invalid data would end up on the bus */ if (_report_pub > 0) { orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + } else { _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); } @@ -310,10 +317,30 @@ GPS::task_main() } if (!_healthy) { - warnx("module found"); + char *mode_str = "unknown"; + + switch (_mode) { + case GPS_DRIVER_MODE_UBX: + mode_str = "ubx"; + break; + + case GPS_DRIVER_MODE_MTK: + mode_str = "mtk"; + break; + + case GPS_DRIVER_MODE_NMEA: + mode_str = "nmea"; + break; + + default: + break; + } + + warnx("module found: %s", mode_str); _healthy = true; } } + if (_healthy) { warnx("module lost"); _healthy = false; @@ -322,24 +349,28 @@ GPS::task_main() lock(); } + lock(); /* select next mode */ switch (_mode) { - case GPS_DRIVER_MODE_UBX: - _mode = GPS_DRIVER_MODE_MTK; - break; - case GPS_DRIVER_MODE_MTK: - _mode = GPS_DRIVER_MODE_UBX; - break; - // case GPS_DRIVER_MODE_NMEA: - // _mode = GPS_DRIVER_MODE_UBX; - // break; - default: - break; + case GPS_DRIVER_MODE_UBX: + _mode = GPS_DRIVER_MODE_MTK; + break; + + case GPS_DRIVER_MODE_MTK: + _mode = GPS_DRIVER_MODE_UBX; + break; + + // case GPS_DRIVER_MODE_NMEA: + // _mode = GPS_DRIVER_MODE_UBX; + // break; + default: + break; } } + debug("exiting"); ::close(_serial_fd); @@ -361,22 +392,27 @@ void GPS::print_info() { switch (_mode) { - case GPS_DRIVER_MODE_UBX: - warnx("protocol: UBX"); - break; - case GPS_DRIVER_MODE_MTK: - warnx("protocol: MTK"); - break; - case GPS_DRIVER_MODE_NMEA: - warnx("protocol: NMEA"); - break; - default: - break; + case GPS_DRIVER_MODE_UBX: + warnx("protocol: UBX"); + break; + + case GPS_DRIVER_MODE_MTK: + warnx("protocol: MTK"); + break; + + case GPS_DRIVER_MODE_NMEA: + warnx("protocol: NMEA"); + break; + + default: + break; } + warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); + if (_report.timestamp_position != 0) { warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type, - (double)((float)(hrt_absolute_time() - _report.timestamp_position) / 1000000.0f)); + (double)((float)(hrt_absolute_time() - _report.timestamp_position) / 1000000.0f)); warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt); warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate()); warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate()); @@ -428,6 +464,7 @@ start(const char *uart_path) errx(1, "Could not open device path: %s\n", GPS_DEVICE_PATH); goto fail; } + exit(0); fail: @@ -503,7 +540,7 @@ gps_main(int argc, char *argv[]) { /* set to default */ - char* device_name = GPS_DEFAULT_UART_PORT; + char *device_name = GPS_DEFAULT_UART_PORT; /* * Start/load the driver. @@ -513,15 +550,18 @@ gps_main(int argc, char *argv[]) if (argc > 3) { if (!strcmp(argv[2], "-d")) { device_name = argv[3]; + } else { goto out; } } + gps::start(device_name); } if (!strcmp(argv[1], "stop")) gps::stop(); + /* * Test the driver/device. */ diff --git a/src/drivers/gps/gps_helper.cpp b/src/drivers/gps/gps_helper.cpp index ba86d370a8..2e2cbc8ddf 100644 --- a/src/drivers/gps/gps_helper.cpp +++ b/src/drivers/gps/gps_helper.cpp @@ -87,13 +87,15 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud) case 115200: speed = B115200; break; - warnx("try baudrate: %d\n", speed); + warnx("try baudrate: %d\n", speed); default: warnx("ERROR: Unsupported baudrate: %d\n", baud); return -EINVAL; } + struct termios uart_config; + int termios_state; /* fill the struct for the new configuration */ @@ -109,14 +111,17 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud) warnx("ERROR setting config: %d (cfsetispeed)\n", termios_state); return -1; } + if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { warnx("ERROR setting config: %d (cfsetospeed)\n", termios_state); return -1; } + if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) { warnx("ERROR setting baudrate (tcsetattr)\n"); return -1; } + /* XXX if resetting the parser here, ensure it does exist (check for null pointer) */ return 0; } diff --git a/src/drivers/gps/gps_helper.h b/src/drivers/gps/gps_helper.h index defc1a074a..73d4b889cb 100644 --- a/src/drivers/gps/gps_helper.h +++ b/src/drivers/gps/gps_helper.h @@ -33,7 +33,7 @@ * ****************************************************************************/ -/** +/** * @file gps_helper.h */ diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp index 62941d74b3..56b702ea6c 100644 --- a/src/drivers/gps/mtk.cpp +++ b/src/drivers/gps/mtk.cpp @@ -48,9 +48,9 @@ MTK::MTK(const int &fd, struct vehicle_gps_position_s *gps_position) : -_fd(fd), -_gps_position(gps_position), -_mtk_revision(0) + _fd(fd), + _gps_position(gps_position), + _mtk_revision(0) { decode_init(); } @@ -73,24 +73,28 @@ MTK::configure(unsigned &baudrate) warnx("mtk: config write failed"); return -1; } + usleep(10000); if (strlen(MTK_SET_BINARY) != write(_fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) { warnx("mtk: config write failed"); return -1; } + usleep(10000); if (strlen(SBAS_ON) != write(_fd, SBAS_ON, strlen(SBAS_ON))) { warnx("mtk: config write failed"); return -1; } + usleep(10000); if (strlen(WAAS_ON) != write(_fd, WAAS_ON, strlen(WAAS_ON))) { warnx("mtk: config write failed"); return -1; } + usleep(10000); if (strlen(MTK_NAVTHRES_OFF) != write(_fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) { @@ -128,12 +132,15 @@ MTK::receive(unsigned timeout) handle_message(packet); return 1; } + /* in case we keep trying but only get crap from GPS */ - if (time_started + timeout*1000 < hrt_absolute_time() ) { + if (time_started + timeout * 1000 < hrt_absolute_time()) { return -1; } + j++; } + /* everything is read */ j = count = 0; } @@ -181,6 +188,7 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet) if (b == MTK_SYNC1_V16) { _decode_state = MTK_DECODE_GOT_CK_A; _mtk_revision = 16; + } else if (b == MTK_SYNC1_V19) { _decode_state = MTK_DECODE_GOT_CK_A; _mtk_revision = 19; @@ -201,7 +209,7 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet) add_byte_to_checksum(b); // Fill packet buffer - ((uint8_t*)(&packet))[_rx_count] = b; + ((uint8_t *)(&packet))[_rx_count] = b; _rx_count++; /* Packet size minus checksum, XXX ? */ @@ -209,14 +217,17 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet) /* Compare checksum */ if (_rx_ck_a == packet.ck_a && _rx_ck_b == packet.ck_b) { ret = 1; + } else { warnx("MTK Checksum invalid"); ret = -1; } + // Reset state machine to decode next packet decode_init(); } } + return ret; } @@ -226,19 +237,22 @@ MTK::handle_message(gps_mtk_packet_t &packet) if (_mtk_revision == 16) { _gps_position->lat = packet.latitude * 10; // from degrees*1e6 to degrees*1e7 _gps_position->lon = packet.longitude * 10; // from degrees*1e6 to degrees*1e7 + } else if (_mtk_revision == 19) { _gps_position->lat = packet.latitude; // both degrees*1e7 _gps_position->lon = packet.longitude; // both degrees*1e7 + } else { warnx("mtk: unknown revision"); _gps_position->lat = 0; _gps_position->lon = 0; } + _gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm _gps_position->fix_type = packet.fix_type; _gps_position->eph_m = packet.hdop; // XXX: Check this because eph_m is in m and hdop is without unit _gps_position->epv_m = 0.0; //unknown in mtk custom mode - _gps_position->vel_m_s = ((float)packet.ground_speed)*1e-2f; // from cm/s to m/s + _gps_position->vel_m_s = ((float)packet.ground_speed) * 1e-2f; // from cm/s to m/s _gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad _gps_position->satellites_visible = packet.satellites; diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index ba5d14cc49..0b25b379fd 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -60,13 +60,14 @@ #define UBX_CONFIG_TIMEOUT 200 // ms, timeout for waiting ACK #define UBX_PACKET_TIMEOUT 2 // ms, if now data during this delay assume that full update received #define UBX_WAIT_BEFORE_READ 20 // ms, wait before reading to save read() calls +#define DISABLE_MSG_INTERVAL 1000000 // us, try to disable message with this interval UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) : _fd(fd), _gps_position(gps_position), _configured(false), _waiting_for_ack(false), - _disable_cmd_counter(0) + _disable_cmd_last(0) { decode_init(); } @@ -191,35 +192,35 @@ UBX::configure(unsigned &baudrate) configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH, 1); if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("ubx: msg rate configuration failed: NAV POSLLH\n"); + warnx("ubx: msg rate configuration failed: NAV POSLLH"); return 1; } configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, 1); if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("ubx: msg rate configuration failed: NAV TIMEUTC\n"); + warnx("ubx: msg rate configuration failed: NAV TIMEUTC"); return 1; } configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL, 1); if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("ubx: msg rate configuration failed: NAV SOL\n"); + warnx("ubx: msg rate configuration failed: NAV SOL"); return 1; } configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED, 1); if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("ubx: msg rate configuration failed: NAV VELNED\n"); + warnx("ubx: msg rate configuration failed: NAV VELNED"); return 1; } configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5); if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("ubx: msg rate configuration failed: NAV SVINFO\n"); + warnx("ubx: msg rate configuration failed: NAV SVINFO"); return 1; } @@ -271,11 +272,18 @@ UBX::receive(unsigned timeout) if (ret < 0) { /* something went wrong when polling */ + warnx("ubx: poll error"); return -1; } else if (ret == 0) { /* return success after short delay after receiving a packet or timeout after long delay */ - return handled ? 1 : -1; + if (handled) { + return 1; + + } else { + warnx("ubx: timeout - no messages"); + return -1; + } } else if (ret > 0) { /* if we have new data from GPS, go handle it */ @@ -292,8 +300,6 @@ UBX::receive(unsigned timeout) /* pass received bytes to the packet decoder */ for (int i = 0; i < count; i++) { if (parse_char(buf[i]) > 0) { - /* return to configure during configuration or to the gps driver during normal work - * if a packet has arrived */ if (handle_message() > 0) handled = true; } @@ -303,6 +309,7 @@ UBX::receive(unsigned timeout) /* abort after timeout if no useful packets received */ if (time_started + timeout * 1000 < hrt_absolute_time()) { + warnx("ubx: timeout - no useful messages"); return -1; } } @@ -453,16 +460,16 @@ UBX::handle_message() timeinfo.tm_min = packet->min; timeinfo.tm_sec = packet->sec; time_t epoch = mktime(&timeinfo); - + #ifndef CONFIG_RTC - //Since we lack a hardware RTC, set the system time clock based on GPS UTC - //TODO generalize this by moving into gps.cpp? - timespec ts; - ts.tv_sec = epoch; - ts.tv_nsec = packet->time_nanoseconds; - clock_settime(CLOCK_REALTIME,&ts); + //Since we lack a hardware RTC, set the system time clock based on GPS UTC + //TODO generalize this by moving into gps.cpp? + timespec ts; + ts.tv_sec = epoch; + ts.tv_nsec = packet->time_nanoseconds; + clock_settime(CLOCK_REALTIME, &ts); #endif - + _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this _gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f); _gps_position->timestamp_time = hrt_absolute_time(); @@ -564,10 +571,13 @@ UBX::handle_message() if (ret == 0) { /* message not handled */ - warnx("ubx: unknown message received: 0x%02x-0x%02x\n", (unsigned)_message_class, (unsigned)_message_id); + warnx("ubx: unknown message received: 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id); - if ((_disable_cmd_counter = _disable_cmd_counter++ % 10) == 0) { + hrt_abstime t = hrt_absolute_time(); + + if (t > _disable_cmd_last + DISABLE_MSG_INTERVAL) { /* don't attempt for every message to disable, some might not be disabled */ + _disable_cmd_last = t; warnx("ubx: disabling message 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id); configure_message_rate(_message_class, _message_id, 0); } @@ -640,7 +650,7 @@ UBX::add_checksum_to_message(uint8_t *message, const unsigned length) ck_b = ck_b + ck_a; } - /* The checksum is written to the last to bytes of a message */ + /* the checksum is written to the last to bytes of a message */ message[length - 2] = ck_a; message[length - 1] = ck_b; } @@ -669,17 +679,17 @@ UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length) { ssize_t ret = 0; - /* Calculate the checksum now */ + /* calculate the checksum now */ add_checksum_to_message(packet, length); const uint8_t sync_bytes[] = {UBX_SYNC1, UBX_SYNC2}; - /* Start with the two sync bytes */ + /* start with the two sync bytes */ ret += write(fd, sync_bytes, sizeof(sync_bytes)); ret += write(fd, packet, length); if (ret != (int)length + (int)sizeof(sync_bytes)) // XXX is there a neater way to get rid of the unsigned signed warning? - warnx("ubx: config write fail"); + warnx("ubx: configuration write fail"); } void @@ -696,7 +706,7 @@ UBX::send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size) add_checksum((uint8_t *)&header.msg_class, sizeof(header) - 2, ck_a, ck_b); add_checksum((uint8_t *)msg, size, ck_a, ck_b); - // Configure receive check + /* configure ACK check */ _message_class_needed = msg_class; _message_id_needed = msg_id; diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 4fc2769750..76ef873a36 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -347,7 +347,7 @@ private: /** * Add the two checksum bytes to an outgoing message */ - void add_checksum_to_message(uint8_t* message, const unsigned length); + void add_checksum_to_message(uint8_t *message, const unsigned length); /** * Helper to send a config packet @@ -358,7 +358,7 @@ private: void send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size); - void add_checksum(uint8_t* message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b); + void add_checksum(uint8_t *message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b); int wait_for_ack(unsigned timeout); @@ -376,7 +376,7 @@ private: uint8_t _message_class; uint8_t _message_id; unsigned _payload_size; - uint8_t _disable_cmd_counter; + uint8_t _disable_cmd_last; }; #endif /* UBX_H_ */ From 42f930908fc3f60e6305257f25b625830a020a80 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 21 Sep 2013 10:13:50 +0200 Subject: [PATCH 695/872] gps: more cleanup, some more info in 'gps status' --- src/drivers/gps/gps.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index dd21fe61d4..a84cb8e59f 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -321,15 +321,15 @@ GPS::task_main() switch (_mode) { case GPS_DRIVER_MODE_UBX: - mode_str = "ubx"; + mode_str = "UBX"; break; case GPS_DRIVER_MODE_MTK: - mode_str = "mtk"; + mode_str = "MTK"; break; case GPS_DRIVER_MODE_NMEA: - mode_str = "nmea"; + mode_str = "NMEA"; break; default: @@ -371,7 +371,7 @@ GPS::task_main() } - debug("exiting"); + warnx("exiting"); ::close(_serial_fd); @@ -411,9 +411,10 @@ GPS::print_info() warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); if (_report.timestamp_position != 0) { - warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type, - (double)((float)(hrt_absolute_time() - _report.timestamp_position) / 1000000.0f)); + warnx("position lock: %dD, satellites: %d, last update: %fms ago", (int)_report.fix_type, + _report.satellites_visible, (hrt_absolute_time() - _report.timestamp_position) / 1000.0f); warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt); + warnx("eph: %.2fm, epv: %.2fm", _report.eph_m, _report.epv_m); warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate()); warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate()); warnx("rate publication:\t%6.2f Hz", (double)_rate); From 32fbf80ab84d3d8ebcb93ec1d7f20470ebb4db1f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 21 Sep 2013 10:17:00 +0200 Subject: [PATCH 696/872] mavlink: EPH/EPV casting issue fixed --- src/modules/mavlink/orb_listener.c | 23 ++++++++++++++++++++--- 1 file changed, 20 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index d11a67fc6f..ed02b17e1e 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -99,6 +99,8 @@ struct listener { uintptr_t arg; }; +uint16_t cm_uint16_from_m_float(float m); + static void l_sensor_combined(const struct listener *l); static void l_vehicle_attitude(const struct listener *l); static void l_vehicle_gps_position(const struct listener *l); @@ -150,6 +152,19 @@ static const struct listener listeners[] = { static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]); +uint16_t +cm_uint16_from_m_float(float m) +{ + if (m < 0.0f) { + return 0; + + } else if (m > 655.35f) { + return 65535; + } + + return m * 0.01f; +} + void l_sensor_combined(const struct listener *l) { @@ -235,8 +250,10 @@ l_vehicle_gps_position(const struct listener *l) /* GPS COG is 0..2PI in degrees * 1e2 */ float cog_deg = gps.cog_rad; + if (cog_deg > M_PI_F) cog_deg -= 2.0f * M_PI_F; + cog_deg *= M_RAD_TO_DEG_F; @@ -247,10 +264,10 @@ l_vehicle_gps_position(const struct listener *l) gps.lat, gps.lon, gps.alt, - gps.eph_m * 1e2f, // from m to cm - gps.epv_m * 1e2f, // from m to cm + cm_uint16_from_m_float(gps.eph_m), + cm_uint16_from_m_float(gps.epv_m), gps.vel_m_s * 1e2f, // from m/s to cm/s - cog_deg * 1e2f, // from rad to deg * 100 + cog_deg * 1e2f, // from deg to deg * 100 gps.satellites_visible); /* update SAT info every 10 seconds */ From b56723af2ad8843870e814c6451189ecddbae750 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 21 Sep 2013 10:20:26 +0200 Subject: [PATCH 697/872] mavlink: bugfix --- src/modules/mavlink/orb_listener.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index ed02b17e1e..cec2fdc431 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -162,7 +162,7 @@ cm_uint16_from_m_float(float m) return 65535; } - return m * 0.01f; + return (uint16_t)(m * 100.0f); } void From 327f1f8001c3564a5a76d59b0b1044301b4cebf0 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 21 Sep 2013 11:23:42 +0200 Subject: [PATCH 698/872] Look for the appropriate images in the uploader --- src/drivers/px4io/px4io.cpp | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index f9fb9eea96..b66d425dd0 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2245,7 +2245,7 @@ px4io_main(int argc, char *argv[]) } PX4IO_Uploader *up; - const char *fn[5]; + const char *fn[3]; /* work out what we're uploading... */ if (argc > 2) { @@ -2253,11 +2253,19 @@ px4io_main(int argc, char *argv[]) fn[1] = nullptr; } else { - fn[0] = "/etc/extras/px4io-v2_default.bin"; - fn[1] = "/etc/extras/px4io-v1_default.bin"; +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) + fn[0] = "/etc/extras/px4io-v1_default.bin"; + fn[1] = "/fs/microsd/px4io1.bin"; fn[2] = "/fs/microsd/px4io.bin"; - fn[3] = "/fs/microsd/px4io2.bin"; - fn[4] = nullptr; + fn[3] = nullptr; +#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) + fn[0] = "/etc/extras/px4io-v2_default.bin"; + fn[1] = "/fs/microsd/px4io2.bin"; + fn[2] = "/fs/microsd/px4io.bin"; + fn[3] = nullptr; +#else +#error "unknown board" +#endif } up = new PX4IO_Uploader; From f62aeba4207beaeeff63af970ec5d6bb2fb1e8a7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Sep 2013 11:16:19 +0200 Subject: [PATCH 699/872] Cover last potential corner case with mixers, should be totally safe now --- src/modules/systemlib/mixer/mixer.cpp | 6 +++++ .../systemlib/mixer/mixer_multirotor.cpp | 7 ++++++ src/modules/systemlib/mixer/mixer_simple.cpp | 23 +++++++++++++++---- 3 files changed, 32 insertions(+), 4 deletions(-) diff --git a/src/modules/systemlib/mixer/mixer.cpp b/src/modules/systemlib/mixer/mixer.cpp index df0dfe838d..7d9ddba8f2 100644 --- a/src/modules/systemlib/mixer/mixer.cpp +++ b/src/modules/systemlib/mixer/mixer.cpp @@ -142,6 +142,12 @@ NullMixer * NullMixer::from_text(const char *buf, unsigned &buflen) { NullMixer *nm = nullptr; + const char *end = buf + buflen; + + /* require a space or newline at the end of the buffer */ + if (*end != ' ' && *end != '\n' && *end != '\r') { + return nm; + } if ((buflen >= 2) && (buf[0] == 'Z') && (buf[1] == ':')) { nm = new NullMixer; diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 8ded0b05c1..576af5e303 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -181,6 +181,13 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl char geomname[8]; int s[4]; int used; + const char *end = buf + buflen; + + /* require a space or newline at the end of the buffer */ + if (*end != ' ' && *end != '\n' && *end != '\r') { + debug("multirotor parser rejected: No newline / space at end of buf."); + return nullptr; + } if (sscanf(buf, "R: %s %d %d %d %d%n", geomname, &s[0], &s[1], &s[2], &s[3], &used) != 5) { debug("multirotor parse failed on '%s'", buf); diff --git a/src/modules/systemlib/mixer/mixer_simple.cpp b/src/modules/systemlib/mixer/mixer_simple.cpp index 07dc5f37fa..44b6470f07 100644 --- a/src/modules/systemlib/mixer/mixer_simple.cpp +++ b/src/modules/systemlib/mixer/mixer_simple.cpp @@ -100,8 +100,10 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler int s[5]; buf = findtag(buf, buflen, 'O'); - if ((buf == nullptr) || (buflen < 12)) + if ((buf == nullptr) || (buflen < 12)) { + debug("output parser failed finding tag, ret: '%s'", buf); return -1; + } if ((ret = sscanf(buf, "O: %d %d %d %d %d", &s[0], &s[1], &s[2], &s[3], &s[4])) != 5) { @@ -126,8 +128,10 @@ SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scale int s[5]; buf = findtag(buf, buflen, 'S'); - if ((buf == nullptr) || (buflen < 16)) + if ((buf == nullptr) || (buflen < 16)) { + debug("contorl parser failed finding tag, ret: '%s'", buf); return -1; + } if (sscanf(buf, "S: %u %u %d %d %d %d %d", &u[0], &u[1], &s[0], &s[1], &s[2], &s[3], &s[4]) != 7) { @@ -156,6 +160,12 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c int used; const char *end = buf + buflen; + /* require a space or newline at the end of the buffer */ + if (*end != ' ' && *end != '\n' && *end != '\r') { + debug("simple parser rejected: No newline / space at end of buf."); + goto out; + } + /* get the base info for the mixer */ if (sscanf(buf, "M: %u%n", &inputs, &used) != 1) { debug("simple parse failed on '%s'", buf); @@ -173,15 +183,20 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c mixinfo->control_count = inputs; - if (parse_output_scaler(end - buflen, buflen, mixinfo->output_scaler)) + if (parse_output_scaler(end - buflen, buflen, mixinfo->output_scaler)) { + debug("simple mixer parser failed parsing out scaler tag, ret: '%s'", buf); goto out; + } for (unsigned i = 0; i < inputs; i++) { if (parse_control_scaler(end - buflen, buflen, mixinfo->controls[i].scaler, mixinfo->controls[i].control_group, - mixinfo->controls[i].control_index)) + mixinfo->controls[i].control_index)) { + debug("simple mixer parser failed parsing ctrl scaler tag, ret: '%s'", buf); goto out; + } + } sm = new SimpleMixer(control_cb, cb_handle, mixinfo); From ebd16975d01b3a2200e94e164199b88527a27e3c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Sep 2013 11:30:37 +0200 Subject: [PATCH 700/872] An even number of bytes is when modulo 2 is zero, not modulo 1 is one. --- 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 f9fb9eea96..aca411b1e5 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1509,7 +1509,7 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) * even. */ unsigned total_len = sizeof(px4io_mixdata) + count; - if (total_len % 1) { + if (total_len % 2) { msg->text[count] = '\0'; total_len++; } From 9eb4e05c9dd1c9116fbe58da76fa5f750cc8911a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Sep 2013 12:04:30 +0200 Subject: [PATCH 701/872] Hotfix: Silence GPS driver if no GPS is connected --- src/drivers/drv_gps.h | 3 +-- src/drivers/gps/gps.cpp | 15 --------------- src/drivers/gps/ubx.cpp | 1 - 3 files changed, 1 insertion(+), 18 deletions(-) diff --git a/src/drivers/drv_gps.h b/src/drivers/drv_gps.h index 1dda8ef0b8..398cf48701 100644 --- a/src/drivers/drv_gps.h +++ b/src/drivers/drv_gps.h @@ -51,8 +51,7 @@ typedef enum { GPS_DRIVER_MODE_NONE = 0, GPS_DRIVER_MODE_UBX, - GPS_DRIVER_MODE_MTK, - GPS_DRIVER_MODE_NMEA, + GPS_DRIVER_MODE_MTK } gps_driver_mode_t; diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index a84cb8e59f..fc500a9ecd 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -279,10 +279,6 @@ GPS::task_main() _Helper = new MTK(_serial_fd, &_report); break; - case GPS_DRIVER_MODE_NMEA: - //_Helper = new NMEA(); //TODO: add NMEA - break; - default: break; } @@ -328,10 +324,6 @@ GPS::task_main() mode_str = "MTK"; break; - case GPS_DRIVER_MODE_NMEA: - mode_str = "NMEA"; - break; - default: break; } @@ -362,9 +354,6 @@ GPS::task_main() _mode = GPS_DRIVER_MODE_UBX; break; - // case GPS_DRIVER_MODE_NMEA: - // _mode = GPS_DRIVER_MODE_UBX; - // break; default: break; } @@ -400,10 +389,6 @@ GPS::print_info() warnx("protocol: MTK"); break; - case GPS_DRIVER_MODE_NMEA: - warnx("protocol: NMEA"); - break; - default: break; } diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 0b25b379fd..86291901cb 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -281,7 +281,6 @@ UBX::receive(unsigned timeout) return 1; } else { - warnx("ubx: timeout - no messages"); return -1; } From 8483670fed557cc371da2a088c40bed38864a858 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Sep 2013 12:17:07 +0200 Subject: [PATCH 702/872] Hot hotfix: Change the number of preallocated smeaphores on both boards to the same value. --- 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 8f99143102..372453fb6d 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -410,7 +410,7 @@ CONFIG_START_DAY=1 CONFIG_DEV_CONSOLE=y # CONFIG_MUTEX_TYPES is not set CONFIG_PRIORITY_INHERITANCE=y -CONFIG_SEM_PREALLOCHOLDERS=8 +CONFIG_SEM_PREALLOCHOLDERS=0 CONFIG_SEM_NNESTPRIO=8 # CONFIG_FDCLONE_DISABLE is not set CONFIG_FDCLONE_STDIO=y From 9424728af9ca0c78890845052589a9a5751ff084 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Sep 2013 14:19:47 +0200 Subject: [PATCH 703/872] Fix a whole bunch of sanity checks across all mixers --- src/modules/px4iofirmware/mixer.cpp | 2 +- src/modules/systemlib/mixer/mixer.cpp | 16 ++++++++-- .../systemlib/mixer/mixer_multirotor.cpp | 18 +++++++++--- src/modules/systemlib/mixer/mixer_simple.cpp | 29 +++++++++++++------ 4 files changed, 48 insertions(+), 17 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 0edd91b243..30aff7d20a 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -221,7 +221,7 @@ mixer_tick(void) } /* do the calculations during the ramp for all at once */ - if(esc_state == ESC_RAMP) { + if (esc_state == ESC_RAMP) { ramp_promille = (1000*(hrt_elapsed_time(&esc_init_time)-ESC_INIT_TIME_US))/ESC_RAMP_TIME_US; } diff --git a/src/modules/systemlib/mixer/mixer.cpp b/src/modules/systemlib/mixer/mixer.cpp index 7d9ddba8f2..b1bb1a66d7 100644 --- a/src/modules/systemlib/mixer/mixer.cpp +++ b/src/modules/systemlib/mixer/mixer.cpp @@ -144,9 +144,19 @@ NullMixer::from_text(const char *buf, unsigned &buflen) NullMixer *nm = nullptr; const char *end = buf + buflen; - /* require a space or newline at the end of the buffer */ - if (*end != ' ' && *end != '\n' && *end != '\r') { - return nm; + /* enforce that the mixer ends with space or a new line */ + for (int i = buflen - 1; i >= 0; i--) { + if (buf[i] == '\0') + continue; + + /* require a space or newline at the end of the buffer, fail on printable chars */ + if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') { + /* found a line ending or space, so no split symbols / numbers. good. */ + break; + } else { + return nm; + } + } if ((buflen >= 2) && (buf[0] == 'Z') && (buf[1] == ':')) { diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 576af5e303..2446cc3fbe 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -183,10 +183,20 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl int used; const char *end = buf + buflen; - /* require a space or newline at the end of the buffer */ - if (*end != ' ' && *end != '\n' && *end != '\r') { - debug("multirotor parser rejected: No newline / space at end of buf."); - return nullptr; + /* enforce that the mixer ends with space or a new line */ + for (int i = buflen - 1; i >= 0; i--) { + if (buf[i] == '\0') + continue; + + /* require a space or newline at the end of the buffer, fail on printable chars */ + if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') { + /* found a line ending or space, so no split symbols / numbers. good. */ + break; + } else { + debug("simple parser rejected: No newline / space at end of buf. (#%d/%d: 0x%02x)", i, buflen-1, buf[i]); + return nullptr; + } + } if (sscanf(buf, "R: %s %d %d %d %d%n", geomname, &s[0], &s[1], &s[2], &s[3], &used) != 5) { diff --git a/src/modules/systemlib/mixer/mixer_simple.cpp b/src/modules/systemlib/mixer/mixer_simple.cpp index 44b6470f07..c8434f991b 100644 --- a/src/modules/systemlib/mixer/mixer_simple.cpp +++ b/src/modules/systemlib/mixer/mixer_simple.cpp @@ -55,7 +55,7 @@ #include "mixer.h" #define debug(fmt, args...) do { } while(0) -//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) +// #define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) SimpleMixer::SimpleMixer(ControlCallback control_cb, uintptr_t cb_handle, @@ -98,6 +98,7 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler { int ret; int s[5]; + int n = -1; buf = findtag(buf, buflen, 'O'); if ((buf == nullptr) || (buflen < 12)) { @@ -105,9 +106,9 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler return -1; } - if ((ret = sscanf(buf, "O: %d %d %d %d %d", - &s[0], &s[1], &s[2], &s[3], &s[4])) != 5) { - debug("scaler parse failed on '%s' (got %d)", buf, ret); + if ((ret = sscanf(buf, "O: %d %d %d %d %d %n", + &s[0], &s[1], &s[2], &s[3], &s[4], &n)) != 5) { + debug("out scaler parse failed on '%s' (got %d, consumed %d)", buf, ret, n); return -1; } skipline(buf, buflen); @@ -160,10 +161,20 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c int used; const char *end = buf + buflen; - /* require a space or newline at the end of the buffer */ - if (*end != ' ' && *end != '\n' && *end != '\r') { - debug("simple parser rejected: No newline / space at end of buf."); - goto out; + /* enforce that the mixer ends with space or a new line */ + for (int i = buflen - 1; i >= 0; i--) { + if (buf[i] == '\0') + continue; + + /* require a space or newline at the end of the buffer, fail on printable chars */ + if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') { + /* found a line ending or space, so no split symbols / numbers. good. */ + break; + } else { + debug("simple parser rejected: No newline / space at end of buf. (#%d/%d: 0x%02x)", i, buflen-1, buf[i]); + goto out; + } + } /* get the base info for the mixer */ @@ -203,7 +214,7 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c if (sm != nullptr) { mixinfo = nullptr; - debug("loaded mixer with %d inputs", inputs); + debug("loaded mixer with %d input(s)", inputs); } else { debug("could not allocate memory for mixer"); From d717b6f940fdeca062d629c652c7dbcb1a42c62e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Sep 2013 14:26:43 +0200 Subject: [PATCH 704/872] Fixed in-air restart order for fixed wing --- ROMFS/px4fmu_common/init.d/100_mpx_easystar | 14 ++++++-------- ROMFS/px4fmu_common/init.d/101_hk_bixler | 12 +++++------- ROMFS/px4fmu_common/init.d/30_io_camflyer | 12 +++++------- ROMFS/px4fmu_common/init.d/31_io_phantom | 12 +++++------- 4 files changed, 21 insertions(+), 29 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/100_mpx_easystar b/ROMFS/px4fmu_common/init.d/100_mpx_easystar index 4ed73909e0..1a05f51400 100644 --- a/ROMFS/px4fmu_common/init.d/100_mpx_easystar +++ b/ROMFS/px4fmu_common/init.d/100_mpx_easystar @@ -29,7 +29,8 @@ if px4io detect then # Start MAVLink (depends on orb) mavlink start - usleep 5000 + + commander start sh /etc/init.d/rc.io # Limit to 100 Hz updates and (implicit) 50 Hz PWM @@ -37,12 +38,14 @@ then else # Start MAVLink (on UART1 / ttyS0) mavlink start -d /dev/ttyS0 - usleep 5000 + + commander start + fmu mode_pwm param set BAT_V_SCALING 0.004593 set EXIT_ON_END yes fi - + # # Start the sensors and test them. # @@ -53,11 +56,6 @@ sh /etc/init.d/rc.sensors # sh /etc/init.d/rc.logging -# -# Start the commander. -# -commander start - # # Start GPS interface (depends on orb) # diff --git a/ROMFS/px4fmu_common/init.d/101_hk_bixler b/ROMFS/px4fmu_common/init.d/101_hk_bixler index b4daa8f5aa..89b3185ad7 100644 --- a/ROMFS/px4fmu_common/init.d/101_hk_bixler +++ b/ROMFS/px4fmu_common/init.d/101_hk_bixler @@ -29,7 +29,8 @@ if px4io detect then # Start MAVLink (depends on orb) mavlink start - usleep 5000 + + commander start sh /etc/init.d/rc.io # Limit to 100 Hz updates and (implicit) 50 Hz PWM @@ -37,7 +38,9 @@ then else # Start MAVLink (on UART1 / ttyS0) mavlink start -d /dev/ttyS0 - usleep 5000 + + commander start + fmu mode_pwm param set BAT_V_SCALING 0.004593 set EXIT_ON_END yes @@ -53,11 +56,6 @@ sh /etc/init.d/rc.sensors # sh /etc/init.d/rc.logging -# -# Start the commander. -# -commander start - # # Start GPS interface (depends on orb) # diff --git a/ROMFS/px4fmu_common/init.d/30_io_camflyer b/ROMFS/px4fmu_common/init.d/30_io_camflyer index ff740b6f23..191d8cd95f 100644 --- a/ROMFS/px4fmu_common/init.d/30_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/30_io_camflyer @@ -29,7 +29,8 @@ if px4io detect then # Start MAVLink (depends on orb) mavlink start - usleep 5000 + + commander start sh /etc/init.d/rc.io # Limit to 100 Hz updates and (implicit) 50 Hz PWM @@ -37,7 +38,9 @@ then else # Start MAVLink (on UART1 / ttyS0) mavlink start -d /dev/ttyS0 - usleep 5000 + + commander start + fmu mode_pwm param set BAT_V_SCALING 0.004593 set EXIT_ON_END yes @@ -53,11 +56,6 @@ sh /etc/init.d/rc.sensors # sh /etc/init.d/rc.logging -# -# Start the commander. -# -commander start - # # Start GPS interface (depends on orb) # diff --git a/ROMFS/px4fmu_common/init.d/31_io_phantom b/ROMFS/px4fmu_common/init.d/31_io_phantom index 838dcb369c..7e0127e79d 100644 --- a/ROMFS/px4fmu_common/init.d/31_io_phantom +++ b/ROMFS/px4fmu_common/init.d/31_io_phantom @@ -29,7 +29,8 @@ if px4io detect then # Start MAVLink (depends on orb) mavlink start - usleep 5000 + + commander start sh /etc/init.d/rc.io # Limit to 100 Hz updates and (implicit) 50 Hz PWM @@ -37,7 +38,9 @@ then else # Start MAVLink (on UART1 / ttyS0) mavlink start -d /dev/ttyS0 - usleep 5000 + + commander start + fmu mode_pwm param set BAT_V_SCALING 0.004593 set EXIT_ON_END yes @@ -53,11 +56,6 @@ sh /etc/init.d/rc.sensors # sh /etc/init.d/rc.logging -# -# Start the commander. -# -commander start - # # Start GPS interface (depends on orb) # From 826d5687be209bc5e42ed97b8a84493913123c2a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Sep 2013 14:43:12 +0200 Subject: [PATCH 705/872] Fixed in-air restart --- src/modules/commander/commander.cpp | 22 ++++++++++++++++++++-- 1 file changed, 20 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 1e86e8e247..fd9067e902 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -470,8 +470,26 @@ 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; + { + transition_result_t arming_res = TRANSITION_NOT_CHANGED; + if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) { + if (safety->safety_switch_available && !safety->safety_off) { + print_reject_arm("NOT ARMING: Press safety switch first."); + arming_res = TRANSITION_DENIED; + + } else { + arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed); + } + + if (arming_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] ARMED by component arm cmd"); + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + mavlink_log_info(mavlink_fd, "[cmd] REJECTING component arm cmd"); + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } + } break; default: From dce13299195dbb14cc70a0390285aaa6ffdd8e5d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 22 Sep 2013 14:54:15 +0200 Subject: [PATCH 706/872] Disable USART1 DMA Rx on FMUv1 --- nuttx-configs/px4fmu-v1/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index ba211ccb23..e43b9c18e8 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -262,7 +262,7 @@ CONFIG_STM32_USART=y # U[S]ART Configuration # # CONFIG_USART1_RS485 is not set -CONFIG_USART1_RXDMA=y +# CONFIG_USART1_RXDMA is not set # CONFIG_USART2_RS485 is not set CONFIG_USART2_RXDMA=y # CONFIG_USART3_RXDMA is not set From 6616aa6f993c0dc767c7fe7b2e616202c79667d5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Sep 2013 14:58:06 +0200 Subject: [PATCH 707/872] Fixed in-air restart, now obeys the right order --- src/drivers/px4io/px4io.cpp | 3 +++ src/modules/commander/commander.cpp | 22 ++++++++++++---------- 2 files changed, 15 insertions(+), 10 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 9597dad9a7..952453a8cc 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -591,6 +591,9 @@ PX4IO::init() if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) && (reg & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + /* get a status update from IO */ + io_get_status(); + mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART"); log("INAIR RESTART RECOVERY (needs commander app running)"); diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index fd9067e902..01b7b84d01 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -824,16 +824,6 @@ int commander_thread_main(int argc, char *argv[]) check_valid(diff_pres.timestamp, DIFFPRESS_TIMEOUT, true, &(status.condition_airspeed_valid), &status_changed); - orb_check(cmd_sub, &updated); - - if (updated) { - /* got command */ - orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); - - /* handle it */ - handle_command(&status, &safety, &control_mode, &cmd, &armed); - } - /* update safety topic */ orb_check(safety_sub, &updated); @@ -1165,6 +1155,18 @@ int commander_thread_main(int argc, char *argv[]) } } + + /* handle commands last, as the system needs to be updated to handle them */ + orb_check(cmd_sub, &updated); + + if (updated) { + /* got command */ + orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); + + /* handle it */ + handle_command(&status, &safety, &control_mode, &cmd, &armed); + } + /* evaluate the navigation state machine */ transition_result_t res = check_navigation_state_machine(&status, &control_mode, &local_position); From af118a3568f2d63219bf97c999912babda588f8c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 21 Sep 2013 17:32:07 +0200 Subject: [PATCH 708/872] Add esc_calib systemcmd to FMUv2 --- makefiles/config_px4fmu-v2_default.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index ba0beec3e1..ca99f84ccf 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -51,6 +51,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 From 30b151b9a83eed7b41243b31a1ebaf37ee663171 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 22 Sep 2013 16:27:52 +0200 Subject: [PATCH 709/872] Improved esc_calib a little, only works on nsh over USB now --- src/systemcmds/esc_calib/esc_calib.c | 56 ++++++++++++---------------- 1 file changed, 23 insertions(+), 33 deletions(-) diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c index 188fa4e371..0d74218422 100644 --- a/src/systemcmds/esc_calib/esc_calib.c +++ b/src/systemcmds/esc_calib/esc_calib.c @@ -1,6 +1,7 @@ /**************************************************************************** * * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Julian Oes * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -63,7 +64,7 @@ static void usage(const char *reason); __EXPORT int esc_calib_main(int argc, char *argv[]); -#define MAX_CHANNELS 8 +#define MAX_CHANNELS 14 static void usage(const char *reason) @@ -97,7 +98,7 @@ esc_calib_main(int argc, char *argv[]) case 'd': dev = optarg; - argc--; + argc=-2; break; default: @@ -124,15 +125,18 @@ esc_calib_main(int argc, char *argv[]) } /* Wait for confirmation */ - int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY); + int console = open("/dev/ttyACM0", O_NONBLOCK | O_RDONLY | O_NOCTTY); if (!console) err(1, "failed opening console"); - warnx("ATTENTION, please remove or fix props before starting calibration!\n" + printf("\nATTENTION, please remove or fix propellers before starting calibration!\n" "\n" - "Also press the arming switch first for safety off\n" + "Make sure\n" + "\t - that the ESCs are not powered\n" + "\t - that safety is off (two short blinks)\n" + "\t - that the controllers are stopped\n" "\n" - "Do you really want to start calibration: y or n?\n"); + "Do you want to start calibration now: y or n?\n"); /* wait for user input */ while (1) { @@ -142,15 +146,15 @@ esc_calib_main(int argc, char *argv[]) break; } else if (c == 0x03 || c == 0x63 || c == 'q') { - warnx("ESC calibration exited"); + printf("ESC calibration exited\n"); close(console); exit(0); } else if (c == 'n' || c == 'N') { - warnx("ESC calibration aborted"); + printf("ESC calibration aborted\n"); close(console); exit(0); } else { - warnx("Unknown input, ESC calibration aborted"); + printf("Unknown input, ESC calibration aborted\n"); close(console); exit(0); } @@ -163,23 +167,14 @@ esc_calib_main(int argc, char *argv[]) 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"); + printf("\nHigh PWM set\n" + "\n" + "Connect battery now and hit ENTER after the ESCs confirm the first calibration step\n" + "\n"); + fflush(stdout); while (1) { @@ -209,7 +204,8 @@ esc_calib_main(int argc, char *argv[]) /* we don't need any more user input */ - warnx("Set low PWM, hit ENTER when finished"); + printf("Low PWM set, hit ENTER when finished\n" + "\n"); while (1) { @@ -227,7 +223,7 @@ esc_calib_main(int argc, char *argv[]) break; } else if (c == 0x03 || c == 0x63 || c == 'q') { - warnx("ESC calibration exited"); + printf("ESC calibration exited\n"); close(console); exit(0); } @@ -237,14 +233,8 @@ esc_calib_main(int argc, char *argv[]) } - warnx("ESC calibration finished"); + printf("ESC calibration finished\n"); close(console); - // XXX disarming not necessaire at the moment - /* Now disarm again */ - // ret = ioctl(fd, PWM_SERVO_DISARM, 0); - - - exit(0); -} \ No newline at end of file +} From bdfc7b9f69ec8a9495e2efecdf7bbe3c627c03b8 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 22 Sep 2013 17:07:02 +0200 Subject: [PATCH 710/872] Listen to all consoles plus some more small fixes --- src/systemcmds/esc_calib/esc_calib.c | 39 +++++++++++++++------------- 1 file changed, 21 insertions(+), 18 deletions(-) diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c index 0d74218422..608c9fff17 100644 --- a/src/systemcmds/esc_calib/esc_calib.c +++ b/src/systemcmds/esc_calib/esc_calib.c @@ -46,6 +46,7 @@ #include #include #include +#include #include #include #include @@ -83,22 +84,26 @@ usage(const char *reason) int esc_calib_main(int argc, char *argv[]) { - const char *dev = PWM_OUTPUT_DEVICE_PATH; + char *dev = PWM_OUTPUT_DEVICE_PATH; char *ep; bool channels_selected[MAX_CHANNELS] = {false}; int ch; int ret; char c; + struct pollfd fds; + fds.fd = 0; /* stdin */ + fds.events = POLLIN; + if (argc < 2) usage(NULL); - while ((ch = getopt(argc, argv, "d:")) != EOF) { + while ((ch = getopt(argc-1, argv, "d:")) != EOF) { switch (ch) { case 'd': dev = optarg; - argc=-2; + argc-=2; break; default: @@ -106,7 +111,7 @@ esc_calib_main(int argc, char *argv[]) } } - if(argc < 1) { + if(argc < 2) { usage("no channels provided"); } @@ -124,11 +129,6 @@ esc_calib_main(int argc, char *argv[]) } } - /* Wait for confirmation */ - int console = open("/dev/ttyACM0", O_NONBLOCK | O_RDONLY | O_NOCTTY); - if (!console) - err(1, "failed opening console"); - printf("\nATTENTION, please remove or fix propellers before starting calibration!\n" "\n" "Make sure\n" @@ -141,21 +141,21 @@ esc_calib_main(int argc, char *argv[]) /* wait for user input */ while (1) { - if (read(console, &c, 1) == 1) { + ret = poll(&fds, 1, 0); + if (ret > 0) { + + read(0, &c, 1); if (c == 'y' || c == 'Y') { break; } else if (c == 0x03 || c == 0x63 || c == 'q') { printf("ESC calibration exited\n"); - close(console); exit(0); } else if (c == 'n' || c == 'N') { printf("ESC calibration aborted\n"); - close(console); exit(0); } else { printf("Unknown input, ESC calibration aborted\n"); - close(console); exit(0); } } @@ -187,13 +187,15 @@ esc_calib_main(int argc, char *argv[]) } } - if (read(console, &c, 1) == 1) { + ret = poll(&fds, 1, 0); + if (ret > 0) { + + read(0, &c, 1); if (c == 13) { break; } else if (c == 0x03 || c == 0x63 || c == 'q') { warnx("ESC calibration exited"); - close(console); exit(0); } } @@ -218,13 +220,15 @@ esc_calib_main(int argc, char *argv[]) } } - if (read(console, &c, 1) == 1) { + ret = poll(&fds, 1, 0); + if (ret > 0) { + + read(0, &c, 1); if (c == 13) { break; } else if (c == 0x03 || c == 0x63 || c == 'q') { printf("ESC calibration exited\n"); - close(console); exit(0); } } @@ -234,7 +238,6 @@ esc_calib_main(int argc, char *argv[]) printf("ESC calibration finished\n"); - close(console); exit(0); } From ad4c28507fd1319ef26eca624ad125b822007b4e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Sep 2013 17:08:29 +0200 Subject: [PATCH 711/872] Hotfixes for HIL and mode switching. --- src/modules/mavlink/mavlink.c | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 47 ++++++++++++++++++----- src/modules/mavlink/orb_topics.h | 1 + src/modules/uORB/topics/vehicle_command.h | 2 +- 4 files changed, 40 insertions(+), 12 deletions(-) diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index cbcd4adfb8..7a5c2dab28 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -531,7 +531,7 @@ int mavlink_thread_main(int argc, char *argv[]) case 'b': baudrate = strtoul(optarg, NULL, 10); - if (baudrate == 0) + if (baudrate < 9600 || baudrate > 921600) errx(1, "invalid baud rate '%s'", optarg); break; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index a3ef1d63b9..7dd9e321fb 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -102,6 +102,7 @@ struct vehicle_global_position_s hil_global_pos; struct vehicle_attitude_s hil_attitude; struct vehicle_gps_position_s hil_gps; struct sensor_combined_s hil_sensors; +struct battery_status_s hil_battery_status; 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; @@ -111,6 +112,12 @@ 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 pub_hil_battery = -1; + +/* packet counter */ +static int hil_counter = 0; +static int hil_frames = 0; +static uint64_t old_timestamp = 0; static orb_advert_t cmd_pub = -1; static orb_advert_t flow_pub = -1; @@ -149,7 +156,8 @@ handle_message(mavlink_message_t *msg) vcmd.param5 = cmd_mavlink.param5; vcmd.param6 = cmd_mavlink.param6; vcmd.param7 = cmd_mavlink.param7; - vcmd.command = cmd_mavlink.command; + // XXX do proper translation + vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command; vcmd.target_system = cmd_mavlink.target_system; vcmd.target_component = cmd_mavlink.target_component; vcmd.source_system = msg->sysid; @@ -207,7 +215,7 @@ handle_message(mavlink_message_t *msg) vcmd.param5 = 0; vcmd.param6 = 0; vcmd.param7 = 0; - vcmd.command = MAV_CMD_DO_SET_MODE; + vcmd.command = VEHICLE_CMD_DO_SET_MODE; vcmd.target_system = new_mode.target_system; vcmd.target_component = MAV_COMP_ID_ALL; vcmd.source_system = msg->sysid; @@ -360,11 +368,6 @@ handle_message(mavlink_message_t *msg) mavlink_hil_sensor_t imu; mavlink_msg_hil_sensor_decode(msg, &imu); - /* packet counter */ - static uint16_t hil_counter = 0; - static uint16_t hil_frames = 0; - static uint64_t old_timestamp = 0; - /* sensors general */ hil_sensors.timestamp = hrt_absolute_time(); @@ -391,9 +394,9 @@ handle_message(mavlink_message_t *msg) hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16 /* adc */ - hil_sensors.adc_voltage_v[0] = 0; - hil_sensors.adc_voltage_v[1] = 0; - hil_sensors.adc_voltage_v[2] = 0; + hil_sensors.adc_voltage_v[0] = 0.0f; + hil_sensors.adc_voltage_v[1] = 0.0f; + hil_sensors.adc_voltage_v[2] = 0.0f; /* magnetometer */ float mga2ga = 1.0e-3f; @@ -506,6 +509,18 @@ handle_message(mavlink_message_t *msg) pub_hil_sensors = orb_advertise(ORB_ID(sensor_combined), &hil_sensors); } + /* fill in HIL battery status */ + hil_battery_status.timestamp = hrt_absolute_time(); + hil_battery_status.voltage_v = 11.1f; + hil_battery_status.current_a = 10.0f; + + /* lazily publish the battery voltage */ + if (pub_hil_battery > 0) { + orb_publish(ORB_ID(battery_status), pub_hil_battery, &hil_battery_status); + } else { + pub_hil_battery = orb_advertise(ORB_ID(battery_status), &hil_battery_status); + } + // increment counters hil_counter++; hil_frames++; @@ -640,6 +655,18 @@ handle_message(mavlink_message_t *msg) } else { orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel); } + + /* fill in HIL battery status */ + hil_battery_status.timestamp = hrt_absolute_time(); + hil_battery_status.voltage_v = 11.1f; + hil_battery_status.current_a = 10.0f; + + /* lazily publish the battery voltage */ + if (pub_hil_battery > 0) { + orb_publish(ORB_ID(battery_status), pub_hil_battery, &hil_battery_status); + } else { + pub_hil_battery = orb_advertise(ORB_ID(battery_status), &hil_battery_status); + } } if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) { diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h index e2e6300463..c5cd0d03e5 100644 --- a/src/modules/mavlink/orb_topics.h +++ b/src/modules/mavlink/orb_topics.h @@ -65,6 +65,7 @@ #include #include #include +#include #include struct mavlink_subscriptions { diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h index a62e38db2f..e6e4743c6a 100644 --- a/src/modules/uORB/topics/vehicle_command.h +++ b/src/modules/uORB/topics/vehicle_command.h @@ -120,7 +120,7 @@ struct vehicle_command_s float param5; /**< Parameter 5, as defined by MAVLink VEHICLE_CMD enum. */ float param6; /**< Parameter 6, as defined by MAVLink VEHICLE_CMD enum. */ float param7; /**< Parameter 7, as defined by MAVLink VEHICLE_CMD enum. */ - uint16_t command; /**< Command ID, as defined MAVLink by VEHICLE_CMD enum. */ + enum VEHICLE_CMD command; /**< Command ID, as defined MAVLink by VEHICLE_CMD enum. */ uint8_t target_system; /**< System which should execute the command */ uint8_t target_component; /**< Component which should execute the command, 0 for all components */ uint8_t source_system; /**< System sending the command */ From 166dba09de38642ec656d857e225c08d9a36fc72 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 22 Sep 2013 17:23:43 +0200 Subject: [PATCH 712/872] px4io test and fmu test now work over USB as well --- src/drivers/px4fmu/fmu.cpp | 17 ++++++++++------- src/drivers/px4io/px4io.cpp | 13 +++++++------ 2 files changed, 17 insertions(+), 13 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 6d4019f249..b1dd55dd78 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1096,10 +1096,11 @@ fmu_start(void) void test(void) { - int fd; + int fd; unsigned servo_count = 0; unsigned pwm_value = 1000; int direction = 1; + int ret; fd = open(PX4FMU_DEVICE_PATH, O_RDWR); @@ -1114,9 +1115,9 @@ test(void) warnx("Testing %u servos", (unsigned)servo_count); - int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY); - if (!console) - err(1, "failed opening console"); + struct pollfd fds; + fds.fd = 0; /* stdin */ + fds.events = POLLIN; warnx("Press CTRL-C or 'c' to abort."); @@ -1166,15 +1167,17 @@ test(void) /* Check if user wants to quit */ char c; - if (read(console, &c, 1) == 1) { - if (c == 0x03 || c == 0x63) { + ret = poll(&fds, 1, 0); + if (ret > 0) { + + read(0, &c, 1); + if (c == 0x03 || c == 0x63 || c == 'q') { warnx("User abort\n"); break; } } } - close(console); close(fd); exit(0); diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 952453a8cc..1336460515 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2131,10 +2131,9 @@ test(void) if (ioctl(fd, PWM_SERVO_ARM, 0)) err(1, "failed to arm servos"); - /* 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"); + struct pollfd fds; + fds.fd = 0; /* stdin */ + fds.events = POLLIN; warnx("Press CTRL-C or 'c' to abort."); @@ -2175,10 +2174,12 @@ test(void) /* Check if user wants to quit */ char c; - if (read(console, &c, 1) == 1) { + ret = poll(&fds, 1, 0); + if (ret > 0) { + + read(0, &c, 1); if (c == 0x03 || c == 0x63 || c == 'q') { warnx("User abort\n"); - close(console); exit(0); } } From 6f54825000572692b1f6fb863c6af35896eb1b93 Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Sun, 22 Sep 2013 23:17:27 -0400 Subject: [PATCH 713/872] Comments in GPS UORB topic don't make sense - Describe long, lat, and alt coordinates properly --- src/modules/uORB/topics/vehicle_gps_position.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h index 0a7fb4e9da..1639a08c2b 100644 --- a/src/modules/uORB/topics/vehicle_gps_position.h +++ b/src/modules/uORB/topics/vehicle_gps_position.h @@ -56,9 +56,9 @@ struct vehicle_gps_position_s { uint64_t timestamp_position; /**< Timestamp for position information */ - int32_t lat; /**< Latitude in 1E7 degrees */ - int32_t lon; /**< Longitude in 1E7 degrees */ - int32_t alt; /**< Altitude in 1E3 meters (millimeters) above MSL */ + int32_t lat; /**< Latitude in 1E-7 degrees */ + int32_t lon; /**< Longitude in 1E-7 degrees */ + int32_t alt; /**< Altitude in 1E-3 meters (millimeters) above MSL */ uint64_t timestamp_variance; float s_variance_m_s; /**< speed accuracy estimate m/s */ From 65bea797b42a9a967772754994c767959481162c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 23 Sep 2013 22:46:50 +0200 Subject: [PATCH 714/872] Enabling navigator, does not do anything useful yet --- 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 bd41f68b65..1dd9490763 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -63,6 +63,7 @@ MODULES += systemcmds/nshterm # General system control # MODULES += modules/commander +MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/mavlink_onboard MODULES += modules/gpio_led diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index ca99f84ccf..e12d61a535 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -62,6 +62,7 @@ MODULES += systemcmds/nshterm # General system control # MODULES += modules/commander +MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/mavlink_onboard From f7090db7081922cc61e79fce800fb6bce84a3836 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 24 Sep 2013 08:22:44 +0200 Subject: [PATCH 715/872] Fix the direction of the override switch for the new state machine --- src/modules/px4iofirmware/controls.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 617b536f48..5c621cfb2e 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -286,7 +286,7 @@ controls_tick() { * requested override. * */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) > RC_CHANNEL_HIGH_THRESH)) + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) < RC_CHANNEL_LOW_THRESH)) override = true; if (override) { From bfe22c114070848d5d4bc0ed4e4378154f274b34 Mon Sep 17 00:00:00 2001 From: "Hyon Lim (Retina)" Date: Wed, 25 Sep 2013 02:02:51 +0900 Subject: [PATCH 716/872] RC3 usually used as Throttle, but trim was set to 1500 by default. It should be 1000 by default. --- src/modules/sensors/sensor_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 4d719e0e15..031d20ded8 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -87,7 +87,7 @@ PARAM_DEFINE_FLOAT(RC2_REV, 1.0f); PARAM_DEFINE_FLOAT(RC2_DZ, 0.0f); PARAM_DEFINE_FLOAT(RC3_MIN, 1000); -PARAM_DEFINE_FLOAT(RC3_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC3_TRIM, 1000); PARAM_DEFINE_FLOAT(RC3_MAX, 2000); PARAM_DEFINE_FLOAT(RC3_REV, 1.0f); PARAM_DEFINE_FLOAT(RC3_DZ, 0.0f); From 4514045fb61479c456bb2bbaea5d3fe116ca705f Mon Sep 17 00:00:00 2001 From: "Hyon Lim (Retina)" Date: Wed, 25 Sep 2013 02:12:55 +0900 Subject: [PATCH 717/872] There were unintialized variables. (control mode was not updated) Also, new flags (xy_valid etc) were considered. --- .../flow_position_control_main.c | 36 +++++++++++++++---- .../flow_position_estimator_main.c | 7 ++++ .../flow_speed_control_main.c | 32 +++++++++++++---- 3 files changed, 62 insertions(+), 13 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 621d3253f0..3125ce2460 100644 --- a/src/examples/flow_position_control/flow_position_control_main.c +++ b/src/examples/flow_position_control/flow_position_control_main.c @@ -67,6 +67,7 @@ #include #include #include +#include #include "flow_position_control_params.h" @@ -153,20 +154,28 @@ flow_position_control_thread_main(int argc, char *argv[]) { /* welcome user */ thread_running = true; - printf("[flow position control] starting\n"); + static int mavlink_fd; + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + mavlink_log_info(mavlink_fd, "[fpc] started"); uint32_t counter = 0; const float time_scale = powf(10.0f,-6.0f); /* structures */ 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; + memset(&att, 0, sizeof(att)); struct manual_control_setpoint_s manual; + memset(&manual, 0, sizeof(manual)); struct filtered_bottom_flow_s filtered_flow; + memset(&filtered_flow, 0, sizeof(filtered_flow)); struct vehicle_local_position_s local_pos; - + memset(&local_pos, 0, sizeof(local_pos)); struct vehicle_bodyframe_speed_setpoint_s speed_sp; + memset(&speed_sp, 0, sizeof(speed_sp)); /* subscribe to attitude, motor setpoints and system state */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -216,6 +225,7 @@ flow_position_control_thread_main(int argc, char *argv[]) perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_position_control_err"); static bool sensors_ready = false; + static bool status_changed = false; while (!thread_should_exit) { @@ -252,7 +262,7 @@ flow_position_control_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); parameters_update(¶m_handles, ¶ms); - printf("[flow position control] parameters updated.\n"); + mavlink_log_info(mavlink_fd,"[fpc] parameters updated."); } /* only run controller if position/speed changed */ @@ -270,6 +280,8 @@ flow_position_control_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(filtered_bottom_flow), filtered_bottom_flow_sub, &filtered_flow); /* get a local copy of local position */ orb_copy(ORB_ID(vehicle_local_position), vehicle_local_position_sub, &local_pos); + /* get a local copy of control mode */ + orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); if (control_mode.flag_control_velocity_enabled) { @@ -277,6 +289,11 @@ flow_position_control_thread_main(int argc, char *argv[]) float manual_roll = manual.roll / params.rc_scale_roll; // 0 to 1 float manual_yaw = manual.yaw / params.rc_scale_yaw; // -1 to 1 + if(status_changed == false) + mavlink_log_info(mavlink_fd,"[fpc] flow POSITION control engaged"); + + status_changed = true; + /* calc dt */ if(last_time == 0) { @@ -296,7 +313,7 @@ flow_position_control_thread_main(int argc, char *argv[]) { flow_sp_sumy = filtered_flow.sumy; update_flow_sp_sumy = false; - } + } /* calc new bodyframe speed setpoints */ float speed_body_x = (flow_sp_sumx - filtered_flow.sumx) * params.pos_p - filtered_flow.vx * params.pos_d; @@ -518,6 +535,11 @@ flow_position_control_thread_main(int argc, char *argv[]) else { /* in manual or stabilized state just reset speed and flow sum setpoint */ + //mavlink_log_info(mavlink_fd,"[fpc] reset speed sp, flow_sp_sumx,y (%f,%f)",filtered_flow.sumx, filtered_flow.sumy); + if(status_changed == true) + mavlink_log_info(mavlink_fd,"[fpc] flow POSITION controller disengaged."); + + status_changed = false; speed_sp.vx = 0.0f; speed_sp.vy = 0.0f; flow_sp_sumx = filtered_flow.sumx; @@ -558,20 +580,20 @@ flow_position_control_thread_main(int argc, char *argv[]) else if (ret == 0) { /* no return value, ignore */ - printf("[flow position control] no attitude received.\n"); + mavlink_log_info(mavlink_fd,"[fpc] no attitude received.\n"); } else { if (fds[0].revents & POLLIN) { sensors_ready = true; - printf("[flow position control] initialized.\n"); + mavlink_log_info(mavlink_fd,"[fpc] initialized.\n"); } } } } - printf("[flow position control] ending now...\n"); + mavlink_log_info(mavlink_fd,"[fpc] ending now...\n"); thread_running = false; 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 5e80066a70..495c415f2b 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_main.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c @@ -345,6 +345,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) local_pos.y = local_pos.y + global_speed[1] * dt; local_pos.vx = global_speed[0]; local_pos.vy = global_speed[1]; + local_pos.xy_valid = true; + local_pos.v_xy_valid = true; } else { @@ -353,6 +355,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) filtered_flow.vy = 0; local_pos.vx = 0; local_pos.vy = 0; + local_pos.xy_valid = false; + local_pos.v_xy_valid = false; } /* filtering ground distance */ @@ -361,6 +365,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) /* not possible to fly */ sonar_valid = false; local_pos.z = 0.0f; + local_pos.z_valid = false; } else { @@ -388,6 +393,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) { local_pos.z = -sonar_new; } + + local_pos.z_valid = true; } filtered_flow.timestamp = hrt_absolute_time(); 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 6af955cd7d..feed0d23cd 100644 --- a/src/examples/flow_speed_control/flow_speed_control_main.c +++ b/src/examples/flow_speed_control/flow_speed_control_main.c @@ -65,6 +65,7 @@ #include #include #include +#include #include "flow_speed_control_params.h" @@ -151,17 +152,23 @@ flow_speed_control_thread_main(int argc, char *argv[]) { /* welcome user */ thread_running = true; - printf("[flow speed control] starting\n"); + static int mavlink_fd; + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + mavlink_log_info(mavlink_fd,"[fsc] started"); uint32_t counter = 0; /* structures */ 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 filtered_bottom_flow_s filtered_flow; + memset(&filtered_flow, 0, sizeof(filtered_flow)); struct vehicle_bodyframe_speed_setpoint_s speed_sp; - + memset(&speed_sp, 0, sizeof(speed_sp)); struct vehicle_attitude_setpoint_s att_sp; + memset(&att_sp, 0, sizeof(att_sp)); /* subscribe to attitude, motor setpoints and system state */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -186,6 +193,7 @@ flow_speed_control_thread_main(int argc, char *argv[]) perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_speed_control_err"); static bool sensors_ready = false; + static bool status_changed = false; while (!thread_should_exit) { @@ -221,7 +229,7 @@ flow_speed_control_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); parameters_update(¶m_handles, ¶ms); - printf("[flow speed control] parameters updated.\n"); + mavlink_log_info(mavlink_fd,"[fsp] parameters updated."); } /* only run controller if position/speed changed */ @@ -237,6 +245,8 @@ flow_speed_control_thread_main(int argc, char *argv[]) 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); + /* get a local copy of control mode */ + orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); if (control_mode.flag_control_velocity_enabled) { @@ -244,6 +254,11 @@ flow_speed_control_thread_main(int argc, char *argv[]) float pitch_body = -(speed_sp.vx - filtered_flow.vx) * params.speed_p; float roll_body = (speed_sp.vy - filtered_flow.vy) * params.speed_p; + if(status_changed == false) + mavlink_log_info(mavlink_fd,"[fsc] flow SPEED control engaged"); + + status_changed = true; + /* limit roll and pitch corrections */ if((pitch_body <= params.limit_pitch) && (pitch_body >= -params.limit_pitch)) { @@ -299,6 +314,11 @@ flow_speed_control_thread_main(int argc, char *argv[]) } else { + if(status_changed == true) + mavlink_log_info(mavlink_fd,"[fsc] flow SPEED controller disengaged."); + + status_changed = false; + /* reset attitude setpoint */ att_sp.roll_body = 0.0f; att_sp.pitch_body = 0.0f; @@ -334,20 +354,20 @@ flow_speed_control_thread_main(int argc, char *argv[]) else if (ret == 0) { /* no return value, ignore */ - printf("[flow speed control] no attitude received.\n"); + mavlink_log_info(mavlink_fd,"[fsc] no attitude received."); } else { if (fds[0].revents & POLLIN) { sensors_ready = true; - printf("[flow speed control] initialized.\n"); + mavlink_log_info(mavlink_fd,"[fsp] initialized."); } } } } - printf("[flow speed control] ending now...\n"); + mavlink_log_info(mavlink_fd,"[fsc] ending now..."); thread_running = false; From 1c891d8a6817c298414c645f93124fcec560869a Mon Sep 17 00:00:00 2001 From: "Hyon Lim (Retina)" Date: Wed, 25 Sep 2013 02:17:06 +0900 Subject: [PATCH 718/872] Airframe No. 21. 22 have been added. That is for general ESC with PX4IO (frame geometry defined) --- ROMFS/px4fmu_common/init.d/rc.custom_io_esc | 120 ++++++++++++++++++++ ROMFS/px4fmu_common/init.d/rcS | 17 +++ 2 files changed, 137 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/rc.custom_io_esc diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_io_esc b/ROMFS/px4fmu_common/init.d/rc.custom_io_esc new file mode 100644 index 0000000000..e645d9d549 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.custom_io_esc @@ -0,0 +1,120 @@ +#!nsh + +# +# 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.002 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_P 0.09 + param set MC_ATT_D 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_P 6.8 + param set MC_YAWPOS_D 0.0 + 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 set NAV_TAKEOFF_ALT 3.0 + param set MPC_TILT_MAX 0.5 + param set MPC_THR_MAX 0.7 + param set MPC_THR_MIN 0.3 + param set MPC_XY_D 0 + param set MPC_XY_P 0.5 + param set MPC_XY_VEL_D 0 + param set MPC_XY_VEL_I 0 + param set MPC_XY_VEL_MAX 3 + param set MPC_XY_VEL_P 0.2 + param set MPC_Z_D 0 + param set MPC_Z_P 1 + param set MPC_Z_VEL_D 0 + param set MPC_Z_VEL_I 0.1 + param set MPC_Z_VEL_MAX 2 + param set MPC_Z_VEL_P 0.20 + + param save +fi + +echo "RC script for PX4FMU + PX4IO + PPM-ESCs running" + +# +# Force some key parameters to sane values +# MAV_TYPE 2 = quadrotor +# +param set MAV_TYPE 2 + +set EXIT_ON_END no + +usleep 10000 + +# +# Start and configure PX4IO or FMU interface +# +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start -d /dev/ttyS1 -b 115200 + usleep 5000 + + sh /etc/init.d/rc.io + + if [ $ESC_MAKER = afro ] + then + # Set PWM values for Afro ESCs + px4io idle 1050 1050 1050 1050 + px4io min 1080 1080 1080 1080 + px4io max 1860 1860 1860 1860 + else + # Set PWM values for typical ESCs + px4io idle 900 900 900 900 + px4io min 1110 1100 1100 1100 + px4io max 1800 1800 1800 1800 + fi +else + fmu mode_pwm + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS1 -b 115200 + usleep 5000 + param set BAT_V_SCALING 0.004593 + set EXIT_ON_END yes +fi + +# +# Load mixer +# +if [ $FRAME_GEOMETRY == x ] +then + echo "Frame geometry X" + mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix +else + if [ $FRAME_GEOMETRY == w ] + then + echo "Frame geometry W" + mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix + else + echo "Frame geometry +" + mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix + fi +fi + +# +# Set PWM output frequency +# +pwm -u 400 -m 0xff + +# +# Start common for all multirotors apps +# +sh /etc/init.d/rc.multirotor + +if [ $EXIT_ON_END == yes ] +then + exit +fi + +echo "Script end" diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 9ec346465a..93e76184d4 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -225,6 +225,23 @@ then sh /etc/init.d/rc.custom_dji_f330_mkblctrl set MODE custom fi + + # PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame + if param compare SYS_AUTOSTART 21 + then + set FRAME_GEOMETRY x + set ESC_MAKER afro + sh /etc/init.d/rc.custom_io_esc + set MODE custom + fi + + # PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame + if param compare SYS_AUTOSTART 22 + then + set FRAME_GEOMETRY w + sh /etc/init.d/rc.custom_io_esc + set MODE custom + fi if param compare SYS_AUTOSTART 30 then From a4956367127845ab7a3ce4dea66b6721a75e2d3d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 25 Sep 2013 22:32:23 +0200 Subject: [PATCH 719/872] easystar startup script: load mixer from sd-card if available --- ROMFS/px4fmu_common/init.d/100_mpx_easystar | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/100_mpx_easystar b/ROMFS/px4fmu_common/init.d/100_mpx_easystar index 1a05f51400..75437f4047 100644 --- a/ROMFS/px4fmu_common/init.d/100_mpx_easystar +++ b/ROMFS/px4fmu_common/init.d/100_mpx_easystar @@ -69,7 +69,14 @@ att_pos_estimator_ekf start # # Load mixer and start controllers (depends on px4io) # -mixer load /dev/pwm_output /etc/mixers/FMU_RET.mix +if [ -f /fs/microsd/etc/mixers/FMU_RET.mix ] +then + echo "Using FMU_RET mixer from sd card" + mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_RET.mix +else + echo "Using standard FMU_RET mixer" + mixer load /dev/pwm_output /etc/mixers/FMU_RET.mix +fi fw_att_control start # Not ready yet for prime-time #fw_pos_control_l1 start From 2c54e827eddef56ff36798159fd119d94627a6eb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 27 Sep 2013 09:24:49 +0200 Subject: [PATCH 720/872] Hotfix: Improved file test --- src/systemcmds/tests/tests_file.c | 63 ++++++++++++++++++++++++++++++- 1 file changed, 61 insertions(+), 2 deletions(-) diff --git a/src/systemcmds/tests/tests_file.c b/src/systemcmds/tests/tests_file.c index 588d648bdf..f36c280613 100644 --- a/src/systemcmds/tests/tests_file.c +++ b/src/systemcmds/tests/tests_file.c @@ -38,7 +38,9 @@ */ #include +#include #include +#include #include #include #include @@ -52,7 +54,7 @@ int test_file(int argc, char *argv[]) { - const iterations = 10; + const iterations = 200; /* check if microSD card is mounted */ struct stat buffer; @@ -63,15 +65,52 @@ test_file(int argc, char *argv[]) uint8_t buf[512]; hrt_abstime start, end; - perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes"); + perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes (aligned)"); int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); memset(buf, 0, sizeof(buf)); + warnx("aligned write - please wait.."); + + if ((0x3 & (uintptr_t)buf)) + warnx("memory is unaligned!"); + start = hrt_absolute_time(); for (unsigned i = 0; i < iterations; i++) { perf_begin(wperf); write(fd, buf, sizeof(buf)); + fsync(fd); + perf_end(wperf); + } + end = hrt_absolute_time(); + + warnx("%dKiB in %llu microseconds", iterations / 2, end - start); + + perf_print_counter(wperf); + perf_free(wperf); + + int ret = unlink("/fs/microsd/testfile"); + + if (ret) + err(1, "UNLINKING FILE FAILED"); + + warnx("unaligned write - please wait.."); + + struct { + uint8_t byte; + uint8_t unaligned[512]; + } unaligned_buf; + + if ((0x3 & (uintptr_t)unaligned_buf.unaligned) == 0) + warnx("creating unaligned memory failed."); + + wperf = perf_alloc(PC_ELAPSED, "SD writes (unaligned)"); + + start = hrt_absolute_time(); + for (unsigned i = 0; i < iterations; i++) { + perf_begin(wperf); + write(fd, unaligned_buf.unaligned, sizeof(unaligned_buf.unaligned)); + fsync(fd); perf_end(wperf); } end = hrt_absolute_time(); @@ -79,9 +118,29 @@ test_file(int argc, char *argv[]) close(fd); warnx("%dKiB in %llu microseconds", iterations / 2, end - start); + perf_print_counter(wperf); perf_free(wperf); + /* list directory */ + DIR *d; + struct dirent *dir; + d = opendir("/fs/microsd"); + if (d) { + + while ((dir = readdir(d)) != NULL) { + //printf("%s\n", dir->d_name); + } + + closedir(d); + + warnx("directory listing ok (FS mounted and readable)"); + + } else { + /* failed opening dir */ + err(1, "FAILED LISTING MICROSD ROOT DIRECTORY"); + } + return 0; } #if 0 From 642081ddfe5857720c7b1df10743a627686b0ac3 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 23 Sep 2013 16:59:48 +0900 Subject: [PATCH 721/872] tone_alarm: add GPS warning tone --- src/drivers/drv_tone_alarm.h | 1 + src/drivers/stm32/tone_alarm/tone_alarm.cpp | 2 ++ 2 files changed, 3 insertions(+) diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h index caf2b0f6ee..2fab37dd21 100644 --- a/src/drivers/drv_tone_alarm.h +++ b/src/drivers/drv_tone_alarm.h @@ -144,6 +144,7 @@ enum { TONE_ARMING_WARNING_TUNE, TONE_BATTERY_WARNING_SLOW_TUNE, TONE_BATTERY_WARNING_FAST_TUNE, + TONE_GPS_WARNING_TUNE, TONE_NUMBER_OF_TUNES }; diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index 9308090366..f36f2091e3 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -333,6 +333,7 @@ ToneAlarm::ToneAlarm() : _default_tunes[TONE_ARMING_WARNING_TUNE] = "MNT75L1O2G"; //arming warning _default_tunes[TONE_BATTERY_WARNING_SLOW_TUNE] = "MBNT100a8"; //battery warning slow _default_tunes[TONE_BATTERY_WARNING_FAST_TUNE] = "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8"; //battery warning fast + _default_tunes[TONE_GPS_WARNING_TUNE] = "MFT255L4AAAL1F#"; //gps warning slow _tune_names[TONE_STARTUP_TUNE] = "startup"; // startup tune _tune_names[TONE_ERROR_TUNE] = "error"; // ERROR tone @@ -342,6 +343,7 @@ ToneAlarm::ToneAlarm() : _tune_names[TONE_ARMING_WARNING_TUNE] = "arming"; // arming warning _tune_names[TONE_BATTERY_WARNING_SLOW_TUNE] = "slow_bat"; // battery warning slow _tune_names[TONE_BATTERY_WARNING_FAST_TUNE] = "fast_bat"; // battery warning fast + _tune_names[TONE_GPS_WARNING_TUNE] = "gps_warning"; // gps warning } ToneAlarm::~ToneAlarm() From 858e6debaf2d09fdeb6f238ff6f09404beb96c5d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Sep 2013 14:06:22 +0200 Subject: [PATCH 722/872] Better voltage scaling for Iris --- ROMFS/px4fmu_common/init.d/16_3dr_iris | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/16_3dr_iris b/ROMFS/px4fmu_common/init.d/16_3dr_iris index b6c5fbdea4..d47ecb3932 100644 --- a/ROMFS/px4fmu_common/init.d/16_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/16_3dr_iris @@ -53,7 +53,7 @@ else mavlink start -d /dev/ttyS0 usleep 5000 fmu mode_pwm - param set BAT_V_SCALING 0.004593 + param set BAT_V_SCALING 0.0098 set EXIT_ON_END yes fi From 252fc30ca7330dac224e087c2f91531c1d19842e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Sep 2013 14:07:01 +0200 Subject: [PATCH 723/872] Start digital airspeed sensors as default --- ROMFS/px4fmu_common/init.d/rc.sensors | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 2828a108bf..f17b650bc2 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -27,6 +27,22 @@ else set BOARD fmuv2 fi +# Start airspeed sensors +if meas_airspeed start +then + echo "using MEAS airspeed sensor" +else + if ets_airspeed start + then + echo "using ETS airspeed sensor (bus 3)" + else + if ets_airspeed start -b 1 + then + echo "Using ETS airspeed sensor (bus 1)" + fi + fi +fi + # # Start the sensor collection task. # IMPORTANT: this also loads param offsets From 1b32ba2436848745e0a78c59fffa0a767cab9d3c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Sep 2013 14:12:50 +0200 Subject: [PATCH 724/872] Hotfix: Ensure that a minimum of 10 degrees pitch is set on takeoff --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index d6d135f9f8..3d5bce134c 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -734,9 +734,10 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* apply minimum pitch and limit roll if target altitude is not within 10 meters */ if (altitude_error > 10.0f) { + /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min), _airspeed.indicated_airspeed_m_s, eas2tas, - true, math::radians(global_triplet.current.param1), + true, math::max(math::radians(global_triplet.current.param1), math::radians(10.0f)), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); From 1a3d17d4e79af8b3868ec08c9404f51b47500369 Mon Sep 17 00:00:00 2001 From: Hyon Lim Date: Tue, 1 Oct 2013 09:15:15 +0900 Subject: [PATCH 725/872] Update sensor_params.c Not necessarily modify this on initial. --- src/modules/sensors/sensor_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 031d20ded8..4d719e0e15 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -87,7 +87,7 @@ PARAM_DEFINE_FLOAT(RC2_REV, 1.0f); PARAM_DEFINE_FLOAT(RC2_DZ, 0.0f); PARAM_DEFINE_FLOAT(RC3_MIN, 1000); -PARAM_DEFINE_FLOAT(RC3_TRIM, 1000); +PARAM_DEFINE_FLOAT(RC3_TRIM, 1500); PARAM_DEFINE_FLOAT(RC3_MAX, 2000); PARAM_DEFINE_FLOAT(RC3_REV, 1.0f); PARAM_DEFINE_FLOAT(RC3_DZ, 0.0f); From baa49080547d740959f96174023a9cd5312924f1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 4 Oct 2013 13:00:12 +0200 Subject: [PATCH 726/872] Changes to pwm systemcmd, basic functionality there, needs polishing --- src/drivers/px4io/px4io.cpp | 104 +-------- src/systemcmds/pwm/pwm.c | 429 ++++++++++++++++++++++++------------ 2 files changed, 286 insertions(+), 247 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 0fed99692b..add20c5511 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1748,6 +1748,7 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) case PWM_SERVO_SET_MIN_PWM: { struct pwm_output_values* pwm = (struct pwm_output_values*)arg; + warnx("Set min values"); set_min_values(pwm->values, pwm->channel_count); } break; @@ -2381,110 +2382,7 @@ 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)"); - } - - int iofd = open(PWM_OUTPUT_DEVICE_PATH, 0); - struct pwm_output_values pwm; - - if (iofd > 0) { - - pwm.channel_count = 0; - - for (unsigned i = 0; i < sizeof(pwm.values) / sizeof(pwm.values[0]); i++) - { - /* set channel to commanline argument or to 900 for non-provided channels */ - if (argc > i + 2) { - pwm.values[i] = atoi(argv[i+2]); - if (pwm.values[i] < 900 || pwm.values[i] > 1200) { - errx(1, "value out of range of 900 < value < 1200. Aborting."); - } - pwm.channel_count++; - } - } - - int ret = ioctl(iofd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm); - - 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)"); - } - - int iofd = open(PWM_OUTPUT_DEVICE_PATH, 0); - struct pwm_output_values pwm; - - if (iofd > 0) { - - pwm.channel_count = 0; - - for (int i = 0; i < sizeof(pwm.values) / sizeof(pwm.values[0]); i++) - { - /* set channel to commanline argument or to 2100 for non-provided channels */ - if (argc > i + 2) { - pwm.values[i] = atoi(argv[i+2]); - if (pwm.values[i] < 1800 || pwm.values[i] > 2100) { - errx(1, "value out of range of 1800 < value < 2100. Aborting."); - } - pwm.channel_count++; - } - } - - int ret = ioctl(iofd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm); - - if (ret != OK) - errx(ret, "failed setting max values"); - } else { - errx(1, "not loaded"); - } - exit(0); - } - - if (!strcmp(argv[1], "idle") || !strcmp(argv[1], "disarmed")) { - - if (argc < 3) { - errx(1, "max command needs at least one channel value (PWM)"); - } - - int iofd = open(PWM_OUTPUT_DEVICE_PATH, 0); - struct pwm_output_values pwm; - - if (iofd > 0) { - - pwm.channel_count = 0; - - for (unsigned i = 0; i < sizeof(pwm.values) / sizeof(pwm.values[0]); i++) - { - /* set channel to commanline argument or to 0 for non-provided channels */ - if (argc > i + 2) { - pwm.values[i] = atoi(argv[i+2]); - if (pwm.values[i] < 900 || pwm.values[i] > 2100) { - errx(1, "value out of range of 900 < value < 2100. Aborting."); - } - pwm.channel_count++; - } - } - - int ret = ioctl(iofd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm); - - if (ret != OK) - errx(ret, "failed setting idle values"); - } else { - errx(1, "not loaded"); - } - exit(0); - } if (!strcmp(argv[1], "recovery")) { diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index c42a36c7f3..58df9cd159 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -45,6 +45,7 @@ #include #include #include +#include #include #include #include @@ -71,16 +72,25 @@ usage(const char *reason) warnx("%s", reason); errx(1, "usage:\n" - "pwm [-v] [-d ] [-u ] [-c ] [-m chanmask ] [arm|disarm] [ ...]\n" - " -v Print information about the PWM device\n" - " PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n" - " PWM update rate for channels in \n" - " Channel group that should update at the alternate rate (may be specified more than once)\n" - " arm | disarm Arm or disarm the ouptut\n" - " ... PWM output values in microseconds to assign to the PWM outputs\n" - " Directly supply alt rate channel mask (debug use only)\n" + "pwm [-v] [-d ] set|config|arm|disarm|info ...\n" + "" + " -v Print verbose information\n" + " -d PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n" "\n" - "When -c is specified, any channel groups not listed with -c will update at the default rate.\n" + "arm Arm output\n" + "disarm Disarm output\n" + "\n" + "set ... Directly set PWM values\n" + "\n" + "config rate Configure PWM rates\n" + " [-c ] Channel group that should update at the alternate rate\n" + " [-m ] Directly supply alt rate channel mask\n" + "\n" + "config min ... Configure minimum PWM values\n" + "config max ... Configure maximum PWM values\n" + "config disarmed ... Configure disarmed PWM values\n" + "\n" + "info Print information about the PWM device\n" ); } @@ -92,100 +102,53 @@ pwm_main(int argc, char *argv[]) unsigned alt_rate = 0; uint32_t alt_channel_groups = 0; bool alt_channels_set = false; - bool print_info = false; + bool print_verbose = false; int ch; int ret; char *ep; - unsigned group; int32_t set_mask = -1; + unsigned group; - if (argc < 2) + if (argc < 1) usage(NULL); - while ((ch = getopt(argc, argv, "c:d:u:vm:")) != EOF) { + while ((ch = getopt(argc, argv, "v:d:")) != EOF) { switch (ch) { - case 'c': - group = strtoul(optarg, &ep, 0); - if ((*ep != '\0') || (group >= 32)) - usage("bad channel_group value"); - alt_channel_groups |= (1 << group); - alt_channels_set = true; - break; case 'd': + if (NULL == strstr(optarg, "/dev/")) { + warnx("device %s not valid", optarg); + usage(NULL); + } dev = optarg; - break; - - case 'u': - alt_rate = strtol(optarg, &ep, 0); - if (*ep != '\0') - usage("bad alt_rate value"); - break; - - case 'm': - set_mask = strtol(optarg, &ep, 0); - if (*ep != '\0') - usage("bad set_mask value"); + argv+=2; + argc-=2; break; case 'v': - print_info = true; + print_verbose = true; + argv+=1; + argc-=1; break; default: - usage(NULL); + break; } } - argc -= optind; - argv += optind; + + /* get rid of cmd name */ + argv+=1; + argc-=1; /* open for ioctl only */ int fd = open(dev, 0); if (fd < 0) err(1, "can't open %s", dev); - /* change alternate PWM rate */ - if (alt_rate > 0) { - ret = ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, alt_rate); - if (ret != OK) - err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)"); - } - /* directly supplied channel mask */ - if (set_mask != -1) { - ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, set_mask); - if (ret != OK) - err(1, "PWM_SERVO_SELECT_UPDATE_RATE"); - } + for (int argi=0; argi sizeof(channel) / sizeof(channel[0])) - err(1, "too many pwm values (max %d)", sizeof(channel) / sizeof(channel[0])); - channel[nchannels] = pwm_value; - nchannels++; + warnx("Outputs disarmed"); + exit(0); - continue; - } - usage("unrecognized option"); - } + } else if (!strcmp(argv[argi], "set")) { - /* print verbose info */ - if (print_info) { - /* get the number of servo channels */ - unsigned count; - ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&count); - if (ret != OK) - err(1, "PWM_SERVO_GET_COUNT"); + /* iterate remaining arguments */ + unsigned nchannels = 0; + unsigned channel[PWM_OUTPUT_MAX_CHANNELS] = {0}; - /* print current servo values */ - for (unsigned i = 0; i < count; i++) { - servo_position_t spos; + while (argc - argi > 1) { + argi++; + unsigned pwm_value = strtol(argv[argi], &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])); + //XXX check for sane values ? + channel[nchannels] = pwm_value; + if (print_verbose) + warnx("Set channel %d: %d us", nchannels+1, channel[nchannels]); - ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&spos); - if (ret == OK) { - printf("channel %u: %uus\n", i, spos); + nchannels++; + + continue; + } + usage("unrecognized option"); + } + + /* perform PWM output */ + if (nchannels) { + + /* Open console directly to grab CTRL-C signal */ + struct pollfd fds; + fds.fd = 0; /* stdin */ + fds.events = POLLIN; + + warnx("Press CTRL-C or 'c' to abort."); + + while (1) { + for (unsigned 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; + ret = poll(&fds, 1, 0); + if (ret > 0) { + + read(0, &c, 1); + if (c == 0x03 || c == 0x63 || c == 'q') { + warnx("User abort\n"); + exit(0); + } + } + } } else { - printf("%u: ERROR\n", i); + usage("no PWM values supplied"); } - } - /* print rate groups */ - for (unsigned i = 0; i < count; i++) { - uint32_t group_mask; + } else if (!strcmp(argv[argi], "config")) { - ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(i), (unsigned long)&group_mask); - if (ret != OK) + struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0}; + + argi++; + + if (!strcmp(argv[argi], "rate")) { + + while ((ch = getopt(argc, argv, "m:c:")) != EOF) { + switch (ch) { + + case 'm': + set_mask = strtol(optarg, &ep, 0); + if (*ep != '\0') + usage("bad set_mask value"); + break; + argi+=2; + + case 'c': + group = strtoul(optarg, &ep, 0); + if ((*ep != '\0') || (group >= 32)) + usage("bad channel_group value"); + alt_channel_groups |= (1 << group); + alt_channels_set = true; + argi+=2; + break; + } + } + argi++; + if (argi >= argc) + usage("no alt_rate value supplied"); + + alt_rate = strtol(argv[argi], &ep, 0); + if (*ep != '\0') + usage("bad alt_rate value"); break; - if (group_mask != 0) { - printf("channel group %u: channels", i); - for (unsigned j = 0; j < 32; j++) - if (group_mask & (1 << j)) - printf(" %u", j); - printf("\n"); - } - } - fflush(stdout); - } - /* perform PWM output */ - if (nchannels) { + /* change alternate PWM rate */ + if (alt_rate > 0) { + ret = ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, alt_rate); + if (ret != OK) + err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)"); + } - /* 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"); + /* directly supplied channel mask */ + if (set_mask != -1) { + ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, set_mask); + if (ret != OK) + err(1, "PWM_SERVO_SELECT_UPDATE_RATE"); + } - warnx("Press CTRL-C or 'c' to abort."); + /* assign alternate rate to channel groups */ + if (alt_channels_set) { + uint32_t mask = 0; - 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); + for (group = 0; group < 32; group++) { + if ((1 << group) & alt_channel_groups) { + uint32_t group_mask; + + ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(group), (unsigned long)&group_mask); + if (ret != OK) + err(1, "PWM_SERVO_GET_RATEGROUP(%u)", group); + + mask |= group_mask; + } + } + + ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, mask); + if (ret != OK) + err(1, "PWM_SERVO_SELECT_UPDATE_RATE"); + } + + + } else if (!strcmp(argv[argi], "min")) { + + /* iterate remaining arguments */ + while (argc - argi > 1) { + argi++; + unsigned pwm_value = strtol(argv[argi], &ep, 0); + if (*ep == '\0') { + if (pwm_values.channel_count > PWM_OUTPUT_MAX_CHANNELS) + err(1, "too many pwm values (max %d)", PWM_OUTPUT_MAX_CHANNELS); + //XXX check for sane values ? + pwm_values.values[pwm_values.channel_count] = pwm_value; + pwm_values.channel_count++; + + continue; + } + usage("unrecognized option"); + } + if (pwm_values.channel_count == 0) { + usage("no PWM values added"); + } else { + + ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); + if (ret != OK) + errx(ret, "failed setting idle values"); + } + + + } else if (!strcmp(argv[argi], "max")) { + + /* iterate remaining arguments */ + while (argc - argi > 1) { + argi++; + unsigned pwm_value = strtol(argv[argi], &ep, 0); + if (*ep == '\0') { + if (pwm_values.channel_count > PWM_OUTPUT_MAX_CHANNELS) + err(1, "too many pwm values (max %d)", PWM_OUTPUT_MAX_CHANNELS); + //XXX check for sane values ? + pwm_values.values[pwm_values.channel_count] = pwm_value; + pwm_values.channel_count++; + + continue; + } + usage("unrecognized option"); + } + if (pwm_values.channel_count == 0) { + usage("no PWM values added"); + } else { + + ret = ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values); + if (ret != OK) + errx(ret, "failed setting idle values"); + } + + + } else if (!strcmp(argv[argi], "disarmed")) { + + /* iterate remaining arguments */ + while (argc - argi > 1) { + argi++; + unsigned pwm_value = strtol(argv[argi], &ep, 0); + if (*ep == '\0') { + if (pwm_values.channel_count > PWM_OUTPUT_MAX_CHANNELS) + err(1, "too many pwm values (max %d)", PWM_OUTPUT_MAX_CHANNELS); + //XXX check for sane values ? + pwm_values.values[pwm_values.channel_count] = pwm_value; + pwm_values.channel_count++; + + continue; + } + usage("unrecognized option"); + } + if (pwm_values.channel_count == 0) { + usage("no PWM values added"); + } else { + + ret = ioctl(fd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm_values); + if (ret != OK) + errx(ret, "failed setting idle values"); + } + + } else { + usage("specify rate, min, max or disarmed"); } - /* abort on user request */ - char c; - if (read(console, &c, 1) == 1) { - if (c == 0x03 || c == 0x63 || c == 'q') { - warnx("User abort\n"); - close(console); - exit(0); + } else if (!strcmp(argv[argi], "info")) { + + /* get the number of servo channels */ + unsigned count; + ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&count); + if (ret != OK) + err(1, "PWM_SERVO_GET_COUNT"); + + /* print current servo values */ + for (unsigned i = 0; i < count; i++) { + servo_position_t spos; + + ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&spos); + if (ret == OK) { + printf("channel %u: %uus\n", i, spos); + } else { + printf("%u: ERROR\n", i); } } - /* rate limit to ~ 20 Hz */ - usleep(50000); + /* print rate groups */ + for (unsigned i = 0; i < count; i++) { + uint32_t group_mask; + + ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(i), (unsigned long)&group_mask); + if (ret != OK) + break; + if (group_mask != 0) { + printf("channel group %u: channels", i); + for (unsigned j = 0; j < 32; j++) + if (group_mask & (1 << j)) + printf(" %u", j); + printf("\n"); + } + } + + } else { + usage("specify arm|disarm|set|config"); } } - exit(0); -} \ No newline at end of file +} + + + From 9ff521711861fce857b6c17c2ec87eaa2073376e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 4 Oct 2013 17:20:34 +0200 Subject: [PATCH 727/872] Some more interface changes, needs testing and cleanup --- src/drivers/px4io/px4io.cpp | 2 - src/systemcmds/pwm/pwm.c | 496 ++++++++++++++++++------------------ 2 files changed, 255 insertions(+), 243 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index add20c5511..ae5728af00 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -957,8 +957,6 @@ PX4IO::set_idle_values(const uint16_t *vals, unsigned len) /* 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); } diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 58df9cd159..44e49dd054 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -72,25 +72,33 @@ usage(const char *reason) warnx("%s", reason); errx(1, "usage:\n" - "pwm [-v] [-d ] set|config|arm|disarm|info ...\n" - "" - " -v Print verbose information\n" - " -d PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n" + "pwm arm|disarm|rate|min|max|disarmed|test|info ...\n" "\n" - "arm Arm output\n" - "disarm Disarm output\n" + " arm Arm output\n" + " disarm Disarm output\n" "\n" - "set ... Directly set PWM values\n" + " rate Configure PWM rates\n" + " [-c ] Channel group that should update at the alternate rate\n" + " [-m ] Directly supply channel mask\n" + " [-a] Configure all outputs\n" + " -r PWM rate (50 to 400 Hz)\n" "\n" - "config rate Configure PWM rates\n" - " [-c ] Channel group that should update at the alternate rate\n" - " [-m ] Directly supply alt rate channel mask\n" + " min ... Configure minimum PWM values\n" + " max ... Configure maximum PWM values\n" + " disarmed ... Configure disarmed PWM values\n" + " [-m ] Directly supply channel mask\n" + " [-a] Configure all outputs\n" + " -p PWM value\n" "\n" - "config min ... Configure minimum PWM values\n" - "config max ... Configure maximum PWM values\n" - "config disarmed ... Configure disarmed PWM values\n" + " test ... Directly set PWM values\n" + " [-m ] Directly supply channel mask\n" + " [-a] Configure all outputs\n" + " -p PWM value\n" "\n" - "info Print information about the PWM device\n" + " info Print information about the PWM device\n" + "\n" + " -v Print verbose information\n" + " -d PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n" ); } @@ -106,13 +114,16 @@ pwm_main(int argc, char *argv[]) int ch; int ret; char *ep; - int32_t set_mask = -1; + uint32_t set_mask = 0; unsigned group; + unsigned long channels; + unsigned single_ch = 0; + unsigned pwm_value = 0; if (argc < 1) usage(NULL); - while ((ch = getopt(argc, argv, "v:d:")) != EOF) { + while ((ch = getopt(argc-1, &argv[1], "d:vc:m:ap:r:")) != EOF) { switch (ch) { case 'd': @@ -121,32 +132,82 @@ pwm_main(int argc, char *argv[]) usage(NULL); } dev = optarg; - argv+=2; - argc-=2; break; case 'v': print_verbose = true; - argv+=1; - argc-=1; break; + case 'c': + /* Read in channels supplied as one int and convert to mask: 1234 -> 0xF */ + channels = strtol(optarg, &ep, 0); + + while ((single_ch = channels % 10)) { + + set_mask |= 1<<(single_ch-1); + channels /= 10; + } + break; + + case 'g': + group = strtoul(optarg, &ep, 0); + if ((*ep != '\0') || (group >= 32)) + usage("bad channel_group value"); + alt_channel_groups |= (1 << group); + alt_channels_set = true; + break; + + case 'm': + /* Read in mask directly */ + set_mask = strtol(optarg, &ep, 0); + if (*ep != '\0') + usage("bad set_mask value"); + break; + + case 'a': + for (unsigned i = 0; i 0) { + warnx("Chose channels: "); + printf(" "); + for (unsigned i = 0; i 1) { - argi++; - unsigned pwm_value = strtol(argv[argi], &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])); - //XXX check for sane values ? - channel[nchannels] = pwm_value; - if (print_verbose) - warnx("Set channel %d: %d us", nchannels+1, channel[nchannels]); - - nchannels++; - - continue; - } - usage("unrecognized option"); - } - - /* perform PWM output */ - if (nchannels) { - - /* Open console directly to grab CTRL-C signal */ - struct pollfd fds; - fds.fd = 0; /* stdin */ - fds.events = POLLIN; - - warnx("Press CTRL-C or 'c' to abort."); - - while (1) { - for (unsigned 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; - ret = poll(&fds, 1, 0); - if (ret > 0) { - - read(0, &c, 1); - if (c == 0x03 || c == 0x63 || c == 'q') { - warnx("User abort\n"); - exit(0); - } - } - } + /* change alternate PWM rate */ + if (alt_rate > 0) { + ret = ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, alt_rate); + if (ret != OK) + err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)"); } else { - usage("no PWM values supplied"); + usage("no alternative rate provided"); } - } else if (!strcmp(argv[argi], "config")) { + /* directly supplied channel mask */ + if (set_mask > 0) { + ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, set_mask); + if (ret != OK) + err(1, "PWM_SERVO_SELECT_UPDATE_RATE"); + } else { + usage("no channel/channel groups selected"); + } + + /* assign alternate rate to channel groups */ + if (alt_channels_set) { + uint32_t mask = 0; + + for (group = 0; group < 32; group++) { + if ((1 << group) & alt_channel_groups) { + uint32_t group_mask; + + ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(group), (unsigned long)&group_mask); + if (ret != OK) + err(1, "PWM_SERVO_GET_RATEGROUP(%u)", group); + + mask |= group_mask; + } + } + + ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, mask); + if (ret != OK) + err(1, "PWM_SERVO_SELECT_UPDATE_RATE"); + } + exit(0); + + } else if (!strcmp(argv[argi], "min")) { + + if (set_mask == 0) { + usage("no channels set"); + } + if (pwm_value == 0) + usage("no PWM value provided"); struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0}; - argi++; - - if (!strcmp(argv[argi], "rate")) { - - while ((ch = getopt(argc, argv, "m:c:")) != EOF) { - switch (ch) { - - case 'm': - set_mask = strtol(optarg, &ep, 0); - if (*ep != '\0') - usage("bad set_mask value"); - break; - argi+=2; - - case 'c': - group = strtoul(optarg, &ep, 0); - if ((*ep != '\0') || (group >= 32)) - usage("bad channel_group value"); - alt_channel_groups |= (1 << group); - alt_channels_set = true; - argi+=2; - break; - } + for (unsigned i = 0; i < servo_count; i++) { + if (set_mask & 1<= argc) - usage("no alt_rate value supplied"); - - alt_rate = strtol(argv[argi], &ep, 0); - if (*ep != '\0') - usage("bad alt_rate value"); - break; - - /* change alternate PWM rate */ - if (alt_rate > 0) { - ret = ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, alt_rate); - if (ret != OK) - err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)"); - } - - /* directly supplied channel mask */ - if (set_mask != -1) { - ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, set_mask); - if (ret != OK) - err(1, "PWM_SERVO_SELECT_UPDATE_RATE"); - } - - /* assign alternate rate to channel groups */ - if (alt_channels_set) { - uint32_t mask = 0; - - for (group = 0; group < 32; group++) { - if ((1 << group) & alt_channel_groups) { - uint32_t group_mask; - - ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(group), (unsigned long)&group_mask); - if (ret != OK) - err(1, "PWM_SERVO_GET_RATEGROUP(%u)", group); - - mask |= group_mask; - } - } - - ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, mask); - if (ret != OK) - err(1, "PWM_SERVO_SELECT_UPDATE_RATE"); - } - - - } else if (!strcmp(argv[argi], "min")) { - - /* iterate remaining arguments */ - while (argc - argi > 1) { - argi++; - unsigned pwm_value = strtol(argv[argi], &ep, 0); - if (*ep == '\0') { - if (pwm_values.channel_count > PWM_OUTPUT_MAX_CHANNELS) - err(1, "too many pwm values (max %d)", PWM_OUTPUT_MAX_CHANNELS); - //XXX check for sane values ? - pwm_values.values[pwm_values.channel_count] = pwm_value; - pwm_values.channel_count++; - - continue; - } - usage("unrecognized option"); - } - if (pwm_values.channel_count == 0) { - usage("no PWM values added"); - } else { - - ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); - if (ret != OK) - errx(ret, "failed setting idle values"); - } - - - } else if (!strcmp(argv[argi], "max")) { - - /* iterate remaining arguments */ - while (argc - argi > 1) { - argi++; - unsigned pwm_value = strtol(argv[argi], &ep, 0); - if (*ep == '\0') { - if (pwm_values.channel_count > PWM_OUTPUT_MAX_CHANNELS) - err(1, "too many pwm values (max %d)", PWM_OUTPUT_MAX_CHANNELS); - //XXX check for sane values ? - pwm_values.values[pwm_values.channel_count] = pwm_value; - pwm_values.channel_count++; - - continue; - } - usage("unrecognized option"); - } - if (pwm_values.channel_count == 0) { - usage("no PWM values added"); - } else { - - ret = ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values); - if (ret != OK) - errx(ret, "failed setting idle values"); - } - - - } else if (!strcmp(argv[argi], "disarmed")) { - - /* iterate remaining arguments */ - while (argc - argi > 1) { - argi++; - unsigned pwm_value = strtol(argv[argi], &ep, 0); - if (*ep == '\0') { - if (pwm_values.channel_count > PWM_OUTPUT_MAX_CHANNELS) - err(1, "too many pwm values (max %d)", PWM_OUTPUT_MAX_CHANNELS); - //XXX check for sane values ? - pwm_values.values[pwm_values.channel_count] = pwm_value; - pwm_values.channel_count++; - - continue; - } - usage("unrecognized option"); - } - if (pwm_values.channel_count == 0) { - usage("no PWM values added"); - } else { - - ret = ioctl(fd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm_values); - if (ret != OK) - errx(ret, "failed setting idle values"); - } - - } else { - usage("specify rate, min, max or disarmed"); } + if (pwm_values.channel_count == 0) { + usage("no PWM values added"); + } else { + + ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); + if (ret != OK) + errx(ret, "failed setting idle values"); + } + exit(0); + + } else if (!strcmp(argv[argi], "max")) { + + if (set_mask == 0) { + usage("no channels set"); + } + if (pwm_value == 0) + usage("no PWM value provided"); + + struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0}; + + for (unsigned i = 0; i < servo_count; i++) { + if (set_mask & 1< 0) { + + read(0, &c, 1); + if (c == 0x03 || c == 0x63 || c == 'q') { + warnx("User abort\n"); + exit(0); + } + } + } + exit(0); + + } else if (!strcmp(argv[argi], "info")) { - /* get the number of servo channels */ - unsigned count; - ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&count); - if (ret != OK) - err(1, "PWM_SERVO_GET_COUNT"); - /* print current servo values */ - for (unsigned i = 0; i < count; i++) { + for (unsigned i = 0; i < servo_count; i++) { servo_position_t spos; ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&spos); @@ -405,7 +418,7 @@ pwm_main(int argc, char *argv[]) } /* print rate groups */ - for (unsigned i = 0; i < count; i++) { + for (unsigned i = 0; i < servo_count; i++) { uint32_t group_mask; ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(i), (unsigned long)&group_mask); @@ -419,12 +432,13 @@ pwm_main(int argc, char *argv[]) printf("\n"); } } + exit(0); - } else { - usage("specify arm|disarm|set|config"); } + argi++; } - exit(0); + usage("specify arm|disarm|set|config|test"); + return 0; } From d1871bd7bb9ae9eefdf1ada56fc3f57428e689eb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Oct 2013 14:18:28 +0200 Subject: [PATCH 728/872] State machine fixes for HIL --- src/modules/commander/commander.cpp | 50 ++++++++++++------- .../commander/state_machine_helper.cpp | 29 +++++++---- src/modules/commander/state_machine_helper.h | 2 +- src/modules/mavlink/waypoints.c | 4 +- src/modules/sensors/sensors.cpp | 3 ++ 5 files changed, 56 insertions(+), 32 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 01b7b84d01..0c3546b954 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -359,23 +359,35 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe case VEHICLE_CMD_DO_SET_MODE: { uint8_t base_mode = (uint8_t) cmd->param1; uint8_t custom_main_mode = (uint8_t) cmd->param2; + transition_result_t arming_res = TRANSITION_NOT_CHANGED; /* set HIL state */ hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF; - hil_state_transition(new_hil_state, status_pub, status, control_mode_pub, control_mode, mavlink_fd); + int hil_ret = hil_state_transition(new_hil_state, status_pub, status, control_mode_pub, control_mode, mavlink_fd); + + /* if HIL got enabled, reset battery status state */ + if (hil_ret == OK && control_mode->flag_system_hil_enabled) { + /* reset the arming mode to disarmed */ + arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_STANDBY, armed); + if (arming_res != TRANSITION_DENIED) { + mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby"); + } else { + mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state"); + } + } // TODO remove debug code //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; + arming_res = TRANSITION_NOT_CHANGED; if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - if (safety->safety_switch_available && !safety->safety_off) { + if ((safety->safety_switch_available && !safety->safety_off) && !control_mode->flag_system_hil_enabled) { print_reject_arm("NOT ARMING: Press safety switch first."); arming_res = TRANSITION_DENIED; } else { - arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed); + arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed); } if (arming_res == TRANSITION_CHANGED) { @@ -385,7 +397,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } 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, safety, new_arming_state, armed); + arming_res = arming_state_transition(status, safety, control_mode, new_arming_state, armed); if (arming_res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command"); @@ -478,7 +490,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe arming_res = TRANSITION_DENIED; } else { - arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed); + arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed); } if (arming_res == TRANSITION_CHANGED) { @@ -532,6 +544,9 @@ static struct actuator_armed_s armed; static struct safety_s safety; +/* flags for control apps */ +struct vehicle_control_mode_s control_mode; + int commander_thread_main(int argc, char *argv[]) { /* not yet initialized */ @@ -584,10 +599,6 @@ int commander_thread_main(int argc, char *argv[]) /* Initialize armed with all false */ memset(&armed, 0, sizeof(armed)); - /* flags for control apps */ - struct vehicle_control_mode_s control_mode; - - /* Initialize all flags to false */ memset(&control_mode, 0, sizeof(control_mode)); @@ -876,8 +887,8 @@ 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 (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 0.001f) { + /* only consider battery voltage if system has been running 2s and battery voltage is higher than 4V */ + if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 4.0f) { status.battery_voltage = battery.voltage_v; status.condition_battery_voltage_valid = true; status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage); @@ -958,9 +969,9 @@ int commander_thread_main(int argc, char *argv[]) battery_tune_played = false; if (armed.armed) { - arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); + arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED_ERROR, &armed); } else { - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); + arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed); } status_changed = true; @@ -969,6 +980,7 @@ int commander_thread_main(int argc, char *argv[]) critical_voltage_counter++; } else { + low_voltage_counter = 0; critical_voltage_counter = 0; } @@ -978,7 +990,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, &safety, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed); } else { // XXX: Add emergency stuff if sensors are lost @@ -1082,7 +1094,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, &safety, new_arming_state, &armed); + res = arming_state_transition(&status, &safety, &control_mode, new_arming_state, &armed); stick_off_counter = 0; } else { @@ -1104,7 +1116,7 @@ int commander_thread_main(int argc, char *argv[]) print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); } else { - res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); + res = arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED, &armed); } stick_on_counter = 0; @@ -1752,7 +1764,7 @@ void *commander_low_prio_loop(void *arg) /* 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)) { + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_INIT, &armed)) { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); break; } @@ -1792,7 +1804,7 @@ void *commander_low_prio_loop(void *arg) else tune_negative(); - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed); break; } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 8ce31550f9..efe12be57e 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -67,7 +67,9 @@ static bool main_state_changed = true; 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) +arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, + const struct vehicle_control_mode_s *control_mode, + arming_state_t new_arming_state, struct actuator_armed_s *armed) { /* * Perform an atomic state update @@ -82,6 +84,13 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * } else { + /* enforce lockdown in HIL */ + if (control_mode->flag_system_hil_enabled) { + armed->lockdown = true; + } else { + armed->lockdown = false; + } + switch (new_arming_state) { case ARMING_STATE_INIT: @@ -98,7 +107,8 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * /* allow coming from INIT and disarming from ARMED */ if (status->arming_state == ARMING_STATE_INIT - || status->arming_state == ARMING_STATE_ARMED) { + || status->arming_state == ARMING_STATE_ARMED + || control_mode->flag_system_hil_enabled) { /* sensors need to be initialized for STANDBY state */ if (status->condition_system_sensors_initialized) { @@ -115,7 +125,7 @@ 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 */ + && (!safety->safety_switch_available || safety->safety_off || control_mode->flag_system_hil_enabled)) { /* only allow arming if safety is off */ ret = TRANSITION_CHANGED; armed->armed = true; armed->ready_to_arm = true; @@ -466,20 +476,17 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s case HIL_STATE_OFF: - 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; - } + /* we're in HIL and unexpected things can happen if we disable HIL now */ + mavlink_log_critical(mavlink_fd, "Not switching off HIL (safety)"); + valid_transition = false; break; case HIL_STATE_ON: if (current_status->arming_state == ARMING_STATE_INIT - || current_status->arming_state == ARMING_STATE_STANDBY) { + || current_status->arming_state == ARMING_STATE_STANDBY + || current_status->arming_state == ARMING_STATE_STANDBY_ERROR) { current_control_mode->flag_system_hil_enabled = true; mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 1641b6f60b..0bfdf36a8f 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); + const struct vehicle_control_mode_s *control_mode, 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); diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index 97472c233c..f03fe4fdf4 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -307,7 +307,9 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ { static uint16_t counter; - if (!global_pos->valid && !local_pos->xy_valid) { + if ((!global_pos->valid && !local_pos->xy_valid) || + /* no waypoint */ + wpm->size == 0) { /* nothing to check here, return */ return; } diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 085da905c6..9ef0d3c830 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1224,6 +1224,9 @@ Sensors::parameter_update_poll(bool forced) void Sensors::adc_poll(struct sensor_combined_s &raw) { + /* only read if publishing */ + if (!_publishing) + return; /* rate limit to 100 Hz */ if (hrt_absolute_time() - _last_adc >= 10000) { From 81e9c06129ff96508377777ab3a405977c873357 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Oct 2013 21:04:59 +0200 Subject: [PATCH 729/872] Robustified flight close to waypoints --- src/lib/ecl/l1/ecl_l1_pos_controller.cpp | 16 ++++++++++++++++ src/lib/ecl/l1/ecl_l1_pos_controller.h | 8 ++++++++ 2 files changed, 24 insertions(+) diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp index 87eb99f16d..0dadf56f39 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp @@ -86,6 +86,7 @@ float ECL_L1_Pos_Controller::crosstrack_error(void) void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position, const math::Vector2f &ground_speed_vector) { + /* this follows the logic presented in [1] */ float eta; @@ -116,6 +117,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c /* calculate the vector from waypoint A to the aircraft */ math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); + math::Vector2f vector_B_to_airplane = get_local_planar_vector(vector_B, vector_curr_position); /* calculate crosstrack error (output only) */ _crosstrack_error = vector_AB % vector_A_to_airplane; @@ -126,6 +128,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c * If the aircraft is already between A and B normal L1 logic is applied. */ float distance_A_to_airplane = vector_A_to_airplane.length(); + float distance_B_to_airplane = vector_B_to_airplane.length(); float alongTrackDist = vector_A_to_airplane * vector_AB; /* extension from [2] */ @@ -143,6 +146,19 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c /* bearing from current position to L1 point */ _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX()); + } else if (distance_B_to_airplane < alongTrackDist || distance_B_to_airplane < _L1_distance) { + + /* fly directly to B */ + /* unit vector from waypoint B to current position */ + math::Vector2f vector_B_to_airplane_unit = vector_B_to_airplane.normalized(); + /* velocity across / orthogonal to line */ + xtrack_vel = ground_speed_vector % (-vector_B_to_airplane_unit); + /* velocity along line */ + ltrack_vel = ground_speed_vector * (-vector_B_to_airplane_unit); + eta = atan2f(xtrack_vel, ltrack_vel); + /* bearing from current position to L1 point */ + _nav_bearing = atan2f(-vector_B_to_airplane_unit.getY() , -vector_B_to_airplane_unit.getX()); + } else { /* calculate eta to fly along the line between A and B */ diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.h b/src/lib/ecl/l1/ecl_l1_pos_controller.h index a6a2c01563..5a17346cb1 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.h +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.h @@ -130,6 +130,14 @@ public: bool reached_loiter_target(); + /** + * Returns true if following a circle (loiter) + */ + bool circle_mode() { + return _circle_mode; + } + + /** * Get the switch distance * From 378041ad31794109b2673adab8102faf6806bd96 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 6 Oct 2013 23:09:55 +0200 Subject: [PATCH 730/872] px4io: make "too high rate" warning consistent with real behavor --- 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 1336460515..cba193208d 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -94,6 +94,8 @@ extern device::Device *PX4IO_serial_interface() weak_function; #define PX4IO_SET_DEBUG _IOC(0xff00, 0) #define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1) +#define UPDATE_INTERVAL_MIN 2 + /** * The PX4IO class. * @@ -790,8 +792,8 @@ PX4IO::task_main() /* adjust update interval */ if (_update_interval != 0) { - if (_update_interval < 5) - _update_interval = 5; + if (_update_interval < UPDATE_INTERVAL_MIN) + _update_interval = UPDATE_INTERVAL_MIN; if (_update_interval > 100) _update_interval = 100; orb_set_interval(_t_actuators, _update_interval); @@ -1942,8 +1944,8 @@ int PX4IO::set_update_rate(int rate) { int interval_ms = 1000 / rate; - if (interval_ms < 3) { - interval_ms = 3; + if (interval_ms < UPDATE_INTERVAL_MIN) { + interval_ms = UPDATE_INTERVAL_MIN; warnx("update rate too high, limiting interval to %d ms (%d Hz).", interval_ms, 1000 / interval_ms); } @@ -2317,7 +2319,7 @@ px4io_main(int argc, char *argv[]) if ((argc > 2)) { g_dev->set_update_rate(atoi(argv[2])); } else { - errx(1, "missing argument (50 - 400 Hz)"); + errx(1, "missing argument (50 - 500 Hz)"); return 1; } exit(0); From 2fd8c6b958bb00c37c8a3fb885b6b657d6c14755 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Oct 2013 13:54:31 +0200 Subject: [PATCH 731/872] multirotor_att_control: cleanup, some refactoring --- .../multirotor_att_control_main.c | 433 +++++++++--------- 1 file changed, 216 insertions(+), 217 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 04582f2a40..44f8f664bc 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -89,18 +89,18 @@ static int 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 vehicle_attitude_s att; 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 sensor_combined_s raw; - memset(&raw, 0, sizeof(raw)); struct offboard_control_setpoint_s offboard_sp; memset(&offboard_sp, 0, sizeof(offboard_sp)); + struct vehicle_control_mode_s control_mode; + memset(&control_mode, 0, sizeof(control_mode)); + struct manual_control_setpoint_s manual; + memset(&manual, 0, sizeof(manual)); + struct sensor_combined_s sensor; + memset(&sensor, 0, sizeof(sensor)); struct vehicle_rates_setpoint_s rates_sp; memset(&rates_sp, 0, sizeof(rates_sp)); struct vehicle_status_s status; @@ -108,29 +108,16 @@ mc_thread_main(int argc, char *argv[]) struct actuator_controls_s actuators; memset(&actuators, 0, sizeof(actuators)); - /* 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 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 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 - * if rate-limiting would be desired later, the line below would - * enable it. - * - * rate-limit the attitude subscription to 200Hz to pace our loop - * orb_set_interval(att_sub, 5); - */ - struct pollfd fds[2] = { - { .fd = att_sub, .events = POLLIN }, - { .fd = param_sub, .events = POLLIN } - }; + /* subscribe */ + int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); + int vehicle_attitude_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + int offboard_control_setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); + int vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); + int vehicle_rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); + int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); /* publish actuator controls */ for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { @@ -146,233 +133,246 @@ mc_thread_main(int argc, char *argv[]) perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "multirotor_att_control_interval"); perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "multirotor_att_control_err"); - /* welcome user */ warnx("starting"); /* store last control mode to detect mode switches */ bool control_yaw_position = true; bool reset_yaw_sp = true; + struct pollfd fds[1] = { + { .fd = vehicle_attitude_sub, .events = POLLIN }, + }; + while (!thread_should_exit) { /* wait for a sensor update, check for exit condition every 500 ms */ - int ret = poll(fds, 2, 500); + int ret = poll(fds, 1, 500); if (ret < 0) { /* poll error, count it in perf */ perf_count(mc_err_perf); - } else if (ret == 0) { - /* no return value, ignore */ - } else { + } else if (ret > 0) { + /* only run controller if attitude changed */ + perf_begin(mc_loop_perf); - /* only update parameters if they changed */ - if (fds[1].revents & POLLIN) { - /* read from param to clear updated flag */ + /* attitude */ + orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); + + bool updated; + + /* parameters */ + orb_check(parameter_update_sub, &updated); + + if (updated) { struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), param_sub, &update); - + orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); /* update parameters */ } - /* only run controller if attitude changed */ - if (fds[0].revents & POLLIN) { + /* control mode */ + orb_check(vehicle_control_mode_sub, &updated); - perf_begin(mc_loop_perf); + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), vehicle_control_mode_sub, &control_mode); + } - /* get a local copy of system state */ - bool updated; - orb_check(control_mode_sub, &updated); + /* manual control setpoint */ + orb_check(manual_control_setpoint_sub, &updated); - if (updated) { - orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); + if (updated) { + orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual); + } + + /* attitude setpoint */ + orb_check(vehicle_attitude_setpoint_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub, &att_sp); + } + + /* offboard control setpoint */ + orb_check(offboard_control_setpoint_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(offboard_control_setpoint), offboard_control_setpoint_sub, &offboard_sp); + } + + /* vehicle status */ + orb_check(vehicle_status_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &status); + } + + /* sensors */ + orb_check(sensor_combined_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); + } + + /* 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 */ + if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) { + rates_sp.roll = offboard_sp.p1; + rates_sp.pitch = offboard_sp.p2; + rates_sp.yaw = offboard_sp.p3; + rates_sp.thrust = offboard_sp.p4; + rates_sp.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); + + } else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) { + att_sp.roll_body = offboard_sp.p1; + att_sp.pitch_body = offboard_sp.p2; + att_sp.yaw_body = offboard_sp.p3; + att_sp.thrust = offboard_sp.p4; + att_sp.timestamp = hrt_absolute_time(); + /* publish the result to the vehicle actuators */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); } - /* 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 attitude setpoint */ - orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp); - /* get a local copy of rates setpoint */ - orb_check(setpoint_sub, &updated); + /* reset yaw setpoint after offboard control */ + reset_yaw_sp = true; - if (updated) { - orb_copy(ORB_ID(offboard_control_setpoint), setpoint_sub, &offboard_sp); - } + } 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 (att_sp.thrust < 0.1f) { + /* no thrust, don't try to control yaw */ + rates_sp.yaw = 0.0f; + control_yaw_position = false; - /* 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); - - /* 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 */ - if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) { - rates_sp.roll = offboard_sp.p1; - rates_sp.pitch = offboard_sp.p2; - rates_sp.yaw = offboard_sp.p3; - rates_sp.thrust = offboard_sp.p4; - rates_sp.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); - - } else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) { - att_sp.roll_body = offboard_sp.p1; - att_sp.pitch_body = offboard_sp.p2; - att_sp.yaw_body = offboard_sp.p3; - att_sp.thrust = offboard_sp.p4; - att_sp.timestamp = hrt_absolute_time(); - /* publish the result to the vehicle actuators */ - 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 (att_sp.thrust < 0.1f) { - /* no thrust, don't try to control yaw */ - rates_sp.yaw = 0.0f; - control_yaw_position = false; - - if (status.condition_landed) { - /* reset yaw setpoint if on ground */ - reset_yaw_sp = true; - } - - } else { - /* 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; - rates_sp.yaw = manual.yaw; - reset_yaw_sp = true; // has no effect on control, just for beautiful log - - } else { - control_yaw_position = true; - } + if (status.condition_landed) { + /* reset yaw setpoint if on ground */ + reset_yaw_sp = 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_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 */ - if (reset_yaw_sp) { - att_sp.yaw_body = att.yaw; - reset_yaw_sp = false; - } - - 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(); - - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - } else { - /* manual rate inputs (ACRO), from RC control or joystick */ - if (control_mode.flag_control_rates_enabled) { - rates_sp.roll = manual.roll; - rates_sp.pitch = manual.pitch; + /* 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; rates_sp.yaw = manual.yaw; - rates_sp.thrust = manual.throttle; - rates_sp.timestamp = hrt_absolute_time(); - } + reset_yaw_sp = true; // has no effect on control, just for beautiful log - /* reset yaw setpoint after ACRO */ - reset_yaw_sp = true; + } 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_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 */ + if (reset_yaw_sp) { + att_sp.yaw_body = att.yaw; + reset_yaw_sp = false; + } + + 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(); + + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + } 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); - } + /* manual rate inputs (ACRO), from RC control or joystick */ + if (control_mode.flag_control_rates_enabled) { + 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(); } - /* reset yaw setpoint after non-manual control */ + /* reset yaw setpoint after ACRO */ reset_yaw_sp = true; } - /* 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_updated = false; - orb_check(rates_sp_sub, &rates_sp_updated); - - if (rates_sp_updated) { - orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp); + } 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); } - - /* apply controller */ - float rates[3]; - rates[0] = att.rollspeed; - 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; - 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); + /* reset yaw setpoint after non-manual control */ + reset_yaw_sp = true; + } - perf_end(mc_loop_perf); - } /* end of poll call for attitude updates */ - } /* end of poll return value check */ + /* 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_updated = false; + orb_check(vehicle_rates_setpoint_sub, &rates_sp_updated); + + if (rates_sp_updated) { + orb_copy(ORB_ID(vehicle_rates_setpoint), vehicle_rates_setpoint_sub, &rates_sp); + } + + /* apply controller */ + float rates[3]; + rates[0] = att.rollspeed; + 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; + 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); + + perf_end(mc_loop_perf); + } } warnx("stopping, disarming motors"); @@ -383,10 +383,9 @@ mc_thread_main(int argc, char *argv[]) orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - - close(att_sub); - close(control_mode_sub); - close(manual_sub); + close(vehicle_attitude_sub); + close(vehicle_control_mode_sub); + close(manual_control_setpoint_sub); close(actuator_pub); close(att_sp_pub); From ea0aa49b546476ef9ca9904b32dc507d66f0ab44 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 7 Oct 2013 16:24:49 +0200 Subject: [PATCH 732/872] pwm info provides more information, some fixes for setting rate/min/max/disarmed --- src/drivers/drv_pwm_output.h | 38 +- src/drivers/hil/hil.cpp | 2 +- src/drivers/mkblctrl/mkblctrl.cpp | 2 +- src/drivers/px4fmu/fmu.cpp | 14 +- src/drivers/px4io/px4io.cpp | 48 ++- src/modules/px4iofirmware/mixer.cpp | 4 +- src/modules/px4iofirmware/protocol.h | 4 +- src/modules/px4iofirmware/px4io.h | 2 +- src/modules/px4iofirmware/registers.c | 55 ++- src/systemcmds/pwm/pwm.c | 521 ++++++++++++++------------ 10 files changed, 396 insertions(+), 294 deletions(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index fc916b5225..76e98597ae 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -101,38 +101,56 @@ ORB_DECLARE(output_pwm); /** disarm all servo outputs (stop generating pulses) */ #define PWM_SERVO_DISARM _IOC(_PWM_SERVO_BASE, 1) +/** get default servo update rate */ +#define PWM_SERVO_GET_DEFAULT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2) + /** set alternate servo update rate */ -#define PWM_SERVO_SET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2) +#define PWM_SERVO_SET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 3) + +/** get alternate servo update rate */ +#define PWM_SERVO_GET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 4) /** get the number of servos in *(unsigned *)arg */ -#define PWM_SERVO_GET_COUNT _IOC(_PWM_SERVO_BASE, 3) +#define PWM_SERVO_GET_COUNT _IOC(_PWM_SERVO_BASE, 5) /** selects servo update rates, one bit per servo. 0 = default (50Hz), 1 = alternate */ -#define PWM_SERVO_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 4) +#define PWM_SERVO_SET_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 6) + +/** check the selected update rates */ +#define PWM_SERVO_GET_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 7) /** set the 'ARM ok' bit, which activates the safety switch */ -#define PWM_SERVO_SET_ARM_OK _IOC(_PWM_SERVO_BASE, 5) +#define PWM_SERVO_SET_ARM_OK _IOC(_PWM_SERVO_BASE, 8) /** clear the 'ARM ok' bit, which deactivates the safety switch */ -#define PWM_SERVO_CLEAR_ARM_OK _IOC(_PWM_SERVO_BASE, 6) +#define PWM_SERVO_CLEAR_ARM_OK _IOC(_PWM_SERVO_BASE, 9) /** start DSM bind */ -#define DSM_BIND_START _IOC(_PWM_SERVO_BASE, 7) +#define DSM_BIND_START _IOC(_PWM_SERVO_BASE, 10) #define DSM2_BIND_PULSES 3 /* DSM_BIND_START ioctl parameter, pulses required to start dsm2 pairing */ #define DSMX_BIND_PULSES 7 /* DSM_BIND_START ioctl parameter, pulses required to start dsmx pairing */ /** power up DSM receiver */ -#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 8) +#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 11) /** set the PWM value when disarmed - should be no PWM (zero) by default */ -#define PWM_SERVO_SET_DISARMED_PWM _IOC(_PWM_SERVO_BASE, 9) +#define PWM_SERVO_SET_DISARMED_PWM _IOC(_PWM_SERVO_BASE, 12) + +/** get the PWM value when disarmed */ +#define PWM_SERVO_GET_DISARMED_PWM _IOC(_PWM_SERVO_BASE, 13) /** set the minimum PWM value the output will send */ -#define PWM_SERVO_SET_MIN_PWM _IOC(_PWM_SERVO_BASE, 10) +#define PWM_SERVO_SET_MIN_PWM _IOC(_PWM_SERVO_BASE, 14) + +/** get the minimum PWM value the output will send */ +#define PWM_SERVO_GET_MIN_PWM _IOC(_PWM_SERVO_BASE, 15) /** set the maximum PWM value the output will send */ -#define PWM_SERVO_SET_MAX_PWM _IOC(_PWM_SERVO_BASE, 11) +#define PWM_SERVO_SET_MAX_PWM _IOC(_PWM_SERVO_BASE, 16) + +/** get the maximum PWM value the output will send */ +#define PWM_SERVO_GET_MAX_PWM _IOC(_PWM_SERVO_BASE, 17) /** set a single servo to a specific value */ #define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo) diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index 3e30e32927..6563c3446e 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -517,7 +517,7 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg) g_hil->set_pwm_rate(arg); break; - case PWM_SERVO_SELECT_UPDATE_RATE: + case PWM_SERVO_SET_SELECT_UPDATE_RATE: // HIL always outputs at the alternate (usually faster) rate break; diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index d0de26a1ad..b93f38cf6b 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -1071,7 +1071,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg) ret = OK; break; - case PWM_SERVO_SELECT_UPDATE_RATE: + case PWM_SERVO_SET_SELECT_UPDATE_RATE: ret = OK; break; diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index b1dd55dd78..3fc75eb29f 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -685,14 +685,26 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) up_pwm_servo_arm(false); break; + case PWM_SERVO_GET_DEFAULT_UPDATE_RATE: + *(uint32_t *)arg = _pwm_default_rate; + break; + case PWM_SERVO_SET_UPDATE_RATE: ret = set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, arg); break; - case PWM_SERVO_SELECT_UPDATE_RATE: + case PWM_SERVO_GET_UPDATE_RATE: + *(uint32_t *)arg = _pwm_alt_rate; + break; + + case PWM_SERVO_SET_SELECT_UPDATE_RATE: ret = set_pwm_rate(arg, _pwm_default_rate, _pwm_alt_rate); break; + case PWM_SERVO_GET_SELECT_UPDATE_RATE: + *(uint32_t *)arg = _pwm_alt_rate_channels; + break; + case PWM_SERVO_SET(5): case PWM_SERVO_SET(4): if (_mode < MODE_6PWM) { diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index ae5728af00..ea3a738224 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -958,7 +958,7 @@ PX4IO::set_idle_values(const uint16_t *vals, unsigned len) return E2BIG; /* copy values to registers in IO */ - return io_reg_set(PX4IO_PAGE_IDLE_PWM, 0, vals, len); + return io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, vals, len); } @@ -1684,7 +1684,7 @@ PX4IO::print_status() printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); printf("\nidle values"); for (unsigned i = 0; i < _max_actuators; i++) - printf(" %u", io_reg_get(PX4IO_PAGE_IDLE_PWM, i)); + printf(" %u", io_reg_get(PX4IO_PAGE_DISARMED_PWM, i)); printf("\n"); } @@ -1716,12 +1716,22 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED, 0); break; + case PWM_SERVO_GET_DEFAULT_UPDATE_RATE: + /* get the default update rate */ + *(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE); + break; + case PWM_SERVO_SET_UPDATE_RATE: /* set the requested alternate rate */ ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE, arg); break; - case PWM_SERVO_SELECT_UPDATE_RATE: { + case PWM_SERVO_GET_UPDATE_RATE: + /* get the alternative update rate */ + *(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE); + break; + + case PWM_SERVO_SET_SELECT_UPDATE_RATE: { /* blindly clear the PWM update alarm - might be set for some other reason */ io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR); @@ -1738,23 +1748,51 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) break; } + case PWM_SERVO_GET_SELECT_UPDATE_RATE: + + *(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES); + break; + case PWM_SERVO_SET_DISARMED_PWM: { struct pwm_output_values* pwm = (struct pwm_output_values*)arg; set_idle_values(pwm->values, pwm->channel_count); + break; } + + case PWM_SERVO_GET_DISARMED_PWM: + + ret = io_reg_get(PX4IO_PAGE_DISARMED_PWM, 0, (uint16_t*)arg, sizeof(struct pwm_output_values)/sizeof(uint16_t)); + if (ret != OK) { + ret = -EIO; + } break; case PWM_SERVO_SET_MIN_PWM: { struct pwm_output_values* pwm = (struct pwm_output_values*)arg; - warnx("Set min values"); set_min_values(pwm->values, pwm->channel_count); + break; } + + case PWM_SERVO_GET_MIN_PWM: + + ret = io_reg_get(PX4IO_PAGE_CONTROL_MIN_PWM, 0, (uint16_t*)arg, sizeof(struct pwm_output_values)/sizeof(uint16_t)); + if (ret != OK) { + ret = -EIO; + } break; case PWM_SERVO_SET_MAX_PWM: { struct pwm_output_values* pwm = (struct pwm_output_values*)arg; set_max_values(pwm->values, pwm->channel_count); + break; } + + case PWM_SERVO_GET_MAX_PWM: + + ret = io_reg_get(PX4IO_PAGE_CONTROL_MAX_PWM, 0, (uint16_t*)arg, sizeof(struct pwm_output_values)/sizeof(uint16_t)); + if (ret != OK) { + ret = -EIO; + } break; case PWM_SERVO_GET_COUNT: @@ -2448,5 +2486,5 @@ px4io_main(int argc, char *argv[]) bind(argc, argv); out: - errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'failsafe', 'min, 'max',\n 'idle', 'bind' or 'update'"); + errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'failsafe', 'bind' or 'update'"); } diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 30aff7d20a..350b93488e 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -307,9 +307,9 @@ mixer_tick(void) up_pwm_servo_set(i, r_page_servos[i]); } else if (mixer_servos_armed && should_always_enable_pwm) { - /* set the idle servo outputs. */ + /* set the disarmed servo outputs. */ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) - up_pwm_servo_set(i, r_page_servo_idle[i]); + up_pwm_servo_set(i, r_page_servo_disarmed[i]); } } diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 0e2cd16898..5e5396782b 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -220,8 +220,8 @@ enum { /* DSM bind states */ /* 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 */ +/* PWM disarmed values that are active, even when SAFETY_SAFE */ +#define PX4IO_PAGE_DISARMED_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 66c4ca9066..6c9007a75a 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -80,7 +80,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 */ +extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ /* * Register aliases. diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 8cb21e54fb..9d3c5ccfd9 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -220,10 +220,10 @@ uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT] = { 2100, 2100, 2100, 2100 /** * PAGE 108 * - * idle PWM values for difficult ESCs + * disarmed PWM values for difficult ESCs * */ -uint16_t r_page_servo_idle[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 }; +uint16_t r_page_servo_disarmed[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 }; int registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) @@ -293,16 +293,15 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* copy channel data */ while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { - if (*values == 0) - /* set to default */ - r_page_servo_control_min[offset] = 900; - - else if (*values > 1200) + if (*values == 0) { + /* ignore 0 */ + } else if (*values > 1200) { r_page_servo_control_min[offset] = 1200; - else if (*values < 900) + } else if (*values < 900) { r_page_servo_control_min[offset] = 900; - else + } else { r_page_servo_control_min[offset] = *values; + } offset++; num_values--; @@ -315,16 +314,15 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* copy channel data */ while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { - if (*values == 0) - /* set to default */ + if (*values == 0) { + /* ignore 0 */ + } else if (*values > 2100) { r_page_servo_control_max[offset] = 2100; - - else if (*values > 2100) - r_page_servo_control_max[offset] = 2100; - else if (*values < 1800) + } else if (*values < 1800) { r_page_servo_control_max[offset] = 1800; - else + } else { r_page_servo_control_max[offset] = *values; + } offset++; num_values--; @@ -332,21 +330,20 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num } break; - case PX4IO_PAGE_IDLE_PWM: + case PX4IO_PAGE_DISARMED_PWM: /* copy channel data */ while ((offset < PX4IO_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; + if (*values == 0) { + /* ignore 0 */ + } else if (*values < 900) { + r_page_servo_disarmed[offset] = 900; + } else if (*values > 2100) { + r_page_servo_disarmed[offset] = 2100; + } else { + r_page_servo_disarmed[offset] = *values; + } /* flag the failsafe values as custom */ r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE; @@ -769,8 +766,8 @@ 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); + case PX4IO_PAGE_DISARMED_PWM: + SELECT_PAGE(r_page_servo_disarmed); break; default: diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 44e49dd054..25f8c4e753 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -77,21 +77,23 @@ usage(const char *reason) " arm Arm output\n" " disarm Disarm output\n" "\n" - " rate Configure PWM rates\n" - " [-c ] Channel group that should update at the alternate rate\n" + " rate Configure PWM rates\n" + " [-g ] Channel group that should update at the alternate rate\n" " [-m ] Directly supply channel mask\n" " [-a] Configure all outputs\n" " -r PWM rate (50 to 400 Hz)\n" "\n" - " min ... Configure minimum PWM values\n" - " max ... Configure maximum PWM values\n" - " disarmed ... Configure disarmed PWM values\n" - " [-m ] Directly supply channel mask\n" + " min ... Configure minimum PWM values\n" + " max ... Configure maximum PWM values\n" + " disarmed ... Configure disarmed PWM values\n" + " [-c ] Supply channels (e.g. 1234)\n" + " [-m ] Directly supply channel mask (e.g. 0xF)\n" " [-a] Configure all outputs\n" " -p PWM value\n" "\n" " test ... Directly set PWM values\n" - " [-m ] Directly supply channel mask\n" + " [-c ] Supply channels (e.g. 1234)\n" + " [-m ] Directly supply channel mask (e.g. 0xF)\n" " [-a] Configure all outputs\n" " -p PWM value\n" "\n" @@ -123,7 +125,7 @@ pwm_main(int argc, char *argv[]) if (argc < 1) usage(NULL); - while ((ch = getopt(argc-1, &argv[1], "d:vc:m:ap:r:")) != EOF) { + while ((ch = getopt(argc-1, &argv[1], "d:vc:g:m:ap:r:")) != EOF) { switch (ch) { case 'd': @@ -140,7 +142,7 @@ pwm_main(int argc, char *argv[]) case 'c': /* Read in channels supplied as one int and convert to mask: 1234 -> 0xF */ - channels = strtol(optarg, &ep, 0); + channels = strtoul(optarg, &ep, 0); while ((single_ch = channels % 10)) { @@ -155,11 +157,12 @@ pwm_main(int argc, char *argv[]) usage("bad channel_group value"); alt_channel_groups |= (1 << group); alt_channels_set = true; + warnx("alt channels set, group: %d", group); break; case 'm': /* Read in mask directly */ - set_mask = strtol(optarg, &ep, 0); + set_mask = strtoul(optarg, &ep, 0); if (*ep != '\0') usage("bad set_mask value"); break; @@ -170,12 +173,12 @@ pwm_main(int argc, char *argv[]) } break; case 'p': - pwm_value = strtol(optarg, &ep, 0); + pwm_value = strtoul(optarg, &ep, 0); if (*ep != '\0') usage("bad PWM value provided"); break; case 'r': - alt_rate = strtol(optarg, &ep, 0); + alt_rate = strtoul(optarg, &ep, 0); if (*ep != '\0') usage("bad alternative rate provided"); break; @@ -205,241 +208,275 @@ pwm_main(int argc, char *argv[]) if (ret != OK) err(1, "PWM_SERVO_GET_COUNT"); - int argi = 1; /* leave away cmd name */ + if (!strcmp(argv[1], "arm")) { + /* 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"); + /* tell IO that the system is armed (it will output values if safety is off) */ + ret = ioctl(fd, PWM_SERVO_ARM, 0); + if (ret != OK) + err(1, "PWM_SERVO_ARM"); - while(argi 0) { + ret = ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, alt_rate); if (ret != OK) - err(1, "PWM_SERVO_SET_ARM_OK"); - /* tell IO that the system is armed (it will output values if safety is off) */ - ret = ioctl(fd, PWM_SERVO_ARM, 0); - if (ret != OK) - err(1, "PWM_SERVO_ARM"); - - if (print_verbose) - warnx("Outputs armed"); - exit(0); - - } else if (!strcmp(argv[argi], "disarm")) { - /* disarm, but do not revoke the SET_ARM_OK flag */ - ret = ioctl(fd, PWM_SERVO_DISARM, 0); - if (ret != OK) - err(1, "PWM_SERVO_DISARM"); - - if (print_verbose) - warnx("Outputs disarmed"); - exit(0); - - }else if (!strcmp(argv[argi], "rate")) { - - /* change alternate PWM rate */ - if (alt_rate > 0) { - ret = ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, alt_rate); - if (ret != OK) - err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)"); - } else { - usage("no alternative rate provided"); - } - - /* directly supplied channel mask */ - if (set_mask > 0) { - ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, set_mask); - if (ret != OK) - err(1, "PWM_SERVO_SELECT_UPDATE_RATE"); - } else { - usage("no channel/channel groups selected"); - } - - /* assign alternate rate to channel groups */ - if (alt_channels_set) { - uint32_t mask = 0; - - for (group = 0; group < 32; group++) { - if ((1 << group) & alt_channel_groups) { - uint32_t group_mask; - - ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(group), (unsigned long)&group_mask); - if (ret != OK) - err(1, "PWM_SERVO_GET_RATEGROUP(%u)", group); - - mask |= group_mask; - } - } - - ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, mask); - if (ret != OK) - err(1, "PWM_SERVO_SELECT_UPDATE_RATE"); - } - exit(0); - - } else if (!strcmp(argv[argi], "min")) { - - if (set_mask == 0) { - usage("no channels set"); - } - if (pwm_value == 0) - usage("no PWM value provided"); - - struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0}; - - for (unsigned i = 0; i < servo_count; i++) { - if (set_mask & 1< 0) { - - read(0, &c, 1); - if (c == 0x03 || c == 0x63 || c == 'q') { - warnx("User abort\n"); - exit(0); - } - } - } - exit(0); - - - } else if (!strcmp(argv[argi], "info")) { - - /* print current servo values */ - for (unsigned i = 0; i < servo_count; i++) { - servo_position_t spos; - - ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&spos); - if (ret == OK) { - printf("channel %u: %uus\n", i, spos); - } else { - printf("%u: ERROR\n", i); - } - } - - /* print rate groups */ - for (unsigned i = 0; i < servo_count; i++) { - uint32_t group_mask; - - ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(i), (unsigned long)&group_mask); - if (ret != OK) - break; - if (group_mask != 0) { - printf("channel group %u: channels", i); - for (unsigned j = 0; j < 32; j++) - if (group_mask & (1 << j)) - printf(" %u", j); - printf("\n"); - } - } - exit(0); - + err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)"); } - argi++; + + /* directly supplied channel mask */ + if (set_mask > 0) { + ret = ioctl(fd, PWM_SERVO_SET_SELECT_UPDATE_RATE, set_mask); + if (ret != OK) + err(1, "PWM_SERVO_SET_SELECT_UPDATE_RATE"); + } + + /* assign alternate rate to channel groups */ + if (alt_channels_set) { + uint32_t mask = 0; + + for (group = 0; group < 32; group++) { + if ((1 << group) & alt_channel_groups) { + uint32_t group_mask; + + ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(group), (unsigned long)&group_mask); + if (ret != OK) + err(1, "PWM_SERVO_GET_RATEGROUP(%u)", group); + + mask |= group_mask; + } + } + + ret = ioctl(fd, PWM_SERVO_SET_SELECT_UPDATE_RATE, mask); + if (ret != OK) + err(1, "PWM_SERVO_SET_SELECT_UPDATE_RATE"); + } + exit(0); + + } else if (!strcmp(argv[1], "min")) { + + if (set_mask == 0) { + usage("no channels set"); + } + if (pwm_value == 0) + usage("no PWM value provided"); + + struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0}; + + for (unsigned i = 0; i < servo_count; i++) { + if (set_mask & 1< 0) { + + read(0, &c, 1); + if (c == 0x03 || c == 0x63 || c == 'q') { + warnx("User abort\n"); + exit(0); + } + } + } + exit(0); + + + } else if (!strcmp(argv[1], "info")) { + + printf("device: %s\n", dev); + + uint32_t info_default_rate; + uint32_t info_alt_rate; + uint32_t info_alt_rate_mask; + + ret = ioctl(fd, PWM_SERVO_GET_DEFAULT_UPDATE_RATE, (unsigned long)&info_default_rate); + if (ret != OK) { + err(1, "PWM_SERVO_GET_DEFAULT_UPDATE_RATE"); + } + + ret = ioctl(fd, PWM_SERVO_GET_UPDATE_RATE, (unsigned long)&info_alt_rate); + if (ret != OK) { + err(1, "PWM_SERVO_GET_UPDATE_RATE"); + } + + ret = ioctl(fd, PWM_SERVO_GET_SELECT_UPDATE_RATE, (unsigned long)&info_alt_rate_mask); + if (ret != OK) { + err(1, "PWM_SERVO_GET_SELECT_UPDATE_RATE"); + } + + struct pwm_output_values disarmed_pwm; + struct pwm_output_values min_pwm; + struct pwm_output_values max_pwm; + + ret = ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, (unsigned long)&disarmed_pwm); + if (ret != OK) { + err(1, "PWM_SERVO_GET_DISARMED_PWM"); + } + ret = ioctl(fd, PWM_SERVO_GET_MIN_PWM, (unsigned long)&min_pwm); + if (ret != OK) { + err(1, "PWM_SERVO_GET_MIN_PWM"); + } + ret = ioctl(fd, PWM_SERVO_GET_MAX_PWM, (unsigned long)&max_pwm); + if (ret != OK) { + err(1, "PWM_SERVO_GET_MAX_PWM"); + } + + /* print current servo values */ + for (unsigned i = 0; i < servo_count; i++) { + servo_position_t spos; + + ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&spos); + if (ret == OK) { + printf("channel %u: %u us", i+1, spos); + + if (info_alt_rate_mask & (1< Date: Mon, 7 Oct 2013 16:34:07 +0200 Subject: [PATCH 733/872] Adapt startup scripts to new pwm systemcmd interface --- ROMFS/px4fmu_common/init.d/10_dji_f330 | 12 ++++++++---- ROMFS/px4fmu_common/init.d/11_dji_f450 | 13 ++++++++----- ROMFS/px4fmu_common/init.d/15_tbs_discovery | 13 ++++++++----- ROMFS/px4fmu_common/init.d/16_3dr_iris | 13 ++++++++----- ROMFS/px4fmu_common/init.d/666_fmu_q_x550 | 2 +- .../init.d/rc.custom_dji_f330_mkblctrl | 10 ++++------ 6 files changed, 37 insertions(+), 26 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10_dji_f330 b/ROMFS/px4fmu_common/init.d/10_dji_f330 index d21759507d..e7a62a4b80 100644 --- a/ROMFS/px4fmu_common/init.d/10_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/10_dji_f330 @@ -61,9 +61,6 @@ then sh /etc/init.d/rc.io # Set PWM values for DJI ESCs - px4io idle 900 900 900 900 - px4io min 1200 1200 1200 1200 - px4io max 1800 1800 1800 1800 else # Start MAVLink (on UART1 / ttyS0) mavlink start -d /dev/ttyS0 @@ -81,7 +78,14 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix # # Set PWM output frequency # -pwm -u 400 -m 0xff +pwm rate -c 1234 -r 400 + +# +# Set disarmed, min and max PWM signals +# +pwm disarmed -c 1234 -p 900 +pwm min -c 1234 -p 1200 +pwm max -c 1234 -p 1800 # # Start common for all multirotors apps diff --git a/ROMFS/px4fmu_common/init.d/11_dji_f450 b/ROMFS/px4fmu_common/init.d/11_dji_f450 index 388f40a477..91847b06bf 100644 --- a/ROMFS/px4fmu_common/init.d/11_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/11_dji_f450 @@ -44,10 +44,6 @@ then usleep 5000 sh /etc/init.d/rc.io - # Set PWM values for DJI ESCs - px4io idle 900 900 900 900 - px4io min 1200 1200 1200 1200 - px4io max 1800 1800 1800 1800 else # Start MAVLink (on UART1 / ttyS0) mavlink start -d /dev/ttyS0 @@ -65,7 +61,14 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix # # Set PWM output frequency # -pwm -u 400 -m 0xff +pwm rate -c 1234 -r 400 + +# +# Set disarmed, min and max PWM signals (for DJI ESCs) +# +pwm disarmed -c 1234 -p 900 +pwm min -c 1234 -p 1200 +pwm max -c 1234 -p 1800 # # Start common for all multirotors apps diff --git a/ROMFS/px4fmu_common/init.d/15_tbs_discovery b/ROMFS/px4fmu_common/init.d/15_tbs_discovery index cbfde6d3c0..65be56df8c 100644 --- a/ROMFS/px4fmu_common/init.d/15_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/15_tbs_discovery @@ -44,10 +44,6 @@ then usleep 5000 sh /etc/init.d/rc.io - # Set PWM values for DJI ESCs - px4io idle 900 900 900 900 - px4io min 1200 1200 1200 1200 - px4io max 1800 1800 1800 1800 else # Start MAVLink (on UART1 / ttyS0) mavlink start -d /dev/ttyS0 @@ -65,7 +61,14 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix # # Set PWM output frequency # -pwm -u 400 -m 0xff +pwm rate -c 1234 -r 400 + +# +# Set disarmed, min and max PWM signals (for DJI ESCs) +# +pwm disarmed -c 1234 -p 900 +pwm min -c 1234 -p 1200 +pwm max -c 1234 -p 1800 # # Start common for all multirotors apps diff --git a/ROMFS/px4fmu_common/init.d/16_3dr_iris b/ROMFS/px4fmu_common/init.d/16_3dr_iris index d47ecb3932..3434735fd7 100644 --- a/ROMFS/px4fmu_common/init.d/16_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/16_3dr_iris @@ -44,10 +44,6 @@ then usleep 5000 sh /etc/init.d/rc.io - # Set PWM values for DJI ESCs - px4io idle 900 900 900 900 - px4io min 1200 1200 1200 1200 - px4io max 1800 1800 1800 1800 else # Start MAVLink (on UART1 / ttyS0) mavlink start -d /dev/ttyS0 @@ -65,7 +61,14 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix # # Set PWM output frequency # -pwm -u 400 -m 0xff +pwm rate -c 1234 -r 400 + +# +# Set disarmed, min and max PWM signals +# +pwm disarmed -c 1234 -p 900 +pwm min -c 1234 -p 1200 +pwm max -c 1234 -p 1800 # # Start common for all multirotors apps diff --git a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 index ae41f2a014..eae37098bd 100644 --- a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 +++ b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 @@ -53,7 +53,7 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix # # Set PWM output frequency # -pwm -u 400 -m 0xff +pwm rate -c 1234 -r 400 # # Start common for all multirotors apps diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl index 51bc61c9e8..a63d0e5f1d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl +++ b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl @@ -83,10 +83,6 @@ then usleep 5000 sh /etc/init.d/rc.io - # Set PWM values for DJI ESCs - px4io idle 900 900 900 900 - px4io min 1200 1200 1200 1200 - px4io max 1800 1800 1800 1800 else fmu mode_pwm # Start MAVLink (on UART1 / ttyS0) @@ -107,9 +103,11 @@ else fi # -# Set PWM output frequency +# Set disarmed, min and max PWM signals # -#pwm -u 400 -m 0xff +pwm disarmed -c 1234 -p 900 +pwm min -c 1234 -p 1200 +pwm max -c 1234 -p 1800 # # Start common for all multirotors apps From 19879432ad6cf709af25192401829719defd2983 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 7 Oct 2013 18:03:05 +0200 Subject: [PATCH 734/872] Trying to get rid of magic PWM numbers --- src/drivers/drv_pwm_output.h | 20 ++++++++++++++++++ src/drivers/px4io/px4io.cpp | 6 +++--- src/modules/px4iofirmware/mixer.cpp | 4 ++-- src/modules/px4iofirmware/registers.c | 30 +++++++++++++-------------- 4 files changed, 40 insertions(+), 20 deletions(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 76e98597ae..5a3a126d04 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -64,6 +64,26 @@ __BEGIN_DECLS */ #define PWM_OUTPUT_MAX_CHANNELS 16 +/** + * Minimum PWM in us + */ +#define PWM_MIN 900 + +/** + * Highest PWM allowed as the minimum PWM + */ +#define PWM_HIGHEST_MIN 1300 + +/** + * Maximum PWM in us + */ +#define PWM_MAX 2100 + +/** + * Lowest PWM allowed as the maximum PWM + */ +#define PWM_LOWEST_MAX 1700 + /** * Servo output signal type, value is actual servo output pulse * width in microseconds. diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index ea3a738224..f9dc3773e2 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1818,7 +1818,7 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) /* TODO: we could go lower for e.g. TurboPWM */ unsigned channel = cmd - PWM_SERVO_SET(0); - if ((channel >= _max_actuators) || (arg < 900) || (arg > 2100)) { + if ((channel >= _max_actuators) || (arg < PWM_MIN) || (arg > PWM_MAX)) { ret = -EINVAL; } else { /* send a direct PWM value */ @@ -2402,8 +2402,8 @@ px4io_main(int argc, char *argv[]) /* 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."); + if (failsafe[i] < PWM_MIN || failsafe[i] > PWM_AX) { + errx(1, "value out of range of %d < value < %d. Aborting.", PWM_MIN, PWM_MAX); } } else { /* a zero value will result in stopping to output any pulse */ diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 350b93488e..352b93e858 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -242,8 +242,8 @@ mixer_tick(void) 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); + * (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*PWM_MAX - ramp_promille*r_page_servo_control_min[i] - (1000-ramp_promille)*PWM_MIN)/2/1000 + + (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*PWM_MAX + ramp_promille*r_page_servo_control_min[i] + (1000-ramp_promille)*PWM_MIN)/2/1000); break; case ESC_ON: diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 9d3c5ccfd9..30e6f7de27 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -207,7 +207,7 @@ uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0 }; * minimum PWM values when armed * */ -uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 }; +uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT] = { PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN }; /** * PAGE 107 @@ -215,7 +215,7 @@ uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 90 * maximum PWM values when armed * */ -uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT] = { 2100, 2100, 2100, 2100, 2100, 2100, 2100, 2100 }; +uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT] = { PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX }; /** * PAGE 108 @@ -223,7 +223,7 @@ uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT] = { 2100, 2100, 2100, 2100 * disarmed PWM values for difficult ESCs * */ -uint16_t r_page_servo_disarmed[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 }; +uint16_t r_page_servo_disarmed[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 }; int registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) @@ -295,10 +295,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num if (*values == 0) { /* ignore 0 */ - } else if (*values > 1200) { - r_page_servo_control_min[offset] = 1200; - } else if (*values < 900) { - r_page_servo_control_min[offset] = 900; + } else if (*values > PWM_HIGHEST_MIN) { + r_page_servo_control_min[offset] = PWM_HIGHEST_MIN; + } else if (*values < PWM_MIN) { + r_page_servo_control_min[offset] = PWM_MIN; } else { r_page_servo_control_min[offset] = *values; } @@ -316,10 +316,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num if (*values == 0) { /* ignore 0 */ - } else if (*values > 2100) { - r_page_servo_control_max[offset] = 2100; - } else if (*values < 1800) { - r_page_servo_control_max[offset] = 1800; + } else if (*values > PWM_MAX) { + r_page_servo_control_max[offset] = PWM_MAX; + } else if (*values < PWM_LOWEST_MAX) { + r_page_servo_control_max[offset] = PWM_LOWEST_MAX; } else { r_page_servo_control_max[offset] = *values; } @@ -337,10 +337,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num if (*values == 0) { /* ignore 0 */ - } else if (*values < 900) { - r_page_servo_disarmed[offset] = 900; - } else if (*values > 2100) { - r_page_servo_disarmed[offset] = 2100; + } else if (*values < PWM_MIN) { + r_page_servo_disarmed[offset] = PWM_MIN; + } else if (*values > PWM_MAX) { + r_page_servo_disarmed[offset] = PWM_MAX; } else { r_page_servo_disarmed[offset] = *values; } From 6e7300fb927535f7727ff6eeb2a47cad9353a346 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Oct 2013 22:02:46 +0200 Subject: [PATCH 735/872] px4io: major optimization and cleanup --- src/drivers/px4io/px4io.cpp | 102 +++++++++++++++++------------------- 1 file changed, 47 insertions(+), 55 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index cba193208d..b6392b5b12 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -94,7 +94,9 @@ extern device::Device *PX4IO_serial_interface() weak_function; #define PX4IO_SET_DEBUG _IOC(0xff00, 0) #define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1) -#define UPDATE_INTERVAL_MIN 2 +#define UPDATE_INTERVAL_MIN 2 // 2 ms -> 500 Hz +#define ORB_CHECK_INTERVAL 200000 // 200 ms -> 5 Hz +#define IO_POLL_INTERVAL 20000 // 20 ms -> 50 Hz /** * The PX4IO class. @@ -734,7 +736,8 @@ PX4IO::task_main_trampoline(int argc, char *argv[]) void PX4IO::task_main() { - hrt_abstime last_poll_time = 0; + hrt_abstime poll_last = 0; + hrt_abstime orb_check_last = 0; log("starting"); @@ -747,18 +750,10 @@ PX4IO::task_main() _t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1)); orb_set_interval(_t_actuators, 20); /* default to 50Hz */ - _t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */ - _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. */ - _t_vehicle_command = orb_subscribe(ORB_ID(vehicle_command)); - orb_set_interval(_t_param, 1000); /* 1Hz update rate max. */ if ((_t_actuators < 0) || (_t_actuator_armed < 0) || @@ -770,17 +765,9 @@ PX4IO::task_main() } /* poll descriptor */ - pollfd fds[5]; + pollfd fds[1]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; - fds[1].fd = _t_actuator_armed; - fds[1].events = POLLIN; - fds[2].fd = _t_vehicle_control_mode; - fds[2].events = POLLIN; - fds[3].fd = _t_param; - fds[3].events = POLLIN; - fds[4].fd = _t_vehicle_command; - fds[4].events = POLLIN; log("ready"); @@ -802,7 +789,7 @@ PX4IO::task_main() /* sleep waiting for topic updates, but no more than 20ms */ unlock(); - int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 20); + int ret = ::poll(fds, 1, 20); lock(); /* this would be bad... */ @@ -815,58 +802,66 @@ PX4IO::task_main() hrt_abstime now = hrt_absolute_time(); /* if we have new control data from the ORB, handle it */ - if (fds[0].revents & POLLIN) + if (fds[0].revents & POLLIN) { io_set_control_state(); - - /* if we have an arming state update, handle it */ - if ((fds[1].revents & POLLIN) || (fds[2].revents & POLLIN)) - io_set_arming_state(); - - /* if we have a vehicle command, handle it */ - if (fds[4].revents & POLLIN) { - struct vehicle_command_s cmd; - orb_copy(ORB_ID(vehicle_command), _t_vehicle_command, &cmd); - // Check for a DSM pairing command - if ((cmd.command == VEHICLE_CMD_START_RX_PAIR) && (cmd.param1== 0.0f)) { - dsm_bind_ioctl((int)cmd.param2); - } } - /* - * If it's time for another tick of the polling status machine, - * try it now. - */ - if ((now - last_poll_time) >= 20000) { + if (now >= poll_last + IO_POLL_INTERVAL) { + /* run at 50Hz */ + poll_last = now; - /* - * Pull status and alarms from IO. - */ + /* pull status and alarms from IO */ io_get_status(); - /* - * Get raw R/C input from IO. - */ + /* get raw R/C input from IO */ io_publish_raw_rc(); - /* - * Fetch mixed servo controls and PWM outputs from IO. - * - * XXX We could do this at a reduced rate in many/most cases. - */ + /* fetch mixed servo controls and PWM outputs from IO */ io_publish_mixed_controls(); io_publish_pwm_outputs(); + } + + if (now >= orb_check_last + ORB_CHECK_INTERVAL) { + /* run at 5Hz */ + orb_check_last = now; + + /* check updates on uORB topics and handle it */ + bool updated = false; + + /* arming state */ + orb_check(_t_actuator_armed, &updated); + if (!updated) { + orb_check(_t_vehicle_control_mode, &updated); + } + if (updated) { + io_set_arming_state(); + } + + /* vehicle command */ + orb_check(_t_vehicle_command, &updated); + if (updated) { + struct vehicle_command_s cmd; + orb_copy(ORB_ID(vehicle_command), _t_vehicle_command, &cmd); + // Check for a DSM pairing command + if ((cmd.command == VEHICLE_CMD_START_RX_PAIR) && (cmd.param1== 0.0f)) { + dsm_bind_ioctl((int)cmd.param2); + } + } /* * If parameters have changed, re-send RC mappings to IO * * XXX this may be a bit spammy */ - if (fds[3].revents & POLLIN) { + orb_check(_t_param, &updated); + if (updated) { parameter_update_s pupdate; + orb_copy(ORB_ID(parameter_update), _t_param, &pupdate); + int32_t dsm_bind_val; param_t dsm_bind_param; - // See if bind parameter has been set, and reset it to -1 + /* see if bind parameter has been set, and reset it to -1 */ param_get(dsm_bind_param = param_find("RC_DSM_BIND"), &dsm_bind_val); if (dsm_bind_val > -1) { dsm_bind_ioctl(dsm_bind_val); @@ -874,9 +869,6 @@ PX4IO::task_main() param_set(dsm_bind_param, &dsm_bind_val); } - /* copy to reset the notification */ - orb_copy(ORB_ID(parameter_update), _t_param, &pupdate); - /* re-upload RC input config as it may have changed */ io_set_rc_config(); } From 87e1ffe0ba293c62b882a8ae9729878e36a95c4c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Oct 2013 22:04:06 +0200 Subject: [PATCH 736/872] px4io: code style fixed --- src/drivers/px4io/px4io.cpp | 554 +++++++++++++++++++++++------------- 1 file changed, 349 insertions(+), 205 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index b6392b5b12..f3fa3ed29f 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -108,35 +108,35 @@ class PX4IO : public device::CDev public: /** * Constructor. - * + * * Initialize all class variables. */ PX4IO(device::Device *interface); /** * Destructor. - * + * * Wait for worker thread to terminate. */ virtual ~PX4IO(); /** * Initialize the PX4IO class. - * + * * Retrieve relevant initial system parameters. Initialize PX4IO registers. */ virtual int init(); /** * Detect if a PX4IO is connected. - * + * * Only validate if there is a PX4IO to talk to. */ virtual int detect(); /** * IO Control handler. - * + * * Handle all IOCTL calls to the PX4IO file descriptor. * * @param[in] filp file handle (not used). This function is always called directly through object reference @@ -147,7 +147,7 @@ public: /** * write handler. - * + * * Handle writes to the PX4IO file descriptor. * * @param[in] filp file handle (not used). This function is always called directly through object reference @@ -214,8 +214,7 @@ public: * * @param[in] enable true=DSM satellite VCC is controlled by relay1, false=DSM satellite VCC not controlled */ - inline void set_dsm_vcc_ctl(bool enable) - { + inline void set_dsm_vcc_ctl(bool enable) { _dsm_vcc_ctl = enable; }; @@ -224,8 +223,7 @@ public: * * @return true=DSM satellite VCC is controlled by relay1, false=DSM satellite VCC not controlled */ - inline bool get_dsm_vcc_ctl() - { + inline bool get_dsm_vcc_ctl() { return _dsm_vcc_ctl; }; #endif @@ -401,7 +399,7 @@ private: /** * Send mixer definition text to IO */ - int mixer_send(const char *buf, unsigned buflen, unsigned retries=3); + int mixer_send(const char *buf, unsigned buflen, unsigned retries = 3); /** * Handle a status update from IO. @@ -467,12 +465,12 @@ PX4IO::PX4IO(device::Device *interface) : _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_per_volt(90.0f / 5.0f), // this matches the 3DR current sensor _battery_amp_bias(0), _battery_mamphour_total(0), _battery_last_timestamp(0) #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 - ,_dsm_vcc_ctl(false) + , _dsm_vcc_ctl(false) #endif { @@ -515,19 +513,22 @@ PX4IO::detect() /* do regular cdev init */ ret = CDev::init(); + if (ret != OK) return ret; /* get some parameters */ unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); + if (protocol != PX4IO_PROTOCOL_VERSION) { if (protocol == _io_reg_get_error) { log("IO not installed"); + } else { log("IO version error"); mavlink_log_emergency(_mavlink_fd, "IO VERSION MISMATCH, PLEASE UPGRADE SOFTWARE!"); } - + return -1; } } @@ -546,22 +547,26 @@ PX4IO::init() /* do regular cdev init */ ret = CDev::init(); + if (ret != OK) 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_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 > 32) || (_max_transfer < 16) || (_max_transfer > 255) || @@ -571,6 +576,7 @@ PX4IO::init() mavlink_log_emergency(_mavlink_fd, "[IO] config read fail, abort."); return -1; } + if (_max_rc_input > RC_INPUT_MAX_CHANNELS) _max_rc_input = RC_INPUT_MAX_CHANNELS; @@ -585,6 +591,7 @@ PX4IO::init() /* 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; @@ -598,8 +605,8 @@ PX4IO::init() /* get a status update from IO */ io_get_status(); - mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART"); - log("INAIR RESTART RECOVERY (needs commander app running)"); + 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 @@ -611,7 +618,7 @@ PX4IO::init() struct actuator_armed_s safety; uint64_t try_start_time = hrt_absolute_time(); bool updated = false; - + /* keep checking for an update, ensure we got a arming information, not something that was published a long time ago. */ do { @@ -627,7 +634,7 @@ PX4IO::init() usleep(10000); /* abort after 5s */ - if ((hrt_absolute_time() - try_start_time)/1000 > 3000) { + if ((hrt_absolute_time() - try_start_time) / 1000 > 3000) { log("failed to recover from in-air restart (1), aborting IO driver init."); return 1; } @@ -667,7 +674,7 @@ PX4IO::init() usleep(50000); /* abort after 5s */ - if ((hrt_absolute_time() - try_start_time)/1000 > 2000) { + if ((hrt_absolute_time() - try_start_time) / 1000 > 2000) { log("failed to recover from in-air restart (2), aborting IO driver init."); return 1; } @@ -678,25 +685,28 @@ PX4IO::init() log("re-sending arm cmd"); } - /* keep waiting for state change for 2 s */ + /* keep waiting for state change for 2 s */ } while (!safety.armed); - /* regular boot, no in-air restart, init IO */ + /* regular boot, no in-air restart, init IO */ + } else { /* dis-arm IO before touching anything */ - 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 | - PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE, 0); + 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 | + PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE, 0); if (_rc_handling_disabled) { ret = io_disable_rc_handling(); + } else { /* publish RC config to IO */ ret = io_set_rc_config(); + if (ret != OK) { log("failed to update RC input config"); mavlink_log_info(_mavlink_fd, "[IO] RC config upload fail"); @@ -756,10 +766,10 @@ PX4IO::task_main() _t_vehicle_command = orb_subscribe(ORB_ID(vehicle_command)); if ((_t_actuators < 0) || - (_t_actuator_armed < 0) || - (_t_vehicle_control_mode < 0) || - (_t_param < 0) || - (_t_vehicle_command < 0)) { + (_t_actuator_armed < 0) || + (_t_vehicle_control_mode < 0) || + (_t_param < 0) || + (_t_vehicle_command < 0)) { log("subscription(s) failed"); goto out; } @@ -781,8 +791,10 @@ PX4IO::task_main() if (_update_interval != 0) { if (_update_interval < UPDATE_INTERVAL_MIN) _update_interval = UPDATE_INTERVAL_MIN; + if (_update_interval > 100) _update_interval = 100; + orb_set_interval(_t_actuators, _update_interval); _update_interval = 0; } @@ -830,20 +842,24 @@ PX4IO::task_main() /* arming state */ orb_check(_t_actuator_armed, &updated); + if (!updated) { orb_check(_t_vehicle_control_mode, &updated); } + if (updated) { io_set_arming_state(); } /* vehicle command */ orb_check(_t_vehicle_command, &updated); + if (updated) { struct vehicle_command_s cmd; orb_copy(ORB_ID(vehicle_command), _t_vehicle_command, &cmd); + // Check for a DSM pairing command - if ((cmd.command == VEHICLE_CMD_START_RX_PAIR) && (cmd.param1== 0.0f)) { + if ((cmd.command == VEHICLE_CMD_START_RX_PAIR) && (cmd.param1 == 0.0f)) { dsm_bind_ioctl((int)cmd.param2); } } @@ -854,6 +870,7 @@ PX4IO::task_main() * XXX this may be a bit spammy */ orb_check(_t_param, &updated); + if (updated) { parameter_update_s pupdate; orb_copy(ORB_ID(parameter_update), _t_param, &pupdate); @@ -863,6 +880,7 @@ PX4IO::task_main() /* see if bind parameter has been set, and reset it to -1 */ param_get(dsm_bind_param = param_find("RC_DSM_BIND"), &dsm_bind_val); + if (dsm_bind_val > -1) { dsm_bind_ioctl(dsm_bind_val); dsm_bind_val = -1; @@ -899,7 +917,7 @@ PX4IO::io_set_control_state() /* get controls */ orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : - ORB_ID(actuator_controls_1), _t_actuators, &controls); + ORB_ID(actuator_controls_1), _t_actuators, &controls); for (unsigned i = 0; i < _max_controls; i++) regs[i] = FLOAT_TO_REG(controls.control[i]); @@ -972,17 +990,21 @@ PX4IO::io_set_arming_state() if (armed.armed && !armed.lockdown) { set |= PX4IO_P_SETUP_ARMING_FMU_ARMED; + } else { clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED; } + if (armed.ready_to_arm) { set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; + } else { clear |= PX4IO_P_SETUP_ARMING_IO_ARM_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; } @@ -1027,22 +1049,27 @@ PX4IO::io_set_rc_config() * autopilots / GCS'. */ param_get(param_find("RC_MAP_ROLL"), &ichan); + if ((ichan >= 0) && (ichan < (int)_max_rc_input)) input_map[ichan - 1] = 0; param_get(param_find("RC_MAP_PITCH"), &ichan); + if ((ichan >= 0) && (ichan < (int)_max_rc_input)) input_map[ichan - 1] = 1; param_get(param_find("RC_MAP_YAW"), &ichan); + if ((ichan >= 0) && (ichan < (int)_max_rc_input)) input_map[ichan - 1] = 2; param_get(param_find("RC_MAP_THROTTLE"), &ichan); + if ((ichan >= 0) && (ichan < (int)_max_rc_input)) input_map[ichan - 1] = 3; ichan = 4; + for (unsigned i = 0; i < _max_rc_input; i++) if (input_map[i] == -1) input_map[i] = ichan++; @@ -1097,6 +1124,7 @@ PX4IO::io_set_rc_config() /* send channel config to IO */ ret = io_reg_set(PX4IO_PAGE_RC_CONFIG, offset, regs, PX4IO_P_RC_CONFIG_STRIDE); + if (ret != OK) { log("rc config upload failed"); break; @@ -1124,13 +1152,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_SAFETY_OFF && !(status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) - && !(status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { + && !(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_SAFETY_OFF | PX4IO_P_STATUS_FLAGS_ARM_SYNC); /* set new status */ _status = status; _status &= PX4IO_P_STATUS_FLAGS_SAFETY_OFF; + } else if (!(_status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { /* set the sync flag */ @@ -1154,6 +1183,7 @@ PX4IO::io_handle_status(uint16_t status) if (status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { safety.safety_off = true; safety.safety_switch_available = true; + } else { safety.safety_off = false; safety.safety_switch_available = true; @@ -1162,6 +1192,7 @@ PX4IO::io_handle_status(uint16_t status) /* 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); } @@ -1177,11 +1208,13 @@ PX4IO::dsm_bind_ioctl(int dsmMode) if ((dsmMode == 0) || (dsmMode == 1)) { mavlink_log_info(_thread_mavlink_fd, "[IO] binding dsm%c rx", (dsmMode == 0) ? '2' : 'x'); ioctl(nullptr, DSM_BIND_START, (dsmMode == 0) ? DSM2_BIND_PULSES : DSMX_BIND_PULSES); + } else { mavlink_log_info(_thread_mavlink_fd, "[IO] invalid dsm bind mode, bind request rejected"); } + } else { - mavlink_log_info(_thread_mavlink_fd, "[IO] system armed, bind request rejected"); + mavlink_log_info(_thread_mavlink_fd, "[IO] system armed, bind request rejected"); } } @@ -1207,12 +1240,13 @@ PX4IO::io_get_status() /* get STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT in that order */ ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, ®s[0], sizeof(regs) / sizeof(regs[0])); + if (ret != OK) return ret; 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; @@ -1226,22 +1260,24 @@ PX4IO::io_get_status() regs[3] contains the raw ADC count, as 12 bit ADC value, with full range being 3.3v */ - battery_status.current_a = regs[3] * (3.3f/4096.0f) * _battery_amp_per_volt; + battery_status.current_a = regs[3] * (3.3f / 4096.0f) * _battery_amp_per_volt; battery_status.current_a += _battery_amp_bias; /* integrate battery over time to get total mAh used */ if (_battery_last_timestamp != 0) { - _battery_mamphour_total += battery_status.current_a * - (battery_status.timestamp - _battery_last_timestamp) * 1.0e-3f / 3600; + _battery_mamphour_total += battery_status.current_a * + (battery_status.timestamp - _battery_last_timestamp) * 1.0e-3f / 3600; } + battery_status.discharged_mah = _battery_mamphour_total; _battery_last_timestamp = battery_status.timestamp; /* lazily publish the battery voltage */ if (_to_battery > 0) { orb_publish(ORB_ID(battery_status), _to_battery, &battery_status); + } else { _to_battery = orb_advertise(ORB_ID(battery_status), &battery_status); } @@ -1258,7 +1294,7 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) /* we don't have the status bits, so input_source has to be set elsewhere */ input_rc.input_source = RC_INPUT_SOURCE_UNKNOWN; - + static const unsigned prolog = (PX4IO_P_RAW_RC_BASE - PX4IO_P_RAW_RC_COUNT); uint16_t regs[RC_INPUT_MAX_CHANNELS + prolog]; @@ -1269,6 +1305,7 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) */ 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; @@ -1277,8 +1314,10 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) * 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; } @@ -1301,16 +1340,20 @@ PX4IO::io_publish_raw_rc() rc_val.timestamp = hrt_absolute_time(); int ret = io_get_raw_rc_input(rc_val); + if (ret != OK) return ret; /* sort out the source of the values */ if (_status & PX4IO_P_STATUS_FLAGS_RC_PPM) { rc_val.input_source = RC_INPUT_SOURCE_PX4IO_PPM; + } else if (_status & PX4IO_P_STATUS_FLAGS_RC_DSM) { rc_val.input_source = RC_INPUT_SOURCE_PX4IO_SPEKTRUM; + } else if (_status & PX4IO_P_STATUS_FLAGS_RC_SBUS) { rc_val.input_source = RC_INPUT_SOURCE_PX4IO_SBUS; + } else { rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN; } @@ -1318,7 +1361,8 @@ PX4IO::io_publish_raw_rc() /* lazily advertise on first publication */ if (_to_input_rc == 0) { _to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_val); - } else { + + } else { orb_publish(ORB_ID(input_rc), _to_input_rc, &rc_val); } @@ -1343,6 +1387,7 @@ PX4IO::io_publish_mixed_controls() /* get actuator controls from IO */ uint16_t act[_max_actuators]; int ret = io_reg_get(PX4IO_PAGE_ACTUATORS, 0, act, _max_actuators); + if (ret != OK) return ret; @@ -1352,16 +1397,17 @@ PX4IO::io_publish_mixed_controls() /* laxily advertise on first publication */ if (_to_actuators_effective == 0) { - _to_actuators_effective = - orb_advertise((_primary_pwm_device ? - ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : - ORB_ID(actuator_controls_effective_1)), - &controls_effective); + _to_actuators_effective = + orb_advertise((_primary_pwm_device ? + ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : + ORB_ID(actuator_controls_effective_1)), + &controls_effective); + } else { - orb_publish((_primary_pwm_device ? - ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : - ORB_ID(actuator_controls_effective_1)), - _to_actuators_effective, &controls_effective); + orb_publish((_primary_pwm_device ? + ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : + ORB_ID(actuator_controls_effective_1)), + _to_actuators_effective, &controls_effective); } return OK; @@ -1381,26 +1427,29 @@ PX4IO::io_publish_pwm_outputs() /* get servo values from IO */ uint16_t ctl[_max_actuators]; int ret = io_reg_get(PX4IO_PAGE_SERVOS, 0, ctl, _max_actuators); + if (ret != OK) return ret; /* convert from register format to float */ for (unsigned i = 0; i < _max_actuators; i++) outputs.output[i] = ctl[i]; + outputs.noutputs = _max_actuators; /* lazily advertise on first publication */ if (_to_outputs == 0) { _to_outputs = orb_advertise((_primary_pwm_device ? - ORB_ID_VEHICLE_CONTROLS : - ORB_ID(actuator_outputs_1)), - &outputs); + ORB_ID_VEHICLE_CONTROLS : + ORB_ID(actuator_outputs_1)), + &outputs); + } else { orb_publish((_primary_pwm_device ? - ORB_ID_VEHICLE_CONTROLS : - ORB_ID(actuator_outputs_1)), - _to_outputs, - &outputs); + ORB_ID_VEHICLE_CONTROLS : + ORB_ID(actuator_outputs_1)), + _to_outputs, + &outputs); } return OK; @@ -1416,10 +1465,12 @@ 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 != (int)num_values) { debug("io_reg_set(%u,%u,%u): error %d", page, offset, num_values, ret); return -1; } + return OK; } @@ -1439,10 +1490,12 @@ 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 != (int)num_values) { debug("io_reg_get(%u,%u,%u): data error %d", page, offset, num_values, ret); return -1; } + return OK; } @@ -1464,8 +1517,10 @@ 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 != OK) return ret; + value &= ~clearbits; value |= setbits; @@ -1506,6 +1561,7 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) * even. */ unsigned total_len = sizeof(px4io_mixdata) + count; + if (total_len % 2) { msg->text[count] = '\0'; total_len++; @@ -1546,48 +1602,48 @@ PX4IO::print_status() { /* basic configuration */ 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_HARDWARE_VERSION), - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_BOOTLOADER_VERSION), - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER)); + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_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", - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT), - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT), - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT), - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT), - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT)); + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT)); /* status */ printf("%u bytes free\n", - io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM)); + io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM)); 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%s\n", - flags, - ((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" : ""), - (((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_STATUS_FLAGS_RC_DSM11))) ? " DSM10" : ""), - (((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_STATUS_FLAGS_RC_DSM11)) ? " DSM11" : ""), - ((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS) ? " SBUS" : ""), - ((flags & PX4IO_P_STATUS_FLAGS_FMU_OK) ? " FMU_OK" : " FMU_FAIL"), - ((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PWM_PASSTHROUGH" : ""), - ((flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ? " MIXER_OK" : " MIXER_FAIL"), - ((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"), - ((flags & PX4IO_P_STATUS_FLAGS_INIT_OK) ? " INIT_OK" : " INIT_FAIL"), - ((flags & PX4IO_P_STATUS_FLAGS_FAILSAFE) ? " FAILSAFE" : "")); + flags, + ((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" : ""), + (((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_STATUS_FLAGS_RC_DSM11))) ? " DSM10" : ""), + (((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_STATUS_FLAGS_RC_DSM11)) ? " DSM11" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS) ? " SBUS" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_FMU_OK) ? " FMU_OK" : " FMU_FAIL"), + ((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PWM_PASSTHROUGH" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ? " MIXER_OK" : " MIXER_FAIL"), + ((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"), + ((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%s\n", - alarms, - ((alarms & PX4IO_P_STATUS_ALARMS_VBATT_LOW) ? " VBATT_LOW" : ""), - ((alarms & PX4IO_P_STATUS_ALARMS_TEMPERATURE) ? " TEMPERATURE" : ""), - ((alarms & PX4IO_P_STATUS_ALARMS_SERVO_CURRENT) ? " SERVO_CURRENT" : ""), - ((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_VSERVO_FAULT) ? " VSERVO_FAULT" : "")); + alarms, + ((alarms & PX4IO_P_STATUS_ALARMS_VBATT_LOW) ? " VBATT_LOW" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_TEMPERATURE) ? " TEMPERATURE" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_SERVO_CURRENT) ? " SERVO_CURRENT" : ""), + ((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_VSERVO_FAULT) ? " VSERVO_FAULT" : "")); /* now clear alarms */ io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, 0xFFFF); @@ -1600,87 +1656,107 @@ PX4IO::print_status() (double)_battery_amp_per_volt, (double)_battery_amp_bias, (double)_battery_mamphour_total); + } else if (_hardware == 2) { 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_VSERVO_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)); + printf("\n"); printf("servos"); + for (unsigned i = 0; i < _max_actuators; i++) printf(" %u", io_reg_get(PX4IO_PAGE_SERVOS, i)); + printf("\n"); uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); printf("%d raw R/C inputs", raw_inputs); + for (unsigned i = 0; i < raw_inputs; i++) printf(" %u", io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i)); + printf("\n"); uint16_t mapped_inputs = io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_VALID); printf("mapped R/C inputs 0x%04x", mapped_inputs); + for (unsigned i = 0; i < _max_rc_input; i++) { if (mapped_inputs & (1 << i)) printf(" %u:%d", i, REG_TO_SIGNED(io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_BASE + i))); } + printf("\n"); uint16_t adc_inputs = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT); printf("ADC inputs"); + for (unsigned i = 0; i < adc_inputs; i++) printf(" %u", io_reg_get(PX4IO_PAGE_RAW_ADC_INPUT, i)); + printf("\n"); /* 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%s%s\n", - arming, - ((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" : ""), - ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : "")); + arming, + ((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" : ""), + ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : "")); #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 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), - io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE), - io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS)); + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS)); #endif #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 printf("rates 0x%04x default %u alt %u\n", - io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES), - io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE), - io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE)); + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE)); #endif printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG)); printf("controls"); + for (unsigned i = 0; i < _max_controls; i++) printf(" %u", io_reg_get(PX4IO_PAGE_CONTROLS, i)); + printf("\n"); + for (unsigned i = 0; i < _max_rc_input; i++) { unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i; uint16_t options = io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_OPTIONS); printf("input %u min %u center %u max %u deadzone %u assigned %u options 0x%04x%s%s\n", - i, - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MIN), - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_CENTER), - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MAX), - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_DEADZONE), - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_ASSIGNMENT), - options, - ((options & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) ? " ENABLED" : ""), - ((options & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) ? " REVERSED" : "")); + i, + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MIN), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_CENTER), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MAX), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_DEADZONE), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_ASSIGNMENT), + options, + ((options & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) ? " ENABLED" : ""), + ((options & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) ? " REVERSED" : "")); } + printf("failsafe"); + for (unsigned i = 0; i < _max_actuators; i++) printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); + printf("\nidle values"); + for (unsigned i = 0; i < _max_actuators; i++) printf(" %u", io_reg_get(PX4IO_PAGE_IDLE_PWM, i)); + printf("\n"); } @@ -1719,27 +1795,29 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) case PWM_SERVO_SELECT_UPDATE_RATE: { - /* blindly clear the PWM update alarm - might be set for some other reason */ - io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR); - - /* attempt to set the rate map */ - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES, arg); - - /* check that the changes took */ - uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS); - if (alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) { - ret = -EINVAL; + /* blindly clear the PWM update alarm - might be set for some other reason */ io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR); + + /* attempt to set the rate map */ + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES, arg); + + /* check that the changes took */ + uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS); + + if (alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) { + ret = -EINVAL; + io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR); + } + + break; } - break; - } case PWM_SERVO_GET_COUNT: *(unsigned *)arg = _max_actuators; break; case DSM_BIND_START: - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down); + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down); usleep(500000); io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out); io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up); @@ -1755,68 +1833,80 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) case PWM_SERVO_SET(0) ... PWM_SERVO_SET(PWM_OUTPUT_MAX_CHANNELS - 1): { - /* TODO: we could go lower for e.g. TurboPWM */ - unsigned channel = cmd - PWM_SERVO_SET(0); - if ((channel >= _max_actuators) || (arg < 900) || (arg > 2100)) { - ret = -EINVAL; - } else { - /* send a direct PWM value */ - ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, channel, arg); - } + /* TODO: we could go lower for e.g. TurboPWM */ + unsigned channel = cmd - PWM_SERVO_SET(0); - break; - } + if ((channel >= _max_actuators) || (arg < 900) || (arg > 2100)) { + ret = -EINVAL; + + } else { + /* send a direct PWM value */ + ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, channel, arg); + } + + break; + } case PWM_SERVO_GET(0) ... PWM_SERVO_GET(PWM_OUTPUT_MAX_CHANNELS - 1): { - unsigned channel = cmd - PWM_SERVO_GET(0); + unsigned channel = cmd - PWM_SERVO_GET(0); + + if (channel >= _max_actuators) { + ret = -EINVAL; - if (channel >= _max_actuators) { - ret = -EINVAL; - } else { - /* fetch a current PWM value */ - uint32_t value = io_reg_get(PX4IO_PAGE_SERVOS, channel); - if (value == _io_reg_get_error) { - ret = -EIO; } else { - *(servo_position_t *)arg = value; + /* fetch a current PWM value */ + uint32_t value = io_reg_get(PX4IO_PAGE_SERVOS, channel); + + if (value == _io_reg_get_error) { + ret = -EIO; + + } else { + *(servo_position_t *)arg = value; + } } + + break; } - break; - } case PWM_SERVO_GET_RATEGROUP(0) ... PWM_SERVO_GET_RATEGROUP(PWM_OUTPUT_MAX_CHANNELS - 1): { - unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0); + unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0); - *(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; - } + *(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; + } case GPIO_RESET: { #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 - uint32_t bits = (1 << _max_relays) - 1; - /* don't touch relay1 if it's controlling RX vcc */ - if (_dsm_vcc_ctl) - bits &= ~PX4IO_P_SETUP_RELAYS_POWER1; - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, bits, 0); + uint32_t bits = (1 << _max_relays) - 1; + + /* don't touch relay1 if it's controlling RX vcc */ + if (_dsm_vcc_ctl) + bits &= ~PX4IO_P_SETUP_RELAYS_POWER1; + + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, bits, 0); #endif #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 - ret = -EINVAL; + ret = -EINVAL; #endif - break; - } + break; + } case GPIO_SET: #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 arg &= ((1 << _max_relays) - 1); + /* don't touch relay1 if it's controlling RX vcc */ if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1)) { ret = -EINVAL; break; } + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg); #endif #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 @@ -1827,12 +1917,14 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) case GPIO_CLEAR: #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 arg &= ((1 << _max_relays) - 1); + /* don't touch relay1 if it's controlling RX vcc */ if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1)) { ret = -EINVAL; break; } - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0); + + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0); #endif #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 ret = -EINVAL; @@ -1842,8 +1934,10 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) case GPIO_GET: #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 *(uint32_t *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS); + if (*(uint32_t *)arg == _io_reg_get_error) ret = -EIO; + #endif #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 ret = -EINVAL; @@ -1859,53 +1953,60 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) break; case MIXERIOCLOADBUF: { - const char *buf = (const char *)arg; - ret = mixer_send(buf, strnlen(buf, 2048)); - break; - } + const char *buf = (const char *)arg; + ret = mixer_send(buf, strnlen(buf, 2048)); + break; + } case RC_INPUT_GET: { - uint16_t status; - rc_input_values *rc_val = (rc_input_values *)arg; + uint16_t status; + rc_input_values *rc_val = (rc_input_values *)arg; - ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, &status, 1); - if (ret != OK) - break; + ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, &status, 1); - /* if no R/C input, don't try to fetch anything */ - if (!(status & PX4IO_P_STATUS_FLAGS_RC_OK)) { - ret = -ENOTCONN; + if (ret != OK) + break; + + /* if no R/C input, don't try to fetch anything */ + if (!(status & PX4IO_P_STATUS_FLAGS_RC_OK)) { + ret = -ENOTCONN; + break; + } + + /* sort out the source of the values */ + if (status & PX4IO_P_STATUS_FLAGS_RC_PPM) { + rc_val->input_source = RC_INPUT_SOURCE_PX4IO_PPM; + + } else if (status & PX4IO_P_STATUS_FLAGS_RC_DSM) { + rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SPEKTRUM; + + } else if (status & PX4IO_P_STATUS_FLAGS_RC_SBUS) { + rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SBUS; + + } else { + rc_val->input_source = RC_INPUT_SOURCE_UNKNOWN; + } + + /* read raw R/C input values */ + ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE, &(rc_val->values[0]), _max_rc_input); break; } - /* sort out the source of the values */ - if (status & PX4IO_P_STATUS_FLAGS_RC_PPM) { - rc_val->input_source = RC_INPUT_SOURCE_PX4IO_PPM; - } else if (status & PX4IO_P_STATUS_FLAGS_RC_DSM) { - rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SPEKTRUM; - } else if (status & PX4IO_P_STATUS_FLAGS_RC_SBUS) { - rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SBUS; - } else { - rc_val->input_source = RC_INPUT_SOURCE_UNKNOWN; - } - - /* read raw R/C input values */ - ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE, &(rc_val->values[0]), _max_rc_input); - break; - } - case PX4IO_SET_DEBUG: /* set the debug level */ ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG, arg); break; case PX4IO_INAIR_RESTART_ENABLE: + /* set/clear the 'in-air restart' bit */ if (arg) { ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK); + } else { ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK, 0); } + break; default: @@ -1924,11 +2025,14 @@ PX4IO::write(file * /*filp*/, const char *buffer, size_t len) if (count > _max_actuators) count = _max_actuators; + if (count > 0) { int ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, (uint16_t *)buffer, count); + if (ret != OK) return ret; } + return count * 2; } @@ -1936,6 +2040,7 @@ int PX4IO::set_update_rate(int rate) { int interval_ms = 1000 / rate; + if (interval_ms < UPDATE_INTERVAL_MIN) { interval_ms = UPDATE_INTERVAL_MIN; warnx("update rate too high, limiting interval to %d ms (%d Hz).", interval_ms, 1000 / interval_ms); @@ -1968,22 +2073,27 @@ 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) 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"); @@ -2017,7 +2127,7 @@ start(int argc, char *argv[]) if (argc > 1) { if (!strcmp(argv[1], "norc")) { - if(g_dev->disable_rc_handling()) + if (g_dev->disable_rc_handling()) warnx("Failed disabling RC handling"); } else { @@ -2034,6 +2144,7 @@ start(int argc, char *argv[]) g_dev->ioctl(nullptr, DSM_BIND_POWER_UP, 0); } } + #endif exit(0); } @@ -2061,6 +2172,7 @@ detect(int argc, char *argv[]) if (ret) { /* nonzero, error */ exit(1); + } else { exit(0); } @@ -2075,8 +2187,10 @@ bind(int argc, char *argv[]) errx(1, "px4io must be started first"); #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + if (!g_dev->get_dsm_vcc_ctl()) errx(1, "DSM bind feature not enabled"); + #endif if (argc < 3) @@ -2086,9 +2200,10 @@ bind(int argc, char *argv[]) pulses = DSM2_BIND_PULSES; else if (!strcmp(argv[2], "dsmx")) pulses = DSMX_BIND_PULSES; - else + else errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]); - if (g_dev->system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + + if (g_dev->system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) errx(1, "system must not be armed"); #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 @@ -2119,6 +2234,7 @@ test(void) /* 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"); @@ -2135,22 +2251,27 @@ test(void) /* sweep all servos between 1000..2000 */ servo_position_t servos[servo_count]; + for (unsigned i = 0; i < servo_count; i++) servos[i] = pwm_value; 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; } @@ -2162,6 +2283,7 @@ test(void) 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]); } @@ -2169,9 +2291,11 @@ test(void) /* Check if user wants to quit */ char c; ret = poll(&fds, 1, 0); + if (ret > 0) { read(0, &c, 1); + if (c == 0x03 || c == 0x63 || c == 'q') { warnx("User abort\n"); exit(0); @@ -2310,20 +2434,24 @@ px4io_main(int argc, char *argv[]) if ((argc > 2)) { g_dev->set_update_rate(atoi(argv[2])); + } else { errx(1, "missing argument (50 - 500 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); } @@ -2340,10 +2468,12 @@ px4io_main(int argc, char *argv[]) /* set channel to commandline argument or to 900 for non-provided channels */ if (argc > i + 2) { - failsafe[i] = atoi(argv[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; @@ -2354,6 +2484,7 @@ px4io_main(int argc, char *argv[]) if (ret != OK) errx(ret, "failed setting failsafe values"); + exit(0); } @@ -2368,14 +2499,15 @@ px4io_main(int argc, char *argv[]) /* set values for first 8 channels, fill unassigned channels with 900. */ uint16_t min[8]; - for (unsigned i = 0; i < sizeof(min) / sizeof(min[0]); i++) - { + for (unsigned 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]); + 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] = 0; @@ -2386,9 +2518,11 @@ px4io_main(int argc, char *argv[]) if (ret != OK) errx(ret, "failed setting min values"); + } else { errx(1, "not loaded"); } + exit(0); } @@ -2403,14 +2537,15 @@ px4io_main(int argc, char *argv[]) /* set values for first 8 channels, fill unassigned channels with 2100. */ uint16_t max[8]; - for (unsigned i = 0; i < sizeof(max) / sizeof(max[0]); i++) - { + for (unsigned 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]); + 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] = 0; @@ -2421,9 +2556,11 @@ px4io_main(int argc, char *argv[]) if (ret != OK) errx(ret, "failed setting max values"); + } else { errx(1, "not loaded"); } + exit(0); } @@ -2438,14 +2575,15 @@ px4io_main(int argc, char *argv[]) /* set values for first 8 channels, fill unassigned channels with 0. */ uint16_t idle[8]; - for (unsigned i = 0; i < sizeof(idle) / sizeof(idle[0]); i++) - { + for (unsigned 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]); + 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; @@ -2456,9 +2594,11 @@ px4io_main(int argc, char *argv[]) if (ret != OK) errx(ret, "failed setting idle values"); + } else { errx(1, "not loaded"); } + exit(0); } @@ -2467,7 +2607,7 @@ px4io_main(int argc, char *argv[]) /* * Enable in-air restart support. * We can cheat and call the driver directly, as it - * doesn't reference filp in ioctl() + * doesn't reference filp in ioctl() */ g_dev->ioctl(NULL, PX4IO_INAIR_RESTART_ENABLE, 1); exit(0); @@ -2494,19 +2634,23 @@ px4io_main(int argc, char *argv[]) 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(nullptr, 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); } @@ -2527,6 +2671,6 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "bind")) bind(argc, argv); - out: +out: errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'failsafe', 'min, 'max',\n 'idle', 'bind' or 'update'"); } From 3fd20481888fd9e63806f988153abe4bf82e7a04 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Oct 2013 22:16:57 +0200 Subject: [PATCH 737/872] commander: remove duplicate publishing of vehicle_control_mode and actuator_armed topics, remove unused "counter" field from topics --- src/modules/commander/commander.cpp | 24 +++++-------------- .../commander/state_machine_helper.cpp | 1 - .../uORB/topics/vehicle_control_mode.h | 1 - 3 files changed, 6 insertions(+), 20 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 01b7b84d01..562790a7d8 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1181,31 +1181,19 @@ int commander_thread_main(int argc, char *argv[]) bool main_state_changed = check_main_state_changed(); bool navigation_state_changed = check_navigation_state_changed(); + hrt_abstime t1 = hrt_absolute_time(); + + if (navigation_state_changed || arming_state_changed) { + control_mode.flag_armed = armed.armed; // copy armed state to vehicle_control_mode topic + } + 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; } - hrt_abstime t1 = hrt_absolute_time(); - - /* publish arming state */ - if (arming_state_changed) { - armed.timestamp = t1; - orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); - } - - /* publish control mode */ - 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); - } - /* 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 = t1; orb_publish(ORB_ID(vehicle_status), status_pub, &status); control_mode.timestamp = t1; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 8ce31550f9..e4d235a6bc 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -497,7 +497,6 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s if (valid_transition) { current_status->hil_state = new_state; - current_status->counter++; current_status->timestamp = hrt_absolute_time(); orb_publish(ORB_ID(vehicle_status), status_pub, current_status); diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 093c6775d1..38fb74c9b9 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -61,7 +61,6 @@ */ struct vehicle_control_mode_s { - 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; From 69fda8a05908a4871756b91ba68d84ff18a37bcc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Oct 2013 09:12:03 +0200 Subject: [PATCH 738/872] Added useful default gains --- ROMFS/px4fmu_common/init.d/1000_rc_fw.hil | 1 + ROMFS/px4fmu_common/init.d/100_mpx_easystar | 22 ++++++++++++++++++++- ROMFS/px4fmu_common/init.d/101_hk_bixler | 22 ++++++++++++++++++++- ROMFS/px4fmu_common/init.d/31_io_phantom | 22 ++++++++++++++++++++- ROMFS/px4fmu_common/init.d/cmp_test | 9 +++++++++ 5 files changed, 73 insertions(+), 3 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/cmp_test diff --git a/ROMFS/px4fmu_common/init.d/1000_rc_fw.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw.hil index 11318023c7..6e29bd6f80 100644 --- a/ROMFS/px4fmu_common/init.d/1000_rc_fw.hil +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw.hil @@ -32,6 +32,7 @@ then param set FW_T_SINK_MAX 5.0 param set FW_T_SINK_MIN 4.0 param set FW_Y_ROLLFF 1.1 + param set FW_L1_PERIOD 16 param set SYS_AUTOCONFIG 0 param save diff --git a/ROMFS/px4fmu_common/init.d/100_mpx_easystar b/ROMFS/px4fmu_common/init.d/100_mpx_easystar index 75437f4047..828540ad90 100644 --- a/ROMFS/px4fmu_common/init.d/100_mpx_easystar +++ b/ROMFS/px4fmu_common/init.d/100_mpx_easystar @@ -8,7 +8,27 @@ echo "[init] PX4FMU v1, v2 with or without IO on EasyStar" if param compare SYS_AUTOCONFIG 1 then # Set all params here, then disable autoconfig - # TODO + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 15 + param set FW_P_LIM_MAX 50 + param set FW_P_LIM_MIN -50 + param set FW_P_P 60 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 1.1 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 100 + param set FW_R_RMAX 100 + param set FW_THR_CRUISE 0.65 + param set FW_THR_MAX 1 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5.0 + param set FW_T_SINK_MIN 4.0 + param set FW_Y_ROLLFF 1.1 + param set FW_L1_PERIOD 16 param set SYS_AUTOCONFIG 0 param save diff --git a/ROMFS/px4fmu_common/init.d/101_hk_bixler b/ROMFS/px4fmu_common/init.d/101_hk_bixler index 89b3185ad7..5a9cb2171a 100644 --- a/ROMFS/px4fmu_common/init.d/101_hk_bixler +++ b/ROMFS/px4fmu_common/init.d/101_hk_bixler @@ -8,7 +8,27 @@ echo "[init] PX4FMU v1, v2 with or without IO on HK Bixler" if param compare SYS_AUTOCONFIG 1 then # Set all params here, then disable autoconfig - # TODO + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 15 + param set FW_P_LIM_MAX 50 + param set FW_P_LIM_MIN -50 + param set FW_P_P 60 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 1.1 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 100 + param set FW_R_RMAX 100 + param set FW_THR_CRUISE 0.65 + param set FW_THR_MAX 1 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5.0 + param set FW_T_SINK_MIN 4.0 + param set FW_Y_ROLLFF 1.1 + param set FW_L1_PERIOD 16 param set SYS_AUTOCONFIG 0 param save diff --git a/ROMFS/px4fmu_common/init.d/31_io_phantom b/ROMFS/px4fmu_common/init.d/31_io_phantom index 7e0127e79d..8fe94452f8 100644 --- a/ROMFS/px4fmu_common/init.d/31_io_phantom +++ b/ROMFS/px4fmu_common/init.d/31_io_phantom @@ -8,7 +8,27 @@ echo "[init] PX4FMU v1, v2 with or without IO on Phantom FPV" if param compare SYS_AUTOCONFIG 1 then # Set all params here, then disable autoconfig - # TODO + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 15 + param set FW_P_LIM_MAX 50 + param set FW_P_LIM_MIN -50 + param set FW_P_P 60 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 1.1 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 100 + param set FW_R_RMAX 100 + param set FW_THR_CRUISE 0.65 + param set FW_THR_MAX 1 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5.0 + param set FW_T_SINK_MIN 4.0 + param set FW_Y_ROLLFF 1.1 + param set FW_L1_PERIOD 17 param set SYS_AUTOCONFIG 0 param save diff --git a/ROMFS/px4fmu_common/init.d/cmp_test b/ROMFS/px4fmu_common/init.d/cmp_test new file mode 100644 index 0000000000..f86f4f85bb --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/cmp_test @@ -0,0 +1,9 @@ +#!nsh + +cp /etc/extras/px4io-v2_default.bin /fs/microsd/px4io.loaded +if cmp /etc/extras/px4io-v2_default.bin /fs/microsd/px4io.loaded +then + echo "CMP returned true" +else + echo "CMP returned false" +fi \ No newline at end of file From 5bc7d7c00f1f572825ce0db5f7fb24b8d77872a3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Oct 2013 09:13:41 +0200 Subject: [PATCH 739/872] Fixed turn radius return value --- src/lib/ecl/l1/ecl_l1_pos_controller.cpp | 25 ++++++++++++++---------- 1 file changed, 15 insertions(+), 10 deletions(-) diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp index 0dadf56f39..c5c0c7a3c1 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp @@ -70,7 +70,7 @@ float ECL_L1_Pos_Controller::target_bearing() float ECL_L1_Pos_Controller::switch_distance(float wp_radius) { /* following [2], switching on L1 distance */ - return math::min(wp_radius, _L1_distance); + return math::max(wp_radius, _L1_distance); } bool ECL_L1_Pos_Controller::reached_loiter_target(void) @@ -117,7 +117,6 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c /* calculate the vector from waypoint A to the aircraft */ math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); - math::Vector2f vector_B_to_airplane = get_local_planar_vector(vector_B, vector_curr_position); /* calculate crosstrack error (output only) */ _crosstrack_error = vector_AB % vector_A_to_airplane; @@ -128,10 +127,13 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c * If the aircraft is already between A and B normal L1 logic is applied. */ float distance_A_to_airplane = vector_A_to_airplane.length(); - float distance_B_to_airplane = vector_B_to_airplane.length(); float alongTrackDist = vector_A_to_airplane * vector_AB; - /* extension from [2] */ + /* estimate airplane position WRT to B */ + math::Vector2f vector_B_to_airplane_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized(); + float bearing_wp_b = atan2f(-vector_B_to_airplane_unit.getY() , -vector_B_to_airplane_unit.getX()); + + /* extension from [2], fly directly to A */ if (distance_A_to_airplane > _L1_distance && alongTrackDist / math::max(distance_A_to_airplane , 1.0f) < -0.7071f) { /* calculate eta to fly to waypoint A */ @@ -146,19 +148,21 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c /* bearing from current position to L1 point */ _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX()); - } else if (distance_B_to_airplane < alongTrackDist || distance_B_to_airplane < _L1_distance) { +// XXX this can be useful as last-resort guard, but is currently not needed +#if 0 + } else if (absf(bearing_wp_b) > math::radians(80.0f)) { + /* extension, fly back to waypoint */ - /* fly directly to B */ - /* unit vector from waypoint B to current position */ - math::Vector2f vector_B_to_airplane_unit = vector_B_to_airplane.normalized(); + /* calculate eta to fly to waypoint B */ + /* velocity across / orthogonal to line */ xtrack_vel = ground_speed_vector % (-vector_B_to_airplane_unit); /* velocity along line */ ltrack_vel = ground_speed_vector * (-vector_B_to_airplane_unit); eta = atan2f(xtrack_vel, ltrack_vel); /* bearing from current position to L1 point */ - _nav_bearing = atan2f(-vector_B_to_airplane_unit.getY() , -vector_B_to_airplane_unit.getX()); - + _nav_bearing = bearing_wp_b; +#endif } else { /* calculate eta to fly along the line between A and B */ @@ -178,6 +182,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c eta = eta1 + eta2; /* bearing from current position to L1 point */ _nav_bearing = atan2f(vector_AB.getY(), vector_AB.getX()) + eta1; + } /* limit angle to +-90 degrees */ From d59cdf6fcc99e85cc1d897637b1bf9e18269f77c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Oct 2013 09:14:55 +0200 Subject: [PATCH 740/872] Added support for dynamic turn radii --- src/modules/mavlink/mavlink.c | 8 ++-- src/modules/mavlink/missionlib.c | 33 +++++++++++---- src/modules/mavlink/orb_listener.c | 21 ++++++++++ src/modules/mavlink/orb_topics.h | 5 +++ src/modules/mavlink/waypoints.c | 40 +++++++++++++------ src/modules/mavlink/waypoints.h | 3 +- .../topics/vehicle_global_position_setpoint.h | 2 + 7 files changed, 86 insertions(+), 26 deletions(-) diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 7a5c2dab28..7c10e297b9 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -690,25 +690,25 @@ int mavlink_thread_main(int argc, char *argv[]) lowspeed_counter++; - mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos); + mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap); /* sleep quarter the time */ usleep(25000); /* check if waypoint has been reached against the last positions */ - mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos); + mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap); /* sleep quarter the time */ usleep(25000); /* send parameters at 20 Hz (if queued for sending) */ mavlink_pm_queued_send(); - mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos); + mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap); /* sleep quarter the time */ usleep(25000); - mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos); + mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap); if (baudrate > 57600) { mavlink_pm_queued_send(); diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c index 3175e64ce2..e8d707948a 100644 --- a/src/modules/mavlink/missionlib.c +++ b/src/modules/mavlink/missionlib.c @@ -170,6 +170,28 @@ bool set_special_fields(float param1, float param2, float param3, float param4, sp->param2 = param2; sp->param3 = param3; sp->param4 = param4; + + + /* define the turn distance */ + float orbit = 15.0f; + + if (command == (int)MAV_CMD_NAV_WAYPOINT) { + + orbit = param2; + + } else if (command == (int)MAV_CMD_NAV_LOITER_TURNS || + command == (int)MAV_CMD_NAV_LOITER_TIME || + command == (int)MAV_CMD_NAV_LOITER_UNLIM) { + + orbit = param3; + } else { + + // XXX set default orbit via param + // 15 initialized above + } + + sp->turn_distance_xy = orbit; + sp->turn_distance_z = orbit; } /** @@ -223,10 +245,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, int last_setpoint_index = -1; bool last_setpoint_valid = false; - /* at first waypoint, but cycled once through mission */ - if (index == 0 && last_waypoint_index > 0) { - last_setpoint_index = last_waypoint_index; - } else { + if (index > 0) { last_setpoint_index = index - 1; } @@ -251,10 +270,8 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, int next_setpoint_index = -1; bool next_setpoint_valid = false; - /* at last waypoint, try to re-loop through mission as default */ - if (index == (wpm->size - 1) && wpm->size > 1) { - next_setpoint_index = 0; - } else if (wpm->size > 1) { + /* next waypoint */ + if (wpm->size > 1) { next_setpoint_index = index + 1; } diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index cec2fdc431..0e0ce27235 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -67,6 +67,7 @@ extern bool gcs_link; struct vehicle_global_position_s global_pos; struct vehicle_local_position_s local_pos; +struct navigation_capabilities_s nav_cap; struct vehicle_status_s v_status; struct rc_channels_s rc; struct rc_input_values rc_raw; @@ -122,6 +123,7 @@ static void l_optical_flow(const struct listener *l); static void l_vehicle_rates_setpoint(const struct listener *l); static void l_home(const struct listener *l); static void l_airspeed(const struct listener *l); +static void l_nav_cap(const struct listener *l); static const struct listener listeners[] = { {l_sensor_combined, &mavlink_subs.sensor_sub, 0}, @@ -148,6 +150,7 @@ static const struct listener listeners[] = { {l_vehicle_rates_setpoint, &mavlink_subs.rates_setpoint_sub, 0}, {l_home, &mavlink_subs.home_sub, 0}, {l_airspeed, &mavlink_subs.airspeed_sub, 0}, + {l_nav_cap, &mavlink_subs.navigation_capabilities_sub, 0}, }; static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]); @@ -691,6 +694,19 @@ l_airspeed(const struct listener *l) mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, alt, climb); } +void +l_nav_cap(const struct listener *l) +{ + + orb_copy(ORB_ID(navigation_capabilities), mavlink_subs.navigation_capabilities_sub, &nav_cap); + + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + hrt_absolute_time() / 1000, + "turn dist", + nav_cap.turn_distance); + +} + static void * uorb_receive_thread(void *arg) { @@ -837,6 +853,11 @@ uorb_receive_start(void) mavlink_subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); orb_set_interval(mavlink_subs.airspeed_sub, 200); /* 5Hz updates */ + /* --- NAVIGATION CAPABILITIES --- */ + mavlink_subs.navigation_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); + orb_set_interval(mavlink_subs.navigation_capabilities_sub, 500); /* 2Hz updates */ + nav_cap.turn_distance = 0.0f; + /* start the listener loop */ pthread_attr_t uorb_attr; pthread_attr_init(&uorb_attr); diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h index c5cd0d03e5..91773843b6 100644 --- a/src/modules/mavlink/orb_topics.h +++ b/src/modules/mavlink/orb_topics.h @@ -67,6 +67,7 @@ #include #include #include +#include struct mavlink_subscriptions { int sensor_sub; @@ -92,6 +93,7 @@ struct mavlink_subscriptions { int rates_setpoint_sub; int home_sub; int airspeed_sub; + int navigation_capabilities_sub; }; extern struct mavlink_subscriptions mavlink_subs; @@ -102,6 +104,9 @@ extern struct vehicle_global_position_s global_pos; /** Local position */ extern struct vehicle_local_position_s local_pos; +/** navigation capabilities */ +extern struct navigation_capabilities_s nav_cap; + /** Vehicle status */ extern struct vehicle_status_s v_status; diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index f03fe4fdf4..ddad5f0df6 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -251,9 +251,8 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq) * * The distance calculation is based on the WGS84 geoid (GPS) */ -float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt) +float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt, float *dist_xy, float *dist_z) { - static uint16_t counter; if (seq < wpm->size) { mavlink_mission_item_t *wp = &(wpm->waypoints[seq]); @@ -274,20 +273,21 @@ float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float float dxy = radius_earth * c; float dz = alt - wp->z; + *dist_xy = fabsf(dxy); + *dist_z = fabsf(dz); + return sqrtf(dxy * dxy + dz * dz); } else { return -1.0f; } - counter++; - } /* * Calculate distance in local frame (NED) */ -float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float z) +float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float z, float *dist_xy, float *dist_z) { if (seq < wpm->size) { mavlink_mission_item_t *cur = &(wpm->waypoints[seq]); @@ -296,6 +296,9 @@ float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float float dy = (cur->y - y); float dz = (cur->z - z); + *dist_xy = sqrtf(dx * dx + dy * dy); + *dist_z = fabsf(dz); + return sqrtf(dx * dx + dy * dy + dz * dz); } else { @@ -303,7 +306,7 @@ float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float } } -void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_s *global_pos, struct vehicle_local_position_s *local_pos) +void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_s *global_pos, struct vehicle_local_position_s *local_pos, float turn_distance) { static uint16_t counter; @@ -332,26 +335,37 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ orbit = 15.0f; } + /* keep vertical orbit */ + float vertical_switch_distance = orbit; + + /* Take the larger turn distance - orbit or turn_distance */ + if (orbit < turn_distance) + orbit = turn_distance; + int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame; float dist = -1.0f; + float dist_xy = -1.0f; + float dist_z = -1.0f; + if (coordinate_frame == (int)MAV_FRAME_GLOBAL) { - dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt); + dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt, &dist_xy, &dist_z); } else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) { - dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->relative_alt); + dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->relative_alt, &dist_xy, &dist_z); } else if (coordinate_frame == (int)MAV_FRAME_LOCAL_ENU || coordinate_frame == (int)MAV_FRAME_LOCAL_NED) { - dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z); + dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z, &dist_xy, &dist_z); } else if (coordinate_frame == (int)MAV_FRAME_MISSION) { /* Check if conditions of mission item are satisfied */ // XXX TODO } - if (dist >= 0.f && dist <= orbit) { + if (dist >= 0.f && dist_xy <= orbit && dist_z >= 0.0f && dist_z <= vertical_switch_distance) { wpm->pos_reached = true; } + // 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); @@ -465,7 +479,7 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ } -int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, struct vehicle_local_position_s *local_position) +int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, struct vehicle_local_position_s *local_position, struct navigation_capabilities_s *nav_cap) { /* check for timed-out operations */ if (now - wpm->timestamp_lastaction > wpm->timeout && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { @@ -488,7 +502,7 @@ int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_positio } } - check_waypoints_reached(now, global_position, local_position); + check_waypoints_reached(now, global_position, local_position, nav_cap->turn_distance); return OK; } @@ -1011,5 +1025,5 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi } } - check_waypoints_reached(now, global_pos, local_pos); + // check_waypoints_reached(now, global_pos, local_pos); } diff --git a/src/modules/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h index 96a0ecd308..d7d6b31dc3 100644 --- a/src/modules/mavlink/waypoints.h +++ b/src/modules/mavlink/waypoints.h @@ -55,6 +55,7 @@ #include #include #include +#include // FIXME XXX - TO BE MOVED TO XML enum MAVLINK_WPM_STATES { @@ -120,7 +121,7 @@ typedef struct mavlink_wpm_storage mavlink_wpm_storage; void mavlink_wpm_init(mavlink_wpm_storage *state); int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, - struct vehicle_local_position_s *local_pos); + struct vehicle_local_position_s *local_pos, struct navigation_capabilities_s *nav_cap); void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehicle_global_position_s *global_pos , struct vehicle_local_position_s *local_pos); diff --git a/src/modules/uORB/topics/vehicle_global_position_setpoint.h b/src/modules/uORB/topics/vehicle_global_position_setpoint.h index 5c8ce1e4d6..a56434d3b0 100644 --- a/src/modules/uORB/topics/vehicle_global_position_setpoint.h +++ b/src/modules/uORB/topics/vehicle_global_position_setpoint.h @@ -72,6 +72,8 @@ struct vehicle_global_position_setpoint_s float param2; float param3; float param4; + float turn_distance_xy; /**< The distance on the plane which will mark this as reached */ + float turn_distance_z; /**< The distance in Z direction which will mark this as reached */ }; /** From a3bdf536e5f6b95d54ef6684d7092ebff2d23dc8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Oct 2013 09:17:58 +0200 Subject: [PATCH 741/872] Dynamic integral resets for straight and circle mode, announcing turn radius now --- .../fw_att_control/fw_att_control_main.cpp | 4 +++ .../fw_pos_control_l1_main.cpp | 31 ++++++++++++++++++- .../uORB/topics/vehicle_attitude_setpoint.h | 2 ++ 3 files changed, 36 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 6b98003fd2..ae9aaa2da6 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -593,6 +593,10 @@ FixedwingAttitudeControl::task_main() pitch_sp = _att_sp.pitch_body; throttle_sp = _att_sp.thrust; + /* reset integrals where needed */ + if (_att_sp.roll_reset_integral) + _roll_ctrl.reset_integrator(); + } else { /* * Scale down roll and pitch as the setpoints are radians diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 3d5bce134c..cd4a0d58ef 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -74,6 +74,7 @@ #include #include #include +#include #include #include #include @@ -129,9 +130,11 @@ private: int _accel_sub; /**< body frame accelerations */ orb_advert_t _attitude_sp_pub; /**< attitude setpoint */ + orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */ struct vehicle_attitude_s _att; /**< vehicle attitude */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ + struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ struct airspeed_s _airspeed; /**< airspeed */ struct vehicle_control_mode_s _control_mode; /**< vehicle status */ @@ -312,6 +315,7 @@ FixedwingPositionControl::FixedwingPositionControl() : /* publications */ _attitude_sp_pub(-1), + _nav_capabilities_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")), @@ -323,6 +327,8 @@ FixedwingPositionControl::FixedwingPositionControl() : _groundspeed_undershoot(0.0f), _global_pos_valid(false) { + _nav_capabilities.turn_distance = 0.0f; + _parameter_handles.l1_period = param_find("FW_L1_PERIOD"); _parameter_handles.l1_damping = param_find("FW_L1_DAMPING"); _parameter_handles.loiter_hold_radius = param_find("FW_LOITER_R"); @@ -625,6 +631,9 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio // else integrators should be constantly reset. if (_control_mode.flag_control_position_enabled) { + /* get circle mode */ + bool was_circle_mode = _l1_control.circle_mode(); + /* execute navigation once we have a setpoint */ if (_setpoint_valid) { @@ -642,7 +651,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else { /* - * No valid next waypoint, go for heading hold. + * No valid previous waypoint, go for the current wp. * This is automatically handled by the L1 library. */ prev_wp.setX(global_triplet.current.lat / 1e7f); @@ -810,6 +819,11 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } } + if (was_circle_mode && !_l1_control.circle_mode()) { + /* just kicked out of loiter, reset roll integrals */ + _att_sp.roll_reset_integral = true; + } + } else if (0/* seatbelt mode enabled */) { /** SEATBELT FLIGHT **/ @@ -968,6 +982,21 @@ FixedwingPositionControl::task_main() _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); } + float turn_distance = _l1_control.switch_distance(_global_triplet.current.turn_distance_xy); + + /* lazily publish navigation capabilities */ + if (turn_distance != _nav_capabilities.turn_distance && turn_distance > 0) { + + /* set new turn distance */ + _nav_capabilities.turn_distance = turn_distance; + + if (_nav_capabilities_pub > 0) { + orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities); + } else { + _nav_capabilities_pub = orb_advertise(ORB_ID(navigation_capabilities), &_nav_capabilities); + } + } + } } diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h index 686fd765c5..1a245132a8 100644 --- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h +++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h @@ -72,6 +72,8 @@ struct vehicle_attitude_setpoint_s float thrust; /**< Thrust in Newton the power system should generate */ + bool roll_reset_integral; /**< Reset roll integral part (navigation logic change) */ + }; /** From 5e3bdd77890c25b62e46f2f4f1238ac932801b12 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 8 Oct 2013 09:38:04 +0200 Subject: [PATCH 742/872] mavlink_onboard: major optimization, cleanup and minor fixes, WIP --- src/modules/mavlink/mavlink_receiver.cpp | 2 +- src/modules/mavlink/orb_listener.c | 2 +- src/modules/mavlink_onboard/mavlink.c | 20 ++++++------- .../mavlink_onboard/mavlink_receiver.c | 29 +++++++++++-------- 4 files changed, 29 insertions(+), 24 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 7dd9e321fb..b315f3efe2 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -733,7 +733,7 @@ receive_thread(void *arg) mavlink_message_t msg; - prctl(PR_SET_NAME, "mavlink uart rcv", getpid()); + prctl(PR_SET_NAME, "mavlink_uart_rcv", getpid()); while (!thread_should_exit) { diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index cec2fdc431..83c930a69f 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -695,7 +695,7 @@ static void * uorb_receive_thread(void *arg) { /* Set thread name */ - prctl(PR_SET_NAME, "mavlink orb rcv", getpid()); + prctl(PR_SET_NAME, "mavlink_orb_rcv", getpid()); /* * set up poll to block for new data, diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c index e713449823..0edb53a3e0 100644 --- a/src/modules/mavlink_onboard/mavlink.c +++ b/src/modules/mavlink_onboard/mavlink.c @@ -167,12 +167,12 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf case 921600: speed = B921600; break; default: - fprintf(stderr, "[mavlink] ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud); + warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600", baud); return -EINVAL; } /* open uart */ - printf("[mavlink] UART is %s, baudrate is %d\n", uart_name, baud); + warnx("UART is %s, baudrate is %d", uart_name, baud); uart = open(uart_name, O_RDWR | O_NOCTTY); /* Try to set baud rate */ @@ -183,7 +183,7 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf if (strcmp(uart_name, "/dev/ttyACM0") != OK) { /* Back up the original uart configuration to restore it after exit */ if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { - fprintf(stderr, "[mavlink] ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state); + warnx("ERROR getting baudrate / termios config for %s: %d", uart_name, termios_state); close(uart); return -1; } @@ -196,14 +196,14 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf /* Set baud rate */ if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { - fprintf(stderr, "[mavlink] ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); + warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)", uart_name, termios_state); close(uart); return -1; } if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { - fprintf(stderr, "[mavlink] ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name); + warnx("ERROR setting baudrate / termios config for %s (tcsetattr)", uart_name); close(uart); return -1; } @@ -481,9 +481,9 @@ int mavlink_thread_main(int argc, char *argv[]) static void usage() { - fprintf(stderr, "usage: mavlink start [-d ] [-b ]\n" - " mavlink stop\n" - " mavlink status\n"); + fprintf(stderr, "usage: mavlink_onboard start [-d ] [-b ]\n" + " mavlink_onboard stop\n" + " mavlink_onboard status\n"); exit(1); } @@ -499,7 +499,7 @@ int mavlink_onboard_main(int argc, char *argv[]) /* this is not an error */ if (thread_running) - errx(0, "mavlink already running\n"); + errx(0, "already running"); thread_should_exit = false; mavlink_task = task_spawn_cmd("mavlink_onboard", @@ -516,7 +516,7 @@ int mavlink_onboard_main(int argc, char *argv[]) while (thread_running) { usleep(200000); } - warnx("terminated."); + warnx("terminated"); exit(0); } diff --git a/src/modules/mavlink_onboard/mavlink_receiver.c b/src/modules/mavlink_onboard/mavlink_receiver.c index 0236e61264..4f62b5fcc2 100644 --- a/src/modules/mavlink_onboard/mavlink_receiver.c +++ b/src/modules/mavlink_onboard/mavlink_receiver.c @@ -104,7 +104,7 @@ handle_message(mavlink_message_t *msg) //check for MAVLINK terminate command if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) { /* This is the link shutdown command, terminate mavlink */ - printf("[mavlink] Terminating .. \n"); + warnx("terminating..."); fflush(stdout); usleep(50000); @@ -284,28 +284,33 @@ receive_thread(void *arg) int uart_fd = *((int*)arg); const int timeout = 1000; - uint8_t ch; + uint8_t buf[32]; mavlink_message_t msg; - prctl(PR_SET_NAME, "mavlink offb rcv", getpid()); + prctl(PR_SET_NAME, "mavlink_onboard_rcv", getpid()); + + struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } }; + + ssize_t nread = 0; while (!thread_should_exit) { - - struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } }; - if (poll(fds, 1, timeout) > 0) { - /* non-blocking read until buffer is empty */ - int nread = 0; + if (nread < sizeof(buf)) { + /* to avoid reading very small chunks wait for data before reading */ + usleep(1000); + } - do { - nread = read(uart_fd, &ch, 1); + /* non-blocking read. read may return negative values */ + ssize_t nread = read(uart_fd, buf, sizeof(buf)); - if (mavlink_parse_char(chan, ch, &msg, &status)) { //parse the char + /* if read failed, this loop won't execute */ + for (ssize_t i = 0; i < nread; i++) { + if (mavlink_parse_char(chan, buf[i], &msg, &status)) { /* handle generic messages and commands */ handle_message(&msg); } - } while (nread > 0); + } } } From d70d8ae68e4a2971e714aa766cf92874d144a5f4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 8 Oct 2013 11:26:27 +0200 Subject: [PATCH 743/872] mavlink, mavlink_onboard: bugfixes, code style fixed --- src/modules/mavlink/mavlink_receiver.cpp | 72 ++++++++++++++++--- .../mavlink_onboard/mavlink_receiver.c | 65 +++++++++-------- 2 files changed, 98 insertions(+), 39 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index b315f3efe2..8243748dc8 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -167,6 +167,7 @@ handle_message(mavlink_message_t *msg) /* check if topic is advertised */ if (cmd_pub <= 0) { cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + } else { /* publish */ orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); @@ -437,9 +438,11 @@ handle_message(mavlink_message_t *msg) if (pub_hil_airspeed < 0) { pub_hil_airspeed = orb_advertise(ORB_ID(airspeed), &airspeed); + } else { orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed); } + //warnx("SENSOR: IAS: %6.2f TAS: %6.2f", airspeed.indicated_airspeed_m_s, airspeed.true_airspeed_m_s); /* individual sensor publications */ @@ -455,49 +458,72 @@ handle_message(mavlink_message_t *msg) 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); } @@ -505,6 +531,7 @@ handle_message(mavlink_message_t *msg) /* publish combined sensor topic */ if (pub_hil_sensors > 0) { orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors); + } else { pub_hil_sensors = orb_advertise(ORB_ID(sensor_combined), &hil_sensors); } @@ -517,6 +544,7 @@ handle_message(mavlink_message_t *msg) /* lazily publish the battery voltage */ if (pub_hil_battery > 0) { orb_publish(ORB_ID(battery_status), pub_hil_battery, &hil_battery_status); + } else { pub_hil_battery = orb_advertise(ORB_ID(battery_status), &hil_battery_status); } @@ -527,7 +555,7 @@ handle_message(mavlink_message_t *msg) // output if ((timestamp - old_timestamp) > 10000000) { - printf("receiving hil sensor at %d hz\n", hil_frames/10); + printf("receiving hil sensor at %d hz\n", hil_frames / 10); old_timestamp = timestamp; hil_frames = 0; } @@ -552,9 +580,11 @@ handle_message(mavlink_message_t *msg) /* gps.cog is in degrees 0..360 * 100, heading is -PI..+PI */ float heading_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f; + /* go back to -PI..PI */ if (heading_rad > M_PI_F) heading_rad -= 2.0f * M_PI_F; + hil_gps.vel_n_m_s = gps.vn * 1e-2f; // from cm to m hil_gps.vel_e_m_s = gps.ve * 1e-2f; // from cm to m hil_gps.vel_d_m_s = gps.vd * 1e-2f; // from cm to m @@ -567,6 +597,7 @@ handle_message(mavlink_message_t *msg) /* publish GPS measurement data */ if (pub_hil_gps > 0) { orb_publish(ORB_ID(vehicle_gps_position), pub_hil_gps, &hil_gps); + } else { pub_hil_gps = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); } @@ -585,6 +616,7 @@ handle_message(mavlink_message_t *msg) if (pub_hil_airspeed < 0) { pub_hil_airspeed = orb_advertise(ORB_ID(airspeed), &airspeed); + } else { orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed); } @@ -602,6 +634,7 @@ handle_message(mavlink_message_t *msg) if (pub_hil_global_pos > 0) { orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos); + } else { pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); } @@ -613,8 +646,8 @@ handle_message(mavlink_message_t *msg) /* set rotation matrix */ for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) - hil_attitude.R[i][j] = C_nb(i, j); - + hil_attitude.R[i][j] = C_nb(i, j); + hil_attitude.R_valid = true; /* set quaternion */ @@ -636,22 +669,32 @@ handle_message(mavlink_message_t *msg) if (pub_hil_attitude > 0) { orb_publish(ORB_ID(vehicle_attitude), pub_hil_attitude, &hil_attitude); + } 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); } @@ -664,6 +707,7 @@ handle_message(mavlink_message_t *msg) /* lazily publish the battery voltage */ if (pub_hil_battery > 0) { orb_publish(ORB_ID(battery_status), pub_hil_battery, &hil_battery_status); + } else { pub_hil_battery = orb_advertise(ORB_ID(battery_status), &hil_battery_status); } @@ -735,15 +779,21 @@ receive_thread(void *arg) prctl(PR_SET_NAME, "mavlink_uart_rcv", getpid()); + struct pollfd fds[1]; + fds[0].fd = uart_fd; + fds[0].events = POLLIN; + + ssize_t nread = 0; + while (!thread_should_exit) { - - struct pollfd fds[1]; - fds[0].fd = uart_fd; - fds[0].events = POLLIN; - if (poll(fds, 1, timeout) > 0) { + if (nread < sizeof(buf)) { + /* to avoid reading very small chunks wait for data before reading */ + usleep(1000); + } + /* non-blocking read. read may return negative values */ - ssize_t nread = read(uart_fd, buf, sizeof(buf)); + nread = read(uart_fd, buf, sizeof(buf)); /* if read failed, this loop won't execute */ for (ssize_t i = 0; i < nread; i++) { @@ -751,10 +801,10 @@ receive_thread(void *arg) /* handle generic messages and commands */ handle_message(&msg); - /* Handle packet with waypoint component */ + /* handle packet with waypoint component */ mavlink_wpm_message_handler(&msg, &global_pos, &local_pos); - /* Handle packet with parameter component */ + /* handle packet with parameter component */ mavlink_pm_message_handler(MAVLINK_COMM_0, &msg); } } diff --git a/src/modules/mavlink_onboard/mavlink_receiver.c b/src/modules/mavlink_onboard/mavlink_receiver.c index 4f62b5fcc2..4658bcc1d4 100644 --- a/src/modules/mavlink_onboard/mavlink_receiver.c +++ b/src/modules/mavlink_onboard/mavlink_receiver.c @@ -100,7 +100,7 @@ handle_message(mavlink_message_t *msg) mavlink_msg_command_long_decode(msg, &cmd_mavlink); if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid) - || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { + || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { //check for MAVLINK terminate command if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) { /* This is the link shutdown command, terminate mavlink */ @@ -132,6 +132,7 @@ handle_message(mavlink_message_t *msg) if (cmd_pub <= 0) { cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); } + /* publish */ orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); } @@ -156,6 +157,7 @@ handle_message(mavlink_message_t *msg) /* check if topic is advertised */ if (flow_pub <= 0) { flow_pub = orb_advertise(ORB_ID(optical_flow), &f); + } else { /* publish */ orb_publish(ORB_ID(optical_flow), flow_pub, &f); @@ -186,6 +188,7 @@ handle_message(mavlink_message_t *msg) /* check if topic is advertised */ if (cmd_pub <= 0) { cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + } else { /* create command */ orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); @@ -203,6 +206,7 @@ handle_message(mavlink_message_t *msg) if (vicon_position_pub <= 0) { vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position); + } else { orb_publish(ORB_ID(vehicle_vicon_position), vicon_position_pub, &vicon_position); } @@ -219,7 +223,7 @@ handle_message(mavlink_message_t *msg) /* switch to a receiving link mode */ gcs_link = false; - /* + /* * rate control mode - defined by MAVLink */ @@ -227,33 +231,37 @@ handle_message(mavlink_message_t *msg) bool ml_armed = false; switch (quad_motors_setpoint.mode) { - case 0: - ml_armed = false; - break; - case 1: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES; - ml_armed = true; + case 0: + ml_armed = false; + break; - break; - case 2: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; - ml_armed = true; + case 1: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES; + ml_armed = true; - break; - case 3: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY; - break; - case 4: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION; - break; + break; + + case 2: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; + ml_armed = true; + + break; + + case 3: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY; + break; + + case 4: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION; + break; } - offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid-1] / (float)INT16_MAX; - offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid-1] / (float)INT16_MAX; - offboard_control_sp.p3= (float)quad_motors_setpoint.yaw[mavlink_system.sysid-1] / (float)INT16_MAX; - offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid-1]/(float)UINT16_MAX; + offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / (float)INT16_MAX; + offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / (float)INT16_MAX; + offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / (float)INT16_MAX; + offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX; - if (quad_motors_setpoint.thrust[mavlink_system.sysid-1] == 0) { + if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) { ml_armed = false; } @@ -265,6 +273,7 @@ handle_message(mavlink_message_t *msg) /* check if topic has to be advertised */ if (offboard_control_sp_pub <= 0) { offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); + } else { /* Publish */ orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp); @@ -281,7 +290,7 @@ handle_message(mavlink_message_t *msg) static void * receive_thread(void *arg) { - int uart_fd = *((int*)arg); + int uart_fd = *((int *)arg); const int timeout = 1000; uint8_t buf[32]; @@ -302,7 +311,7 @@ receive_thread(void *arg) } /* non-blocking read. read may return negative values */ - ssize_t nread = read(uart_fd, buf, sizeof(buf)); + nread = read(uart_fd, buf, sizeof(buf)); /* if read failed, this loop won't execute */ for (ssize_t i = 0; i < nread; i++) { @@ -324,8 +333,8 @@ receive_start(int uart) pthread_attr_init(&receiveloop_attr); struct sched_param param; - param.sched_priority = SCHED_PRIORITY_MAX - 40; - (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); + param.sched_priority = SCHED_PRIORITY_MAX - 40; + (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); pthread_attr_setstacksize(&receiveloop_attr, 2048); From b2555d70e37d33d09459b0c73637f7efd6cd9a95 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 8 Oct 2013 13:39:08 +0200 Subject: [PATCH 744/872] sensors: minor optimization, cleanup and refactoring --- src/modules/sensors/sensors.cpp | 239 ++++++++++++++++---------------- 1 file changed, 118 insertions(+), 121 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 085da905c6..be599762fa 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -68,7 +68,6 @@ #include #include -#include #include #include @@ -105,22 +104,22 @@ * 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 +#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 +#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 @@ -134,8 +133,6 @@ */ #define PCB_TEMP_ESTIMATE_DEG 5.0f -#define PPM_INPUT_TIMEOUT_INTERVAL 50000 /**< 50 ms timeout / 20 Hz */ - #define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg) /** @@ -143,70 +140,68 @@ * This enum maps from board attitude to airframe attitude. */ 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 + 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; +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 } +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 } }; /** @@ -239,12 +234,12 @@ public: private: static const unsigned _rc_max_chan_count = RC_CHANNELS_MAX; /**< maximum number of r/c channels we handle */ - hrt_abstime _ppm_last_valid; /**< last time we got a valid ppm signal */ + hrt_abstime _rc_last_valid; /**< last time we got a valid RC signal */ /** - * Gather and publish PPM input data. + * Gather and publish RC input data. */ - void ppm_poll(); + void rc_poll(); /* XXX should not be here - should be own driver */ int _fd_adc; /**< ADC driver handle */ @@ -501,7 +496,7 @@ Sensors *g_sensors = nullptr; } Sensors::Sensors() : - _ppm_last_valid(0), + _rc_last_valid(0), _fd_adc(-1), _last_adc(0), @@ -532,8 +527,8 @@ Sensors::Sensors() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")), - _board_rotation(3,3), - _external_mag_rotation(3,3), + _board_rotation(3, 3), + _external_mag_rotation(3, 3), _mag_is_external(false) { @@ -660,9 +655,9 @@ int Sensors::parameters_update() { bool rc_valid = true; - float tmpScaleFactor = 0.0f; - float tmpRevFactor = 0.0f; - + float tmpScaleFactor = 0.0f; + float tmpRevFactor = 0.0f; + /* rc values */ for (unsigned int i = 0; i < RC_CHANNELS_MAX; i++) { @@ -674,19 +669,19 @@ Sensors::parameters_update() tmpScaleFactor = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]); tmpRevFactor = tmpScaleFactor * _parameters.rev[i]; - + /* handle blowup in the scaling factor calculation */ if (!isfinite(tmpScaleFactor) || (tmpRevFactor < 0.000001f) || - (tmpRevFactor > 0.2f) ) { + (tmpRevFactor > 0.2f)) { warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, tmpScaleFactor, (int)(_parameters.rev[i])); /* scaling factors do not make sense, lock them down */ _parameters.scaling_factor[i] = 0.0f; rc_valid = false; + + } else { + _parameters.scaling_factor[i] = tmpScaleFactor; } - else { - _parameters.scaling_factor[i] = tmpScaleFactor; - } } /* handle wrong values */ @@ -812,7 +807,7 @@ void Sensors::get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix) { /* first set to zero */ - rot_matrix->Matrix::zero(3,3); + 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; @@ -823,7 +818,7 @@ Sensors::get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix) 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); + (*rot_matrix)(i, j) = R(i, j); } void @@ -841,7 +836,7 @@ Sensors::accel_init() // XXX do the check more elegantly - #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 /* set the accel internal sampling rate up to at leat 1000Hz */ ioctl(fd, ACCELIOCSSAMPLERATE, 1000); @@ -849,7 +844,7 @@ Sensors::accel_init() /* set the driver to poll at 1000Hz */ ioctl(fd, SENSORIOCSPOLLRATE, 1000); - #elif CONFIG_ARCH_BOARD_PX4FMU_V2 +#elif CONFIG_ARCH_BOARD_PX4FMU_V2 /* set the accel internal sampling rate up to at leat 800Hz */ ioctl(fd, ACCELIOCSSAMPLERATE, 800); @@ -857,10 +852,10 @@ 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 +#else +#error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2 - #endif +#endif warnx("using system accel"); close(fd); @@ -882,7 +877,7 @@ Sensors::gyro_init() // XXX do the check more elegantly - #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 /* set the gyro internal sampling rate up to at least 1000Hz */ if (ioctl(fd, GYROIOCSSAMPLERATE, 1000) != OK) @@ -892,7 +887,7 @@ Sensors::gyro_init() if (ioctl(fd, SENSORIOCSPOLLRATE, 1000) != OK) ioctl(fd, SENSORIOCSPOLLRATE, 800); - #else +#else /* set the gyro internal sampling rate up to at least 760Hz */ ioctl(fd, GYROIOCSSAMPLERATE, 760); @@ -900,7 +895,7 @@ Sensors::gyro_init() /* set the driver to poll at 760Hz */ ioctl(fd, SENSORIOCSPOLLRATE, 760); - #endif +#endif warnx("using system gyro"); close(fd); @@ -924,23 +919,28 @@ Sensors::mag_init() 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"); } } - + ret = ioctl(fd, MAGIOCGEXTERNAL, 0); + if (ret < 0) errx(1, "FATAL: unknown if magnetometer is external or onboard"); else if (ret == 1) @@ -993,7 +993,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); math::Vector3 vect = {accel_report.x, accel_report.y, accel_report.z}; - vect = _board_rotation*vect; + vect = _board_rotation * vect; raw.accelerometer_m_s2[0] = vect(0); raw.accelerometer_m_s2[1] = vect(1); @@ -1019,7 +1019,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); math::Vector3 vect = {gyro_report.x, gyro_report.y, gyro_report.z}; - vect = _board_rotation*vect; + vect = _board_rotation * vect; raw.gyro_rad_s[0] = vect(0); raw.gyro_rad_s[1] = vect(1); @@ -1047,9 +1047,9 @@ Sensors::mag_poll(struct sensor_combined_s &raw) math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z}; if (_mag_is_external) - vect = _external_mag_rotation*vect; + vect = _external_mag_rotation * vect; else - vect = _board_rotation*vect; + vect = _board_rotation * vect; raw.magnetometer_ga[0] = vect(0); raw.magnetometer_ga[1] = vect(1); @@ -1094,8 +1094,8 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) raw.differential_pressure_counter++; _airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_pa); - _airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_pa + raw.baro_pres_mbar*1e2f, - raw.baro_pres_mbar*1e2f, raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); + _airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_pa + raw.baro_pres_mbar * 1e2f, + raw.baro_pres_mbar * 1e2f, raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); /* announce the airspeed if needed, just publish else */ if (_airspeed_pub > 0) { @@ -1233,12 +1233,12 @@ Sensors::adc_poll(struct sensor_combined_s &raw) int ret = read(_fd_adc, &buf_adc, sizeof(buf_adc)); for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) { - + if (ret >= (int)sizeof(buf_adc[0])) { /* Save raw voltage values */ if (i < (sizeof(raw.adc_voltage_v)) / sizeof(raw.adc_voltage_v[0])) { - raw.adc_voltage_v[i] = buf_adc[i].am_data / (4096.0f / 3.3f); + raw.adc_voltage_v[i] = buf_adc[i].am_data / (4096.0f / 3.3f); } /* look for specific channels and process the raw voltage to measurement data */ @@ -1266,12 +1266,12 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } else { _battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status); } - } + } } else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { /* calculate airspeed, raw is the difference from */ - float voltage = (float)(buf_adc[i].am_data ) * 3.3f / 4096.0f * 2.0f; //V_ref/4096 * (voltage divider factor) + float voltage = (float)(buf_adc[i].am_data) * 3.3f / 4096.0f * 2.0f; //V_ref/4096 * (voltage divider factor) /** * The voltage divider pulls the signal down, only act on @@ -1303,17 +1303,13 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } void -Sensors::ppm_poll() +Sensors::rc_poll() { + bool rc_updated; + orb_check(_rc_sub, &rc_updated); - /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ - struct pollfd fds[1]; - fds[0].fd = _rc_sub; - fds[0].events = POLLIN; - /* check non-blocking for new data */ - int poll_ret = poll(fds, 1, 0); - - if (poll_ret > 0) { + if (rc_updated) { + /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ struct rc_input_values rc_input; orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); @@ -1349,7 +1345,7 @@ Sensors::ppm_poll() channel_limit = _rc_max_chan_count; /* we are accepting this message */ - _ppm_last_valid = rc_input.timestamp; + _rc_last_valid = rc_input.timestamp; /* Read out values from raw message */ for (unsigned int i = 0; i < channel_limit; i++) { @@ -1359,6 +1355,7 @@ Sensors::ppm_poll() */ if (rc_input.values[i] < _parameters.min[i]) rc_input.values[i] = _parameters.min[i]; + if (rc_input.values[i] > _parameters.max[i]) rc_input.values[i] = _parameters.max[i]; @@ -1619,7 +1616,7 @@ Sensors::task_main() orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw); /* Look for new r/c input data */ - ppm_poll(); + rc_poll(); perf_end(_loop_perf); } @@ -1637,11 +1634,11 @@ Sensors::start() /* start the task */ _sensors_task = task_spawn_cmd("sensors_task", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2048, - (main_t)&Sensors::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2048, + (main_t)&Sensors::task_main_trampoline, + nullptr); if (_sensors_task < 0) { warn("task start failed"); From 1b9e2af7425615130ddbcf79b89859c97a791a9c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 8 Oct 2013 17:03:57 +0200 Subject: [PATCH 745/872] Moved PWM ramp to systemlib --- src/drivers/px4io/px4io.cpp | 2 +- src/modules/px4iofirmware/mixer.cpp | 143 +++++--------------- src/modules/px4iofirmware/module.mk | 1 + src/modules/px4iofirmware/px4io.c | 6 + src/modules/px4iofirmware/px4io.h | 7 + src/modules/systemlib/pwm_limit/pwm_limit.c | 116 ++++++++++++++++ src/modules/systemlib/pwm_limit/pwm_limit.h | 79 +++++++++++ 7 files changed, 241 insertions(+), 113 deletions(-) create mode 100644 src/modules/systemlib/pwm_limit/pwm_limit.c create mode 100644 src/modules/systemlib/pwm_limit/pwm_limit.h diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index f9dc3773e2..18c9ef0bd0 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2402,7 +2402,7 @@ px4io_main(int argc, char *argv[]) /* 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] < PWM_MIN || failsafe[i] > PWM_AX) { + if (failsafe[i] < PWM_MIN || failsafe[i] > PWM_MAX) { errx(1, "value out of range of %d < value < %d. Aborting.", PWM_MIN, PWM_MAX); } } else { diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 352b93e858..5232f9b963 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -47,6 +47,7 @@ #include #include +#include #include extern "C" { @@ -59,12 +60,6 @@ extern "C" { */ #define FMU_INPUT_DROP_LIMIT_US 200000 -/* - * 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 #define PITCH 1 @@ -76,15 +71,6 @@ extern "C" { static bool mixer_servos_armed = false; static bool should_arm = false; static bool should_always_enable_pwm = false; -static uint64_t esc_init_time; - -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 { @@ -165,102 +151,6 @@ mixer_tick(void) r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE); } - /* - * Run the mixers. - */ - if (source == MIX_FAILSAFE) { - - /* copy failsafe values to the servo outputs */ - for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { - r_page_servos[i] = r_page_servo_failsafe[i]; - - /* safe actuators for FMU feedback */ - r_page_actuators[i] = (r_page_servos[i] - 1500) / 600.0f; - } - - - } else if (source != MIX_NONE) { - - float outputs[PX4IO_SERVO_COUNT]; - unsigned mixed; - - 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; - } - - /* 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], PX4IO_SERVO_COUNT); - - /* scale to PWM and update the servo outputs as required */ - for (unsigned i = 0; i < mixed; i++) { - - /* save actuator values for FMU readback */ - r_page_actuators[i] = FLOAT_TO_REG(outputs[i]); - - 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)*PWM_MAX - ramp_promille*r_page_servo_control_min[i] - (1000-ramp_promille)*PWM_MIN)/2/1000 - + (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*PWM_MAX + ramp_promille*r_page_servo_control_min[i] + (1000-ramp_promille)*PWM_MIN)/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 < PX4IO_SERVO_COUNT; i++) - r_page_servos[i] = 0; - } - /* * Decide whether the servos should be armed right now. * @@ -285,6 +175,36 @@ mixer_tick(void) && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK); + /* + * Run the mixers. + */ + if (source == MIX_FAILSAFE) { + + /* copy failsafe values to the servo outputs */ + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { + r_page_servos[i] = r_page_servo_failsafe[i]; + + /* safe actuators for FMU feedback */ + r_page_actuators[i] = (r_page_servos[i] - 1500) / 600.0f; + } + + + } else if (source != MIX_NONE) { + + float outputs[PX4IO_SERVO_COUNT]; + unsigned mixed; + + /* mix */ + mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT); + + pwm_limit.nchannels = mixed; + + pwm_limit_calc(should_arm, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + + for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) + r_page_servos[i] = 0; + } + if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) { /* need to arm, but not armed */ up_pwm_servo_arm(true); @@ -298,7 +218,6 @@ mixer_tick(void) mixer_servos_armed = false; r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED); isr_debug(5, "> PWM disabled"); - } if (mixer_servos_armed && should_arm) { diff --git a/src/modules/px4iofirmware/module.mk b/src/modules/px4iofirmware/module.mk index 59f470a94e..01869569f6 100644 --- a/src/modules/px4iofirmware/module.mk +++ b/src/modules/px4iofirmware/module.mk @@ -14,6 +14,7 @@ SRCS = adc.c \ ../systemlib/mixer/mixer_group.cpp \ ../systemlib/mixer/mixer_multirotor.cpp \ ../systemlib/mixer/mixer_simple.cpp \ + ../systemlib/pwm_limit/pwm_limit.c ifeq ($(BOARD),px4io-v1) SRCS += i2c.c diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index e70b3fe880..71d6490291 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -50,6 +50,7 @@ #include #include +#include #include @@ -64,6 +65,8 @@ struct sys_state_s system_state; static struct hrt_call serial_dma_call; +pwm_limit_t pwm_limit; + /* * a set of debug buffers to allow us to send debug information from ISRs */ @@ -174,6 +177,9 @@ user_start(int argc, char *argv[]) struct mallinfo minfo = mallinfo(); lowsyslog("MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks); + /* initialize PWM limit lib */ + pwm_limit_init(&pwm_limit); + #if 0 /* not enough memory, lock down */ if (minfo.mxordblk < 500) { diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 6c9007a75a..4fea0288c4 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -46,6 +46,8 @@ #include "protocol.h" +#include + /* * Constants and limits. */ @@ -122,6 +124,11 @@ struct sys_state_s { extern struct sys_state_s system_state; +/* + * PWM limit structure + */ +extern pwm_limit_t pwm_limit; + /* * GPIO handling. */ diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c new file mode 100644 index 0000000000..6752475780 --- /dev/null +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -0,0 +1,116 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * 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 pwm_limit.c + * + * Lib to limit PWM output + * + * @author Julian Oes + */ + +#include "pwm_limit.h" +#include +#include +#include + +__EXPORT pwm_limit_init(pwm_limit_t *limit) +{ + limit->nchannels = 0; + limit->state = LIMIT_STATE_OFF; + limit->time_armed = 0; + return; +} + +__EXPORT void pwm_limit_calc(const bool armed, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output_requested, uint16_t *effective_pwm, pwm_limit_t *limit) +{ + /* first evaluate state changes */ + switch (limit->state) { + case LIMIT_STATE_OFF: + if (armed) + limit->state = LIMIT_STATE_RAMP; + limit->time_armed = hrt_absolute_time(); + break; + case LIMIT_STATE_INIT: + if (!armed) + limit->state = LIMIT_STATE_OFF; + else if (hrt_absolute_time() - limit->time_armed >= INIT_TIME_US) + limit->state = LIMIT_STATE_RAMP; + break; + case LIMIT_STATE_RAMP: + if (!armed) + limit->state = LIMIT_STATE_OFF; + else if (hrt_absolute_time() - limit->time_armed >= INIT_TIME_US + RAMP_TIME_US) + limit->state = LIMIT_STATE_ON; + break; + case LIMIT_STATE_ON: + if (!armed) + limit->state = LIMIT_STATE_OFF; + break; + default: + break; + } + + unsigned progress; + uint16_t temp_pwm; + + /* then set effective_pwm based on state */ + switch (limit->state) { + case LIMIT_STATE_OFF: + case LIMIT_STATE_INIT: + for (unsigned i=0; inchannels; i++) { + effective_pwm[i] = disarmed_pwm[i]; + } + break; + case LIMIT_STATE_RAMP: + + progress = (hrt_absolute_time() - INIT_TIME_US - limit->time_armed)*10000 / RAMP_TIME_US; + for (unsigned i=0; inchannels; i++) { + + temp_pwm = output_requested[i] * (max_pwm[i] - min_pwm[i])/2 + (max_pwm[i] + min_pwm[i])/2; + /* already follow user/controller input if higher than min_pwm */ + effective_pwm[i] = (disarmed_pwm[i]*(10000-progress) + (temp_pwm > min_pwm[i] ? temp_pwm : min_pwm[i])*progress)/10000; + + } + break; + case LIMIT_STATE_ON: + for (unsigned i=0; inchannels; i++) { + effective_pwm[i] = output_requested[i] * (max_pwm[i] - min_pwm[i])/2 + (max_pwm[i] + min_pwm[i])/2; + } + break; + default: + break; + } + return; +} diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.h b/src/modules/systemlib/pwm_limit/pwm_limit.h new file mode 100644 index 0000000000..f8c6370e81 --- /dev/null +++ b/src/modules/systemlib/pwm_limit/pwm_limit.h @@ -0,0 +1,79 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * 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 pwm_limit.h + * + * Lib to limit PWM output + * + * @author Julian Oes + */ + +#ifndef PWM_LIMIT_H_ +#define PWM_LIMIT_H_ + +#include +#include + +__BEGIN_DECLS + +/* + * time for the ESCs to initialize + * (this is not actually needed if PWM is sent right after boot) + */ +#define INIT_TIME_US 500000 +/* + * time to slowly ramp up the ESCs + */ +#define RAMP_TIME_US 2500000 + +typedef struct { + enum { + LIMIT_STATE_OFF = 0, + LIMIT_STATE_INIT, + LIMIT_STATE_RAMP, + LIMIT_STATE_ON + } state; + uint64_t time_armed; + unsigned nchannels; +} pwm_limit_t; + +__EXPORT void pwm_limit_init(pwm_limit_t *limit); + +__EXPORT void pwm_limit_calc(const bool armed, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output_requested, uint16_t *effective_pwm, pwm_limit_t *limit); + + +__END_DECLS + +#endif /* PWM_LIMIT_H_ */ From b25b9d37d53d3caab7c7eb2eed6f5cdbdd3f8804 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 9 Oct 2013 09:00:22 +0200 Subject: [PATCH 746/872] Small function definition correction --- src/modules/systemlib/pwm_limit/pwm_limit.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c index 6752475780..f67b5edf25 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.c +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -45,7 +45,7 @@ #include #include -__EXPORT pwm_limit_init(pwm_limit_t *limit) +__EXPORT void pwm_limit_init(pwm_limit_t *limit) { limit->nchannels = 0; limit->state = LIMIT_STATE_OFF; From d63ad0fb817d76d2d818db47805b46742d2aae68 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 9 Oct 2013 09:26:18 +0200 Subject: [PATCH 747/872] Added debug output printing capabilities for IOv2 --- src/drivers/px4io/px4io.cpp | 88 +++++++++++++++++++++++++++++++++---- 1 file changed, 80 insertions(+), 8 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 7f67d02b53..38e183608a 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -193,6 +193,11 @@ public: */ int set_idle_values(const uint16_t *vals, unsigned len); + /** + * Disable RC input handling + */ + int disable_rc_handling(); + /** * Print IO status. * @@ -201,9 +206,9 @@ public: void print_status(); /** - * Disable RC input handling + * Fetch and print debug console output. */ - int disable_rc_handling(); + int print_debug(); #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 /** @@ -1531,9 +1536,53 @@ PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t return io_reg_set(page, offset, value); } +int +PX4IO::print_debug() +{ +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + int io_fd = -1; + + if (io_fd < 0) { + io_fd = ::open("/dev/ttyS0", O_RDONLY | O_NONBLOCK); + } + + /* read IO's output */ + if (io_fd > 0) { + pollfd fds[1]; + fds[0].fd = io_fd; + fds[0].events = POLLIN; + + usleep(500); + int pret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), 10); + + if (pret > 0) { + int count; + char buf[65]; + + do { + count = ::read(io_fd, buf, sizeof(buf) - 1); + if (count > 0) { + /* enforce null termination */ + buf[count] = '\0'; + warnx("IO CONSOLE: %s", buf); + } + + } while (count > 0); + } + + ::close(io_fd); + return 0; + } +#endif + return 1; + +} + int PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) { + /* get debug level */ + int debuglevel = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG); uint8_t frame[_max_transfer]; @@ -1572,6 +1621,15 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2); + /* print mixer chunk */ + if (debuglevel > 5 || ret) { + + warnx("fmu sent: \"%s\"", msg->text); + + /* read IO's output */ + print_debug(); + } + if (ret) { log("mixer send error %d", ret); return ret; @@ -1581,6 +1639,11 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) } while (buflen > 0); + /* ensure a closing newline */ + msg->text[0] = '\n'; + msg->text[1] = '\0'; + int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, 1); + retries--; log("mixer sent"); @@ -2242,28 +2305,37 @@ test(void) void monitor(void) { + /* clear screen */ + printf("\033[2J"); + unsigned cancels = 3; - printf("Hit three times to exit monitor mode\n"); for (;;) { pollfd fds[1]; fds[0].fd = 0; fds[0].events = POLLIN; - poll(fds, 1, 500); + poll(fds, 1, 2000); if (fds[0].revents == POLLIN) { int c; read(0, &c, 1); - if (cancels-- == 0) + if (cancels-- == 0) { + printf("\033[H"); /* move cursor home and clear screen */ exit(0); + } } -#warning implement this + if (g_dev != nullptr) { -// if (g_dev != nullptr) -// g_dev->dump_one = true; + printf("\033[H"); /* move cursor home and clear screen */ + (void)g_dev->print_status(); + (void)g_dev->print_debug(); + printf("[ Use 'px4io debug ' for more output. Hit three times to exit monitor mode ]\n"); + } else { + errx(1, "driver not loaded, exiting"); + } } } From ed00567400bd6ce24e25dc1038ce40f959205a68 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 9 Oct 2013 22:23:10 +0200 Subject: [PATCH 748/872] Extended file test for alignment --- src/systemcmds/tests/tests_file.c | 77 ++++++++++++++++++------------- 1 file changed, 44 insertions(+), 33 deletions(-) diff --git a/src/systemcmds/tests/tests_file.c b/src/systemcmds/tests/tests_file.c index f36c280613..372bc5c908 100644 --- a/src/systemcmds/tests/tests_file.c +++ b/src/systemcmds/tests/tests_file.c @@ -63,24 +63,38 @@ test_file(int argc, char *argv[]) return 1; } - uint8_t buf[512]; + uint8_t write_buf[512 + 64]; + + /* fill write buffer with known values */ + for (int i = 0; i < sizeof(write_buf); i++) { + /* this will wrap, but we just need a known value with spacing */ + write_buf[i] = i+11; + } + + uint8_t read_buf[512 + 64]; hrt_abstime start, end; perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes (aligned)"); int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); - memset(buf, 0, sizeof(buf)); - warnx("aligned write - please wait.."); - - if ((0x3 & (uintptr_t)buf)) - warnx("memory is unaligned!"); + warnx("testing aligned and unaligned writes - please wait.."); start = hrt_absolute_time(); for (unsigned i = 0; i < iterations; i++) { perf_begin(wperf); - write(fd, buf, sizeof(buf)); + int wret = write(fd, write_buf + (i % 64), 512); + + if (wret != 512) { + warn("WRITE ERROR!"); + + if ((0x3 & (uintptr_t)(write_buf + (i % 64)))) + warnx("memory is unaligned, align shift: %d", (i % 64)); + + } + fsync(fd); perf_end(wperf); + } end = hrt_absolute_time(); @@ -89,39 +103,36 @@ test_file(int argc, char *argv[]) perf_print_counter(wperf); perf_free(wperf); + /* read back data for validation */ + for (unsigned i = 0; i < iterations; i++) { + int rret = read(fd, read_buf, 512); + + /* compare value */ + + for (int j = 0; j < 512; j++) { + if (read_buf[j] != write_buf[j + (i % 64)]) { + warnx("COMPARISON ERROR: byte %d, align shift: %d", j, (i % 64)); + } + } + + } + + /* read back data for alignment checks */ + // for (unsigned i = 0; i < iterations; i++) { + // perf_begin(wperf); + // int rret = read(fd, buf + (i % 64), sizeof(buf)); + // fsync(fd); + // perf_end(wperf); + + // } + int ret = unlink("/fs/microsd/testfile"); if (ret) err(1, "UNLINKING FILE FAILED"); - warnx("unaligned write - please wait.."); - - struct { - uint8_t byte; - uint8_t unaligned[512]; - } unaligned_buf; - - if ((0x3 & (uintptr_t)unaligned_buf.unaligned) == 0) - warnx("creating unaligned memory failed."); - - wperf = perf_alloc(PC_ELAPSED, "SD writes (unaligned)"); - - start = hrt_absolute_time(); - for (unsigned i = 0; i < iterations; i++) { - perf_begin(wperf); - write(fd, unaligned_buf.unaligned, sizeof(unaligned_buf.unaligned)); - fsync(fd); - perf_end(wperf); - } - end = hrt_absolute_time(); - close(fd); - warnx("%dKiB in %llu microseconds", iterations / 2, end - start); - - perf_print_counter(wperf); - perf_free(wperf); - /* list directory */ DIR *d; struct dirent *dir; From 1ca718b57fe0af737b0d2fb4e903162e5bb56852 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Oct 2013 00:03:57 +0200 Subject: [PATCH 749/872] Slight fix to test, but still fails validating written data after non-aligned writes --- src/systemcmds/tests/tests_file.c | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/src/systemcmds/tests/tests_file.c b/src/systemcmds/tests/tests_file.c index 372bc5c908..e9db4716d6 100644 --- a/src/systemcmds/tests/tests_file.c +++ b/src/systemcmds/tests/tests_file.c @@ -103,18 +103,35 @@ test_file(int argc, char *argv[]) perf_print_counter(wperf); perf_free(wperf); + close(fd); + + fd = open("/fs/microsd/testfile", O_RDONLY); + /* read back data for validation */ for (unsigned i = 0; i < iterations; i++) { int rret = read(fd, read_buf, 512); + + if (rret != 512) { + warn("READ ERROR!"); + break; + } /* compare value */ + bool compare_ok = true; for (int j = 0; j < 512; j++) { - if (read_buf[j] != write_buf[j + (i % 64)]) { + if (read_buf[j] != write_buf[j + 1/*+ (i % 64)*/]) { warnx("COMPARISON ERROR: byte %d, align shift: %d", j, (i % 64)); + compare_ok = false; + break; } } + if (!compare_ok) { + warnx("ABORTING FURTHER COMPARISON DUE TO ERROR"); + break; + } + } /* read back data for alignment checks */ From 13b07efc49584ea8b864403ff15bae9042ffb3af Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Oct 2013 00:12:39 +0200 Subject: [PATCH 750/872] added hw test, added better io debugging --- makefiles/config_px4fmu-v2_default.mk | 3 ++ src/drivers/px4io/px4io.cpp | 9 ++-- src/examples/hwtest/hwtest.c | 74 +++++++++++++++++++++++++++ src/examples/hwtest/module.mk | 40 +++++++++++++++ 4 files changed, 123 insertions(+), 3 deletions(-) create mode 100644 src/examples/hwtest/hwtest.c create mode 100644 src/examples/hwtest/module.mk diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index e12d61a535..66216f8fe2 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -128,6 +128,9 @@ MODULES += lib/geo # https://pixhawk.ethz.ch/px4/dev/debug_values #MODULES += examples/px4_mavlink_debug +# Hardware test +MODULES += examples/hwtest + # # Transitional support - add commands from the NuttX export archive. # diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 969dbefea2..f6d4f1ed40 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1545,7 +1545,7 @@ PX4IO::print_debug() int io_fd = -1; if (io_fd < 0) { - io_fd = ::open("/dev/ttyS0", O_RDONLY | O_NONBLOCK); + io_fd = ::open("/dev/ttyS0", O_RDONLY | O_NONBLOCK | O_NOCTTY); } /* read IO's output */ @@ -1555,7 +1555,7 @@ PX4IO::print_debug() fds[0].events = POLLIN; usleep(500); - int pret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), 10); + int pret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), 0); if (pret > 0) { int count; @@ -1606,6 +1606,8 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) memcpy(&msg->text[0], buf, count); buf += count; buflen -= count; + } else { + continue; } /* @@ -1644,7 +1646,8 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) /* ensure a closing newline */ msg->text[0] = '\n'; msg->text[1] = '\0'; - int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, 1); + + int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2); retries--; diff --git a/src/examples/hwtest/hwtest.c b/src/examples/hwtest/hwtest.c new file mode 100644 index 0000000000..06337be32d --- /dev/null +++ b/src/examples/hwtest/hwtest.c @@ -0,0 +1,74 @@ +/**************************************************************************** + * + * 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 hwtest.c + * + * Simple functional hardware test. + * + * @author Lorenz Meier + */ + +#include +#include +#include +#include +#include +#include + +__EXPORT int ex_hwtest_main(int argc, char *argv[]); + +int ex_hwtest_main(int argc, char *argv[]) +{ + struct actuator_controls_s actuators; + memset(&actuators, 0, sizeof(actuators)); + orb_advert_t actuator_pub_fd = orb_advertise(ORB_ID(actuator_controls_0), &actuators); + + int i; + float rcvalue = -1.0f; + hrt_abstime stime; + + while (true) { + stime = hrt_absolute_time(); + while (hrt_absolute_time() - stime < 1000000) { + for (i=0; i<8; i++) + actuators.control[i] = rcvalue; + actuators.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(actuator_controls_0), actuator_pub_fd, &actuators); + } + warnx("servos set to %.1f", rcvalue); + rcvalue *= -1.0f; + } + + return OK; +} diff --git a/src/examples/hwtest/module.mk b/src/examples/hwtest/module.mk new file mode 100644 index 0000000000..6e70863edb --- /dev/null +++ b/src/examples/hwtest/module.mk @@ -0,0 +1,40 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Hardware test example application +# + +MODULE_COMMAND = ex_hwtest + +SRCS = hwtest.c From 21dcdf11cf8f05d94b17cd0b5cce058b45ee3923 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Oct 2013 00:14:03 +0200 Subject: [PATCH 751/872] WIP, typo fix --- src/systemcmds/tests/tests_file.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/systemcmds/tests/tests_file.c b/src/systemcmds/tests/tests_file.c index e9db4716d6..58eeb6fdfd 100644 --- a/src/systemcmds/tests/tests_file.c +++ b/src/systemcmds/tests/tests_file.c @@ -82,7 +82,7 @@ test_file(int argc, char *argv[]) start = hrt_absolute_time(); for (unsigned i = 0; i < iterations; i++) { perf_begin(wperf); - int wret = write(fd, write_buf + (i % 64), 512); + int wret = write(fd, write_buf + 1/*+ (i % 64)*/, 512); if (wret != 512) { warn("WRITE ERROR!"); @@ -109,7 +109,7 @@ test_file(int argc, char *argv[]) /* read back data for validation */ for (unsigned i = 0; i < iterations; i++) { - int rret = read(fd, read_buf, 512); + int rret = read(fd, read_buf + 0, 512); if (rret != 512) { warn("READ ERROR!"); @@ -120,7 +120,7 @@ test_file(int argc, char *argv[]) bool compare_ok = true; for (int j = 0; j < 512; j++) { - if (read_buf[j] != write_buf[j + 1/*+ (i % 64)*/]) { + if ((read_buf + 0)[j] != write_buf[j + 1/*+ (i % 64)*/]) { warnx("COMPARISON ERROR: byte %d, align shift: %d", j, (i % 64)); compare_ok = false; break; From 8407677f20ecc7ea5374a0ded41263ede019d9f6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Oct 2013 00:15:39 +0200 Subject: [PATCH 752/872] Updated error message --- src/systemcmds/tests/tests_file.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/tests/tests_file.c b/src/systemcmds/tests/tests_file.c index 58eeb6fdfd..ab3dd78309 100644 --- a/src/systemcmds/tests/tests_file.c +++ b/src/systemcmds/tests/tests_file.c @@ -121,7 +121,7 @@ test_file(int argc, char *argv[]) for (int j = 0; j < 512; j++) { if ((read_buf + 0)[j] != write_buf[j + 1/*+ (i % 64)*/]) { - warnx("COMPARISON ERROR: byte %d, align shift: %d", j, (i % 64)); + warnx("COMPARISON ERROR: byte %d, align shift: %d", j, 1/*(i % 64)*/); compare_ok = false; break; } From e0e708241b965f364fd49fa0d2270de5ad517a70 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Oct 2013 08:34:08 +0200 Subject: [PATCH 753/872] More testing output --- src/systemcmds/tests/tests_file.c | 126 ++++++++++++++++++++++++++---- 1 file changed, 112 insertions(+), 14 deletions(-) diff --git a/src/systemcmds/tests/tests_file.c b/src/systemcmds/tests/tests_file.c index ab3dd78309..aa0e163d70 100644 --- a/src/systemcmds/tests/tests_file.c +++ b/src/systemcmds/tests/tests_file.c @@ -77,7 +77,7 @@ test_file(int argc, char *argv[]) int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); - warnx("testing aligned and unaligned writes - please wait.."); + warnx("testing unaligned writes - please wait.."); start = hrt_absolute_time(); for (unsigned i = 0; i < iterations; i++) { @@ -104,12 +104,11 @@ test_file(int argc, char *argv[]) perf_free(wperf); close(fd); - fd = open("/fs/microsd/testfile", O_RDONLY); /* read back data for validation */ for (unsigned i = 0; i < iterations; i++) { - int rret = read(fd, read_buf + 0, 512); + int rret = read(fd, read_buf, 512); if (rret != 512) { warn("READ ERROR!"); @@ -120,7 +119,7 @@ test_file(int argc, char *argv[]) bool compare_ok = true; for (int j = 0; j < 512; j++) { - if ((read_buf + 0)[j] != write_buf[j + 1/*+ (i % 64)*/]) { + if (read_buf[j] != write_buf[j + 1/*+ (i % 64)*/]) { warnx("COMPARISON ERROR: byte %d, align shift: %d", j, 1/*(i % 64)*/); compare_ok = false; break; @@ -134,22 +133,121 @@ test_file(int argc, char *argv[]) } - /* read back data for alignment checks */ - // for (unsigned i = 0; i < iterations; i++) { - // perf_begin(wperf); - // int rret = read(fd, buf + (i % 64), sizeof(buf)); - // fsync(fd); - // perf_end(wperf); - - // } + /* + * ALIGNED WRITES AND UNALIGNED READS + */ int ret = unlink("/fs/microsd/testfile"); + fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); + + warnx("testing aligned writes - please wait.."); + + start = hrt_absolute_time(); + for (unsigned i = 0; i < iterations; i++) { + perf_begin(wperf); + int wret = write(fd, write_buf, 512); + + if (wret != 512) { + warn("WRITE ERROR!"); + } + + perf_end(wperf); + + } + + fsync(fd); + + warnx("reading data aligned.."); + + close(fd); + fd = open("/fs/microsd/testfile", O_RDONLY); + + bool align_read_ok = true; + + /* read back data unaligned */ + for (unsigned i = 0; i < iterations; i++) { + int rret = read(fd, read_buf, 512); + + if (rret != 512) { + warn("READ ERROR!"); + break; + } + + /* compare value */ + bool compare_ok = true; + + for (int j = 0; j < 512; j++) { + if (read_buf[j] != write_buf[j]) { + warnx("COMPARISON ERROR: byte %d: %u != %u", j, (unsigned int)read_buf[j], (unsigned int)write_buf[j]); + align_read_ok = false; + break; + } + } + + if (!align_read_ok) { + warnx("ABORTING FURTHER COMPARISON DUE TO ERROR"); + break; + } + + } + + warnx("align read result: %s\n", (align_read_ok) ? "OK" : "ERROR"); + + warnx("reading data unaligned.."); + + close(fd); + fd = open("/fs/microsd/testfile", O_RDONLY); + + bool unalign_read_ok = true; + int unalign_read_err_count = 0; + + memset(read_buf, 0, sizeof(read_buf)); + read_buf[0] = 0; + + warnx("printing first 10 pairs:"); + + uint8_t *read_ptr = &read_buf[1]; + + /* read back data unaligned */ + for (unsigned i = 0; i < iterations; i++) { + int rret = read(fd, read_ptr, 512); + + warnx("first byte: %u (should be zero)", (unsigned int)read_buf[0]); + + if (rret != 512) { + warn("READ ERROR!"); + break; + } + + for (int j = 0; j < 512; j++) { + + if (i == 0 && j < 10) { + warnx("read: %u, expect: %u", (unsigned int)(read_buf + 1)[j], (unsigned int)write_buf[j]); + } + + if ((read_buf + 1)[j] != write_buf[j]) { + warnx("COMPARISON ERROR: byte %d, align shift: %d: %u != %u", j, 1/*(i % 64)*/, (unsigned int)read_buf[j], (unsigned int)write_buf[j]); + unalign_read_ok = false; + unalign_read_err_count++; + + if (unalign_read_err_count > 10) + break; + } + } + + if (!unalign_read_ok) { + warnx("ABORTING FURTHER COMPARISON DUE TO ERROR"); + break; + } + + } + + ret = unlink("/fs/microsd/testfile"); + close(fd); if (ret) err(1, "UNLINKING FILE FAILED"); - close(fd); - /* list directory */ DIR *d; struct dirent *dir; From 6ffa2955b919a18abd94dd990c3447dfc68dd5c2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Oct 2013 08:37:24 +0200 Subject: [PATCH 754/872] Typo in debug output --- src/systemcmds/tests/tests_file.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/tests/tests_file.c b/src/systemcmds/tests/tests_file.c index aa0e163d70..14532e48c8 100644 --- a/src/systemcmds/tests/tests_file.c +++ b/src/systemcmds/tests/tests_file.c @@ -226,7 +226,7 @@ test_file(int argc, char *argv[]) } if ((read_buf + 1)[j] != write_buf[j]) { - warnx("COMPARISON ERROR: byte %d, align shift: %d: %u != %u", j, 1/*(i % 64)*/, (unsigned int)read_buf[j], (unsigned int)write_buf[j]); + warnx("COMPARISON ERROR: byte %d, align shift: %d: %u != %u", j, 1/*(i % 64)*/, (unsigned int)read_ptr[j], (unsigned int)write_buf[j]); unalign_read_ok = false; unalign_read_err_count++; From 6745a910718b2a5fed313187f77a04de383aa8f7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Oct 2013 08:42:54 +0200 Subject: [PATCH 755/872] Added alignment attribute --- src/systemcmds/tests/tests_file.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/systemcmds/tests/tests_file.c b/src/systemcmds/tests/tests_file.c index 14532e48c8..3c0c48cfe6 100644 --- a/src/systemcmds/tests/tests_file.c +++ b/src/systemcmds/tests/tests_file.c @@ -63,7 +63,7 @@ test_file(int argc, char *argv[]) return 1; } - uint8_t write_buf[512 + 64]; + uint8_t write_buf[512 + 64] __attribute__((aligned(64))); /* fill write buffer with known values */ for (int i = 0; i < sizeof(write_buf); i++) { @@ -71,7 +71,7 @@ test_file(int argc, char *argv[]) write_buf[i] = i+11; } - uint8_t read_buf[512 + 64]; + uint8_t read_buf[512 + 64] __attribute__((aligned(64))); hrt_abstime start, end; perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes (aligned)"); @@ -87,8 +87,8 @@ test_file(int argc, char *argv[]) if (wret != 512) { warn("WRITE ERROR!"); - if ((0x3 & (uintptr_t)(write_buf + (i % 64)))) - warnx("memory is unaligned, align shift: %d", (i % 64)); + if ((0x3 & (uintptr_t)(write_buf + 1 /* (i % 64)*/))) + warnx("memory is unaligned, align shift: %d", 1/*(i % 64)*/); } From 44deeb85c09451865c7d869f495a31f1b4348fec Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Oct 2013 21:15:03 +0200 Subject: [PATCH 756/872] We compile with GCC 4.7.4 --- .../att_pos_estimator_ekf/KalmanNav.hpp | 26 +++++++------- src/modules/controllib/block/BlockParam.hpp | 29 +++++++++++---- src/modules/controllib/blocks.hpp | 36 +++++++++---------- 3 files changed, 54 insertions(+), 37 deletions(-) diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp index f01ee03553..799fc2fb9e 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp @@ -166,19 +166,19 @@ protected: double lat, lon; /**< lat, lon radians */ float alt; /**< altitude, meters */ // parameters - control::BlockParam _vGyro; /**< gyro process noise */ - control::BlockParam _vAccel; /**< accelerometer process noise */ - control::BlockParam _rMag; /**< magnetometer measurement noise */ - control::BlockParam _rGpsVel; /**< gps velocity measurement noise */ - control::BlockParam _rGpsPos; /**< gps position measurement noise */ - control::BlockParam _rGpsAlt; /**< gps altitude measurement noise */ - control::BlockParam _rPressAlt; /**< press altitude measurement noise */ - control::BlockParam _rAccel; /**< accelerometer measurement noise */ - control::BlockParam _magDip; /**< magnetic inclination with level */ - control::BlockParam _magDec; /**< magnetic declination, clockwise rotation */ - control::BlockParam _g; /**< gravitational constant */ - control::BlockParam _faultPos; /**< fault detection threshold for position */ - control::BlockParam _faultAtt; /**< fault detection threshold for attitude */ + control::BlockParamFloat _vGyro; /**< gyro process noise */ + control::BlockParamFloat _vAccel; /**< accelerometer process noise */ + control::BlockParamFloat _rMag; /**< magnetometer measurement noise */ + control::BlockParamFloat _rGpsVel; /**< gps velocity measurement noise */ + control::BlockParamFloat _rGpsPos; /**< gps position measurement noise */ + control::BlockParamFloat _rGpsAlt; /**< gps altitude measurement noise */ + control::BlockParamFloat _rPressAlt; /**< press altitude measurement noise */ + control::BlockParamFloat _rAccel; /**< accelerometer measurement noise */ + control::BlockParamFloat _magDip; /**< magnetic inclination with level */ + control::BlockParamFloat _magDec; /**< magnetic declination, clockwise rotation */ + control::BlockParamFloat _g; /**< gravitational constant */ + control::BlockParamFloat _faultPos; /**< fault detection threshold for position */ + control::BlockParamFloat _faultAtt; /**< fault detection threshold for attitude */ // status bool _attitudeInitialized; bool _positionInitialized; diff --git a/src/modules/controllib/block/BlockParam.hpp b/src/modules/controllib/block/BlockParam.hpp index 58a9bfc0d9..36bc8c24ba 100644 --- a/src/modules/controllib/block/BlockParam.hpp +++ b/src/modules/controllib/block/BlockParam.hpp @@ -69,22 +69,39 @@ protected: /** * Parameters that are tied to blocks for updating and nameing. */ -template -class __EXPORT BlockParam : public BlockParamBase + +class __EXPORT BlockParamFloat : public BlockParamBase { public: - BlockParam(Block *block, const char *name, bool parent_prefix=true) : + BlockParamFloat(Block *block, const char *name, bool parent_prefix=true) : BlockParamBase(block, name, parent_prefix), _val() { update(); } - T get() { return _val; } - void set(T val) { _val = val; } + float get() { return _val; } + void set(float val) { _val = val; } void update() { if (_handle != PARAM_INVALID) param_get(_handle, &_val); } protected: - T _val; + float _val; +}; + +class __EXPORT BlockParamInt : public BlockParamBase +{ +public: + BlockParamInt(Block *block, const char *name, bool parent_prefix=true) : + BlockParamBase(block, name, parent_prefix), + _val() { + update(); + } + int get() { return _val; } + void set(int val) { _val = val; } + void update() { + if (_handle != PARAM_INVALID) param_get(_handle, &_val); + } +protected: + int _val; }; } // namespace control diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp index fefe99702d..66e9290381 100644 --- a/src/modules/controllib/blocks.hpp +++ b/src/modules/controllib/blocks.hpp @@ -74,8 +74,8 @@ public: float getMax() { return _max.get(); } protected: // attributes - BlockParam _min; - BlockParam _max; + control::BlockParamFloat _min; + control::BlockParamFloat _max; }; int __EXPORT blockLimitTest(); @@ -99,7 +99,7 @@ public: float getMax() { return _max.get(); } protected: // attributes - BlockParam _max; + control::BlockParamFloat _max; }; int __EXPORT blockLimitSymTest(); @@ -126,7 +126,7 @@ public: protected: // attributes float _state; - BlockParam _fCut; + control::BlockParamFloat _fCut; }; int __EXPORT blockLowPassTest(); @@ -157,7 +157,7 @@ protected: // attributes float _u; /**< previous input */ float _y; /**< previous output */ - BlockParam _fCut; /**< cut-off frequency, Hz */ + control::BlockParamFloat _fCut; /**< cut-off frequency, Hz */ }; int __EXPORT blockHighPassTest(); @@ -273,7 +273,7 @@ public: // accessors float getKP() { return _kP.get(); } protected: - BlockParam _kP; + control::BlockParamFloat _kP; }; int __EXPORT blockPTest(); @@ -303,8 +303,8 @@ public: BlockIntegral &getIntegral() { return _integral; } private: BlockIntegral _integral; - BlockParam _kP; - BlockParam _kI; + control::BlockParamFloat _kP; + control::BlockParamFloat _kI; }; int __EXPORT blockPITest(); @@ -334,8 +334,8 @@ public: BlockDerivative &getDerivative() { return _derivative; } private: BlockDerivative _derivative; - BlockParam _kP; - BlockParam _kD; + control::BlockParamFloat _kP; + control::BlockParamFloat _kD; }; int __EXPORT blockPDTest(); @@ -372,9 +372,9 @@ private: // attributes BlockIntegral _integral; BlockDerivative _derivative; - BlockParam _kP; - BlockParam _kI; - BlockParam _kD; + control::BlockParamFloat _kP; + control::BlockParamFloat _kI; + control::BlockParamFloat _kD; }; int __EXPORT blockPIDTest(); @@ -404,7 +404,7 @@ public: float get() { return _val; } private: // attributes - BlockParam _trim; + control::BlockParamFloat _trim; BlockLimit _limit; float _val; }; @@ -439,8 +439,8 @@ public: float getMax() { return _max.get(); } private: // attributes - BlockParam _min; - BlockParam _max; + control::BlockParamFloat _min; + control::BlockParamFloat _max; }; int __EXPORT blockRandUniformTest(); @@ -486,8 +486,8 @@ public: float getStdDev() { return _stdDev.get(); } private: // attributes - BlockParam _mean; - BlockParam _stdDev; + control::BlockParamFloat _mean; + control::BlockParamFloat _stdDev; }; int __EXPORT blockRandGaussTest(); From 42c412f6ddf3963c8a9bbef5ba37896a24e1f0fa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Oct 2013 21:15:44 +0200 Subject: [PATCH 757/872] Makefile cleanup --- makefiles/config_px4fmu-v1_default.mk | 5 ++++- makefiles/config_px4fmu-v2_default.mk | 6 +++++- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 1dd9490763..46640f3c51 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -134,7 +134,10 @@ MODULES += lib/geo # Tutorial code from # https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control -MODULES += examples/fixedwing_control +#MODULES += examples/fixedwing_control + +# Hardware test +#MODULES += examples/hwtest # # Transitional support - add commands from the NuttX export archive. diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 66216f8fe2..bb589cb9f3 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -128,8 +128,12 @@ MODULES += lib/geo # https://pixhawk.ethz.ch/px4/dev/debug_values #MODULES += examples/px4_mavlink_debug +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control +#MODULES += examples/fixedwing_control + # Hardware test -MODULES += examples/hwtest +#MODULES += examples/hwtest # # Transitional support - add commands from the NuttX export archive. From 73f507daa6f31443e1938556c08aee016efaf345 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Oct 2013 09:12:30 +0200 Subject: [PATCH 758/872] Make tests file complete up to 64 byte shifts and 1.5 K chunks --- src/systemcmds/tests/tests_file.c | 344 +++++++++++++++--------------- 1 file changed, 170 insertions(+), 174 deletions(-) diff --git a/src/systemcmds/tests/tests_file.c b/src/systemcmds/tests/tests_file.c index 3c0c48cfe6..a41d26bda7 100644 --- a/src/systemcmds/tests/tests_file.c +++ b/src/systemcmds/tests/tests_file.c @@ -54,7 +54,8 @@ int test_file(int argc, char *argv[]) { - const iterations = 200; + const unsigned iterations = 100; + const unsigned alignments = 65; /* check if microSD card is mounted */ struct stat buffer; @@ -63,194 +64,189 @@ test_file(int argc, char *argv[]) return 1; } - uint8_t write_buf[512 + 64] __attribute__((aligned(64))); + /* perform tests for a range of chunk sizes */ + unsigned chunk_sizes[] = {1, 5, 8, 16, 32, 33, 64, 70, 128, 133, 256, 300, 512, 555, 1024, 1500}; - /* fill write buffer with known values */ - for (int i = 0; i < sizeof(write_buf); i++) { - /* this will wrap, but we just need a known value with spacing */ - write_buf[i] = i+11; - } + for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) { - uint8_t read_buf[512 + 64] __attribute__((aligned(64))); - hrt_abstime start, end; - perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes (aligned)"); + printf("\n====== FILE TEST: %u bytes chunks ======", chunk_sizes[c]); - int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); + for (unsigned a = 0; a < alignments; a++) { - warnx("testing unaligned writes - please wait.."); + warnx("----- alignment test: %u bytes -----", a); - start = hrt_absolute_time(); - for (unsigned i = 0; i < iterations; i++) { - perf_begin(wperf); - int wret = write(fd, write_buf + 1/*+ (i % 64)*/, 512); + uint8_t write_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); - if (wret != 512) { - warn("WRITE ERROR!"); - - if ((0x3 & (uintptr_t)(write_buf + 1 /* (i % 64)*/))) - warnx("memory is unaligned, align shift: %d", 1/*(i % 64)*/); - - } - - fsync(fd); - perf_end(wperf); - - } - end = hrt_absolute_time(); - - warnx("%dKiB in %llu microseconds", iterations / 2, end - start); - - perf_print_counter(wperf); - perf_free(wperf); - - close(fd); - fd = open("/fs/microsd/testfile", O_RDONLY); - - /* read back data for validation */ - for (unsigned i = 0; i < iterations; i++) { - int rret = read(fd, read_buf, 512); - - if (rret != 512) { - warn("READ ERROR!"); - break; - } - - /* compare value */ - bool compare_ok = true; - - for (int j = 0; j < 512; j++) { - if (read_buf[j] != write_buf[j + 1/*+ (i % 64)*/]) { - warnx("COMPARISON ERROR: byte %d, align shift: %d", j, 1/*(i % 64)*/); - compare_ok = false; - break; - } - } - - if (!compare_ok) { - warnx("ABORTING FURTHER COMPARISON DUE TO ERROR"); - break; - } - - } - - /* - * ALIGNED WRITES AND UNALIGNED READS - */ - - int ret = unlink("/fs/microsd/testfile"); - fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); - - warnx("testing aligned writes - please wait.."); - - start = hrt_absolute_time(); - for (unsigned i = 0; i < iterations; i++) { - perf_begin(wperf); - int wret = write(fd, write_buf, 512); - - if (wret != 512) { - warn("WRITE ERROR!"); - } - - perf_end(wperf); - - } - - fsync(fd); - - warnx("reading data aligned.."); - - close(fd); - fd = open("/fs/microsd/testfile", O_RDONLY); - - bool align_read_ok = true; - - /* read back data unaligned */ - for (unsigned i = 0; i < iterations; i++) { - int rret = read(fd, read_buf, 512); - - if (rret != 512) { - warn("READ ERROR!"); - break; - } - - /* compare value */ - bool compare_ok = true; - - for (int j = 0; j < 512; j++) { - if (read_buf[j] != write_buf[j]) { - warnx("COMPARISON ERROR: byte %d: %u != %u", j, (unsigned int)read_buf[j], (unsigned int)write_buf[j]); - align_read_ok = false; - break; - } - } - - if (!align_read_ok) { - warnx("ABORTING FURTHER COMPARISON DUE TO ERROR"); - break; - } - - } - - warnx("align read result: %s\n", (align_read_ok) ? "OK" : "ERROR"); - - warnx("reading data unaligned.."); - - close(fd); - fd = open("/fs/microsd/testfile", O_RDONLY); - - bool unalign_read_ok = true; - int unalign_read_err_count = 0; - - memset(read_buf, 0, sizeof(read_buf)); - read_buf[0] = 0; - - warnx("printing first 10 pairs:"); - - uint8_t *read_ptr = &read_buf[1]; - - /* read back data unaligned */ - for (unsigned i = 0; i < iterations; i++) { - int rret = read(fd, read_ptr, 512); - - warnx("first byte: %u (should be zero)", (unsigned int)read_buf[0]); - - if (rret != 512) { - warn("READ ERROR!"); - break; - } - - for (int j = 0; j < 512; j++) { - - if (i == 0 && j < 10) { - warnx("read: %u, expect: %u", (unsigned int)(read_buf + 1)[j], (unsigned int)write_buf[j]); + /* fill write buffer with known values */ + for (int i = 0; i < sizeof(write_buf); i++) { + /* this will wrap, but we just need a known value with spacing */ + write_buf[i] = i+11; } - if ((read_buf + 1)[j] != write_buf[j]) { - warnx("COMPARISON ERROR: byte %d, align shift: %d: %u != %u", j, 1/*(i % 64)*/, (unsigned int)read_ptr[j], (unsigned int)write_buf[j]); - unalign_read_ok = false; - unalign_read_err_count++; + uint8_t read_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); + hrt_abstime start, end; + //perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes (aligned)"); + + int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); + + warnx("testing unaligned writes - please wait.."); + + start = hrt_absolute_time(); + for (unsigned i = 0; i < iterations; i++) { + //perf_begin(wperf); + int wret = write(fd, write_buf + a, chunk_sizes[c]); + + if (wret != chunk_sizes[c]) { + warn("WRITE ERROR!"); + + if ((0x3 & (uintptr_t)(write_buf + a))) + errx(1, "memory is unaligned, align shift: %d", a); + + } + + fsync(fd); + //perf_end(wperf); + + } + end = hrt_absolute_time(); + + //warnx("%dKiB in %llu microseconds", iterations / 2, end - start); + + //perf_print_counter(wperf); + //perf_free(wperf); + + close(fd); + fd = open("/fs/microsd/testfile", O_RDONLY); + + /* read back data for validation */ + for (unsigned i = 0; i < iterations; i++) { + int rret = read(fd, read_buf, chunk_sizes[c]); + + if (rret != chunk_sizes[c]) { + errx(1, "READ ERROR!"); + } - if (unalign_read_err_count > 10) - break; + /* compare value */ + bool compare_ok = true; + + for (int j = 0; j < chunk_sizes[c]; j++) { + if (read_buf[j] != write_buf[j + a]) { + warnx("COMPARISON ERROR: byte %d, align shift: %d", j, a); + compare_ok = false; + break; + } + } + + if (!compare_ok) { + errx(1, "ABORTING FURTHER COMPARISON DUE TO ERROR"); + } + } - } - if (!unalign_read_ok) { - warnx("ABORTING FURTHER COMPARISON DUE TO ERROR"); - break; - } + /* + * ALIGNED WRITES AND UNALIGNED READS + */ + close(fd); + int ret = unlink("/fs/microsd/testfile"); + fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); + + warnx("testing aligned writes - please wait.."); + + start = hrt_absolute_time(); + for (unsigned i = 0; i < iterations; i++) { + int wret = write(fd, write_buf, chunk_sizes[c]); + + if (wret != chunk_sizes[c]) { + err(1, "WRITE ERROR!"); + } + + } + + fsync(fd); + + warnx("reading data aligned.."); + + close(fd); + fd = open("/fs/microsd/testfile", O_RDONLY); + + bool align_read_ok = true; + + /* read back data unaligned */ + for (unsigned i = 0; i < iterations; i++) { + int rret = read(fd, read_buf, chunk_sizes[c]); + + if (rret != chunk_sizes[c]) { + err(1, "READ ERROR!"); + } + + /* compare value */ + bool compare_ok = true; + + for (int j = 0; j < chunk_sizes[c]; j++) { + if (read_buf[j] != write_buf[j]) { + warnx("COMPARISON ERROR: byte %d: %u != %u", j, (unsigned int)read_buf[j], (unsigned int)write_buf[j]); + align_read_ok = false; + break; + } + } + + if (!align_read_ok) { + errx(1, "ABORTING FURTHER COMPARISON DUE TO ERROR"); + } + + } + + warnx("align read result: %s\n", (align_read_ok) ? "OK" : "ERROR"); + + warnx("reading data unaligned.."); + + close(fd); + fd = open("/fs/microsd/testfile", O_RDONLY); + + bool unalign_read_ok = true; + int unalign_read_err_count = 0; + + memset(read_buf, 0, sizeof(read_buf)); + + /* read back data unaligned */ + for (unsigned i = 0; i < iterations; i++) { + int rret = read(fd, read_buf + a, chunk_sizes[c]); + + if (rret != chunk_sizes[c]) { + err(1, "READ ERROR!"); + } + + for (int j = 0; j < chunk_sizes[c]; j++) { + + if ((read_buf + a)[j] != write_buf[j]) { + warnx("COMPARISON ERROR: byte %d, align shift: %d: %u != %u", j, a, (unsigned int)read_buf[j + a], (unsigned int)write_buf[j]); + unalign_read_ok = false; + unalign_read_err_count++; + + if (unalign_read_err_count > 10) + break; + } + } + + if (!unalign_read_ok) { + errx(1, "ABORTING FURTHER COMPARISON DUE TO ERROR"); + } + + } + + ret = unlink("/fs/microsd/testfile"); + close(fd); + + if (ret) + err(1, "UNLINKING FILE FAILED"); + + } } - ret = unlink("/fs/microsd/testfile"); - close(fd); - - if (ret) - err(1, "UNLINKING FILE FAILED"); - /* list directory */ - DIR *d; - struct dirent *dir; + DIR *d; + struct dirent *dir; d = opendir("/fs/microsd"); if (d) { From d144213527f9bdab92da9f81bb7647931314fa01 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Oct 2013 09:13:54 +0200 Subject: [PATCH 759/872] Added non-binary number between 8 and 16 --- src/systemcmds/tests/tests_file.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/tests/tests_file.c b/src/systemcmds/tests/tests_file.c index a41d26bda7..46e495f586 100644 --- a/src/systemcmds/tests/tests_file.c +++ b/src/systemcmds/tests/tests_file.c @@ -65,7 +65,7 @@ test_file(int argc, char *argv[]) } /* perform tests for a range of chunk sizes */ - unsigned chunk_sizes[] = {1, 5, 8, 16, 32, 33, 64, 70, 128, 133, 256, 300, 512, 555, 1024, 1500}; + unsigned chunk_sizes[] = {1, 5, 8, 13, 16, 32, 33, 64, 70, 128, 133, 256, 300, 512, 555, 1024, 1500}; for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) { From d04321bcf86d75f3f948998e871140d92967f1b6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Oct 2013 09:14:57 +0200 Subject: [PATCH 760/872] Hotfix: Typo --- src/systemcmds/tests/tests_file.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/tests/tests_file.c b/src/systemcmds/tests/tests_file.c index 46e495f586..d8eac5efc6 100644 --- a/src/systemcmds/tests/tests_file.c +++ b/src/systemcmds/tests/tests_file.c @@ -69,7 +69,7 @@ test_file(int argc, char *argv[]) for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) { - printf("\n====== FILE TEST: %u bytes chunks ======", chunk_sizes[c]); + printf("\n====== FILE TEST: %u bytes chunks ======\n", chunk_sizes[c]); for (unsigned a = 0; a < alignments; a++) { From 1306c9de7b946783ff1143bb42a33734e9380e2c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Oct 2013 09:28:44 +0200 Subject: [PATCH 761/872] Output improvements --- src/systemcmds/tests/tests_file.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/systemcmds/tests/tests_file.c b/src/systemcmds/tests/tests_file.c index d8eac5efc6..7f3a654bf7 100644 --- a/src/systemcmds/tests/tests_file.c +++ b/src/systemcmds/tests/tests_file.c @@ -73,6 +73,7 @@ test_file(int argc, char *argv[]) for (unsigned a = 0; a < alignments; a++) { + printf("\n"); warnx("----- alignment test: %u bytes -----", a); uint8_t write_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); From e2fef6b374e14f8c0f383557cf3569f8265ba034 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 11 Oct 2013 10:16:45 +0200 Subject: [PATCH 762/872] Use unsigned for channel counts --- src/drivers/drv_pwm_output.h | 2 +- src/drivers/hil/hil.cpp | 2 +- src/modules/uORB/topics/actuator_outputs.h | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 5a3a126d04..3357e67a50 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -99,7 +99,7 @@ typedef uint16_t servo_position_t; struct pwm_output_values { /** desired pulse widths for each of the supported channels */ servo_position_t values[PWM_OUTPUT_MAX_CHANNELS]; - int channel_count; + unsigned channel_count; }; /* diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index 6563c3446e..c1d73dd87f 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -404,7 +404,7 @@ HIL::task_main() for (unsigned i = 0; i < num_outputs; i++) { /* last resort: catch NaN, INF and out-of-band errors */ - if (i < (unsigned)outputs.noutputs && + if (i < outputs.noutputs && isfinite(outputs.output[i]) && outputs.output[i] >= -1.0f && outputs.output[i] <= 1.0f) { diff --git a/src/modules/uORB/topics/actuator_outputs.h b/src/modules/uORB/topics/actuator_outputs.h index 30895ca83f..4461404230 100644 --- a/src/modules/uORB/topics/actuator_outputs.h +++ b/src/modules/uORB/topics/actuator_outputs.h @@ -60,7 +60,7 @@ struct actuator_outputs_s { uint64_t timestamp; /**< output timestamp in us since system boot */ float output[NUM_ACTUATOR_OUTPUTS]; /**< output data, in natural output units */ - int noutputs; /**< valid outputs */ + unsigned noutputs; /**< valid outputs */ }; /** From 3dc2bdfa22f0ac152392299628e037e3a6120c2e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 11 Oct 2013 10:19:50 +0200 Subject: [PATCH 763/872] Changed pwm_limit interface a bit --- src/modules/px4iofirmware/mixer.cpp | 4 +--- src/modules/systemlib/pwm_limit/pwm_limit.c | 19 ++++++++++--------- src/modules/systemlib/pwm_limit/pwm_limit.h | 8 +++----- 3 files changed, 14 insertions(+), 17 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 5232f9b963..05897b4ce6 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -197,9 +197,7 @@ mixer_tick(void) /* mix */ mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT); - pwm_limit.nchannels = mixed; - - pwm_limit_calc(should_arm, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) r_page_servos[i] = 0; diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c index f67b5edf25..3187a4fa2e 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.c +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -45,15 +45,14 @@ #include #include -__EXPORT void pwm_limit_init(pwm_limit_t *limit) +void pwm_limit_init(pwm_limit_t *limit) { - limit->nchannels = 0; limit->state = LIMIT_STATE_OFF; limit->time_armed = 0; return; } -__EXPORT void pwm_limit_calc(const bool armed, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output_requested, uint16_t *effective_pwm, pwm_limit_t *limit) +void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, float *output, uint16_t *effective_pwm, pwm_limit_t *limit) { /* first evaluate state changes */ switch (limit->state) { @@ -89,24 +88,26 @@ __EXPORT void pwm_limit_calc(const bool armed, const uint16_t *disarmed_pwm, con switch (limit->state) { case LIMIT_STATE_OFF: case LIMIT_STATE_INIT: - for (unsigned i=0; inchannels; i++) { + for (unsigned i=0; itime_armed)*10000 / RAMP_TIME_US; - for (unsigned i=0; inchannels; i++) { + for (unsigned i=0; i min_pwm[i] ? temp_pwm : min_pwm[i])*progress)/10000; - + output[i] = (float)progress/10000.0f * output[i]; } break; case LIMIT_STATE_ON: - for (unsigned i=0; inchannels; i++) { - effective_pwm[i] = output_requested[i] * (max_pwm[i] - min_pwm[i])/2 + (max_pwm[i] + min_pwm[i])/2; + for (unsigned i=0; i #include -__BEGIN_DECLS - /* * time for the ESCs to initialize * (this is not actually needed if PWM is sent right after boot) @@ -66,13 +64,13 @@ typedef struct { LIMIT_STATE_ON } state; uint64_t time_armed; - unsigned nchannels; } pwm_limit_t; +__BEGIN_DECLS + __EXPORT void pwm_limit_init(pwm_limit_t *limit); -__EXPORT void pwm_limit_calc(const bool armed, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output_requested, uint16_t *effective_pwm, pwm_limit_t *limit); - +__EXPORT void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, float *output, uint16_t *effective_pwm, pwm_limit_t *limit); __END_DECLS From 96111a67a65486ce8b99987e51fb11da54789379 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 11 Oct 2013 10:21:22 +0200 Subject: [PATCH 764/872] Tought the fmu driver the new pwm limit interface --- src/drivers/px4fmu/fmu.cpp | 146 ++++++++++++++++++++++++++++++----- src/drivers/px4fmu/module.mk | 3 +- 2 files changed, 127 insertions(+), 22 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 3fc75eb29f..7f30487bff 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -59,11 +59,12 @@ #include #include -# include +#include #include #include #include +#include #include #include @@ -72,7 +73,7 @@ #include #include -#include + #ifdef HRT_PPM_CHANNEL # include #endif @@ -100,7 +101,7 @@ public: int set_pwm_alt_channels(uint32_t channels); private: - static const unsigned _max_actuators = 4; + static const unsigned _max_actuators = 6; Mode _mode; unsigned _pwm_default_rate; @@ -122,6 +123,11 @@ private: actuator_controls_s _controls; + pwm_limit_t _pwm_limit; + uint16_t _disarmed_pwm[_max_actuators]; + uint16_t _min_pwm[_max_actuators]; + uint16_t _max_pwm[_max_actuators]; + static void task_main_trampoline(int argc, char *argv[]); void task_main() __attribute__((noreturn)); @@ -203,7 +209,10 @@ PX4FMU::PX4FMU() : _primary_pwm_device(false), _task_should_exit(false), _armed(false), - _mixers(nullptr) + _mixers(nullptr), + _disarmed_pwm({0}), + _min_pwm({PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN}), + _max_pwm({PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX}) { _debug_enabled = true; } @@ -457,6 +466,9 @@ PX4FMU::task_main() rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM; #endif + /* initialize PWM limit lib */ + pwm_limit_init(&_pwm_limit); + log("starting"); /* loop until killed */ @@ -530,32 +542,35 @@ PX4FMU::task_main() outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); outputs.timestamp = hrt_absolute_time(); - // XXX output actual limited values - memcpy(&controls_effective, &_controls, sizeof(controls_effective)); - - orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective); - /* iterate actuators */ for (unsigned i = 0; i < num_outputs; i++) { - /* last resort: catch NaN, INF and out-of-band errors */ - if (i < outputs.noutputs && - isfinite(outputs.output[i]) && - outputs.output[i] >= -1.0f && - outputs.output[i] <= 1.0f) { - /* scale for PWM output 900 - 2100us */ - outputs.output[i] = 1500 + (600 * outputs.output[i]); - } else { + if (i >= outputs.noutputs || + !isfinite(outputs.output[i]) || + outputs.output[i] < -1.0f || + outputs.output[i] > 1.0f) { /* * Value is NaN, INF or out of band - set to the minimum value. * This will be clearly visible on the servo status and will limit the risk of accidentally * spinning motors. It would be deadly in flight. */ - outputs.output[i] = 900; + outputs.output[i] = -1.0f; } + } - /* output to the servo */ - up_pwm_servo_set(i, outputs.output[i]); + uint16_t pwm_limited[num_outputs]; + + pwm_limit_calc(_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit); + + /* output actual limited values */ + for (unsigned i = 0; i < num_outputs; i++) { + controls_effective.control_effective[i] = (float)pwm_limited[i]; + } + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective); + + /* output to the servos */ + for (unsigned i = 0; i < num_outputs; i++) { + up_pwm_servo_set(i, pwm_limited[i]); } /* and publish for anyone that cares to see */ @@ -705,6 +720,95 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) *(uint32_t *)arg = _pwm_alt_rate_channels; break; + case PWM_SERVO_SET_DISARMED_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values*)arg; + /* discard if too many values are sent */ + if (pwm->channel_count > _max_actuators) { + ret = -EINVAL; + break; + } + for (unsigned i = 0; i < pwm->channel_count; i++) { + if (pwm->values[i] == 0) { + /* ignore 0 */ + } else if (pwm->values[i] > PWM_MAX) { + _disarmed_pwm[i] = PWM_MAX; + } else if (pwm->values[i] < PWM_MIN) { + _disarmed_pwm[i] = PWM_MIN; + } else { + _disarmed_pwm[i] = pwm->values[i]; + } + } + break; + } + case PWM_SERVO_GET_DISARMED_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values*)arg; + for (unsigned i = 0; i < _max_actuators; i++) { + pwm->values[i] = _disarmed_pwm[i]; + } + pwm->channel_count = _max_actuators; + break; + } + + case PWM_SERVO_SET_MIN_PWM: { + struct pwm_output_values* pwm = (struct pwm_output_values*)arg; + /* discard if too many values are sent */ + if (pwm->channel_count > _max_actuators) { + ret = -EINVAL; + break; + } + for (unsigned i = 0; i < pwm->channel_count; i++) { + if (pwm->values[i] == 0) { + /* ignore 0 */ + } else if (pwm->values[i] > PWM_HIGHEST_MIN) { + _min_pwm[i] = PWM_HIGHEST_MIN; + } else if (pwm->values[i] < PWM_MIN) { + _min_pwm[i] = PWM_MIN; + } else { + _min_pwm[i] = pwm->values[i]; + } + } + break; + } + case PWM_SERVO_GET_MIN_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values*)arg; + for (unsigned i = 0; i < _max_actuators; i++) { + pwm->values[i] = _min_pwm[i]; + } + pwm->channel_count = _max_actuators; + arg = (unsigned long)&pwm; + break; + } + + case PWM_SERVO_SET_MAX_PWM: { + struct pwm_output_values* pwm = (struct pwm_output_values*)arg; + /* discard if too many values are sent */ + if (pwm->channel_count > _max_actuators) { + ret = -EINVAL; + break; + } + for (unsigned i = 0; i < pwm->channel_count; i++) { + if (pwm->values[i] == 0) { + /* ignore 0 */ + } else if (pwm->values[i] < PWM_LOWEST_MAX) { + _max_pwm[i] = PWM_LOWEST_MAX; + } else if (pwm->values[i] > PWM_MAX) { + _max_pwm[i] = PWM_MAX; + } else { + _max_pwm[i] = pwm->values[i]; + } + } + break; + } + case PWM_SERVO_GET_MAX_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values*)arg; + for (unsigned i = 0; i < _max_actuators; i++) { + pwm->values[i] = _max_pwm[i]; + } + pwm->channel_count = _max_actuators; + arg = (unsigned long)&pwm; + break; + } + case PWM_SERVO_SET(5): case PWM_SERVO_SET(4): if (_mode < MODE_6PWM) { @@ -1148,7 +1252,7 @@ test(void) } } else { // and use write interface for the other direction - int ret = write(fd, servos, sizeof(servos)); + 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); } diff --git a/src/drivers/px4fmu/module.mk b/src/drivers/px4fmu/module.mk index 05bc7a5b33..d918abd572 100644 --- a/src/drivers/px4fmu/module.mk +++ b/src/drivers/px4fmu/module.mk @@ -3,4 +3,5 @@ # MODULE_COMMAND = fmu -SRCS = fmu.cpp +SRCS = fmu.cpp \ + ../../modules/systemlib/pwm_limit/pwm_limit.c From 6b079f41b27442d2efbe5467be2be0d8607b6746 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 11 Oct 2013 10:21:37 +0200 Subject: [PATCH 765/872] Small warning fix --- 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 18c9ef0bd0..8a6390ad78 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2400,7 +2400,7 @@ px4io_main(int argc, char *argv[]) for (unsigned 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) { + if ((unsigned)argc > i + 2) { failsafe[i] = atoi(argv[i+2]); if (failsafe[i] < PWM_MIN || failsafe[i] > PWM_MAX) { errx(1, "value out of range of %d < value < %d. Aborting.", PWM_MIN, PWM_MAX); From 9cd3c40606f023a7943b1418a748abb046e36ecb Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 11 Oct 2013 10:48:49 +0200 Subject: [PATCH 766/872] Set the PWM values only once or continuous if specified --- src/systemcmds/pwm/pwm.c | 63 +++++++++++++++++++++++++--------------- 1 file changed, 40 insertions(+), 23 deletions(-) diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 25f8c4e753..b3975de9d0 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -72,7 +72,7 @@ usage(const char *reason) warnx("%s", reason); errx(1, "usage:\n" - "pwm arm|disarm|rate|min|max|disarmed|test|info ...\n" + "pwm arm|disarm|rate|min|max|disarmed|set|info ...\n" "\n" " arm Arm output\n" " disarm Disarm output\n" @@ -91,10 +91,11 @@ usage(const char *reason) " [-a] Configure all outputs\n" " -p PWM value\n" "\n" - " test ... Directly set PWM values\n" + " set ... Directly set PWM values\n" " [-c ] Supply channels (e.g. 1234)\n" " [-m ] Directly supply channel mask (e.g. 0xF)\n" " [-a] Configure all outputs\n" + " [-e] Repeat setting the values until stopped\n" " -p PWM value\n" "\n" " info Print information about the PWM device\n" @@ -113,6 +114,7 @@ pwm_main(int argc, char *argv[]) uint32_t alt_channel_groups = 0; bool alt_channels_set = false; bool print_verbose = false; + bool repeated_pwm_output = false; int ch; int ret; char *ep; @@ -125,7 +127,7 @@ pwm_main(int argc, char *argv[]) if (argc < 1) usage(NULL); - while ((ch = getopt(argc-1, &argv[1], "d:vc:g:m:ap:r:")) != EOF) { + while ((ch = getopt(argc-1, &argv[1], "d:vc:g:m:ap:r:e")) != EOF) { switch (ch) { case 'd': @@ -182,6 +184,9 @@ pwm_main(int argc, char *argv[]) if (*ep != '\0') usage("bad alternative rate provided"); break; + case 'e': + repeated_pwm_output = true; + break; default: break; } @@ -357,7 +362,7 @@ pwm_main(int argc, char *argv[]) } exit(0); - } else if (!strcmp(argv[1], "test")) { + } else if (!strcmp(argv[1], "set")) { if (set_mask == 0) { usage("no channels set"); @@ -367,14 +372,38 @@ pwm_main(int argc, char *argv[]) /* perform PWM output */ - /* Open console directly to grab CTRL-C signal */ - struct pollfd fds; - fds.fd = 0; /* stdin */ - fds.events = POLLIN; + if (repeated_pwm_output) { - warnx("Press CTRL-C or 'c' to abort."); + /* Open console directly to grab CTRL-C signal */ + struct pollfd fds; + fds.fd = 0; /* stdin */ + fds.events = POLLIN; - while (1) { + warnx("Press CTRL-C or 'c' to abort."); + + while (1) { + for (unsigned i = 0; i < servo_count; i++) { + if (set_mask & 1< 0) { + + read(0, &c, 1); + if (c == 0x03 || c == 0x63 || c == 'q') { + warnx("User abort\n"); + exit(0); + } + } + } + } else { + /* only set the values once */ for (unsigned i = 0; i < servo_count; i++) { if (set_mask & 1< 0) { - - read(0, &c, 1); - if (c == 0x03 || c == 0x63 || c == 'q') { - warnx("User abort\n"); - exit(0); - } - } } exit(0); @@ -476,7 +493,7 @@ pwm_main(int argc, char *argv[]) exit(0); } - usage("specify arm|disarm|rate|min|max|disarmed|test|info"); + usage("specify arm|disarm|rate|min|max|disarmed|set|info"); return 0; } From 326f241185f45d9e2d4377e8096a8a2f05f65b0d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 11 Oct 2013 12:11:25 +0200 Subject: [PATCH 767/872] Enable PWM when disarmed on the fmu side --- src/drivers/px4fmu/fmu.cpp | 27 +++++++++++++++++++++++---- 1 file changed, 23 insertions(+), 4 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 7f30487bff..8e9e8103ff 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -118,6 +118,7 @@ private: volatile bool _task_should_exit; bool _armed; + bool _pwm_on; MixerGroup *_mixers; @@ -127,6 +128,7 @@ private: uint16_t _disarmed_pwm[_max_actuators]; uint16_t _min_pwm[_max_actuators]; uint16_t _max_pwm[_max_actuators]; + unsigned _num_disarmed_set; static void task_main_trampoline(int argc, char *argv[]); void task_main() __attribute__((noreturn)); @@ -209,10 +211,12 @@ PX4FMU::PX4FMU() : _primary_pwm_device(false), _task_should_exit(false), _armed(false), + _pwm_on(false), _mixers(nullptr), _disarmed_pwm({0}), _min_pwm({PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN}), - _max_pwm({PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX}) + _max_pwm({PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX}), + _num_disarmed_set(0) { _debug_enabled = true; } @@ -585,11 +589,16 @@ PX4FMU::task_main() /* get new value */ orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa); - /* update PWM servo armed status if armed and not locked down */ + /* update the armed status and check that we're not locked down */ bool set_armed = aa.armed && !aa.lockdown; - if (set_armed != _armed) { + if (_armed != set_armed) _armed = set_armed; - up_pwm_servo_arm(set_armed); + + /* update PWM status if armed or if disarmed PWM values are set */ + bool pwm_on = (aa.armed || _num_disarmed_set > 0); + if (_pwm_on != pwm_on) { + _pwm_on = pwm_on; + up_pwm_servo_arm(pwm_on); } } @@ -738,6 +747,16 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) _disarmed_pwm[i] = pwm->values[i]; } } + + /* + * update the counter + * this is needed to decide if disarmed PWM output should be turned on or not + */ + _num_disarmed_set = 0; + for (unsigned i = 0; i < _max_actuators; i++) { + if (_disarmed_pwm[i] > 0) + _num_disarmed_set++; + } break; } case PWM_SERVO_GET_DISARMED_PWM: { From 3cbe1ee1a8308e2efc017374a6d297761c6c5226 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 11 Oct 2013 16:33:52 +0200 Subject: [PATCH 768/872] Revert "Set the PWM values only once or continuous if specified" This reverts commit 9cd3c40606f023a7943b1418a748abb046e36ecb. --- src/systemcmds/pwm/pwm.c | 63 +++++++++++++++------------------------- 1 file changed, 23 insertions(+), 40 deletions(-) diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index b3975de9d0..25f8c4e753 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -72,7 +72,7 @@ usage(const char *reason) warnx("%s", reason); errx(1, "usage:\n" - "pwm arm|disarm|rate|min|max|disarmed|set|info ...\n" + "pwm arm|disarm|rate|min|max|disarmed|test|info ...\n" "\n" " arm Arm output\n" " disarm Disarm output\n" @@ -91,11 +91,10 @@ usage(const char *reason) " [-a] Configure all outputs\n" " -p PWM value\n" "\n" - " set ... Directly set PWM values\n" + " test ... Directly set PWM values\n" " [-c ] Supply channels (e.g. 1234)\n" " [-m ] Directly supply channel mask (e.g. 0xF)\n" " [-a] Configure all outputs\n" - " [-e] Repeat setting the values until stopped\n" " -p PWM value\n" "\n" " info Print information about the PWM device\n" @@ -114,7 +113,6 @@ pwm_main(int argc, char *argv[]) uint32_t alt_channel_groups = 0; bool alt_channels_set = false; bool print_verbose = false; - bool repeated_pwm_output = false; int ch; int ret; char *ep; @@ -127,7 +125,7 @@ pwm_main(int argc, char *argv[]) if (argc < 1) usage(NULL); - while ((ch = getopt(argc-1, &argv[1], "d:vc:g:m:ap:r:e")) != EOF) { + while ((ch = getopt(argc-1, &argv[1], "d:vc:g:m:ap:r:")) != EOF) { switch (ch) { case 'd': @@ -184,9 +182,6 @@ pwm_main(int argc, char *argv[]) if (*ep != '\0') usage("bad alternative rate provided"); break; - case 'e': - repeated_pwm_output = true; - break; default: break; } @@ -362,7 +357,7 @@ pwm_main(int argc, char *argv[]) } exit(0); - } else if (!strcmp(argv[1], "set")) { + } else if (!strcmp(argv[1], "test")) { if (set_mask == 0) { usage("no channels set"); @@ -372,38 +367,14 @@ pwm_main(int argc, char *argv[]) /* perform PWM output */ - if (repeated_pwm_output) { + /* Open console directly to grab CTRL-C signal */ + struct pollfd fds; + fds.fd = 0; /* stdin */ + fds.events = POLLIN; - /* Open console directly to grab CTRL-C signal */ - struct pollfd fds; - fds.fd = 0; /* stdin */ - fds.events = POLLIN; + warnx("Press CTRL-C or 'c' to abort."); - warnx("Press CTRL-C or 'c' to abort."); - - while (1) { - for (unsigned i = 0; i < servo_count; i++) { - if (set_mask & 1< 0) { - - read(0, &c, 1); - if (c == 0x03 || c == 0x63 || c == 'q') { - warnx("User abort\n"); - exit(0); - } - } - } - } else { - /* only set the values once */ + while (1) { for (unsigned i = 0; i < servo_count; i++) { if (set_mask & 1< 0) { + + read(0, &c, 1); + if (c == 0x03 || c == 0x63 || c == 'q') { + warnx("User abort\n"); + exit(0); + } + } } exit(0); @@ -493,7 +476,7 @@ pwm_main(int argc, char *argv[]) exit(0); } - usage("specify arm|disarm|rate|min|max|disarmed|set|info"); + usage("specify arm|disarm|rate|min|max|disarmed|test|info"); return 0; } From 5d36971689566e2142a16643a77337f2e3613c35 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 11 Oct 2013 16:59:21 +0200 Subject: [PATCH 769/872] Base max actuators on board revision --- src/drivers/px4fmu/fmu.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 8e9e8103ff..e729612cc3 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -101,7 +101,12 @@ public: int set_pwm_alt_channels(uint32_t channels); private: +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) + static const unsigned _max_actuators = 4; +#endif +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) static const unsigned _max_actuators = 6; +#endif Mode _mode; unsigned _pwm_default_rate; @@ -214,10 +219,13 @@ PX4FMU::PX4FMU() : _pwm_on(false), _mixers(nullptr), _disarmed_pwm({0}), - _min_pwm({PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN}), - _max_pwm({PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX}), _num_disarmed_set(0) { + for (unsigned i = 0; i < _max_actuators; i++) { + _min_pwm[i] = PWM_MIN; + _max_pwm[i] = PWM_MAX; + } + _debug_enabled = true; } From f1c399a60b3bf5a81740eab67016732714573dfa Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Sat, 12 Oct 2013 01:14:03 -0400 Subject: [PATCH 770/872] Add support for 8 channel DSMX sattelite pairing --- src/drivers/drv_pwm_output.h | 1 + src/drivers/px4io/px4io.cpp | 15 ++++++++++----- src/modules/px4iofirmware/dsm.c | 2 +- 3 files changed, 12 insertions(+), 6 deletions(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 94e923d71a..fc96bd8484 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -120,6 +120,7 @@ ORB_DECLARE(output_pwm); #define DSM2_BIND_PULSES 3 /* DSM_BIND_START ioctl parameter, pulses required to start dsm2 pairing */ #define DSMX_BIND_PULSES 7 /* DSM_BIND_START ioctl parameter, pulses required to start dsmx pairing */ +#define DSMX8_BIND_PULSES 10 /* DSM_BIND_START ioctl parameter, pulses required to start 8 or more channel dsmx pairing */ /** Power up DSM receiver */ #define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 8) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index f6d4f1ed40..d9869bf943 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -82,6 +82,7 @@ #include #include #include + #include #include @@ -709,7 +710,6 @@ PX4IO::init() /* regular boot, no in-air restart, init IO */ } else { - /* dis-arm IO before touching anything */ io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED | @@ -1210,13 +1210,13 @@ PX4IO::dsm_bind_ioctl(int dsmMode) if (!(_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) { /* 0: dsm2, 1:dsmx */ if ((dsmMode == 0) || (dsmMode == 1)) { - mavlink_log_info(_thread_mavlink_fd, "[IO] binding dsm%c rx", (dsmMode == 0) ? '2' : 'x'); - ioctl(nullptr, DSM_BIND_START, (dsmMode == 0) ? DSM2_BIND_PULSES : DSMX_BIND_PULSES); + mavlink_log_info(_thread_mavlink_fd, "[IO] binding dsm%s rx", (dsmMode == 0) ? "2" : ((dsmMode == 1) ? "x" : "x8")); + ioctl(nullptr, DSM_BIND_START, (dsmMode == 0) ? DSM2_BIND_PULSES : ((dsmMode == 1) ? DSMX_BIND_PULSES : DSMX8_BIND_PULSES)); } else { mavlink_log_info(_thread_mavlink_fd, "[IO] invalid dsm bind mode, bind request rejected"); } } else { - mavlink_log_info(_thread_mavlink_fd, "[IO] system armed, bind request rejected"); + mavlink_log_info(_thread_mavlink_fd, "[IO] system armed, bind request rejected"); } } @@ -1870,7 +1870,7 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) usleep(500000); io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out); io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up); - usleep(50000); + usleep(72000); io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (arg << 4)); usleep(50000); io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart); @@ -2213,8 +2213,13 @@ bind(int argc, char *argv[]) pulses = DSM2_BIND_PULSES; else if (!strcmp(argv[2], "dsmx")) pulses = DSMX_BIND_PULSES; + else if (!strcmp(argv[2], "dsmx8")) + pulses = DSMX8_BIND_PULSES; else errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]); + // Test for custom pulse parameter + if (argc > 3) + pulses = atoi(argv[3]); if (g_dev->system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) errx(1, "system must not be armed"); diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index 4339766568..fd3b720150 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -285,10 +285,10 @@ dsm_bind(uint16_t cmd, int pulses) /*Pulse RX pin a number of times*/ for (int i = 0; i < pulses; i++) { + up_udelay(25); stm32_gpiowrite(usart1RxAsOutp, false); up_udelay(25); stm32_gpiowrite(usart1RxAsOutp, true); - up_udelay(25); } break; From 4984ab4418e800db1af1f1fb63ff5b6d77aa8e89 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Oct 2013 20:14:47 +0200 Subject: [PATCH 771/872] Comment fix --- src/drivers/px4io/px4io_uploader.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index fe8561a0b8..d01dedb0dd 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -237,7 +237,7 @@ PX4IO_Uploader::recv(uint8_t &c, unsigned timeout) fds[0].fd = _io_fd; fds[0].events = POLLIN; - /* wait 100 ms for a character */ + /* wait ms for a character */ int ret = ::poll(&fds[0], 1, timeout); if (ret < 1) { From c5b890e87d545328734a815d90ce16d396630048 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Oct 2013 20:17:59 +0200 Subject: [PATCH 772/872] Moved mixer file load / compression into mixer library. --- src/modules/systemlib/mixer/mixer.cpp | 2 + src/modules/systemlib/mixer/mixer.h | 2 + src/modules/systemlib/mixer/mixer_load.c | 99 +++++++++++++++++++ src/modules/systemlib/mixer/mixer_load.h | 51 ++++++++++ .../systemlib/mixer/mixer_multirotor.cpp | 4 +- src/modules/systemlib/mixer/module.mk | 3 +- src/systemcmds/mixer/{mixer.c => mixer.cpp} | 55 ++--------- src/systemcmds/mixer/module.mk | 2 +- 8 files changed, 167 insertions(+), 51 deletions(-) create mode 100644 src/modules/systemlib/mixer/mixer_load.c create mode 100644 src/modules/systemlib/mixer/mixer_load.h rename src/systemcmds/mixer/{mixer.c => mixer.cpp} (73%) diff --git a/src/modules/systemlib/mixer/mixer.cpp b/src/modules/systemlib/mixer/mixer.cpp index b1bb1a66d7..b4b82c539f 100644 --- a/src/modules/systemlib/mixer/mixer.cpp +++ b/src/modules/systemlib/mixer/mixer.cpp @@ -50,6 +50,8 @@ #include #include #include +#include +#include #include "mixer.h" diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index bbfa130a9d..6c322ff92e 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -128,7 +128,9 @@ #ifndef _SYSTEMLIB_MIXER_MIXER_H #define _SYSTEMLIB_MIXER_MIXER_H value +#include #include "drivers/drv_mixer.h" +#include "mixer_load.h" /** * Abstract class defining a mixer mixing zero or more inputs to diff --git a/src/modules/systemlib/mixer/mixer_load.c b/src/modules/systemlib/mixer/mixer_load.c new file mode 100644 index 0000000000..18c4e474a6 --- /dev/null +++ b/src/modules/systemlib/mixer/mixer_load.c @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * 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 mixer_load.c + * + * Programmable multi-channel mixer library. + */ + +#include +#include +#include +#include +#include + +#include "mixer_load.h" + +int load_mixer_file(const char *fname, char *buf) +{ + FILE *fp; + char line[120]; + + /* open the mixer definition file */ + fp = fopen(fname, "r"); + if (fp == NULL) + err(1, "can't open %s", fname); + + /* read valid lines from the file into a buffer */ + buf[0] = '\0'; + for (;;) { + + /* get a line, bail on error/EOF */ + line[0] = '\0'; + if (fgets(line, sizeof(line), fp) == NULL) + break; + + /* if the line doesn't look like a mixer definition line, skip it */ + if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':')) + continue; + + /* compact whitespace in the buffer */ + char *t, *f; + for (f = buf; *f != '\0'; f++) { + /* scan for space characters */ + if (*f == ' ') { + /* look for additional spaces */ + t = f + 1; + while (*t == ' ') + t++; + if (*t == '\0') { + /* strip trailing whitespace */ + *f = '\0'; + } else if (t > (f + 1)) { + memmove(f + 1, t, strlen(t) + 1); + } + } + } + + /* if the line is too long to fit in the buffer, bail */ + if ((strlen(line) + strlen(buf) + 1) >= sizeof(buf)) + break; + + /* add the line to the buffer */ + strcat(buf, line); + } + + return 0; +} + diff --git a/src/modules/systemlib/mixer/mixer_load.h b/src/modules/systemlib/mixer/mixer_load.h new file mode 100644 index 0000000000..b2327a4f72 --- /dev/null +++ b/src/modules/systemlib/mixer/mixer_load.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * 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 mixer_load.h + * + */ + + +#ifndef _SYSTEMLIB_MIXER_LOAD_H +#define _SYSTEMLIB_MIXER_LOAD_H value + +#include + +__BEGIN_DECLS + +__EXPORT int load_mixer_file(const char *fname, char *buf); + +__END_DECLS + +#endif diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 2446cc3fbe..89afc272c0 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -130,7 +130,7 @@ const MultirotorMixer::Rotor _config_octa_plus[] = { { 1.000000, 0.000000, -1.00 }, { -1.000000, 0.000000, -1.00 }, }; -const MultirotorMixer::Rotor *_config_index[MultirotorMixer::Geometry::MAX_GEOMETRY] = { +const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = { &_config_quad_x[0], &_config_quad_plus[0], &_config_quad_v[0], @@ -140,7 +140,7 @@ const MultirotorMixer::Rotor *_config_index[MultirotorMixer::Geometry::MAX_GEOME &_config_octa_x[0], &_config_octa_plus[0], }; -const unsigned _config_rotor_count[MultirotorMixer::Geometry::MAX_GEOMETRY] = { +const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = { 4, /* quad_x */ 4, /* quad_plus */ 4, /* quad_v */ diff --git a/src/modules/systemlib/mixer/module.mk b/src/modules/systemlib/mixer/module.mk index 4d45e1c506..fc7485e202 100644 --- a/src/modules/systemlib/mixer/module.mk +++ b/src/modules/systemlib/mixer/module.mk @@ -39,4 +39,5 @@ LIBNAME = mixerlib SRCS = mixer.cpp \ mixer_group.cpp \ mixer_multirotor.cpp \ - mixer_simple.cpp + mixer_simple.cpp \ + mixer_load.c diff --git a/src/systemcmds/mixer/mixer.c b/src/systemcmds/mixer/mixer.cpp similarity index 73% rename from src/systemcmds/mixer/mixer.c rename to src/systemcmds/mixer/mixer.cpp index e642ed0676..b1ebebbb44 100644 --- a/src/systemcmds/mixer/mixer.c +++ b/src/systemcmds/mixer/mixer.cpp @@ -47,10 +47,15 @@ #include #include -#include +#include #include -__EXPORT int mixer_main(int argc, char *argv[]); +/** + * Mixer utility for loading mixer files to devices + * + * @ingroup apps + */ +extern "C" __EXPORT int mixer_main(int argc, char *argv[]); static void usage(const char *reason) noreturn_function; static void load(const char *devname, const char *fname) noreturn_function; @@ -87,8 +92,6 @@ static void load(const char *devname, const char *fname) { int dev; - FILE *fp; - char line[120]; char buf[2048]; /* open the device */ @@ -99,49 +102,7 @@ load(const char *devname, const char *fname) if (ioctl(dev, MIXERIOCRESET, 0)) err(1, "can't reset mixers on %s", devname); - /* open the mixer definition file */ - fp = fopen(fname, "r"); - if (fp == NULL) - err(1, "can't open %s", fname); - - /* read valid lines from the file into a buffer */ - buf[0] = '\0'; - for (;;) { - - /* get a line, bail on error/EOF */ - line[0] = '\0'; - if (fgets(line, sizeof(line), fp) == NULL) - break; - - /* if the line doesn't look like a mixer definition line, skip it */ - if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':')) - continue; - - /* compact whitespace in the buffer */ - char *t, *f; - for (f = buf; *f != '\0'; f++) { - /* scan for space characters */ - if (*f == ' ') { - /* look for additional spaces */ - t = f + 1; - while (*t == ' ') - t++; - if (*t == '\0') { - /* strip trailing whitespace */ - *f = '\0'; - } else if (t > (f + 1)) { - memmove(f + 1, t, strlen(t) + 1); - } - } - } - - /* if the line is too long to fit in the buffer, bail */ - if ((strlen(line) + strlen(buf) + 1) >= sizeof(buf)) - break; - - /* add the line to the buffer */ - strcat(buf, line); - } + load_mixer_file(fname, &buf[0]); /* XXX pass the buffer to the device */ int ret = ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf); diff --git a/src/systemcmds/mixer/module.mk b/src/systemcmds/mixer/module.mk index d5a3f9f77b..cdbff75f04 100644 --- a/src/systemcmds/mixer/module.mk +++ b/src/systemcmds/mixer/module.mk @@ -36,7 +36,7 @@ # MODULE_COMMAND = mixer -SRCS = mixer.c +SRCS = mixer.cpp MODULE_STACKSIZE = 4096 From 7d6a4ece6b1875bab62c4dbcb95fcc9fa5661674 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Oct 2013 20:19:09 +0200 Subject: [PATCH 773/872] Added mixer test --- src/systemcmds/tests/module.mk | 1 + src/systemcmds/tests/test_mixer.cpp | 74 +++++++++++++++++++++++++++++ src/systemcmds/tests/tests.h | 5 ++ src/systemcmds/tests/tests_main.c | 1 + 4 files changed, 81 insertions(+) create mode 100644 src/systemcmds/tests/test_mixer.cpp diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index 754d3a0da9..6729ce4ae3 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -23,6 +23,7 @@ SRCS = test_adc.c \ test_uart_console.c \ test_uart_loopback.c \ test_uart_send.c \ + test_mixer.cpp \ tests_file.c \ tests_main.c \ tests_param.c diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp new file mode 100644 index 0000000000..acb7bd88f6 --- /dev/null +++ b/src/systemcmds/tests/test_mixer.cpp @@ -0,0 +1,74 @@ +/**************************************************************************** + * + * 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 test_mixer.hpp + * + * Mixer load test + */ + +#include + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "tests.h" + +int test_mixer(int argc, char *argv[]) +{ + warnx("testing mixer"); + + char *filename = "/etc/mixers/IO_pass.mix"; + + if (argc > 1) + filename = argv[1]; + + warnx("loading: %s", filename); + + char buf[2048]; + + load_mixer_file(filename, &buf[0]); + + warnx("loaded: %s", &buf[0]); + + return 0; +} diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h index c02ea6808f..c483108cf0 100644 --- a/src/systemcmds/tests/tests.h +++ b/src/systemcmds/tests/tests.h @@ -76,6 +76,8 @@ * Public Function Prototypes ****************************************************************************/ +__BEGIN_DECLS + extern int test_sensors(int argc, char *argv[]); extern int test_gpio(int argc, char *argv[]); extern int test_hrt(int argc, char *argv[]); @@ -98,5 +100,8 @@ extern int test_jig_voltages(int argc, char *argv[]); extern int test_param(int argc, char *argv[]); extern int test_bson(int argc, char *argv[]); extern int test_file(int argc, char *argv[]); +extern int test_mixer(int argc, char *argv[]); + +__END_DECLS #endif /* __APPS_PX4_TESTS_H */ diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 9f8c5c9eae..9eb9c632ec 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -110,6 +110,7 @@ const struct { {"param", test_param, 0}, {"bson", test_bson, 0}, {"file", test_file, 0}, + {"mixer", test_mixer, OPT_NOJIGTEST | OPT_NOALLTEST}, {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST}, {NULL, NULL, 0} }; From 42b75ae8963b2f711a72ac1cb6cfd1b44bd826b2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Oct 2013 20:19:34 +0200 Subject: [PATCH 774/872] Added host-building mixer test --- Tools/tests-host/.gitignore | 2 ++ Tools/tests-host/Makefile | 36 +++++++++++++++++++++++++++++ Tools/tests-host/arch/board/board.h | 0 Tools/tests-host/mixer_test.cpp | 12 ++++++++++ Tools/tests-host/nuttx/config.h | 0 5 files changed, 50 insertions(+) create mode 100644 Tools/tests-host/.gitignore create mode 100644 Tools/tests-host/Makefile create mode 100644 Tools/tests-host/arch/board/board.h create mode 100644 Tools/tests-host/mixer_test.cpp create mode 100644 Tools/tests-host/nuttx/config.h diff --git a/Tools/tests-host/.gitignore b/Tools/tests-host/.gitignore new file mode 100644 index 0000000000..61e0915515 --- /dev/null +++ b/Tools/tests-host/.gitignore @@ -0,0 +1,2 @@ +./obj/* +mixer_test diff --git a/Tools/tests-host/Makefile b/Tools/tests-host/Makefile new file mode 100644 index 0000000000..c603b2aa2a --- /dev/null +++ b/Tools/tests-host/Makefile @@ -0,0 +1,36 @@ + +CC=g++ +CFLAGS=-I. -I../../src/modules -I ../../src/include -I../../src/drivers -I../../src -D__EXPORT="" -Dnullptr="0" + +ODIR=obj +LDIR =../lib + +LIBS=-lm + +#_DEPS = test.h +#DEPS = $(patsubst %,$(IDIR)/%,$(_DEPS)) + +_OBJ = mixer_test.o test_mixer.o mixer_simple.o mixer_multirotor.o mixer.o mixer_group.o +OBJ = $(patsubst %,$(ODIR)/%,$(_OBJ)) + +#$(DEPS) +$(ODIR)/%.o: %.cpp + $(CC) -c -o $@ $< $(CFLAGS) + +$(ODIR)/%.o: ../../src/systemcmds/tests/%.c + $(CC) -c -o $@ $< $(CFLAGS) + +$(ODIR)/%.o: ../../src/modules/systemlib/%.cpp + $(CC) -c -o $@ $< $(CFLAGS) + +$(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.cpp + $(CC) -c -o $@ $< $(CFLAGS) + +# +mixer_test: $(OBJ) + g++ -o $@ $^ $(CFLAGS) $(LIBS) + +.PHONY: clean + +clean: + rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ \ No newline at end of file diff --git a/Tools/tests-host/arch/board/board.h b/Tools/tests-host/arch/board/board.h new file mode 100644 index 0000000000..e69de29bb2 diff --git a/Tools/tests-host/mixer_test.cpp b/Tools/tests-host/mixer_test.cpp new file mode 100644 index 0000000000..5d92040f11 --- /dev/null +++ b/Tools/tests-host/mixer_test.cpp @@ -0,0 +1,12 @@ +#include +#include + +extern int test_mixer(int argc, char *argv[]); + +int main(int argc, char *argv[]) { + warnx("Host execution started"); + + char* args[] = {argv[0], "../../ROMFS/px4fmu_common/mixers/IO_pass.mix"}; + + test_mixer(2, args); +} \ No newline at end of file diff --git a/Tools/tests-host/nuttx/config.h b/Tools/tests-host/nuttx/config.h new file mode 100644 index 0000000000..e69de29bb2 From 1dc9569e31717aefab8e05b858122f433dab1698 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Oct 2013 11:44:26 +0200 Subject: [PATCH 775/872] Fixed mixer chunk load and line ending detection for good. --- src/modules/systemlib/mixer/mixer.cpp | 27 ++++ src/modules/systemlib/mixer/mixer.h | 23 ++++ src/modules/systemlib/mixer/mixer_group.cpp | 15 ++ src/modules/systemlib/mixer/mixer_load.c | 15 +- src/modules/systemlib/mixer/mixer_load.h | 2 +- .../systemlib/mixer/mixer_multirotor.cpp | 10 +- src/modules/systemlib/mixer/mixer_simple.cpp | 62 +++------ src/systemcmds/mixer/mixer.cpp | 2 +- src/systemcmds/tests/test_mixer.cpp | 129 +++++++++++++++++- 9 files changed, 229 insertions(+), 56 deletions(-) diff --git a/src/modules/systemlib/mixer/mixer.cpp b/src/modules/systemlib/mixer/mixer.cpp index b4b82c539f..cce46bf5fc 100644 --- a/src/modules/systemlib/mixer/mixer.cpp +++ b/src/modules/systemlib/mixer/mixer.cpp @@ -116,6 +116,33 @@ Mixer::scale_check(struct mixer_scaler_s &scaler) return 0; } +const char * +Mixer::findtag(const char *buf, unsigned &buflen, char tag) +{ + while (buflen >= 2) { + if ((buf[0] == tag) && (buf[1] == ':')) + return buf; + buf++; + buflen--; + } + return nullptr; +} + +const char * +Mixer::skipline(const char *buf, unsigned &buflen) +{ + const char *p; + + /* if we can find a CR or NL in the buffer, skip up to it */ + if ((p = (const char *)memchr(buf, '\r', buflen)) || (p = (const char *)memchr(buf, '\n', buflen))) { + /* skip up to it AND one beyond - could be on the NUL symbol now */ + buflen -= (p - buf) + 1; + return p + 1; + } + + return nullptr; +} + /****************************************************************************/ NullMixer::NullMixer() : diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index 6c322ff92e..723bf9f3b7 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -212,6 +212,24 @@ protected: */ static int scale_check(struct mixer_scaler_s &scaler); + /** + * Find a tag + * + * @param buf The buffer to operate on. + * @param buflen length of the buffer. + * @param tag character to search for. + */ + static const char * findtag(const char *buf, unsigned &buflen, char tag); + + /** + * Skip a line + * + * @param buf The buffer to operate on. + * @param buflen length of the buffer. + * @return 0 / OK if a line could be skipped, 1 else + */ + static const char * skipline(const char *buf, unsigned &buflen); + private: }; @@ -240,6 +258,11 @@ public: */ void reset(); + /** + * Count the mixers in the group. + */ + unsigned count(); + /** * Adds mixers to the group based on a text description in a buffer. * diff --git a/src/modules/systemlib/mixer/mixer_group.cpp b/src/modules/systemlib/mixer/mixer_group.cpp index 1dbc512cdb..3ed99fba09 100644 --- a/src/modules/systemlib/mixer/mixer_group.cpp +++ b/src/modules/systemlib/mixer/mixer_group.cpp @@ -111,6 +111,20 @@ MixerGroup::mix(float *outputs, unsigned space) return index; } +unsigned +MixerGroup::count() +{ + Mixer *mixer = _first; + unsigned index = 0; + + while ((mixer != nullptr)) { + mixer = mixer->_next; + index++; + } + + return index; +} + void MixerGroup::groups_required(uint32_t &groups) { @@ -170,6 +184,7 @@ MixerGroup::load_from_buf(const char *buf, unsigned &buflen) /* only adjust buflen if parsing was successful */ buflen = resid; + debug("SUCCESS - buflen: %d", buflen); } else { /* diff --git a/src/modules/systemlib/mixer/mixer_load.c b/src/modules/systemlib/mixer/mixer_load.c index 18c4e474a6..a55ddf8a35 100644 --- a/src/modules/systemlib/mixer/mixer_load.c +++ b/src/modules/systemlib/mixer/mixer_load.c @@ -38,22 +38,22 @@ */ #include -#include #include #include #include #include "mixer_load.h" -int load_mixer_file(const char *fname, char *buf) +int load_mixer_file(const char *fname, char *buf, unsigned maxlen) { FILE *fp; char line[120]; /* open the mixer definition file */ fp = fopen(fname, "r"); - if (fp == NULL) - err(1, "can't open %s", fname); + if (fp == NULL) { + return 1; + } /* read valid lines from the file into a buffer */ buf[0] = '\0'; @@ -70,7 +70,7 @@ int load_mixer_file(const char *fname, char *buf) /* compact whitespace in the buffer */ char *t, *f; - for (f = buf; *f != '\0'; f++) { + for (f = line; *f != '\0'; f++) { /* scan for space characters */ if (*f == ' ') { /* look for additional spaces */ @@ -87,8 +87,9 @@ int load_mixer_file(const char *fname, char *buf) } /* if the line is too long to fit in the buffer, bail */ - if ((strlen(line) + strlen(buf) + 1) >= sizeof(buf)) - break; + if ((strlen(line) + strlen(buf) + 1) >= maxlen) { + return 1; + } /* add the line to the buffer */ strcat(buf, line); diff --git a/src/modules/systemlib/mixer/mixer_load.h b/src/modules/systemlib/mixer/mixer_load.h index b2327a4f72..4b7091d5b2 100644 --- a/src/modules/systemlib/mixer/mixer_load.h +++ b/src/modules/systemlib/mixer/mixer_load.h @@ -44,7 +44,7 @@ __BEGIN_DECLS -__EXPORT int load_mixer_file(const char *fname, char *buf); +__EXPORT int load_mixer_file(const char *fname, char *buf, unsigned maxlen); __END_DECLS diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 89afc272c0..b89f341b60 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -205,11 +205,17 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl } if (used > (int)buflen) { - debug("multirotor spec used %d of %u", used, buflen); + debug("OVERFLOW: multirotor spec used %d of %u", used, buflen); return nullptr; } - buflen -= used; + buf = skipline(buf, buflen); + if (buf == nullptr) { + debug("no line ending, line is incomplete"); + return nullptr; + } + + debug("remaining in buf: %d, first char: %c", buflen, buf[0]); if (!strcmp(geomname, "4+")) { geometry = MultirotorMixer::QUAD_PLUS; diff --git a/src/modules/systemlib/mixer/mixer_simple.cpp b/src/modules/systemlib/mixer/mixer_simple.cpp index c8434f991b..c3985b5de6 100644 --- a/src/modules/systemlib/mixer/mixer_simple.cpp +++ b/src/modules/systemlib/mixer/mixer_simple.cpp @@ -55,7 +55,7 @@ #include "mixer.h" #define debug(fmt, args...) do { } while(0) -// #define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) +//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) SimpleMixer::SimpleMixer(ControlCallback control_cb, uintptr_t cb_handle, @@ -71,28 +71,6 @@ SimpleMixer::~SimpleMixer() free(_info); } -static const char * -findtag(const char *buf, unsigned &buflen, char tag) -{ - while (buflen >= 2) { - if ((buf[0] == tag) && (buf[1] == ':')) - return buf; - buf++; - buflen--; - } - return nullptr; -} - -static void -skipline(const char *buf, unsigned &buflen) -{ - const char *p; - - /* if we can find a CR or NL in the buffer, skip up to it */ - if ((p = (const char *)memchr(buf, '\r', buflen)) || (p = (const char *)memchr(buf, '\n', buflen))) - buflen -= (p - buf); -} - int SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler) { @@ -111,7 +89,12 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler debug("out scaler parse failed on '%s' (got %d, consumed %d)", buf, ret, n); return -1; } - skipline(buf, buflen); + + buf = skipline(buf, buflen); + if (buf == nullptr) { + debug("no line ending, line is incomplete"); + return -1; + } scaler.negative_scale = s[0] / 10000.0f; scaler.positive_scale = s[1] / 10000.0f; @@ -130,7 +113,7 @@ SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scale buf = findtag(buf, buflen, 'S'); if ((buf == nullptr) || (buflen < 16)) { - debug("contorl parser failed finding tag, ret: '%s'", buf); + debug("control parser failed finding tag, ret: '%s'", buf); return -1; } @@ -139,7 +122,12 @@ SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scale debug("control parse failed on '%s'", buf); return -1; } - skipline(buf, buflen); + + buf = skipline(buf, buflen); + if (buf == nullptr) { + debug("no line ending, line is incomplete"); + return -1; + } control_group = u[0]; control_index = u[1]; @@ -161,29 +149,17 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c int used; const char *end = buf + buflen; - /* enforce that the mixer ends with space or a new line */ - for (int i = buflen - 1; i >= 0; i--) { - if (buf[i] == '\0') - continue; - - /* require a space or newline at the end of the buffer, fail on printable chars */ - if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') { - /* found a line ending or space, so no split symbols / numbers. good. */ - break; - } else { - debug("simple parser rejected: No newline / space at end of buf. (#%d/%d: 0x%02x)", i, buflen-1, buf[i]); - goto out; - } - - } - /* get the base info for the mixer */ if (sscanf(buf, "M: %u%n", &inputs, &used) != 1) { debug("simple parse failed on '%s'", buf); goto out; } - buflen -= used; + buf = skipline(buf, buflen); + if (buf == nullptr) { + debug("no line ending, line is incomplete"); + goto out; + } mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(inputs)); diff --git a/src/systemcmds/mixer/mixer.cpp b/src/systemcmds/mixer/mixer.cpp index b1ebebbb44..6da39d371b 100644 --- a/src/systemcmds/mixer/mixer.cpp +++ b/src/systemcmds/mixer/mixer.cpp @@ -102,7 +102,7 @@ load(const char *devname, const char *fname) if (ioctl(dev, MIXERIOCRESET, 0)) err(1, "can't reset mixers on %s", devname); - load_mixer_file(fname, &buf[0]); + load_mixer_file(fname, &buf[0], sizeof(buf)); /* XXX pass the buffer to the device */ int ret = ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf); diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index acb7bd88f6..4da86042d2 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -46,6 +46,7 @@ #include #include #include +#include #include #include @@ -53,6 +54,11 @@ #include "tests.h" +static int mixer_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &control); + int test_mixer(int argc, char *argv[]) { warnx("testing mixer"); @@ -66,9 +72,128 @@ int test_mixer(int argc, char *argv[]) char buf[2048]; - load_mixer_file(filename, &buf[0]); + load_mixer_file(filename, &buf[0], sizeof(buf)); + unsigned loaded = strlen(buf); - warnx("loaded: %s", &buf[0]); + warnx("loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded); + + /* load the mixer in chunks, like + * in the case of a remote load, + * e.g. on PX4IO. + */ + + unsigned nused = 0; + + const unsigned chunk_size = 64; + + MixerGroup mixer_group(mixer_callback, 0); + + /* load at once test */ + unsigned xx = loaded; + mixer_group.load_from_buf(&buf[0], xx); + warnx("complete buffer load: loaded %u mixers", mixer_group.count()); + if (mixer_group.count() != 8) + return 1; + + unsigned empty_load = 2; + char empty_buf[2]; + empty_buf[0] = ' '; + empty_buf[1] = '\0'; + mixer_group.reset(); + mixer_group.load_from_buf(&empty_buf[0], empty_load); + warnx("empty buffer load: loaded %u mixers, used: %u", mixer_group.count(), empty_load); + if (empty_load != 0) + return 1; + + /* FIRST mark the mixer as invalid */ + bool mixer_ok = false; + /* THEN actually delete it */ + mixer_group.reset(); + char mixer_text[256]; /* large enough for one mixer */ + unsigned mixer_text_length = 0; + + unsigned transmitted = 0; + + warnx("transmitted: %d, loaded: %d", transmitted, loaded); + + while (transmitted < loaded) { + + unsigned text_length = (loaded - transmitted > chunk_size) ? chunk_size : loaded - transmitted; + + /* check for overflow - this would be really fatal */ + if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) { + bool mixer_ok = false; + return 1; + } + + /* append mixer text and nul-terminate */ + memcpy(&mixer_text[mixer_text_length], &buf[transmitted], text_length); + mixer_text_length += text_length; + mixer_text[mixer_text_length] = '\0'; + warnx("buflen %u, text:\n\"%s\"", mixer_text_length, &mixer_text[0]); + + /* process the text buffer, adding new mixers as their descriptions can be parsed */ + unsigned resid = mixer_text_length; + mixer_group.load_from_buf(&mixer_text[0], resid); + + /* if anything was parsed */ + if (resid != mixer_text_length) { + + /* only set mixer ok if no residual is left over */ + if (resid == 0) { + mixer_ok = true; + } else { + /* not yet reached the end of the mixer, set as not ok */ + mixer_ok = false; + } + + warnx("used %u", mixer_text_length - resid); + + /* copy any leftover text to the base of the buffer for re-use */ + if (resid > 0) + memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid); + + mixer_text_length = resid; + } + + transmitted += text_length; + } + + warnx("chunked load: loaded %u mixers", mixer_group.count()); + + if (mixer_group.count() != 8) + return 1; + + /* load multirotor at once test */ + mixer_group.reset(); + + if (argc > 2) + filename = argv[2]; + else + filename = "/etc/mixers/FMU_quad_w.mix"; + + load_mixer_file(filename, &buf[0], sizeof(buf)); + loaded = strlen(buf); + + warnx("loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded); + + unsigned mc_loaded = loaded; + mixer_group.load_from_buf(&buf[0], mc_loaded); + warnx("complete buffer load: loaded %u mixers", mixer_group.count()); + if (mixer_group.count() != 8) + return 1; +} + +static int +mixer_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &control) +{ + if (control_group != 0) + return -1; + + control = 0.0f; return 0; } From 5671df0af4e258ef0a83377cdbd50e59734aa00b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Oct 2013 11:44:42 +0200 Subject: [PATCH 776/872] Improved mixer tests --- Tools/tests-host/Makefile | 7 +++++-- Tools/tests-host/mixer_test.cpp | 8 ++++---- 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/Tools/tests-host/Makefile b/Tools/tests-host/Makefile index c603b2aa2a..97410ff47c 100644 --- a/Tools/tests-host/Makefile +++ b/Tools/tests-host/Makefile @@ -10,14 +10,14 @@ LIBS=-lm #_DEPS = test.h #DEPS = $(patsubst %,$(IDIR)/%,$(_DEPS)) -_OBJ = mixer_test.o test_mixer.o mixer_simple.o mixer_multirotor.o mixer.o mixer_group.o +_OBJ = mixer_test.o test_mixer.o mixer_simple.o mixer_multirotor.o mixer.o mixer_group.o mixer_load.o OBJ = $(patsubst %,$(ODIR)/%,$(_OBJ)) #$(DEPS) $(ODIR)/%.o: %.cpp $(CC) -c -o $@ $< $(CFLAGS) -$(ODIR)/%.o: ../../src/systemcmds/tests/%.c +$(ODIR)/%.o: ../../src/systemcmds/tests/%.cpp $(CC) -c -o $@ $< $(CFLAGS) $(ODIR)/%.o: ../../src/modules/systemlib/%.cpp @@ -26,6 +26,9 @@ $(ODIR)/%.o: ../../src/modules/systemlib/%.cpp $(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.cpp $(CC) -c -o $@ $< $(CFLAGS) +$(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.c + $(CC) -c -o $@ $< $(CFLAGS) + # mixer_test: $(OBJ) g++ -o $@ $^ $(CFLAGS) $(LIBS) diff --git a/Tools/tests-host/mixer_test.cpp b/Tools/tests-host/mixer_test.cpp index 5d92040f11..042322aadc 100644 --- a/Tools/tests-host/mixer_test.cpp +++ b/Tools/tests-host/mixer_test.cpp @@ -1,12 +1,12 @@ #include #include - -extern int test_mixer(int argc, char *argv[]); +#include "../../src/systemcmds/tests/tests.h" int main(int argc, char *argv[]) { warnx("Host execution started"); - char* args[] = {argv[0], "../../ROMFS/px4fmu_common/mixers/IO_pass.mix"}; + char* args[] = {argv[0], "../../ROMFS/px4fmu_common/mixers/IO_pass.mix", + "../../ROMFS/px4fmu_common/mixers/FMU_quad_w.mix"}; - test_mixer(2, args); + test_mixer(3, args); } \ No newline at end of file From 8aaf285ac5eca73853300d38c8121e9c2757fef2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Oct 2013 12:38:26 +0200 Subject: [PATCH 777/872] Python exception handling from muggenhor --- Tools/px_uploader.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index 64af672a34..a2d7d371df 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -430,7 +430,7 @@ while True: # Windows, don't open POSIX ports if not "/" in port: up = uploader(port, args.baud) - except: + except Exception: # open failed, rate-limit our attempts time.sleep(0.05) @@ -443,7 +443,7 @@ while True: up.identify() print("Found board %x,%x bootloader rev %x on %s" % (up.board_type, up.board_rev, up.bl_rev, port)) - except: + except Exception: # most probably a timeout talking to the port, no bootloader, try to reboot the board print("attempting reboot on %s..." % port) up.send_reboot() From f475ffda04e49255d99eb7d7da537432d00c8151 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Oct 2013 15:54:02 +0200 Subject: [PATCH 778/872] Set better default gains for Iris and F330 --- ROMFS/px4fmu_common/init.d/10_dji_f330 | 6 +++--- ROMFS/px4fmu_common/init.d/16_3dr_iris | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10_dji_f330 b/ROMFS/px4fmu_common/init.d/10_dji_f330 index d21759507d..b8fe5fc31c 100644 --- a/ROMFS/px4fmu_common/init.d/10_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/10_dji_f330 @@ -10,12 +10,12 @@ then # Set all params here, then disable autoconfig param set SYS_AUTOCONFIG 0 - param set MC_ATTRATE_D 0.002 + param set MC_ATTRATE_D 0.004 param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.09 + 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 6.8 + param set MC_ATT_P 7.0 param set MC_YAWPOS_D 0.0 param set MC_YAWPOS_I 0.0 param set MC_YAWPOS_P 2.0 diff --git a/ROMFS/px4fmu_common/init.d/16_3dr_iris b/ROMFS/px4fmu_common/init.d/16_3dr_iris index d47ecb3932..3ac086b915 100644 --- a/ROMFS/px4fmu_common/init.d/16_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/16_3dr_iris @@ -10,12 +10,12 @@ then # Set all params here, then disable autoconfig param set SYS_AUTOCONFIG 0 - param set MC_ATTRATE_D 0.006 + param set MC_ATTRATE_D 0.004 param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.17 + param set MC_ATTRATE_P 0.13 param set MC_ATT_D 0.0 param set MC_ATT_I 0.0 - param set MC_ATT_P 5.0 + param set MC_ATT_P 9.0 param set MC_YAWPOS_D 0.0 param set MC_YAWPOS_I 0.15 param set MC_YAWPOS_P 0.5 From af7288ed936d642f3141a3e8792799909d351885 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Oct 2013 09:56:57 +0200 Subject: [PATCH 779/872] Enable position control for Easystar type planes --- ROMFS/px4fmu_common/init.d/100_mpx_easystar | 3 +-- ROMFS/px4fmu_common/init.d/101_hk_bixler | 3 +-- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/100_mpx_easystar b/ROMFS/px4fmu_common/init.d/100_mpx_easystar index 828540ad90..1aa1ef494e 100644 --- a/ROMFS/px4fmu_common/init.d/100_mpx_easystar +++ b/ROMFS/px4fmu_common/init.d/100_mpx_easystar @@ -98,8 +98,7 @@ else mixer load /dev/pwm_output /etc/mixers/FMU_RET.mix fi fw_att_control start -# Not ready yet for prime-time -#fw_pos_control_l1 start +fw_pos_control_l1 start if [ $EXIT_ON_END == yes ] then diff --git a/ROMFS/px4fmu_common/init.d/101_hk_bixler b/ROMFS/px4fmu_common/init.d/101_hk_bixler index 5a9cb2171a..4d8a24c4ae 100644 --- a/ROMFS/px4fmu_common/init.d/101_hk_bixler +++ b/ROMFS/px4fmu_common/init.d/101_hk_bixler @@ -91,8 +91,7 @@ att_pos_estimator_ekf start # mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix fw_att_control start -# Not ready yet for prime-time -#fw_pos_control_l1 start +fw_pos_control_l1 start if [ $EXIT_ON_END == yes ] then From 114b7b696d54d031c899db4ffb91c125204fbba2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 14 Oct 2013 11:14:56 +0200 Subject: [PATCH 780/872] sdlog2: VER message added instead of FWRV --- src/modules/sdlog2/sdlog2.c | 35 ++++++++++++++-- src/modules/sdlog2/sdlog2_messages.h | 24 +++-------- src/modules/sdlog2/sdlog2_version.h | 62 ++++++++++++++++++++++++++++ 3 files changed, 99 insertions(+), 22 deletions(-) create mode 100644 src/modules/sdlog2/sdlog2_version.h diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index ec8033202d..edb21c7ab2 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -90,6 +90,7 @@ #include "logbuffer.h" #include "sdlog2_format.h" #include "sdlog2_messages.h" +#include "sdlog2_version.h" #define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \ log_msgs_written++; \ @@ -182,6 +183,10 @@ static void sdlog2_stop_log(void); */ static void write_formats(int fd); +/** + * Write version message to log file. + */ +static void write_version(int fd); static bool file_exist(const char *filename); @@ -359,6 +364,9 @@ static void *logwriter_thread(void *arg) /* write log messages formats */ write_formats(log_file); + /* write version */ + write_version(log_file); + int poll_count = 0; void *read_ptr; @@ -487,14 +495,13 @@ void sdlog2_stop_log() sdlog2_status(); } - void write_formats(int fd) { /* construct message format packet */ struct { LOG_PACKET_HEADER; struct log_format_s body; - } log_format_packet = { + } log_msg_format = { LOG_PACKET_HEADER_INIT(LOG_FORMAT_MSG), }; @@ -502,13 +509,33 @@ void write_formats(int fd) int i; for (i = 0; i < log_formats_num; i++) { - log_format_packet.body = log_formats[i]; - log_bytes_written += write(fd, &log_format_packet, sizeof(log_format_packet)); + log_msg_format.body = log_formats[i]; + log_bytes_written += write(fd, &log_msg_format, sizeof(log_msg_format)); } fsync(fd); } +void write_version(int fd) +{ + /* construct version message */ + struct { + LOG_PACKET_HEADER; + struct log_VER_s body; + } log_msg_VER = { + LOG_PACKET_HEADER_INIT(127), + }; + + /* fill message format packet for each format and write to log */ + int i; + + strncpy(log_msg_VER.body.fw_git, FW_GIT, sizeof(log_msg_VER.body.fw_git)); + strncpy(log_msg_VER.body.arch, HW_ARCH, sizeof(log_msg_VER.body.arch)); + log_bytes_written += write(fd, &log_msg_VER, sizeof(log_msg_VER)); + + fsync(fd); +} + int sdlog2_thread_main(int argc, char *argv[]) { mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 0e88da0547..9f709e4591 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -253,28 +253,16 @@ struct log_GVSP_s { float vz; }; -/* --- FWRV - FIRMWARE REVISION --- */ -#define LOG_FWRV_MSG 20 -struct log_FWRV_s { - char fw_revision[64]; +/* --- VER - VERSION --- */ +#define LOG_VER_MSG 127 +struct log_VER_s { + char arch[16]; + char fw_git[64]; }; #pragma pack(pop) - -/* - GIT_VERSION is defined at build time via a Makefile call to the - git command line. We create a fake log message format for - the firmware revision "FWRV" that is written to every log - header. This makes it easier to determine which version - of the firmware was running when a log was created. - */ -#define FREEZE_STR(s) #s -#define STRINGIFY(s) FREEZE_STR(s) -#define FW_VERSION_STR STRINGIFY(GIT_VERSION) - /* construct list of all message formats */ - static const struct log_format_s log_formats[] = { LOG_FORMAT(TIME, "Q", "StartTime"), LOG_FORMAT(ATT, "ffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate"), @@ -295,7 +283,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"), LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), - LOG_FORMAT(FWRV,"Z",FW_VERSION_STR), + LOG_FORMAT(VER, "NZ", "Arch,FwGit"), }; static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); diff --git a/src/modules/sdlog2/sdlog2_version.h b/src/modules/sdlog2/sdlog2_version.h new file mode 100644 index 0000000000..c6a9ba6387 --- /dev/null +++ b/src/modules/sdlog2/sdlog2_version.h @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * 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 sdlog2_version.h + * + * Tools for system version detection. + * + * @author Anton Babushkin + */ + +#ifndef SDLOG2_VERSION_H_ +#define SDLOG2_VERSION_H_ + +/* + GIT_VERSION is defined at build time via a Makefile call to the + git command line. + */ +#define FREEZE_STR(s) #s +#define STRINGIFY(s) FREEZE_STR(s) +#define FW_GIT STRINGIFY(GIT_VERSION) + +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 +#define HW_ARCH "PX4FMU_V1" +#endif + +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +#define HW_ARCH "PX4FMU_V2" +#endif + +#endif /* SDLOG2_VERSION_H_ */ From 57b8dee7092eb84e8f4f31dcee85736eedb2ca8f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Oct 2013 13:41:37 +0200 Subject: [PATCH 781/872] Bring back proper log conversion copy operation --- src/modules/sdlog2/sdlog2.c | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index ec8033202d..080aa550cb 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -583,6 +583,15 @@ int sdlog2_thread_main(int argc, char *argv[]) errx(1, "unable to create logging folder, exiting."); } + const char *converter_in = "/etc/logging/conv.zip"; + char* converter_out = malloc(120); + sprintf(converter_out, "%s/conv.zip", folder_path); + + if (file_copy(converter_in, converter_out)) { + errx(1, "unable to copy conversion scripts, exiting."); + } + free(converter_out); + /* only print logging path, important to find log file later */ warnx("logging to directory: %s", folder_path); @@ -1251,7 +1260,7 @@ int file_copy(const char *file_old, const char *file_new) fclose(source); fclose(target); - return ret; + return OK; } void handle_command(struct vehicle_command_s *cmd) From c6b58491bbd7390650723c65b4d4e9ec0922c8de Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Oct 2013 22:18:44 +0200 Subject: [PATCH 782/872] Work queue in RGB driver without work_cancel() --- src/drivers/rgbled/rgbled.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index fedff769b8..aeb7e2f787 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -248,6 +248,11 @@ RGBLED::led_trampoline(void *arg) void RGBLED::led() { + if (!_should_run) { + _running = false; + return; + } + switch (_mode) { case RGBLED_MODE_BLINK_SLOW: case RGBLED_MODE_BLINK_NORMAL: @@ -471,11 +476,6 @@ RGBLED::set_mode(rgbled_mode_t mode) 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); - } } } From fbe595a591734bffa95d28125b8e0bda117d7314 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Oct 2013 23:10:12 +0200 Subject: [PATCH 783/872] Fixed some stupid compile errors --- src/drivers/rgbled/rgbled.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index aeb7e2f787..ea87b37d94 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -103,6 +103,7 @@ private: bool _running; int _led_interval; + bool _should_run; int _counter; void set_color(rgbled_color_t ledcolor); @@ -136,6 +137,7 @@ RGBLED::RGBLED(int bus, int rgbled) : _brightness(1.0f), _running(false), _led_interval(0), + _should_run(false), _counter(0) { memset(&_work, 0, sizeof(_work)); @@ -414,10 +416,10 @@ RGBLED::set_mode(rgbled_mode_t mode) { if (mode != _mode) { _mode = mode; - bool should_run = false; switch (mode) { case RGBLED_MODE_OFF: + _should_run = false; send_led_enable(false); break; @@ -428,7 +430,7 @@ RGBLED::set_mode(rgbled_mode_t mode) break; case RGBLED_MODE_BLINK_SLOW: - should_run = true; + _should_run = true; _counter = 0; _led_interval = 2000; _brightness = 1.0f; @@ -436,7 +438,7 @@ RGBLED::set_mode(rgbled_mode_t mode) break; case RGBLED_MODE_BLINK_NORMAL: - should_run = true; + _should_run = true; _counter = 0; _led_interval = 500; _brightness = 1.0f; @@ -444,7 +446,7 @@ RGBLED::set_mode(rgbled_mode_t mode) break; case RGBLED_MODE_BLINK_FAST: - should_run = true; + _should_run = true; _counter = 0; _led_interval = 100; _brightness = 1.0f; @@ -452,14 +454,14 @@ RGBLED::set_mode(rgbled_mode_t mode) break; case RGBLED_MODE_BREATHE: - should_run = true; + _should_run = true; _counter = 0; _led_interval = 25; send_led_enable(true); break; case RGBLED_MODE_PATTERN: - should_run = true; + _should_run = true; _counter = 0; _brightness = 1.0f; send_led_enable(true); @@ -471,7 +473,7 @@ RGBLED::set_mode(rgbled_mode_t mode) } /* if it should run now, start the workq */ - if (should_run && !_running) { + if (_should_run && !_running) { _running = true; work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1); } From 3dcd5dbd0e25432049e5ba6971851931764ae043 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Oct 2013 08:39:57 +0200 Subject: [PATCH 784/872] Piping through manual control channels --- .../fw_att_control/fw_att_control_main.cpp | 4 ++++ .../multirotor_att_control_main.c | 6 +++++ src/modules/sensors/sensors.cpp | 22 +++++++++++++++++++ 3 files changed, 32 insertions(+) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index ae9aaa2da6..eb67874db4 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -683,6 +683,10 @@ FixedwingAttitudeControl::task_main() _actuators.control[4] = _manual.flaps; } + _actuators.control[5] = _manual.aux1; + _actuators.control[6] = _manual.aux2; + _actuators.control[7] = _manual.aux3; + /* lazily publish the setpoint only once available */ _actuators.timestamp = hrt_absolute_time(); 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 44f8f664bc..60a211817c 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -368,6 +368,12 @@ mc_thread_main(int argc, char *argv[]) actuators.control[3] = 0.0f; } + /* fill in manual control values */ + actuators.control[4] = manual.flaps; + actuators.control[5] = manual.aux1; + actuators.control[6] = manual.aux2; + actuators.control[7] = manual.aux3; + actuators.timestamp = hrt_absolute_time(); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 3fcacaf8f9..630131e1e8 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -74,6 +74,7 @@ #include #include #include +#include #include #include #include @@ -264,6 +265,7 @@ private: orb_advert_t _sensor_pub; /**< combined sensor data topic */ orb_advert_t _manual_control_pub; /**< manual control signal topic */ + orb_advert_t _actuator_group_3_pub; /**< manual control as actuator topic */ orb_advert_t _rc_pub; /**< raw r/c control topic */ orb_advert_t _battery_pub; /**< battery status */ orb_advert_t _airspeed_pub; /**< airspeed */ @@ -519,6 +521,7 @@ Sensors::Sensors() : /* publications */ _sensor_pub(-1), _manual_control_pub(-1), + _actuator_group_3_pub(-1), _rc_pub(-1), _battery_pub(-1), _airspeed_pub(-1), @@ -1318,6 +1321,7 @@ Sensors::rc_poll() orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); struct manual_control_setpoint_s manual_control; + struct actuator_controls_s actuator_group_3; /* initialize to default values */ manual_control.roll = NAN; @@ -1485,6 +1489,16 @@ Sensors::rc_poll() manual_control.aux5 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_5]].scaled); } + /* copy from mapped manual control to control group 3 */ + actuator_group_3.control[0] = manual_control.roll; + actuator_group_3.control[1] = manual_control.pitch; + actuator_group_3.control[2] = manual_control.yaw; + actuator_group_3.control[3] = manual_control.throttle; + actuator_group_3.control[4] = manual_control.flaps; + actuator_group_3.control[5] = manual_control.aux1; + actuator_group_3.control[6] = manual_control.aux2; + actuator_group_3.control[7] = manual_control.aux3; + /* check if ready for publishing */ if (_rc_pub > 0) { orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc); @@ -1501,6 +1515,14 @@ Sensors::rc_poll() } else { _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control); } + + /* check if ready for publishing */ + if (_actuator_group_3_pub > 0) { + orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3); + + } else { + _actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3); + } } } From 99068e864beaabf3b2dfabfb2e2f1c6cb7df7095 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Oct 2013 09:10:40 +0200 Subject: [PATCH 785/872] Enable payload channels as direct pass-through from manual control --- ROMFS/px4fmu_common/init.d/rcS | 6 +++--- ROMFS/px4fmu_common/mixers/FMU_AERT.mix | 20 ++++++++++++++++++++ ROMFS/px4fmu_common/mixers/FMU_AET.mix | 20 ++++++++++++++++++++ ROMFS/px4fmu_common/mixers/FMU_Q.mix | 18 ++++++++++++++++++ ROMFS/px4fmu_common/mixers/FMU_RET.mix | 20 ++++++++++++++++++++ ROMFS/px4fmu_common/mixers/FMU_X5.mix | 19 +++++++++++++++++++ ROMFS/px4fmu_common/mixers/FMU_delta.mix | 20 ++++++++++++++++++++ ROMFS/px4fmu_common/mixers/FMU_hex_+.mix | 11 +++++++++++ ROMFS/px4fmu_common/mixers/FMU_hex_x.mix | 11 +++++++++++ ROMFS/px4fmu_common/mixers/FMU_quad_+.mix | 20 ++++++++++++++++++++ ROMFS/px4fmu_common/mixers/FMU_quad_v.mix | 19 +++++++++++++++++++ ROMFS/px4fmu_common/mixers/FMU_quad_w.mix | 19 +++++++++++++++++++ ROMFS/px4fmu_common/mixers/FMU_quad_x.mix | 19 +++++++++++++++++++ src/modules/sensors/sensor_params.c | 5 +++-- 14 files changed, 222 insertions(+), 5 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 93e76184d4..e0813b0319 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -166,11 +166,11 @@ then set MODE custom fi - if param compare SYS_AUTOSTART 10 - then + #if param compare SYS_AUTOSTART 10 + #then sh /etc/init.d/10_dji_f330 set MODE custom - fi + #fi if param compare SYS_AUTOSTART 11 then diff --git a/ROMFS/px4fmu_common/mixers/FMU_AERT.mix b/ROMFS/px4fmu_common/mixers/FMU_AERT.mix index 75e82bb00a..0ec663e35e 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_AERT.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_AERT.mix @@ -62,3 +62,23 @@ range. Inputs below zero are treated as zero. M: 1 O: 10000 10000 0 -10000 10000 S: 0 3 0 20000 -10000 -10000 10000 + + +Gimbal / flaps / payload mixer for last four channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_AET.mix b/ROMFS/px4fmu_common/mixers/FMU_AET.mix index 20cb88b916..c73cb2a62d 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_AET.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_AET.mix @@ -58,3 +58,23 @@ range. Inputs below zero are treated as zero. M: 1 O: 10000 10000 0 -10000 10000 S: 0 3 0 20000 -10000 -10000 10000 + + +Gimbal / flaps / payload mixer for last four channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_Q.mix b/ROMFS/px4fmu_common/mixers/FMU_Q.mix index ebcb66b248..17ff711513 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_Q.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_Q.mix @@ -50,3 +50,21 @@ M: 1 O: 10000 10000 0 -10000 10000 S: 0 3 0 20000 -10000 -10000 10000 +Gimbal / flaps / payload mixer for last four channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_RET.mix b/ROMFS/px4fmu_common/mixers/FMU_RET.mix index 95beb8927c..f07c34ac8a 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_RET.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_RET.mix @@ -51,3 +51,23 @@ range. Inputs below zero are treated as zero. M: 1 O: 10000 10000 0 -10000 10000 S: 0 3 0 20000 -10000 -10000 10000 + + +Gimbal / payload mixer for last four channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_X5.mix b/ROMFS/px4fmu_common/mixers/FMU_X5.mix index 9f81e1dc3a..6108683549 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_X5.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_X5.mix @@ -48,3 +48,22 @@ M: 1 O: 10000 10000 0 -10000 10000 S: 0 3 0 20000 -10000 -10000 10000 +Gimbal / payload mixer for last four channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 + diff --git a/ROMFS/px4fmu_common/mixers/FMU_delta.mix b/ROMFS/px4fmu_common/mixers/FMU_delta.mix index 9814667041..f0aa6650d9 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_delta.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_delta.mix @@ -48,3 +48,23 @@ M: 1 O: 10000 10000 0 -10000 10000 S: 0 3 0 20000 -10000 -10000 10000 + +Gimbal / flaps / payload mixer for last four channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 + diff --git a/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix b/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix index b5e38ce9ef..f8f9f0e4dc 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix @@ -5,3 +5,14 @@ This file defines a single mixer for a hexacopter in the + configuration. All co are mixed 100%. R: 6+ 10000 10000 10000 0 + +Gimbal / payload mixer for last two channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix b/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix index 8e8d122adc..26b40b9e95 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix @@ -5,3 +5,14 @@ This file defines a single mixer for a hexacopter in the X configuration. All c are mixed 100%. R: 6x 10000 10000 10000 0 + +Gimbal / payload mixer for last two channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_quad_+.mix b/ROMFS/px4fmu_common/mixers/FMU_quad_+.mix index dfdf1d58e0..cd9a9cfab6 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_quad_+.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_quad_+.mix @@ -5,3 +5,23 @@ This file defines a single mixer for a quadrotor in the + configuration. All con are mixed 100%. R: 4+ 10000 10000 10000 0 + + +Gimbal / payload mixer for last four channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_quad_v.mix b/ROMFS/px4fmu_common/mixers/FMU_quad_v.mix index 2a4a0f3419..520aba6359 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_quad_v.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_quad_v.mix @@ -5,3 +5,22 @@ This file defines a single mixer for a quadrotor in the V configuration. All co are mixed 100%. R: 4v 10000 10000 10000 0 + +Gimbal / payload mixer for last four channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_quad_w.mix b/ROMFS/px4fmu_common/mixers/FMU_quad_w.mix index 81b4af30b2..58e6af74ba 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_quad_w.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_quad_w.mix @@ -4,3 +4,22 @@ Multirotor mixer for PX4FMU This file defines a single mixer for a quadrotor with a wide configuration. All controls are mixed 100%. R: 4w 10000 10000 10000 0 + +Gimbal / payload mixer for last four channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_quad_x.mix b/ROMFS/px4fmu_common/mixers/FMU_quad_x.mix index 12a3bee20c..fa21c80128 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_quad_x.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_quad_x.mix @@ -5,3 +5,22 @@ This file defines a single mixer for a quadrotor in the X configuration. All co are mixed 100%. R: 4x 10000 10000 10000 0 + +Gimbal / payload mixer for last four channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 4d719e0e15..682cb3d81c 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -193,8 +193,9 @@ PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); 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_AUX1, 0); /**< default function: camera pitch */ +PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /**< default function: camera roll */ +PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); /**< default function: camera azimuth / yaw */ PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f); PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f); From b5d3355dfb513bbfab362734a672f8c51fc10402 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Oct 2013 09:22:47 +0200 Subject: [PATCH 786/872] Fix accidentally comitted hardcoded autostart --- ROMFS/px4fmu_common/init.d/rcS | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index e0813b0319..93e76184d4 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -166,11 +166,11 @@ then set MODE custom fi - #if param compare SYS_AUTOSTART 10 - #then + if param compare SYS_AUTOSTART 10 + then sh /etc/init.d/10_dji_f330 set MODE custom - #fi + fi if param compare SYS_AUTOSTART 11 then From 39336fd58533c85ef6bad93b80dd51e62b6eba3a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Oct 2013 10:46:28 +0200 Subject: [PATCH 787/872] Better defaults for RC SCALE --- ROMFS/px4fmu_common/init.d/1000_rc_fw.hil | 2 ++ ROMFS/px4fmu_common/init.d/100_mpx_easystar | 2 ++ ROMFS/px4fmu_common/init.d/101_hk_bixler | 2 ++ ROMFS/px4fmu_common/init.d/31_io_phantom | 2 ++ 4 files changed, 8 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/1000_rc_fw.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw.hil index 6e29bd6f80..5e4028bbb8 100644 --- a/ROMFS/px4fmu_common/init.d/1000_rc_fw.hil +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw.hil @@ -33,6 +33,8 @@ then param set FW_T_SINK_MIN 4.0 param set FW_Y_ROLLFF 1.1 param set FW_L1_PERIOD 16 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 param set SYS_AUTOCONFIG 0 param save diff --git a/ROMFS/px4fmu_common/init.d/100_mpx_easystar b/ROMFS/px4fmu_common/init.d/100_mpx_easystar index 1aa1ef494e..4f843e9aa1 100644 --- a/ROMFS/px4fmu_common/init.d/100_mpx_easystar +++ b/ROMFS/px4fmu_common/init.d/100_mpx_easystar @@ -29,6 +29,8 @@ then param set FW_T_SINK_MIN 4.0 param set FW_Y_ROLLFF 1.1 param set FW_L1_PERIOD 16 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 param set SYS_AUTOCONFIG 0 param save diff --git a/ROMFS/px4fmu_common/init.d/101_hk_bixler b/ROMFS/px4fmu_common/init.d/101_hk_bixler index 4d8a24c4ae..cef86c34d0 100644 --- a/ROMFS/px4fmu_common/init.d/101_hk_bixler +++ b/ROMFS/px4fmu_common/init.d/101_hk_bixler @@ -29,6 +29,8 @@ then param set FW_T_SINK_MIN 4.0 param set FW_Y_ROLLFF 1.1 param set FW_L1_PERIOD 16 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 param set SYS_AUTOCONFIG 0 param save diff --git a/ROMFS/px4fmu_common/init.d/31_io_phantom b/ROMFS/px4fmu_common/init.d/31_io_phantom index 8fe94452f8..6528337454 100644 --- a/ROMFS/px4fmu_common/init.d/31_io_phantom +++ b/ROMFS/px4fmu_common/init.d/31_io_phantom @@ -29,6 +29,8 @@ then param set FW_T_SINK_MIN 4.0 param set FW_Y_ROLLFF 1.1 param set FW_L1_PERIOD 17 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 param set SYS_AUTOCONFIG 0 param save From 8ed0796448e49ae34dbfdf31c056e402834c0b8f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Oct 2013 22:17:53 +0200 Subject: [PATCH 788/872] L1 control diagram illustrating corner case --- Documentation/l1_control.odg | Bin 0 -> 11753 bytes 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 Documentation/l1_control.odg diff --git a/Documentation/l1_control.odg b/Documentation/l1_control.odg new file mode 100644 index 0000000000000000000000000000000000000000..69910c677459aae4664a08b1bbeb6aaed48721ad GIT binary patch literal 11753 zcmeHtbzGEd)Bg&BfOJYIEgefY(jwhR!?H9i>@Hn`Gzf?wAkxw$-5^RRh)8#LhjjiH z&+*acJf7!w-uLf!{4CykUo+pCYwqiwz2=%xRY1N;2mqh~03lk6QmM%7tt*v4e+-GzI71zTzS|?@U}_JB3Bf@^zZxd=E4R>h zMSt9Jv$J-v6{WF)!ySbA6$47K~tWe(y1 z+kvmA4&wl_137;1UR(H2jk@Oj)g(tK7={4+?&p7s@vF?QSP%&0Uq)a1;}3UzH=T=< zlZWGv??0@D*<0KFQ;pZWY#@leBf?HIJMgbcO&zT5|0&Eh@AsMh3SbE}H%A!qpAQ9L zGdG7Nhq)=-l+DE&>`ME?@;~PB-)AfMm)V*_O_*Vs*fkLs5pV@D{&1`L98fVl)sxqv)uTxMoq zbAB#1PChQ6Ihc>lRFI#SO8~?sV8O}9Xa1+^pIZLV`mc8zcD?XnaBC3NcUM8ouCWmd z&DjAiO2f@Z!yzW}=W`VL!}CAB{R`py?LPt#8XDU7h=vGa{~}Dp6$}i&b|dUM2uW^P{*3dBL}#D(%V2 z-=Zn2_Pq-RZI1e5x>QPT2)o~)X4;GF7bp^Dcqh8gWDI|uvzq3z<+XO{82U}{AZ;^~ z1m&HUlE1n~h)8dLtzC(+>+6R*eE9O@j6o|jrgSFHp?Wv;N2b=#1+x;?*lsaHU+Xe=EnSlQ>i4@vV^ObbCOji}iS^Lp$>^*(LFcS)#Si z{D;Tu@fpXJPXc&5z1>iJ<8RMxbU#{JIVLQiA(j%^cl{juXyu_}-Fn@F!$46JK`(%zFlZx!Hbioe1S57SDI#^Eoasg#TaZ5 zMq9ILdPw4CL$&{*^KpAFjv8+`Y5Bt`{4MTiQQ{X`fwQ)fI#*D(k7gNBWCiA)4tKB^ z88F&EYKKnCp8BnkHf^+OmW;DorWS+|Ws!5yuo#}W?1lzJS?Y|Tl|t(rW=pZN)1tDNt@T75_onIAd+KnvUv|=C zc*9wHx^E^ZThEa1WFp~!@iAOibY{ZR8`>WV*Y@Ndi3I96&-YI-8wA~_Ug1DDen(pj zAtq{F2vDP3xYJ78KAL30#P>}9Gqz$ZJG1mgka7%+?Z_3`JIba6PJuieD-lt)XlreK zF5bu;%YV6(JUd~;bo3kzy>^Iytt#+}atFC8UXs#k)8pd~^y96fs$f2UUf^le86!Y~ zFbqSm*bhY01l5gvw|ii$+c9i21dI)x;pO3noIK0lQ|Wcp4wdSWW|j@xN?=>46U}lBo4hoYnA)ra=Ra>?R-mG7AfV} z=!z*$?WOA;U37*Cs^sw16LTY6wt93B8!fSaK$kq>O)dIix)`5uv`m}fO>mtQI%`OJ z@#`g!qalHudFAx|F=>JiHQ9meWgl6~3Bm1o?CezP5$L&AI!rzgh?k_%OxLN#(XA;< z#sYx?f+)`1C9c;GD4V**Z-@sr6|2-Cp=zK>x}<3@TAobY&Z$32#d4Us>V#jRsw&(< z!&QkJ!$3jo`Dg)vzmK7}5it}5ae!ZM`4Ay=({LO*{t?f+M@P}LJnvwv<})TLGUH3l z`pU|TrTH+21|XYqX2&Q13e0r^1%D42WgXf$w!0XxR3l_DaRf5{t zCM7h>MHnBTIOxzVRRq=XHJ;Z!R{T<&)OIhS(Uw2;wWWG(D)T^U3iH7B82jX&hl-c| zg(%u$*O_ZlLub6k8GfB;Zi7hY{b`G=$x4;}3TLf5nokYu6fi#w6!hHPJjgv2it41= z&h^Y(OCk4Un_GpFSfvF!e^q@-{(Ntf3jYD2CuWFIsQ%*n>G6H>>E^SxdhC~5vVuZ| zg|hN$b)QfZ%}Co`WCH3uV}_J6KIKutZ%Hwbe0odbl)WtRatTbH=IDX<#OPsouXAn0 zkw!u^Uw-(+k!!xcu;wixWG*#GhRMaVz5?a>&{aG7>^G-U_to?CZ_8r6O~0M|QkhDN zKaj<^qLNopC)Btt9f`B1+g}8qhX197Wh1c%5k1CSkj2=gD=%_Neiu?1wye&pWaq*& z6lUWoWn1Nl+yR_w{RuOY5=N_97B|YP^xKKdYuuV?bva*P=xXe*f}_LW7>$iH#hsew z#W|hK>IMx(`JrAigWK;{VQoEc)s4{%@9?@h=H++UEU_cWOhf8E-pUT2=q{!$seN5r z>&6p9Inenq!g{*fJIlJ7PmTK>BWnwmP{Ro$!RDnZ)-9j z5N@kd2!KyJ~wju{0Fhg|mP9?Nnkjv#=GD`@2numWP$(6CUQA|TMH zHdEhL+1L0c3^CrLhVMnwo|8JGF%v#5hIiKgEXkMV3j-K?2ZZ$5Sn`SQv#tp&Pc<OvTk>=Z)y(D$Oaf z-a9%L&!&*Be0^z$G8IN`tGSsfVwEH3or&WUz@;6P7lnCs&0>6#3FWO!vw`WY!HST4 z-fc4!v(^nroMm{9C~;5ndnJ)g&o&q*KwLp2RU~!MM}vaMd2z#Z2b;w(P@o2VuI`;t z(VPII8a?4~x_8#8Zm4Eyi(~QmcJq}raGj@5LU}AiP;0};?@dvg6fIRowO1x%bbs=eS&Qj^6o+AO+LFM6Gzx`bDfB2#5x(^y0oJ z6O<45OiN>)N+-Etg@)HHA|FtgNiD5{Pw6A#r5u~HnMrM)!l)uFC%wYsPDU>GG{pO{ z^d#PJ&)DhI=8T?W+^Oqku*dY#T#>;NUVZbRzsi9fyV$%-pi}u0J%99iU|t3$wAS9B z%LhZ!l_*?3LeuKip*G5KCh@QXF(!HJc;}{vCd7MFiZ*mo*eL;Hs!!{2k!6)Buy)dJ zf%mp%jUfdKjGrmL|AD(+9r#I;MyPCJ;N5;W<_2!35p^Ds0@JW!hdzy5Ijc#mPWL%o z|GFo7jIGndwqQZ+I?T>zMk<){z@rU>V$hc>V zPE08E)_phoj0QUlQGw9~ zPB!TMZRWkj=v9p7dnQAkbX&8AJth-r@=MHiiOdY+g{n3*I} z@hz*opo&iqd}UCBX@A^ObFs3ojv_r5xY_-zD>`GXgHXKjW0(w&NsNHufr=Hm zD7Bw8o@zWB$^KO9M>nv<=m4V=MbKF-;N(ljae`vs)Ja4GL+@- z#)->l57O?A_Vyo*d<_+JkgMKgm-06PeTi@qhE=E1(98$fjimd*xg5;x$v+jkvF(8- zqPN$Sil8^kdYFI%k36F54Vb!R zO~=+8Kedijk9_zPZsUPEt*>#%!pETB^uW^+Ot@F2$eqQ26662KtKfa6LWM!7VZ12x zs47MWa{&L_liGVNBMWK~N?5R@Qor`Q>v)CsVOWf6Lrh}!Lly}FWWYQr!SsOcLecWW z&tu%S;vZm#-VA1$5tCm&gnZ-u@Zc-pNlf0^`-y$kjE!|ec6myZdYMu4$J8IHFyK zrueLewq=&?AAE7{NRHC2Jx-s(CMN41avYl2&*t@pb#QQHar*Uj2)$K(hu356TvFk$ z3H6|QMOGx?r3<}qBl`rleuIoLd?y9HKnBM)NKm(V{0nQgpyElhlUEFtHMw2ikhn_}T zM+t`Yi!bwLE5?Qob8K6o;F3_%vQOcu2ENGK(7dTPt{0|wqHak<0tSzLhF-8N&Yx!MoFO#y%jUQ1x%oK1BY{m@&<=MWFT6( z6PJjz{~;)Uz$LAFYlTk!aKI}Px&M;AfbKCxo8KQ{_G4fj)u$>5?fMvmGG8Odk+&#s2ID(ibiQAMK z-6OcKK7*El%ZFkjv^SR|&|2|0ZPEPN z6GdxI!%7CmyprN;=$dmBw!OpSo0fgwG#raxXB7q6a#nW{2QPilA6dnZv*yKzx+W>8 z0KoO<4kD}2v~spLb1=2GgK@xrT(UblSca-9%VA-VVIWwrjE6#6zS`Xj((HH2a#;FfsyMIi1r% z_Xg7Jd&O?@t~!$vkduvr?;XAJRGO0Rl&beusMy1OiHsk&M5m^v-dlH_)=#$O>+eMw zhl`=|2G^d~9TW|5b?eu44YLnF6GJ_S1oMh6fZy-MXGYzdRD(=$4Rr5&*5e{zLD`hz zK5)qS<;B@$l)1*Kn?!l&6rn6h4TZO&R^-J>G(Sl~+4;#T$bgrnL1<)g@0-t#=04Eu z6OZbJNlkFPOB_yml({^`;7EL?krLhH*@@eg+tv2P`mhe{WX?Q&1@!ejR>%@34!t@v z0n?Sfj{0j>LT|gQ6!9S9Kut9KXZ#D4(t)F+Hhq;lS8XLSJnj1)tvR z{a#R}VSjq1qV1TKkQc`F{Kxo2iwBb>>*{NAgr*CkKmp0IdoOLm-{@^q)Q9zU_{bW& zxqO{}v|m~@Mf-+8!|I->aG2O-=}^0zfa6p>NxxpUyVV;3xJz$`R}tD$vf`)UUhGuK z{nCfYZ86;Tja~dXxZ^oGgZ1QnlM;8yW*S)Z??aDW3dEMk$!fPYi?GGS#S8pW_x6tH zqZ|djKl6K_77yuVPl#zH+vGApF&2L!GKeN=Tpz`C0k^x zfVfvck)r(Ma@7_=O_iOr107vMLcV;S=Z6is8kF|#1m?5T-l^gDv_+5$qd<0by>I74 z*E2g5Jc0d{Td;5e?PeA4eey@gFr0mxs__Vxi={AhIjq}Y4OmU#$J9VaD zXuHR_4;&ZG4puuI=~=uVgD%wH$?l@yL!A-W~3QxUrtTSEw#MqtAXI?rHE^p7{`|c(cX}tO}CAaQA zDvVFLO=;ZRq)nO5l<6~!V0EONM8=uK>Kn3@s|qSQw>(%nJQELvh}aKAu4Z1oUd{Nn za67(WxR&Quc37r+^|0SD5VK;j4k^Q2WHcm#t$r6KSlj{a-h)Jp7KuvsE zub$j?@+w!!cbI#g@#RtPWUcOL$o;GK&8_C6t2sX>!h+9~^%t}GM@dsBY@aBFVVW-w zOM56Cs#SK@x5P1wAPTE$_e~ z+mRD{Om9x+RfN9)7Z85ija!_g*KzMl60{*%$>ef+S2d37p53(%oNSPOAFLoYigl(d zNx^osF+a%k#$7of+w)NEvtd_)h@Qi^oTxpu??nE`5)wkWlB6Tk z7qIlCBVcEWg3C9t`;m=tUC~KLYOT_CiIH`_l!2?JescRjY5vT6!yVWxZc{$7~uH^Pn( zM`uSw!O{7DC>N!Uz;^#bx$An2GtA5s`d{QIerG2XVu>hZ{(mH|fAat8o@-10 zyT0E^UAyF`)b9#E48PH33IZYOx8KWM93W>X^v?rLWW?F$t?NoP;Cg#;O@IH8erJq@ z9RyJU{Nb|S8FRgnfx+Do)wu69DE;op`5L^YWhP8EzK7TqJ{u|4!u^dS8)l_9Tu@uS zY}869#J1giM97#wnWFQNI+$XQ*!^jPVswLCn0O46e%WF~y=R~BJG0h&`~opz@v_cW z{L>-}c2E^*8F#1o!2Wkh(KJiwY(xB09F#r}hNt+eo83S3$F`d%?$msdr&gn{j?g6F zWY5pSn{umUd`Y2Gp+_vhFsR z1rH11xp5Eat|Da@yLWbwiECnJQM8G0l)mva65-VIYS`ss>dM-N@FlHU@GY8c28Xx3 zAMP8Ptr=voZK=@4KMTO)eY_al{}lp<(d_D8#TKutt1Yw8$Vxq>7tWO3C1zl>0cdptL`q-%k^?z!2Fl72Zop6cRGzgG%l zn#9&DO

yd{$-KBM33nLbWKjb_j9GK5s16nwR3-p{lKyLhc z3rl)StFzi^Irdp38T?kvO%gcI{-Yx*tw1DS&i5?F(J{A2kzyhl3=`;Wm}p-Jl4oPm z+iMQf$a`qDBq!qR-$){v)F?aYTz`$)L10=MkM>GWCDDpL-ZP3Uz^?>8ws||RRG6`~ zPP9|qh$Qs!Eo3Tp#?Z7!0f~J2E>mA;1M**e5_1$g-(VqCU&i9h7u|ae;@fjb;vS^P zs)oc?za4Iqqq%>&-I+|jtxqi=eWSHPlk@7i_=H;vU4nFrIdOuM$xD#{AYpzfD~6bR zleaNF%GEQkZ>lRW5@bJcsEViAF`6`tD?7j^p=hww3V@)ojVwgq=N2S(L1GY5t-t8c{68F zT1kwB!~i82ZxhIIu|L@{2M9+E&Y(y6nySxK%)6AN7EQg$gl8wU@YG<;H{7l)Aj8V5 zlHi@kjTbB?h?#pe+BMS}k#%Rp$#cF4g`Jzs^I5UCY}S)xX1c%ywLII>P{DaeTSk(Qzks~LCmbq0U7_)rxz8U(@mAVinSbv2>{~r&qG5>&XBk`rL4qa;4`}!s zLf&4^wHAw7pKt}Ei&^BaM!fEFd@oYnBX4t$guYwJE2#8xsIMd}SeyD^lM-g;z`Gh-RFGS^;S!EI3CmciD z1>swFu@*s@CL)yz4$1cBsb#WejSe@b8`ohtM% zi&j*&8w|Ic58YqDx3c-Av)pJ}Xm-EkAO$nbioWh`g{_)O5hy8!#VVHu2}-o+)c92g z_2({@M~Va`=VB0*UkG8mRf#Rvjt*L_m@OpXQ^p-f& zTh|SqWweIox8v>jLFprkW4R+u+Ojm`C&fr<^9F8g+`egO8QIGEAc(-5ItN`<(-OA@ zoJE5#3h!OPsBV1Us8#2k8SaSMfguzlRZRA@UWmKXN{d@|p%rbgMyzc=6OUTUbJFeV z^vrlX0A@6|@Ve@gwcj1YL95}q&K6#^O3_)v$vy3z3qE6~rDUo4pxetjr!g~>;ro~r z89nbLQbVj{((ch=p>g1(kGA1rD5W$D1T2wAubj%zz@32s8LQ_qH2Z62Xx&D1VkI2G zJ+~KE*TboKeA7i5Yp5hSUgUh3F~c>y@Wrd7ts+Vi*L1sU|LWa@>Sm$VyX>&H^Z|G6 zm_xR5^VI7=_I_r;&g-Zf?_B5mP6ypA~< zq}LLja`~}Z)uz15Me@DvI64s2y+NZx!0X0CpSauU)QFlEuu6S!(pb8}w_Q%ELZ?}X zhA|@Pnru+$;lXxw(qVjg(bFs{Q_^NKi{UJ*Z_SU!&_s)ImQ2#$b($Ax?a#37q?T;w zU|2)Mjcdk2i4A=qlXK%!7iN+dR6{!H_IpNoPiXM;QI>M5|D;C!#UeaV8UFNmO$WaV zEACzW5=q2VwrW4DE#smPaJK?UtQx|DRf4J~=XY6>!#i-f1AUXXf9j_3X`{F$rSG%q zM6DH>tec_$+u}rj*jlVlEZ^5FTtVDgn`}|#l^o~?p2K}*(oMwgiAwa5#hK5`c$O5q z)c*c?R|Zd`CL|k4N8*NXDOc8S1)9~{o-PSXQ~Qgk?(H&Vo;Y?6!?d$$ zeS!haU!_xedLEe*sUK4Su`a{#YMQG>MxB5@B#Fpw8&K(G4!QjmOI1$=G#Bw6_z;!S zyj$sU*^tTS`~2k_!{PmoX=ZjPqU)YUy(5PEqmP5-WgRoNKJ@gkGNe^oeaWO2$aEBu zUAB)v&9qFJ=RJc%p@x7$N%VdK?XE zM&384UGujTDKcQp=lKI8)za%VO|f))WXg?e4nQ6HX|J=MD7s=ey@ zt+zjt#pR^7%V=3cae=kC8QC32q4|okZ{m{VdY5#gQ+=c6x^s~G$1dr2`yC?0en&}0 zQ-V!iS(d{d(e-5khJCLc7iqLOe4xT?JYu2Q@+-L>8w|gO_rTqp3wOob|tR9V!LTK>Yy=s>;Xz=|K`--d^9>H#3)py zIDuR%D!dYrZ;knti^!i>PtBTuVdI!Jfx~%^I^Pk|k5wugjI7>J=oTD5qiDulf~537 z)9azit)Y9?V+W%(!5|mo+z2Iqc4{1f32LA+7FG5s^GnyLMpEc&dqw^93e?;52Wl%6 z^V3NeQR^-Bk4PENMaYZe(O52Wdf#Vm@y`uYPI@C>J2fA$*nxio0GL9)cIpizLcrg* zXI^hlf6|X;&A&?hwFmR3gc{=Vx@YQoBl<(?$0Pi;9pJig>L>LfdO3e>qx!4luSb5@ z?E*iE7_s;LrFr0o;NP7H{uHz${`>R6zoPt<0syXau%Gnc?ti8n|B;dXyT^YD0|3`K z*-!HR4cM<4+CRmq{0+{p8QVX_+58R8uNmCm<6P%(KZ*X{e>w@#=l4rS_xDKG3DHk7 z{teR44DX+!#Qg^4XU6vj%8xaBt@}sXcAfM6q^{qf{7nY<-`3N=Pml(&y#AWP{>=aW z-Lzk0<#h)5lXMX%Kl8yK+J8*x*KlwhNq^Eq#=nWHstPEGBXs}(7x9k|@vbH_U4Q$3 DeGk#2 literal 0 HcmV?d00001 From 4532eca4ef6f6170765062ccd2ca7f29814e0b3a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Oct 2013 22:55:16 +0200 Subject: [PATCH 789/872] Covered corner case in L1 controller not adressed so far in existing concepts --- src/lib/ecl/l1/ecl_l1_pos_controller.cpp | 33 +++++++++++++++++------- 1 file changed, 23 insertions(+), 10 deletions(-) diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp index c5c0c7a3c1..daf136d495 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp @@ -130,8 +130,12 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c float alongTrackDist = vector_A_to_airplane * vector_AB; /* estimate airplane position WRT to B */ - math::Vector2f vector_B_to_airplane_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized(); - float bearing_wp_b = atan2f(-vector_B_to_airplane_unit.getY() , -vector_B_to_airplane_unit.getX()); + math::Vector2f vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized(); + + /* calculate angle of airplane position vector relative to line) */ + + // XXX this could probably also be based solely on the dot product + float AB_to_BP_bearing = atan2f(vector_B_to_P_unit % vector_AB, vector_B_to_P_unit * vector_AB); /* extension from [2], fly directly to A */ if (distance_A_to_airplane > _L1_distance && alongTrackDist / math::max(distance_A_to_airplane , 1.0f) < -0.7071f) { @@ -148,21 +152,30 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c /* bearing from current position to L1 point */ _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX()); -// XXX this can be useful as last-resort guard, but is currently not needed -#if 0 - } else if (absf(bearing_wp_b) > math::radians(80.0f)) { - /* extension, fly back to waypoint */ + /* + * If the AB vector and the vector from B to airplane point in the same + * direction, we have missed the waypoint. At +- 90 degrees we are just passing it. + */ + } else if (fabsf(AB_to_BP_bearing) < math::radians(100.0f)) { + /* + * Extension, fly back to waypoint. + * + * This corner case is possible if the system was following + * the AB line from waypoint A to waypoint B, then is + * switched to manual mode (or otherwise misses the waypoint) + * and behind the waypoint continues to follow the AB line. + */ /* calculate eta to fly to waypoint B */ /* velocity across / orthogonal to line */ - xtrack_vel = ground_speed_vector % (-vector_B_to_airplane_unit); + xtrack_vel = ground_speed_vector % (-vector_B_to_P_unit); /* velocity along line */ - ltrack_vel = ground_speed_vector * (-vector_B_to_airplane_unit); + ltrack_vel = ground_speed_vector * (-vector_B_to_P_unit); eta = atan2f(xtrack_vel, ltrack_vel); /* bearing from current position to L1 point */ - _nav_bearing = bearing_wp_b; -#endif + _nav_bearing = atan2f(-vector_B_to_P_unit.getY() , -vector_B_to_P_unit.getX()); + } else { /* calculate eta to fly along the line between A and B */ From 71ac33596836519a341001bb48a8835b8af75cd3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Oct 2013 21:43:11 +0200 Subject: [PATCH 790/872] Small improvements to autoland, ensure that throttle can be shut down close to touch down. Depends on accurate land WP altitude --- src/lib/external_lgpl/tecs/tecs.h | 5 +++ .../fw_pos_control_l1_main.cpp | 32 +++++++++++++++---- .../fw_pos_control_l1_params.c | 1 + 3 files changed, 31 insertions(+), 7 deletions(-) diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 2ae6b28bb4..4a98c8e974 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -94,6 +94,11 @@ public: // Rate of change of velocity along X body axis in m/s^2 float get_VXdot(void) { return _vel_dot; } + + float get_speed_weight() { + return _spdWeight; + } + // log data on internal state of the controller. Called at 10Hz // void log_data(DataFlash_Class &dataflash, uint8_t msgid); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index cd4a0d58ef..3697af225e 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -196,6 +196,8 @@ private: float throttle_max; float throttle_cruise; + float throttle_land_max; + float loiter_hold_radius; } _parameters; /**< local copies of interesting parameters */ @@ -227,6 +229,8 @@ private: param_t throttle_max; param_t throttle_cruise; + param_t throttle_land_max; + param_t loiter_hold_radius; } _parameter_handles; /**< handles for interesting parameters */ @@ -342,6 +346,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.throttle_min = param_find("FW_THR_MIN"); _parameter_handles.throttle_max = param_find("FW_THR_MAX"); _parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE"); + _parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); @@ -404,6 +409,8 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.throttle_max, &(_parameters.throttle_max)); param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise)); + param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max)); + param_get(_parameter_handles.time_const, &(_parameters.time_const)); param_get(_parameter_handles.min_sink_rate, &(_parameters.min_sink_rate)); param_get(_parameter_handles.max_sink_rate, &(_parameters.max_sink_rate)); @@ -625,6 +632,9 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio _tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); float altitude_error = _global_triplet.current.altitude - _global_pos.alt; + /* no throttle limit as default */ + float throttle_max = 1.0f; + /* AUTONOMOUS FLIGHT */ // XXX this should only execute if auto AND safety off (actuators active), @@ -634,11 +644,12 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* get circle mode */ bool was_circle_mode = _l1_control.circle_mode(); + /* restore speed weight, in case changed intermittently (e.g. in landing handling) */ + _tecs.set_speed_weight(_parameters.speed_weight); + /* execute navigation once we have a setpoint */ if (_setpoint_valid) { - float altitude_error = _global_triplet.current.altitude - _global_pos.alt; - /* current waypoint (the one currently heading for) */ math::Vector2f next_wp(global_triplet.current.lat / 1e7f, global_triplet.current.lon / 1e7f); @@ -712,16 +723,23 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ // XXX this could make a great param - if (altitude_error > -20.0f) { - float flare_angle_rad = math::radians(15.0f);//math::radians(global_triplet.current.param1) + if (altitude_error > -10.0f) { + + float flare_angle_rad = math::radians(10.0f);//math::radians(global_triplet.current.param1) + + /* set the speed weight to 0.0 to push the system to control altitude with pitch */ + _tecs.set_speed_weight(0.0f); _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min), _airspeed.indicated_airspeed_m_s, eas2tas, - true, flare_angle_rad, + false, flare_angle_rad, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + /* kill the throttle if param requests it */ + throttle_max = math::min(throttle_max, _parameters.throttle_land_max); + /* limit roll motion to prevent wings from touching the ground first */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-20.0f), math::radians(20.0f)); @@ -785,7 +803,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio _loiter_hold = true; } - float altitude_error = _loiter_hold_alt - _global_pos.alt; + altitude_error = _loiter_hold_alt - _global_pos.alt; math::Vector2f loiter_hold_pos(_loiter_hold_lat, _loiter_hold_lon); @@ -862,7 +880,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } _att_sp.pitch_body = _tecs.get_pitch_demand(); - _att_sp.thrust = _tecs.get_throttle_demand(); + _att_sp.thrust = math::min(_tecs.get_throttle_demand(), throttle_max); return setpoint; } diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index d210ec7126..9b64cb047a 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -72,6 +72,7 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f); +PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 0.0f); PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f); From 013579cffd70f46788a356043563340731beabae Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 17 Oct 2013 07:54:04 +0200 Subject: [PATCH 791/872] More improvements on landing --- .../fw_pos_control_l1_main.cpp | 72 ++++++++++++++++--- .../fw_pos_control_l1_params.c | 2 +- 2 files changed, 64 insertions(+), 10 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 3697af225e..348a17ba63 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -727,15 +727,17 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio if (altitude_error > -10.0f) { float flare_angle_rad = math::radians(10.0f);//math::radians(global_triplet.current.param1) + float land_pitch_min = math::radians(5.0f); + float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; - /* set the speed weight to 0.0 to push the system to control altitude with pitch */ - _tecs.set_speed_weight(0.0f); + // /* set the speed weight to 0.0 to push the system to control altitude with pitch */ + // _tecs.set_speed_weight(0.0f); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, _parameters.airspeed_min, _airspeed.indicated_airspeed_m_s, eas2tas, false, flare_angle_rad, - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + 0.0f, _parameters.throttle_max, throttle_land, + land_pitch_min, math::radians(_parameters.pitch_limit_max)); /* kill the throttle if param requests it */ throttle_max = math::min(throttle_max, _parameters.throttle_land_max); @@ -814,7 +816,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio _att_sp.yaw_body = _l1_control.nav_bearing(); /* climb with full throttle if the altitude error is bigger than 5 meters */ - bool climb_out = (altitude_error > 5); + bool climb_out = (altitude_error > 3); float min_pitch; @@ -842,6 +844,43 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio _att_sp.roll_reset_integral = true; } + } else if (0/* easy mode enabled */) { + + /** EASY FLIGHT **/ + + if (0/* switched from another mode to easy */) { + _seatbelt_hold_heading = _att.yaw; + } + + if (0/* easy on and manual control yaw non-zero */) { + _seatbelt_hold_heading = _att.yaw + _manual.yaw; + } + + /* climb out control */ + bool climb_out = false; + + /* user wants to climb out */ + if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) { + climb_out = true; + } + + /* if in seatbelt mode, set airspeed based on manual control */ + + // XXX check if ground speed undershoot should be applied here + float seatbelt_airspeed = _parameters.airspeed_min + + (_parameters.airspeed_max - _parameters.airspeed_min) * + _manual.throttle; + + _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, + seatbelt_airspeed, + _airspeed.indicated_airspeed_m_s, eas2tas, + false, _parameters.pitch_limit_min, + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + _parameters.pitch_limit_min, _parameters.pitch_limit_max); + } else if (0/* seatbelt mode enabled */) { /** SEATBELT FLIGHT **/ @@ -861,13 +900,28 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.throttle; + /* user switched off throttle */ + if (_manual.throttle < 0.1f) { + throttle_max = 0.0f; + /* switch to pure pitch based altitude control, give up speed */ + _tecs.set_speed_weight(0.0f); + } + + /* climb out control */ + bool climb_out = false; + + /* user wants to climb out */ + if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) { + climb_out = true; + } + _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed); - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); + _att_sp.roll_body = _manual.roll; + _att_sp.yaw_body = _manual.yaw; _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, seatbelt_airspeed, _airspeed.indicated_airspeed_m_s, eas2tas, - false, _parameters.pitch_limit_min, + climb_out, _parameters.pitch_limit_min, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, _parameters.pitch_limit_min, _parameters.pitch_limit_max); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 9b64cb047a..c39df9ae31 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -72,7 +72,7 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f); -PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 0.0f); +PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f); PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f); From 95aba0d70eae6a9aba55d31f223b852df1f82dcb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 17 Oct 2013 09:36:20 +0200 Subject: [PATCH 792/872] Almost perfect landing approach, needs touch-down fine tuning --- .../fw_pos_control_l1_main.cpp | 47 +++++++++++++++---- src/modules/mavlink/waypoints.c | 37 ++++++++++----- 2 files changed, 62 insertions(+), 22 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 348a17ba63..219c6ffa6b 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -635,6 +635,9 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* no throttle limit as default */ float throttle_max = 1.0f; + /* not in non-abort mode for landing yet */ + bool land_noreturn = false; + /* AUTONOMOUS FLIGHT */ // XXX this should only execute if auto AND safety off (actuators active), @@ -717,23 +720,39 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else if (global_triplet.current.nav_cmd == NAV_CMD_LAND) { - _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); + /* switch to heading hold for the last meters, continue heading hold after */ + + float wp_distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), next_wp.getX(), next_wp.getY()); + + if (wp_distance < 5.0f || land_noreturn) { + + /* heading hold, along the line connecting this and the last waypoint */ + float target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY()); + _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed); + + if (altitude_error > -20.0f) + land_noreturn = true; + + } else { + + /* normal navigation */ + _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); + } + _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ // XXX this could make a great param - if (altitude_error > -10.0f) { + float flare_angle_rad = math::radians(10.0f);//math::radians(global_triplet.current.param1) + float land_pitch_min = math::radians(5.0f); + float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; + float airspeed_land = (_parameters.airspeed_min + _parameters.airspeed_trim) / 2.0f; - float flare_angle_rad = math::radians(10.0f);//math::radians(global_triplet.current.param1) - float land_pitch_min = math::radians(5.0f); - float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; + if (altitude_error > -5.0f) { - // /* set the speed weight to 0.0 to push the system to control altitude with pitch */ - // _tecs.set_speed_weight(0.0f); - - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, _parameters.airspeed_min, + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min), _airspeed.indicated_airspeed_m_s, eas2tas, false, flare_angle_rad, 0.0f, _parameters.throttle_max, throttle_land, @@ -743,7 +762,15 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio throttle_max = math::min(throttle_max, _parameters.throttle_land_max); /* limit roll motion to prevent wings from touching the ground first */ - _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-20.0f), math::radians(20.0f)); + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); + + } else if (altitude_error > -20.0f) { + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_land), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, flare_angle_rad, + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); } else { diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index ddad5f0df6..adaf814047 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -399,6 +399,13 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ if (time_elapsed) { if (cur_wp->autocontinue) { + + /* safeguard against invalid missions with last wp autocontinue on */ + if (wpm->current_active_wp_id == wpm->size - 1) { + /* stop handling missions here */ + cur_wp->autocontinue = false; + } + cur_wp->current = 0; float navigation_lat = -1.0f; @@ -419,13 +426,16 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ navigation_frame = MAV_FRAME_LOCAL_NED; } + /* guard against missions without final land waypoint */ /* only accept supported navigation waypoints, skip unknown ones */ do { + /* pick up the last valid navigation waypoint, this will be one we hold on to after the mission */ if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT || wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS || wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME || - wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM) { + wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM || + wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_TAKEOFF) { /* this is a navigation waypoint */ navigation_frame = cur_wp->frame; @@ -434,17 +444,20 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ navigation_alt = cur_wp->z; } - if (wpm->current_active_wp_id == wpm->size - 1 && wpm->size > 1) { - /* the last waypoint was reached, if auto continue is - * activated keep the system loitering there. - */ - cur_wp->command = MAV_CMD_NAV_LOITER_UNLIM; - cur_wp->param3 = 20.0f; // XXX magic number 20 m loiter radius - cur_wp->frame = navigation_frame; - cur_wp->x = navigation_lat; - cur_wp->y = navigation_lon; - cur_wp->z = navigation_alt; - cur_wp->autocontinue = false; + if (wpm->current_active_wp_id == wpm->size - 1) { + + /* if we're not landing at the last nav waypoint, we're falling back to loiter */ + if (wpm->waypoints[wpm->current_active_wp_id].command != (int)MAV_CMD_NAV_LAND) { + /* the last waypoint was reached, if auto continue is + * activated AND it is NOT a land waypoint, keep the system loitering there. + */ + cur_wp->command = MAV_CMD_NAV_LOITER_UNLIM; + cur_wp->param3 = 20.0f; // XXX magic number 20 m loiter radius + cur_wp->frame = navigation_frame; + cur_wp->x = navigation_lat; + cur_wp->y = navigation_lon; + cur_wp->z = navigation_alt; + } /* we risk an endless loop for missions without navigation waypoints, abort. */ break; From 3ca94e7943fc76053114ab97d4b63dc6902fd5bf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 17 Oct 2013 13:38:21 +0200 Subject: [PATCH 793/872] Prevent warnings by data conversion --- mavlink/include/mavlink/v1.0/mavlink_conversions.h | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/mavlink/include/mavlink/v1.0/mavlink_conversions.h b/mavlink/include/mavlink/v1.0/mavlink_conversions.h index d893635770..51afac87c3 100644 --- a/mavlink/include/mavlink/v1.0/mavlink_conversions.h +++ b/mavlink/include/mavlink/v1.0/mavlink_conversions.h @@ -9,6 +9,10 @@ #endif #include +#ifndef M_PI_2 + #define M_PI_2 ((float)asin(1)) +#endif + /** * @file mavlink_conversions.h * @@ -49,12 +53,12 @@ MAVLINK_HELPER void mavlink_dcm_to_euler(const float dcm[3][3], float* roll, flo float phi, theta, psi; theta = asin(-dcm[2][0]); - if (fabs(theta - M_PI_2) < 1.0e-3f) { + if (fabsf(theta - (float)M_PI_2) < 1.0e-3f) { phi = 0.0f; - psi = (atan2(dcm[1][2] - dcm[0][1], + psi = (atan2f(dcm[1][2] - dcm[0][1], dcm[0][2] + dcm[1][1]) + phi); - } else if (fabs(theta + M_PI_2) < 1.0e-3f) { + } else if (fabsf(theta + (float)M_PI_2) < 1.0e-3f) { phi = 0.0f; psi = atan2f(dcm[1][2] - dcm[0][1], dcm[0][2] + dcm[1][1] - phi); @@ -128,4 +132,4 @@ MAVLINK_HELPER void mavlink_euler_to_dcm(float roll, float pitch, float yaw, flo dcm[2][2] = cosPhi * cosThe; } -#endif \ No newline at end of file +#endif From 0f67c5cbb0f69e5b9dda4e4e75120e63e466e1f8 Mon Sep 17 00:00:00 2001 From: Alexander Lourier Date: Fri, 18 Oct 2013 03:47:15 +0400 Subject: [PATCH 794/872] Parameters list generator --- Tools/px4params/README.md | 9 ++ Tools/px4params/dokuwikiout.py | 27 ++++ Tools/px4params/output.py | 5 + Tools/px4params/parser.py | 220 +++++++++++++++++++++++++++ Tools/px4params/px_process_params.py | 61 ++++++++ Tools/px4params/scanner.py | 34 +++++ Tools/px4params/xmlout.py | 22 +++ src/modules/sensors/sensor_params.c | 26 ++++ 8 files changed, 404 insertions(+) create mode 100644 Tools/px4params/README.md create mode 100644 Tools/px4params/dokuwikiout.py create mode 100644 Tools/px4params/output.py create mode 100644 Tools/px4params/parser.py create mode 100755 Tools/px4params/px_process_params.py create mode 100644 Tools/px4params/scanner.py create mode 100644 Tools/px4params/xmlout.py diff --git a/Tools/px4params/README.md b/Tools/px4params/README.md new file mode 100644 index 0000000000..a23b447995 --- /dev/null +++ b/Tools/px4params/README.md @@ -0,0 +1,9 @@ +h1. PX4 Parameters Processor + +It's designed to scan PX4 source codes, find declarations of tunable parameters, +and generate the list in various formats. + +Currently supported formats are: + +* XML for the parametric UI generator +* Human-readable description in DokuWiki format diff --git a/Tools/px4params/dokuwikiout.py b/Tools/px4params/dokuwikiout.py new file mode 100644 index 0000000000..33f76b415e --- /dev/null +++ b/Tools/px4params/dokuwikiout.py @@ -0,0 +1,27 @@ +import output + +class DokuWikiOutput(output.Output): + def Generate(self, groups): + result = "" + for group in groups: + result += "==== %s ====\n\n" % group.GetName() + for param in group.GetParams(): + code = param.GetFieldValue("code") + name = param.GetFieldValue("short_desc") + if code != name: + name = "%s (%s)" % (name, code) + result += "=== %s ===\n\n" % name + long_desc = param.GetFieldValue("long_desc") + if long_desc is not None: + result += "%s\n\n" % long_desc + min_val = param.GetFieldValue("min") + if min_val is not None: + result += "* Minimal value: %s\n" % min_val + max_val = param.GetFieldValue("max") + if max_val is not None: + result += "* Maximal value: %s\n" % max_val + def_val = param.GetFieldValue("default") + if def_val is not None: + result += "* Default value: %s\n" % def_val + result += "\n" + return result diff --git a/Tools/px4params/output.py b/Tools/px4params/output.py new file mode 100644 index 0000000000..c092468713 --- /dev/null +++ b/Tools/px4params/output.py @@ -0,0 +1,5 @@ +class Output(object): + def Save(self, groups, fn): + data = self.Generate(groups) + with open(fn, 'w') as f: + f.write(data) diff --git a/Tools/px4params/parser.py b/Tools/px4params/parser.py new file mode 100644 index 0000000000..251be672f2 --- /dev/null +++ b/Tools/px4params/parser.py @@ -0,0 +1,220 @@ +import sys +import re + +class ParameterGroup(object): + """ + Single parameter group + """ + def __init__(self, name): + self.name = name + self.params = [] + + def AddParameter(self, param): + """ + Add parameter to the group + """ + self.params.append(param) + + def GetName(self): + """ + Get parameter group name + """ + return self.name + + def GetParams(self): + """ + Returns the parsed list of parameters. Every parameter is a Parameter + object. Note that returned object is not a copy. Modifications affect + state of the parser. + """ + return sorted(self.params, + cmp=lambda x, y: cmp(x.GetFieldValue("code"), + y.GetFieldValue("code"))) + +class Parameter(object): + """ + Single parameter + """ + + # Define sorting order of the fields + priority = { + "code": 10, + "type": 9, + "short_desc": 8, + "long_desc": 7, + "default": 6, + "min": 5, + "max": 4, + # all others == 0 (sorted alphabetically) + } + + def __init__(self): + self.fields = {} + + def SetField(self, code, value): + """ + Set named field value + """ + self.fields[code] = value + + def GetFieldCodes(self): + """ + Return list of existing field codes in convenient order + """ + return sorted(self.fields.keys(), + cmp=lambda x, y: cmp(self.priority.get(y, 0), + self.priority.get(x, 0)) or cmp(x, y)) + + def GetFieldValue(self, code): + """ + Return value of the given field code or None if not found. + """ + return self.fields.get(code) + +class Parser(object): + """ + Parses provided data and stores all found parameters internally. + """ + + re_split_lines = re.compile(r'[\r\n]+') + re_comment_start = re.compile(r'^\/\*\*') + re_comment_content = re.compile(r'^\*\s*(.*)') + re_comment_tag = re.compile(r'@([a-zA-Z][a-zA-Z0-9_]*)\s*(.*)') + re_comment_end = re.compile(r'(.*?)\s*\*\/') + re_parameter_definition = re.compile(r'PARAM_DEFINE_([A-Z_][A-Z0-9_]*)\s*\(([A-Z_][A-Z0-9_]*)\s*,\s*([^ ,\)]+)\s*\)\s*;') + re_cut_type_specifier = re.compile(r'[a-z]+$') + re_is_a_number = re.compile(r'^-?[0-9\.]') + re_remove_dots = re.compile(r'\.+$') + + valid_tags = set(["min", "max", "group"]) + + # Order of parameter groups + priority = { + # All other groups = 0 (sort alphabetically) + "Miscellaneous": -10 + } + + def __init__(self): + self.param_groups = {} + + def GetSupportedExtensions(self): + """ + Returns list of supported file extensions that can be parsed by this + parser. + """ + return ["cpp", "c"] + + def Parse(self, contents): + """ + Incrementally parse program contents and append all found parameters + to the list. + """ + # This code is essentially a comment-parsing grammar. "state" + # represents parser state. It contains human-readable state + # names. + state = None + for line in self.re_split_lines.split(contents): + line = line.strip() + # Ignore empty lines + if line == "": + continue + if self.re_comment_start.match(line): + state = "wait-short" + short_desc = None + long_desc = None + tags = {} + elif state is not None and state != "comment-processed": + m = self.re_comment_end.search(line) + if m: + line = m.group(1) + last_comment_line = True + else: + last_comment_line = False + m = self.re_comment_content.match(line) + if m: + comment_content = m.group(1) + if comment_content == "": + # When short comment ends with empty comment line, + # start waiting for the next part - long comment. + if state == "wait-short-end": + state = "wait-long" + else: + m = self.re_comment_tag.match(comment_content) + if m: + tag, desc = m.group(1, 2) + tags[tag] = desc + current_tag = tag + state = "wait-tag-end" + elif state == "wait-short": + # Store first line of the short description + short_desc = comment_content + state = "wait-short-end" + elif state == "wait-short-end": + # Append comment line to the short description + short_desc += "\n" + comment_content + elif state == "wait-long": + # Store first line of the long description + long_desc = comment_content + state = "wait-long-end" + elif state == "wait-long-end": + # Append comment line to the long description + long_desc += "\n" + comment_content + elif state == "wait-tag-end": + # Append comment line to the tag text + tags[current_tag] += "\n" + comment_content + else: + raise AssertionError( + "Invalid parser state: %s" % state) + elif not last_comment_line: + # Invalid comment line (inside comment, but not starting with + # "*" or "*/". Reset parsed content. + state = None + if last_comment_line: + state = "comment-processed" + else: + # Non-empty line outside the comment + m = self.re_parameter_definition.match(line) + if m: + tp, code, defval = m.group(1, 2, 3) + # Remove trailing type specifier from numbers: 0.1f => 0.1 + if self.re_is_a_number.match(defval): + defval = self.re_cut_type_specifier.sub('', defval) + param = Parameter() + param.SetField("code", code) + param.SetField("short_desc", code) + param.SetField("type", tp) + param.SetField("default", defval) + # If comment was found before the parameter declaration, + # inject its data into the newly created parameter. + group = "Miscellaneous" + if state == "comment-processed": + if short_desc is not None: + param.SetField("short_desc", + self.re_remove_dots.sub('', short_desc)) + if long_desc is not None: + param.SetField("long_desc", long_desc) + for tag in tags: + if tag == "group": + group = tags[tag] + elif tag not in self.valid_tags: + sys.stderr.write("Skipping invalid" + "documentation tag: '%s'\n" % tag) + else: + param.SetField(tag, tags[tag]) + # Store the parameter + if group not in self.param_groups: + self.param_groups[group] = ParameterGroup(group) + self.param_groups[group].AddParameter(param) + # Reset parsed comment. + state = None + + def GetParamGroups(self): + """ + Returns the parsed list of parameters. Every parameter is a Parameter + object. Note that returned object is not a copy. Modifications affect + state of the parser. + """ + return sorted(self.param_groups.values(), + cmp=lambda x, y: cmp(self.priority.get(y.GetName(), 0), + self.priority.get(x.GetName(), 0)) or cmp(x.GetName(), + y.GetName())) diff --git a/Tools/px4params/px_process_params.py b/Tools/px4params/px_process_params.py new file mode 100755 index 0000000000..cdf5aba7c0 --- /dev/null +++ b/Tools/px4params/px_process_params.py @@ -0,0 +1,61 @@ +#!/usr/bin/env python +############################################################################ +# +# 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. +# +############################################################################ + +# +# PX4 paramers processor (main executable file) +# +# It scans src/ subdirectory of the project, collects all parameters +# definitions, and outputs list of parameters in XML and DokuWiki formats. +# + +import scanner +import parser +import xmlout +import dokuwikiout + +# Initialize parser +prs = parser.Parser() + +# Scan directories, and parse the files +sc = scanner.Scanner() +sc.ScanDir("../../src", prs) +output = prs.GetParamGroups() + +# Output into XML +out = xmlout.XMLOutput() +out.Save(output, "parameters.xml") + +# Output into DokuWiki +out = dokuwikiout.DokuWikiOutput() +out.Save(output, "parameters.wiki") diff --git a/Tools/px4params/scanner.py b/Tools/px4params/scanner.py new file mode 100644 index 0000000000..b5a1af47c3 --- /dev/null +++ b/Tools/px4params/scanner.py @@ -0,0 +1,34 @@ +import os +import re + +class Scanner(object): + """ + Traverses directory tree, reads all source files, and passes their contents + to the Parser. + """ + + re_file_extension = re.compile(r'\.([^\.]+)$') + + def ScanDir(self, srcdir, parser): + """ + Scans provided path and passes all found contents to the parser using + parser.Parse method. + """ + extensions = set(parser.GetSupportedExtensions()) + for dirname, dirnames, filenames in os.walk(srcdir): + for filename in filenames: + m = self.re_file_extension.search(filename) + if m: + ext = m.group(1) + if ext in extensions: + path = os.path.join(dirname, filename) + self.ScanFile(path, parser) + + def ScanFile(self, path, parser): + """ + Scans provided file and passes its contents to the parser using + parser.Parse method. + """ + with open(path, 'r') as f: + contents = f.read() + parser.Parse(contents) diff --git a/Tools/px4params/xmlout.py b/Tools/px4params/xmlout.py new file mode 100644 index 0000000000..d56802b158 --- /dev/null +++ b/Tools/px4params/xmlout.py @@ -0,0 +1,22 @@ +import output +from xml.dom.minidom import getDOMImplementation + +class XMLOutput(output.Output): + def Generate(self, groups): + impl = getDOMImplementation() + xml_document = impl.createDocument(None, "parameters", None) + xml_parameters = xml_document.documentElement + for group in groups: + xml_group = xml_document.createElement("group") + xml_group.setAttribute("name", group.GetName()) + xml_parameters.appendChild(xml_group) + for param in group.GetParams(): + xml_param = xml_document.createElement("parameter") + xml_group.appendChild(xml_param) + for code in param.GetFieldCodes(): + value = param.GetFieldValue(code) + xml_field = xml_document.createElement(code) + xml_param.appendChild(xml_field) + xml_value = xml_document.createTextNode(value) + xml_field.appendChild(xml_value) + return xml_document.toprettyxml(indent=" ", newl="\n", encoding="utf-8") diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 4d719e0e15..8875f65fc6 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -44,8 +44,34 @@ #include +/** + * Gyro X offset FIXME + * + * This is an X-axis offset for the gyro. + * Adjust it according to the calibration data. + * + * @min -10.0 + * @max 10.0 + * @group Gyro Config + */ PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f); + +/** + * Gyro Y offset FIXME with dot. + * + * @min -10.0 + * @max 10.0 + * @group Gyro Config + */ PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f); + +/** + * Gyro Z offset FIXME + * + * @min -5.0 + * @max 5.0 + * @group Gyro Config + */ PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f); PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f); From 5fafe45745238ae377ed748c6f561a6cbf02221a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 18 Oct 2013 10:14:42 +0200 Subject: [PATCH 795/872] Updated docs --- Documentation/fixed_wing_control.odg | Bin 0 -> 12258 bytes Documentation/l1_control.odg | Bin 11753 -> 0 bytes 2 files changed, 0 insertions(+), 0 deletions(-) create mode 100644 Documentation/fixed_wing_control.odg delete mode 100644 Documentation/l1_control.odg diff --git a/Documentation/fixed_wing_control.odg b/Documentation/fixed_wing_control.odg new file mode 100644 index 0000000000000000000000000000000000000000..0918edcac86ac046cda417e534551bb91480c219 GIT binary patch literal 12258 zcmd6Nby!tf*DqbtAt_x_o7^-g(%l`xW(!Dc*tAF^ohsce-JQ}%gMf%MN_RPajWKi0F~?ArgGa!JfkA!9g0Z%+ z20OUgf{kr$tt>#s4i*p_fRl|W8^qY&!k!Ic3$`(ZfE=yCHV$lNP-9zj3y{6?p8#L& zOBv}Qz`)#I-vGf5#%wOuRzjjTKa@^jsQnG3Ff|7oJ2fTP1_Uv+urU*+)=-yZ<)aoA zK@);Jd13(q3;fW+dSi|;<&CI~y}#qz3IG7k&dzMkTx<}i8GwVIpC9n;E%hI&OhNzQvvq`8ed96(0l-$^ z+p60GIM_GzbZAhv9SKPFt@zlYWgdH8PwGD#*n{v%8kuj z05gE8v4b(IlLgqB=7;6q>+#=f%m0_!nnI18|EmNCJK&awn({lkz%K)plUn4*sJ!hy z5utCL2!evY)rIxOK(GiWI|mml2RkbVr#c6x06V7uH@^_zH+({-Ab~#%@Tzli3Gna= zaPtZQe!Tpy0c>jFa5FhrO&y`%G>IsybMh(h@T&cR{I~ai76ZA0tibjnTz}#I8=%L99y>-+dxz=nlo!%$b?n}xVq)!NGUk+%hI4*RUL-mjQOnp@=dVI#_)CM7 zB$E{nA}Oj5-3!K_o{YWhQ!1end?abz8Yx-Ygxk=Y7BG~A?ytn~ zYB4?4EC}gbbg_emnrIdo>FcY%s@X`7g2_t{ltpKgtmQX*H6bOEU6H|9Fp_wHvo~PQ zWn5L-u`5~Rq-${5M9%u?T`)AhjDVtDdRoYM?ljnzG#(3hOc;+f%O}^JsidQc;i*Zjfhuxha#0fj_;(>AU%&(B9)pBT zVeJBNJw!B+OI22y%a_p4RYQXtvEv4D6h7pw>R@9n5{9s`(N)jk2Ll!Jqzo%&X4wl6 z@4+weUnZ5<6qSM(wZw2W7_sIFe5~2DO?Nanr2=xb@0JnWy?op!pXY~(P-5NI1|{{x z<6iB1PocMqCywOjwsqea_uc#n!oD>H1zZzADoGr~&EmQ%+d(%HV2b1{-p?4jE!}?t z`#Dn14u)4){tC(H$j@Un2^@HaU8mC@*|{IH>*c*!il`%iZiH8V7Hj$N361;vxK~?s z#@ty+j6~FW^d6+E$oZhJoOojax&u!94f<@FuWfp~SR5CoyBKf>6-z2{OxCIjUbKi* zZ(0$rljZezb|%1{4PiGzWRKk6xNR~Tz|gY+cgY)iY$rSRPq#UqP|g< z4W08LJj6!}Ae(IE7%}8$L7xA51}I!ANOU{|XEsfw!6JTm zImNh_arYGjRi~b2%zjn=5Ok_gO&*Mj0XGks)@uF?9DW8I#J-5a({!{P1WrBW2-b)5cKT`6|WFz2ElTnuR1D-g?z@becPr26+&*CrD~=R9y|HS$?T!`;cGld)EJHv(ev^090kgP@3lURJZtiDYT(IdIp3?h0!CP9Z~Sd zIT&R*WRx!X$17NfFff)hFgN$NTf^Tiq<3x>QV_((;dbA5vyN^VOocDi61Wd(VW-Hm zCz?j#)0J^*gb|}7xxT?9StFXMIKxi4Tk9-%}er#w<{Y@K0>n=2bYrmorDz6E`_h>*bp-!Qz?CNz>zF(Z#+5FjP${#C_Z5>o7O zV9lL)inn7zqA(!`kgt-#&)E%6bBfb9t&7ccBr_DZS!5tgZ#aAz;sC`dYAk)@m>$y{ z=?=axbB29b%yy@sN^M7EOHb!@(R)Y8*$KSZh0Ypx_|r(q_tNLG0Lz)%$M0Ld4|QsTr?4p@djT* zy@$R5KbFX!e^7)udq*H-uG3?^Q+7|rY%hpZUbZ9iW_qNNkv~EmXds|BQly3zA64}Z zt=KHL<+Qdw8-kR~DczkNoxwkLCY-onmzlBkl`lW^oz?X!XO-OW_kgHjgGtpG?sg3a z^O1JXG6j#RKCHM>gn33qCmG^ET&Wh3=`ZLyJI1`Uo+Zpp@e-SXKa5ZHBMd#qexGrxG(ao}3e=={%b7oVpLgv?@gQ4nf4C z+rx)k;OprGSeYs?oF;Mwv#tSSRRJcG;p~Z}V*eX+fr1)sP^`?$VTZ(hkqZl!eeQ+S zxA32Z?{DC8)-&CuB3`3i;(Tk68&&!#h`vc0dSv}YVVU`z2ceizcn-7BJG>`s{Rzm@ z?=H%#p)0fIp9~$zdGiNB&Q-e%68rDMI!C2FhRbD56qRON1eiV!6qqx&_JpX-$zi*Y zt+IuC>^;Ur#&H=KLO+H0`B|k!!)yu%!FlFqSpk^m;qU8P<8)q?L=A_1LAoNNyl_p< zy4p-P+|`ukn8MdPV{d9~d<~0yE zokD8r5hROrrI#GlBTpnra72KBk_;Gj zoWdNNxnly81;QLLw0~p$v{7iOhFpg~T8G{7Rj0JVsQA0jt5TweBDhOSSi8N6!E++G zD@#SBp0pQB=E}PqXw^nOKAvc!ACfgHpQ~1FZ!C%2JK|SrIwYDP%+;j2cDR^39i*{( zt|}Dh{3gQ5G9pFW6+?pYqvbNedZzWeOt<`PFBs3Yx?X`}CnhUGqVvF0d@9F8V}ljo zkzpmZ$HLIMkGC8>|p=F>`3Ql}-1?>mq?_H)fnt*9sTPcru}!UNqb(?s`BRB7QJ1$x44b7^ zPhS-bG!2JbqG}H$r_x5_GK(g|sA89%@2_UJ3}bwJE|T8ok9VzwjqyrD_dehy5AWVg zDXSXU5#p?S$_#}clEqOCk?n{*`}n79g04imE}mC|s$2m&3e3si6tY$awp;{Y=#Uia zgbw}r0=}wj^K5Y)i(_)&ku=|nJjV1sv*$pq=-}2av2bV(la6Y$1CG?jDK$-{kMw%i z-aSU%Rqjwe-TH+qf0Z19rf#_r6>q6>+uTJI;klEUz-FX0ll#}2bvw}EkmK?M?^w8x zBrIKGuGdnXviH<&lCRGv=dR9oJRL&o8RLX#^J=Uxn|tOS34u6h#fXih zy!D5nOqYWO61_VmlX#x%87DSzrl@<8eZxpL0=1-jDM%Fc(N900jbLG{%-t_ZAffYc zie}b%1T-03QMl{Q;ttHwWT@iK1$#ax*$6coSvi=p2L;)sEX7Vr!;ALn)cd^UAwkwr|Aq>huJcD;o!qUq=c7%i9f}|8sTFcokcI z`z5(g@(EU7;+sR_WXl|;H}vQFi=#ZF!nTxB^O1$0P`TF!GCnXr;1l&*?Zs~692FAv z>&K&BPNEarG)2Lg5t8#Od_^TbgiCSm@HOeak|4vQ9wD`qFAUlJ`??LhkU&JxH735hjBi*Ao}mQb7*2{g-^XVuM};+M?3JLVL8 zUibOR+?70`>!wezPCHcAbI8xcXavA_zvT2{^rM1}sCAek>#A)8hRGM}!LZrSbr2W} z>_mtF`hfvTIoPHWa)2-j)1k%t!8DKCCpJtZDm8e8JHbFW)_sPSGRdi=HkZ z4t)D@Of!liCy3pt8(WwiZ#Ec9$9ak&dxLt0TA~=Og0BGw0~#f>Ilx@Qi%6lFhEQi# zl#LDWKiDU$6*s1%sC^dVM>!cC&bxzjcIPaUd)1#`DCmWD=q8wo2cCl&$;8|Eu?1zg zb!z_uCS4D%3gz@+UM^%0b~e%d;ASeh#!^khmUxxdmvM`^89HSlDC)T42On4gMB-|m zU);*Lf$cd0cJgD@82#pn>vR3;pXK78(o?=SN;{aWdj41{J3+_w3gxLb^7iM|8YGgi zhNtt-Fs7oaE6Art$Fx2W;Z-z{7frfx(X3ipab-57)EOwIM{NfePNjQyWi`CiD4{J{ zv^1ueA=t8RvWQo3)O1)UxSS${JOQ2|g?eQ`LIDj02Ssp}4TxPBuT%u+S4mM1!Uyd_ zaE?}+j3vvzXsMo+wc1Es`tuVA`)-0Y;+~9A=C*G-gDr_Rfnl1NvTTv~wTo^T6Kq#d zR!l6t{h4zXQBc~;IMRMc1kkHYqC}D>$z|SMb&d#@iHoCAwj21Vyp~+@wTklZ@)Q`2 z;VrQWg-efHKPr_D&hp}Jm~JXiN`AHPG?tyk_7`>~(!_dSaJ;bUUyBn;HUm_&p=?l? z&vrSdk|b@soZmu*Tw8RQHyj(@TiEa(Rq&{%+}yJ!<(nC7ZzPsr%5x zsvX&!=~qrvlvs#}uosT8tH(3k(LoS72W^VesA-NU zxgmHDk~~Y~?GVf-1{vdR@^ZbVoWD5TgF{RCNT2TRH_74nWKR-1ofLCpU%)?{pNEl$ z!*@P-s&dh(l|khR=Tp8^bfi?udM1xUQLZsXzR=#LMyqkvnTva z)VQbwD)ao~U(^Qim?Nv~68q64vJX(BfUMfpYjD8I=v<~Di};-AT|A^0VIUew9ADHh zj8LmSF+bSOcEKl@aFp0QMCEbbR#wRGB{3mP;^qr-7s)q=v-+^{_)`J;VI5s>aaHLK zqo@U{5dGB%>?Yxsz$_i(UN(11OI(R(cQl0xgo@iD_F>JSG_!?>&Q>mdW=aK|q-8U^ z_)4FV>b0#guyz?g@LSz^rjZGKTA<B;NJyPX=>>Y^d1kA8PqW^BU!k|d`=do9Mwij|95+pAcDh;?Jl|gRz zK|`%g5cxGi)Na6dToL%OAVlk)Jvn4UZ&0Fi5uOPvz89nS0Gd(jj@aguf&5{YRjxv} z6*BpWk$xgQ9ABOxRCzOdgHOm|Ba>&;+DK^=s(vrZkYTzP4cf=n{$(@MX+1xr1Cy3W+SN9k^E`&-hV zn(Jr65NG)4KMyBPqu~}_%@B9bmmWS^$c-W#%pl^sI5a0SBYwS2YiT%>geVkFz2Q2q zBuuM6CD)!2u|!>OJf7QIamwZx`%M3O8fWR9Q4C{6X797LUQsAc&aTIi=dDK%dTVYQ zj|BrG9RI_k{}JqQf9gI?cjN2dUPLz@y}G%hwTX?fg_S+P;YX3p*2XMISy2iDjRft6 z1w%$!T=nLE6$}h43<}&$>Dj}yP#745O&Rfr>MluJGYd8u+Av-tisd`P$qcF1F@+q# zsp<~_he;HqeX6ybMgViYv!S^bPtkdQ&skv@{~WAf5yrE$Ro{&N#lpLdv#&=Rlcxb= za|H1QJ$!eM?Dk4(i`5v7uyU?#Y5>iaDim1Pi3-LB@&tr>f)Nne<#=J6taf4BPbGpa z{p1ZBFIL8Snf)Dy?$3#cDq`6vRQd|VB3n&&TZp)9;+?o@Q+O<95}zFPQ3fSp%|8?3 zXsn5NPOx;mSN8UEPmyKX=OrC5(LygyvCXQ82(W>Szias#F7t##XHTE2XA(@xFz0@_ z$FVCn2bxx(pjc`;yhJ^I-)*{3Cn;H=-4a2l?~S!Tdcts1oNVM-hA#6QJA3e-Zh!7M z)tmR#;u+ipt}R6?0dfj9KIdjRsr1&93(`PFyF((bHp7dtx;uG;&0@(~H&5D?t~Z|d zzRYc0I0d)L^?bFXVAmQ=uVlRdli21w;S}8&gxi}5-t%CN8Af4{b z!&lxGD#tXdx0a5KI(#X%d&6}(JR7I1n3w!ABCHJ>Q*^K7)@zzIxCQ5|2q#8GQux&x zH&pF5E}XUG4coC5@6Wov8$w)!8+-Nxk{?M;lK|3!}M zcXmP{W;X%t|BK}9m*=m|b8G2;*Y`WA+a~!b^}BOxhTrHi27zwk_}{{w0FWaT`p20z z{I}!q8-AGEuiP#Db0_`Im?u_{n@I4FCi|T+x8GBH2iKdZ?6+8?-c0y3@NV;5F1ETu zncKsS?iAC_E;R=|d;?0fP&F$KIB$7xetColKuZIF~A$Y2`d(Z=%c{(xzd#c$~}9 zvN4_yDXhIsWP&=zg7$k)FW!+@4TU4lh9(!5w&n>7FMt^4V$tcyB!rktv)pt)aV2soIc;Suqd#7zZfjb9u`a!!JcuKx#=;k3kHw|H)34L*gATj zNe_o3frMr6ZSh^7*D$d5(MI8Jk;1u4M$Q`}vyL_7Y5cNfL|bMgpB$qtgl08z%7l5_ zIq!X#v)sWzITD2!?p(;4f+kfzU}{9Ab((Wwd`ubQhiGWv^&3yF`!|ao^gG{a;zyf9 zVG1gpA5n4f(|ON1SRpbBL+Il5U|F&5xuvt@lz9QH;5;o~Zf2g=2Y4-l;ERWMN`xMw zv6;$AbBe6CF&UF`s>YWsM5bq9IIu3(5~8ik3!_ljx{TbTO9XsU1oCa890*Gdrysq? z$0!HwjG%h&+?RKI{+7onNUU&LJ!Y3FVnBoluGjq`f(CW}VWq?hYI2-%jM|jc;CZSL z-6N*C2jWF)DbY_Jo#gc9(MQiVs(x(KA*2YjJv7T69KDo8!PI>4SeG&3x@#x4c;~C) zs-W6j=p)l!IHE`1Byx|gGdISEn6u|8P=f8^KVn^`d|3t_LRx(7a0`@b7LK;=-K|-; znBeJ&i$QGHt4v%Vsy!ftxvG*JuhWZ^1|HP1z?Y6(1rzm7G6f?Z#2Rg<0PKplF`n)E zb(L6#&E*}v4{$9>K>K1I%GHwPpYEyJo*IwOw}w4kU=m_4kj6EB$Z(~B4US>i6l#>Vz0e1?~GdLEI zf_0)yYWxW_$a`fe>@GL6^+pSo(>r<={Kbc5vC(T7hg0)0*m0vh=12WG$=jFWA3(N?U)D5(UmCffVx$mo_+LSge78Q(HNJ`~<9-V`XUWt%s*EoMd;Tqa37WDH=^4!v8oMe*6b*yx~m>QPL zf{3hsqh+u}3LC5?(blFEW0IuFp~Xvzk*i0V_veZ|jW& zUtLYioEokq@hX~8Ji~jVo)lOXICJpGn}A_~KWXGcReQUCmjHWSngi3*xKNb(wy^qS zKGbQKi6|*sT#W_ea3f@x?!z7jx~;@Q6HUvvw)kVXj#eCgIIp1(MWK$KUJt}4=OtUz zv$J|NQ$ygo^XnzCv0gJhbjf~QhQQ30UEg@zOC*y8d~8IyDo{oH1YVVF;|?&4O570% zM_)vidc-|8tk`y%`O|cEqE`KzCy|=NI27f^=-adXUL*M?q0-$bBEe0ecr+Hmflnmr>4ZMO)-!xmVY<3*g8t&w)J>tYlPEz%&co0! zJk^XOw*=5NycyrMc8W;+EX7V>7a%z@eyx>O-Qn2z1%tp)Dv`bvs4i#;F3qD}$IT$O zVhY@Tlc!n_vi4TR+L(}%?Z0R}X?dTSz@MK;gCx+ihB8|i)cEr0+iJxMab28vG{Yh_ zb>8>Tl#@fLW^dI*gK9in{mEo(u8Bf6&MM`0DYcixMJ(gYl(KQ;s;RSy2{T_@tGe+} z37CE0fRhFci3KT!eLL4@-JaK%)1X6Lbi!(7HwV@&65}angwi=zj;6v%4RO$BW4o(s zd7a~eX1`ZZ(en>Iuz^nSFA44guS^>PQ`k^4Y1j7dc}%VdxO<(+ZwU$=#-XS-rnf%N7j(GNPuV)$w0u4^K&4yCllpu&it* zY1Yz4W!qBoXojAc@t1SncR@=;^6cE*n$sg^H3$`zpofi^3E#cgLZJ{suBt)sJUNgz z!+J>@{N-TC603_0TJg@Ki4PTtsY`RXqGg33Y(^n9C0bt92jocMan=L%IZGJO;S)C_ z3SC)k^c}H1q)lI6lU~={QzC~V2sDtOPFkMk4(W608nXjTk% zaw?l*?yC*fm%t5-te__2S6)7|2=7DJkDe~J4L%-=Ltn{hEjhx&mtOGMHssWTOrXhI zd$2J3Cm$&2lOgn;IeKQ14?|6`$ob~cq$noDnnHCUGKzPffb5PWI*0L!$sBqxOiFU_s?dZBYE>ZN`w&=2!3p4K~ol~3Z$F}Hq8yte0 z4UU3@`a@P3MM;45O}gI`u>H4ufg-iHHtm$?O()FM+ulXFsB~M#qs?jOFp^35sOWHN zJf5PsdDc03YMDn*K<7fM267mGO#mA$m+5nJ~(VI{OA2up1zE4yfiWrnRXG}595lzI19R(y{iGMa0sGY@`?okvzrp#{MfC*jMl;q!_{B-fYqx=}Nx4M7$X}3<^Pa32AyDk6V^8L5*^ydmP zQT-kCr^EMW(|%nmZ(Y8h^!Ntlrvvyy`;RL9x*XiDq(A8)(?6`M%5sQEw`|xqf54l$ L8grY*5A%Nj(}NKq literal 0 HcmV?d00001 diff --git a/Documentation/l1_control.odg b/Documentation/l1_control.odg deleted file mode 100644 index 69910c677459aae4664a08b1bbeb6aaed48721ad..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 11753 zcmeHtbzGEd)Bg&BfOJYIEgefY(jwhR!?H9i>@Hn`Gzf?wAkxw$-5^RRh)8#LhjjiH z&+*acJf7!w-uLf!{4CykUo+pCYwqiwz2=%xRY1N;2mqh~03lk6QmM%7tt*v4e+-GzI71zTzS|?@U}_JB3Bf@^zZxd=E4R>h zMSt9Jv$J-v6{WF)!ySbA6$47K~tWe(y1 z+kvmA4&wl_137;1UR(H2jk@Oj)g(tK7={4+?&p7s@vF?QSP%&0Uq)a1;}3UzH=T=< zlZWGv??0@D*<0KFQ;pZWY#@leBf?HIJMgbcO&zT5|0&Eh@AsMh3SbE}H%A!qpAQ9L zGdG7Nhq)=-l+DE&>`ME?@;~PB-)AfMm)V*_O_*Vs*fkLs5pV@D{&1`L98fVl)sxqv)uTxMoq zbAB#1PChQ6Ihc>lRFI#SO8~?sV8O}9Xa1+^pIZLV`mc8zcD?XnaBC3NcUM8ouCWmd z&DjAiO2f@Z!yzW}=W`VL!}CAB{R`py?LPt#8XDU7h=vGa{~}Dp6$}i&b|dUM2uW^P{*3dBL}#D(%V2 z-=Zn2_Pq-RZI1e5x>QPT2)o~)X4;GF7bp^Dcqh8gWDI|uvzq3z<+XO{82U}{AZ;^~ z1m&HUlE1n~h)8dLtzC(+>+6R*eE9O@j6o|jrgSFHp?Wv;N2b=#1+x;?*lsaHU+Xe=EnSlQ>i4@vV^ObbCOji}iS^Lp$>^*(LFcS)#Si z{D;Tu@fpXJPXc&5z1>iJ<8RMxbU#{JIVLQiA(j%^cl{juXyu_}-Fn@F!$46JK`(%zFlZx!Hbioe1S57SDI#^Eoasg#TaZ5 zMq9ILdPw4CL$&{*^KpAFjv8+`Y5Bt`{4MTiQQ{X`fwQ)fI#*D(k7gNBWCiA)4tKB^ z88F&EYKKnCp8BnkHf^+OmW;DorWS+|Ws!5yuo#}W?1lzJS?Y|Tl|t(rW=pZN)1tDNt@T75_onIAd+KnvUv|=C zc*9wHx^E^ZThEa1WFp~!@iAOibY{ZR8`>WV*Y@Ndi3I96&-YI-8wA~_Ug1DDen(pj zAtq{F2vDP3xYJ78KAL30#P>}9Gqz$ZJG1mgka7%+?Z_3`JIba6PJuieD-lt)XlreK zF5bu;%YV6(JUd~;bo3kzy>^Iytt#+}atFC8UXs#k)8pd~^y96fs$f2UUf^le86!Y~ zFbqSm*bhY01l5gvw|ii$+c9i21dI)x;pO3noIK0lQ|Wcp4wdSWW|j@xN?=>46U}lBo4hoYnA)ra=Ra>?R-mG7AfV} z=!z*$?WOA;U37*Cs^sw16LTY6wt93B8!fSaK$kq>O)dIix)`5uv`m}fO>mtQI%`OJ z@#`g!qalHudFAx|F=>JiHQ9meWgl6~3Bm1o?CezP5$L&AI!rzgh?k_%OxLN#(XA;< z#sYx?f+)`1C9c;GD4V**Z-@sr6|2-Cp=zK>x}<3@TAobY&Z$32#d4Us>V#jRsw&(< z!&QkJ!$3jo`Dg)vzmK7}5it}5ae!ZM`4Ay=({LO*{t?f+M@P}LJnvwv<})TLGUH3l z`pU|TrTH+21|XYqX2&Q13e0r^1%D42WgXf$w!0XxR3l_DaRf5{t zCM7h>MHnBTIOxzVRRq=XHJ;Z!R{T<&)OIhS(Uw2;wWWG(D)T^U3iH7B82jX&hl-c| zg(%u$*O_ZlLub6k8GfB;Zi7hY{b`G=$x4;}3TLf5nokYu6fi#w6!hHPJjgv2it41= z&h^Y(OCk4Un_GpFSfvF!e^q@-{(Ntf3jYD2CuWFIsQ%*n>G6H>>E^SxdhC~5vVuZ| zg|hN$b)QfZ%}Co`WCH3uV}_J6KIKutZ%Hwbe0odbl)WtRatTbH=IDX<#OPsouXAn0 zkw!u^Uw-(+k!!xcu;wixWG*#GhRMaVz5?a>&{aG7>^G-U_to?CZ_8r6O~0M|QkhDN zKaj<^qLNopC)Btt9f`B1+g}8qhX197Wh1c%5k1CSkj2=gD=%_Neiu?1wye&pWaq*& z6lUWoWn1Nl+yR_w{RuOY5=N_97B|YP^xKKdYuuV?bva*P=xXe*f}_LW7>$iH#hsew z#W|hK>IMx(`JrAigWK;{VQoEc)s4{%@9?@h=H++UEU_cWOhf8E-pUT2=q{!$seN5r z>&6p9Inenq!g{*fJIlJ7PmTK>BWnwmP{Ro$!RDnZ)-9j z5N@kd2!KyJ~wju{0Fhg|mP9?Nnkjv#=GD`@2numWP$(6CUQA|TMH zHdEhL+1L0c3^CrLhVMnwo|8JGF%v#5hIiKgEXkMV3j-K?2ZZ$5Sn`SQv#tp&Pc<OvTk>=Z)y(D$Oaf z-a9%L&!&*Be0^z$G8IN`tGSsfVwEH3or&WUz@;6P7lnCs&0>6#3FWO!vw`WY!HST4 z-fc4!v(^nroMm{9C~;5ndnJ)g&o&q*KwLp2RU~!MM}vaMd2z#Z2b;w(P@o2VuI`;t z(VPII8a?4~x_8#8Zm4Eyi(~QmcJq}raGj@5LU}AiP;0};?@dvg6fIRowO1x%bbs=eS&Qj^6o+AO+LFM6Gzx`bDfB2#5x(^y0oJ z6O<45OiN>)N+-Etg@)HHA|FtgNiD5{Pw6A#r5u~HnMrM)!l)uFC%wYsPDU>GG{pO{ z^d#PJ&)DhI=8T?W+^Oqku*dY#T#>;NUVZbRzsi9fyV$%-pi}u0J%99iU|t3$wAS9B z%LhZ!l_*?3LeuKip*G5KCh@QXF(!HJc;}{vCd7MFiZ*mo*eL;Hs!!{2k!6)Buy)dJ zf%mp%jUfdKjGrmL|AD(+9r#I;MyPCJ;N5;W<_2!35p^Ds0@JW!hdzy5Ijc#mPWL%o z|GFo7jIGndwqQZ+I?T>zMk<){z@rU>V$hc>V zPE08E)_phoj0QUlQGw9~ zPB!TMZRWkj=v9p7dnQAkbX&8AJth-r@=MHiiOdY+g{n3*I} z@hz*opo&iqd}UCBX@A^ObFs3ojv_r5xY_-zD>`GXgHXKjW0(w&NsNHufr=Hm zD7Bw8o@zWB$^KO9M>nv<=m4V=MbKF-;N(ljae`vs)Ja4GL+@- z#)->l57O?A_Vyo*d<_+JkgMKgm-06PeTi@qhE=E1(98$fjimd*xg5;x$v+jkvF(8- zqPN$Sil8^kdYFI%k36F54Vb!R zO~=+8Kedijk9_zPZsUPEt*>#%!pETB^uW^+Ot@F2$eqQ26662KtKfa6LWM!7VZ12x zs47MWa{&L_liGVNBMWK~N?5R@Qor`Q>v)CsVOWf6Lrh}!Lly}FWWYQr!SsOcLecWW z&tu%S;vZm#-VA1$5tCm&gnZ-u@Zc-pNlf0^`-y$kjE!|ec6myZdYMu4$J8IHFyK zrueLewq=&?AAE7{NRHC2Jx-s(CMN41avYl2&*t@pb#QQHar*Uj2)$K(hu356TvFk$ z3H6|QMOGx?r3<}qBl`rleuIoLd?y9HKnBM)NKm(V{0nQgpyElhlUEFtHMw2ikhn_}T zM+t`Yi!bwLE5?Qob8K6o;F3_%vQOcu2ENGK(7dTPt{0|wqHak<0tSzLhF-8N&Yx!MoFO#y%jUQ1x%oK1BY{m@&<=MWFT6( z6PJjz{~;)Uz$LAFYlTk!aKI}Px&M;AfbKCxo8KQ{_G4fj)u$>5?fMvmGG8Odk+&#s2ID(ibiQAMK z-6OcKK7*El%ZFkjv^SR|&|2|0ZPEPN z6GdxI!%7CmyprN;=$dmBw!OpSo0fgwG#raxXB7q6a#nW{2QPilA6dnZv*yKzx+W>8 z0KoO<4kD}2v~spLb1=2GgK@xrT(UblSca-9%VA-VVIWwrjE6#6zS`Xj((HH2a#;FfsyMIi1r% z_Xg7Jd&O?@t~!$vkduvr?;XAJRGO0Rl&beusMy1OiHsk&M5m^v-dlH_)=#$O>+eMw zhl`=|2G^d~9TW|5b?eu44YLnF6GJ_S1oMh6fZy-MXGYzdRD(=$4Rr5&*5e{zLD`hz zK5)qS<;B@$l)1*Kn?!l&6rn6h4TZO&R^-J>G(Sl~+4;#T$bgrnL1<)g@0-t#=04Eu z6OZbJNlkFPOB_yml({^`;7EL?krLhH*@@eg+tv2P`mhe{WX?Q&1@!ejR>%@34!t@v z0n?Sfj{0j>LT|gQ6!9S9Kut9KXZ#D4(t)F+Hhq;lS8XLSJnj1)tvR z{a#R}VSjq1qV1TKkQc`F{Kxo2iwBb>>*{NAgr*CkKmp0IdoOLm-{@^q)Q9zU_{bW& zxqO{}v|m~@Mf-+8!|I->aG2O-=}^0zfa6p>NxxpUyVV;3xJz$`R}tD$vf`)UUhGuK z{nCfYZ86;Tja~dXxZ^oGgZ1QnlM;8yW*S)Z??aDW3dEMk$!fPYi?GGS#S8pW_x6tH zqZ|djKl6K_77yuVPl#zH+vGApF&2L!GKeN=Tpz`C0k^x zfVfvck)r(Ma@7_=O_iOr107vMLcV;S=Z6is8kF|#1m?5T-l^gDv_+5$qd<0by>I74 z*E2g5Jc0d{Td;5e?PeA4eey@gFr0mxs__Vxi={AhIjq}Y4OmU#$J9VaD zXuHR_4;&ZG4puuI=~=uVgD%wH$?l@yL!A-W~3QxUrtTSEw#MqtAXI?rHE^p7{`|c(cX}tO}CAaQA zDvVFLO=;ZRq)nO5l<6~!V0EONM8=uK>Kn3@s|qSQw>(%nJQELvh}aKAu4Z1oUd{Nn za67(WxR&Quc37r+^|0SD5VK;j4k^Q2WHcm#t$r6KSlj{a-h)Jp7KuvsE zub$j?@+w!!cbI#g@#RtPWUcOL$o;GK&8_C6t2sX>!h+9~^%t}GM@dsBY@aBFVVW-w zOM56Cs#SK@x5P1wAPTE$_e~ z+mRD{Om9x+RfN9)7Z85ija!_g*KzMl60{*%$>ef+S2d37p53(%oNSPOAFLoYigl(d zNx^osF+a%k#$7of+w)NEvtd_)h@Qi^oTxpu??nE`5)wkWlB6Tk z7qIlCBVcEWg3C9t`;m=tUC~KLYOT_CiIH`_l!2?JescRjY5vT6!yVWxZc{$7~uH^Pn( zM`uSw!O{7DC>N!Uz;^#bx$An2GtA5s`d{QIerG2XVu>hZ{(mH|fAat8o@-10 zyT0E^UAyF`)b9#E48PH33IZYOx8KWM93W>X^v?rLWW?F$t?NoP;Cg#;O@IH8erJq@ z9RyJU{Nb|S8FRgnfx+Do)wu69DE;op`5L^YWhP8EzK7TqJ{u|4!u^dS8)l_9Tu@uS zY}869#J1giM97#wnWFQNI+$XQ*!^jPVswLCn0O46e%WF~y=R~BJG0h&`~opz@v_cW z{L>-}c2E^*8F#1o!2Wkh(KJiwY(xB09F#r}hNt+eo83S3$F`d%?$msdr&gn{j?g6F zWY5pSn{umUd`Y2Gp+_vhFsR z1rH11xp5Eat|Da@yLWbwiECnJQM8G0l)mva65-VIYS`ss>dM-N@FlHU@GY8c28Xx3 zAMP8Ptr=voZK=@4KMTO)eY_al{}lp<(d_D8#TKutt1Yw8$Vxq>7tWO3C1zl>0cdptL`q-%k^?z!2Fl72Zop6cRGzgG%l zn#9&DO

yd{$-KBM33nLbWKjb_j9GK5s16nwR3-p{lKyLhc z3rl)StFzi^Irdp38T?kvO%gcI{-Yx*tw1DS&i5?F(J{A2kzyhl3=`;Wm}p-Jl4oPm z+iMQf$a`qDBq!qR-$){v)F?aYTz`$)L10=MkM>GWCDDpL-ZP3Uz^?>8ws||RRG6`~ zPP9|qh$Qs!Eo3Tp#?Z7!0f~J2E>mA;1M**e5_1$g-(VqCU&i9h7u|ae;@fjb;vS^P zs)oc?za4Iqqq%>&-I+|jtxqi=eWSHPlk@7i_=H;vU4nFrIdOuM$xD#{AYpzfD~6bR zleaNF%GEQkZ>lRW5@bJcsEViAF`6`tD?7j^p=hww3V@)ojVwgq=N2S(L1GY5t-t8c{68F zT1kwB!~i82ZxhIIu|L@{2M9+E&Y(y6nySxK%)6AN7EQg$gl8wU@YG<;H{7l)Aj8V5 zlHi@kjTbB?h?#pe+BMS}k#%Rp$#cF4g`Jzs^I5UCY}S)xX1c%ywLII>P{DaeTSk(Qzks~LCmbq0U7_)rxz8U(@mAVinSbv2>{~r&qG5>&XBk`rL4qa;4`}!s zLf&4^wHAw7pKt}Ei&^BaM!fEFd@oYnBX4t$guYwJE2#8xsIMd}SeyD^lM-g;z`Gh-RFGS^;S!EI3CmciD z1>swFu@*s@CL)yz4$1cBsb#WejSe@b8`ohtM% zi&j*&8w|Ic58YqDx3c-Av)pJ}Xm-EkAO$nbioWh`g{_)O5hy8!#VVHu2}-o+)c92g z_2({@M~Va`=VB0*UkG8mRf#Rvjt*L_m@OpXQ^p-f& zTh|SqWweIox8v>jLFprkW4R+u+Ojm`C&fr<^9F8g+`egO8QIGEAc(-5ItN`<(-OA@ zoJE5#3h!OPsBV1Us8#2k8SaSMfguzlRZRA@UWmKXN{d@|p%rbgMyzc=6OUTUbJFeV z^vrlX0A@6|@Ve@gwcj1YL95}q&K6#^O3_)v$vy3z3qE6~rDUo4pxetjr!g~>;ro~r z89nbLQbVj{((ch=p>g1(kGA1rD5W$D1T2wAubj%zz@32s8LQ_qH2Z62Xx&D1VkI2G zJ+~KE*TboKeA7i5Yp5hSUgUh3F~c>y@Wrd7ts+Vi*L1sU|LWa@>Sm$VyX>&H^Z|G6 zm_xR5^VI7=_I_r;&g-Zf?_B5mP6ypA~< zq}LLja`~}Z)uz15Me@DvI64s2y+NZx!0X0CpSauU)QFlEuu6S!(pb8}w_Q%ELZ?}X zhA|@Pnru+$;lXxw(qVjg(bFs{Q_^NKi{UJ*Z_SU!&_s)ImQ2#$b($Ax?a#37q?T;w zU|2)Mjcdk2i4A=qlXK%!7iN+dR6{!H_IpNoPiXM;QI>M5|D;C!#UeaV8UFNmO$WaV zEACzW5=q2VwrW4DE#smPaJK?UtQx|DRf4J~=XY6>!#i-f1AUXXf9j_3X`{F$rSG%q zM6DH>tec_$+u}rj*jlVlEZ^5FTtVDgn`}|#l^o~?p2K}*(oMwgiAwa5#hK5`c$O5q z)c*c?R|Zd`CL|k4N8*NXDOc8S1)9~{o-PSXQ~Qgk?(H&Vo;Y?6!?d$$ zeS!haU!_xedLEe*sUK4Su`a{#YMQG>MxB5@B#Fpw8&K(G4!QjmOI1$=G#Bw6_z;!S zyj$sU*^tTS`~2k_!{PmoX=ZjPqU)YUy(5PEqmP5-WgRoNKJ@gkGNe^oeaWO2$aEBu zUAB)v&9qFJ=RJc%p@x7$N%VdK?XE zM&384UGujTDKcQp=lKI8)za%VO|f))WXg?e4nQ6HX|J=MD7s=ey@ zt+zjt#pR^7%V=3cae=kC8QC32q4|okZ{m{VdY5#gQ+=c6x^s~G$1dr2`yC?0en&}0 zQ-V!iS(d{d(e-5khJCLc7iqLOe4xT?JYu2Q@+-L>8w|gO_rTqp3wOob|tR9V!LTK>Yy=s>;Xz=|K`--d^9>H#3)py zIDuR%D!dYrZ;knti^!i>PtBTuVdI!Jfx~%^I^Pk|k5wugjI7>J=oTD5qiDulf~537 z)9azit)Y9?V+W%(!5|mo+z2Iqc4{1f32LA+7FG5s^GnyLMpEc&dqw^93e?;52Wl%6 z^V3NeQR^-Bk4PENMaYZe(O52Wdf#Vm@y`uYPI@C>J2fA$*nxio0GL9)cIpizLcrg* zXI^hlf6|X;&A&?hwFmR3gc{=Vx@YQoBl<(?$0Pi;9pJig>L>LfdO3e>qx!4luSb5@ z?E*iE7_s;LrFr0o;NP7H{uHz${`>R6zoPt<0syXau%Gnc?ti8n|B;dXyT^YD0|3`K z*-!HR4cM<4+CRmq{0+{p8QVX_+58R8uNmCm<6P%(KZ*X{e>w@#=l4rS_xDKG3DHk7 z{teR44DX+!#Qg^4XU6vj%8xaBt@}sXcAfM6q^{qf{7nY<-`3N=Pml(&y#AWP{>=aW z-Lzk0<#h)5lXMX%Kl8yK+J8*x*KlwhNq^Eq#=nWHstPEGBXs}(7x9k|@vbH_U4Q$3 DeGk#2 From 40610c7d484d077dc10726628c3adbe01edd91df Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 18 Oct 2013 10:38:51 +0200 Subject: [PATCH 796/872] Fixes, but approach needs proper design --- .../fw_pos_control_l1_main.cpp | 66 ++++++++++++++----- src/modules/mavlink/waypoints.c | 12 ++-- 2 files changed, 57 insertions(+), 21 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 219c6ffa6b..9ed74f0caf 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -158,6 +158,12 @@ private: float _launch_alt; bool _launch_valid; + /* land states */ + /* not in non-abort mode for landing yet */ + bool land_noreturn; + /* heading hold */ + float target_bearing; + /* throttle and airspeed states */ float _airspeed_error; ///< airspeed error to setpoint in m/s bool _airspeed_valid; ///< flag if a valid airspeed estimate exists @@ -329,7 +335,8 @@ FixedwingPositionControl::FixedwingPositionControl() : _airspeed_error(0.0f), _airspeed_valid(false), _groundspeed_undershoot(0.0f), - _global_pos_valid(false) + _global_pos_valid(false), + land_noreturn(false) { _nav_capabilities.turn_distance = 0.0f; @@ -635,9 +642,6 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* no throttle limit as default */ float throttle_max = 1.0f; - /* not in non-abort mode for landing yet */ - bool land_noreturn = false; - /* AUTONOMOUS FLIGHT */ // XXX this should only execute if auto AND safety off (actuators active), @@ -722,15 +726,26 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* switch to heading hold for the last meters, continue heading hold after */ - float wp_distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), next_wp.getX(), next_wp.getY()); - - if (wp_distance < 5.0f || land_noreturn) { + float wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), current_position.getX(), current_position.getY()); + //warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO"); + if (wp_distance < 15.0f || land_noreturn) { /* heading hold, along the line connecting this and the last waypoint */ - float target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY()); + + + // if (global_triplet.previous_valid) { + // target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY()); + // } else { + + if (!land_noreturn) + target_bearing = _att.yaw; + //} + + warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw)); + _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed); - if (altitude_error > -20.0f) + if (altitude_error > -5.0f) land_noreturn = true; } else { @@ -739,6 +754,12 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); } + /* do not go down too early */ + if (wp_distance > 50.0f) { + altitude_error = (_global_triplet.current.altitude + 25.0f) - _global_pos.alt; + } + + _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); @@ -748,15 +769,21 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float flare_angle_rad = math::radians(10.0f);//math::radians(global_triplet.current.param1) float land_pitch_min = math::radians(5.0f); float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; - float airspeed_land = (_parameters.airspeed_min + _parameters.airspeed_trim) / 2.0f; + float airspeed_land = _parameters.airspeed_min; + float airspeed_approach = (_parameters.airspeed_min + _parameters.airspeed_trim) / 2.0f; - if (altitude_error > -5.0f) { + if (altitude_error > -4.0f) { - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min), + /* land with minimal speed */ + + /* force TECS to only control speed with pitch, altitude is only implicitely controlled now */ + _tecs.set_speed_weight(2.0f); + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_land), _airspeed.indicated_airspeed_m_s, eas2tas, false, flare_angle_rad, 0.0f, _parameters.throttle_max, throttle_land, - land_pitch_min, math::radians(_parameters.pitch_limit_max)); + math::radians(-10.0f), math::radians(15.0f)); /* kill the throttle if param requests it */ throttle_max = math::min(throttle_max, _parameters.throttle_land_max); @@ -764,9 +791,11 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* limit roll motion to prevent wings from touching the ground first */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); - } else if (altitude_error > -20.0f) { + } else if (wp_distance < 60.0f && altitude_error > -20.0f) { - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_land), + /* minimize speed to approach speed */ + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_approach), _airspeed.indicated_airspeed_m_s, eas2tas, false, flare_angle_rad, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, @@ -774,6 +803,8 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else { + /* normal cruise speed */ + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), @@ -866,6 +897,11 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } } + /* reset land state */ + if (global_triplet.current.nav_cmd != NAV_CMD_LAND) { + land_noreturn = false; + } + if (was_circle_mode && !_l1_control.circle_mode()) { /* just kicked out of loiter, reset roll integrals */ _att_sp.roll_reset_integral = true; diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index adaf814047..7e4a2688f6 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -398,13 +398,13 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ if (time_elapsed) { - if (cur_wp->autocontinue) { + /* safeguard against invalid missions with last wp autocontinue on */ + if (wpm->current_active_wp_id == wpm->size - 1) { + /* stop handling missions here */ + cur_wp->autocontinue = false; + } - /* safeguard against invalid missions with last wp autocontinue on */ - if (wpm->current_active_wp_id == wpm->size - 1) { - /* stop handling missions here */ - cur_wp->autocontinue = false; - } + if (cur_wp->autocontinue) { cur_wp->current = 0; From 1d3f25ee6c9983ec5da9de4d4f7b463f880f3a87 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 19 Oct 2013 10:43:41 +0200 Subject: [PATCH 797/872] pwm systemcmd can now set the failsafe values, fmu uses failsafe values as well now, fix to only send the appropriate number of pwm values to IO at once --- src/drivers/drv_pwm_output.h | 18 ++- src/drivers/px4fmu/fmu.cpp | 202 ++++++++++++++++---------- src/drivers/px4io/px4io.cpp | 128 ++++++---------- src/modules/px4iofirmware/registers.c | 13 +- src/systemcmds/pwm/pwm.c | 46 +++++- 5 files changed, 233 insertions(+), 174 deletions(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 3357e67a50..f0dd713b27 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -154,23 +154,29 @@ ORB_DECLARE(output_pwm); /** power up DSM receiver */ #define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 11) +/** set the PWM value for failsafe */ +#define PWM_SERVO_SET_FAILSAFE_PWM _IOC(_PWM_SERVO_BASE, 12) + +/** get the PWM value for failsafe */ +#define PWM_SERVO_GET_FAILSAFE_PWM _IOC(_PWM_SERVO_BASE, 13) + /** set the PWM value when disarmed - should be no PWM (zero) by default */ -#define PWM_SERVO_SET_DISARMED_PWM _IOC(_PWM_SERVO_BASE, 12) +#define PWM_SERVO_SET_DISARMED_PWM _IOC(_PWM_SERVO_BASE, 14) /** get the PWM value when disarmed */ -#define PWM_SERVO_GET_DISARMED_PWM _IOC(_PWM_SERVO_BASE, 13) +#define PWM_SERVO_GET_DISARMED_PWM _IOC(_PWM_SERVO_BASE, 15) /** set the minimum PWM value the output will send */ -#define PWM_SERVO_SET_MIN_PWM _IOC(_PWM_SERVO_BASE, 14) +#define PWM_SERVO_SET_MIN_PWM _IOC(_PWM_SERVO_BASE, 16) /** get the minimum PWM value the output will send */ -#define PWM_SERVO_GET_MIN_PWM _IOC(_PWM_SERVO_BASE, 15) +#define PWM_SERVO_GET_MIN_PWM _IOC(_PWM_SERVO_BASE, 17) /** set the maximum PWM value the output will send */ -#define PWM_SERVO_SET_MAX_PWM _IOC(_PWM_SERVO_BASE, 16) +#define PWM_SERVO_SET_MAX_PWM _IOC(_PWM_SERVO_BASE, 18) /** get the maximum PWM value the output will send */ -#define PWM_SERVO_GET_MAX_PWM _IOC(_PWM_SERVO_BASE, 17) +#define PWM_SERVO_GET_MAX_PWM _IOC(_PWM_SERVO_BASE, 19) /** set a single servo to a specific value */ #define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index e729612cc3..b7b2f7b33d 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.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 @@ -78,6 +78,12 @@ # include #endif +/* + * This is the analog to FMU_INPUT_DROP_LIMIT_US on the IO side + */ + +#define CONTROL_INPUT_DROP_LIMIT_MS 20 + class PX4FMU : public device::CDev { public: @@ -130,9 +136,11 @@ private: actuator_controls_s _controls; pwm_limit_t _pwm_limit; + uint16_t _failsafe_pwm[_max_actuators]; uint16_t _disarmed_pwm[_max_actuators]; uint16_t _min_pwm[_max_actuators]; uint16_t _max_pwm[_max_actuators]; + unsigned _num_failsafe_set; unsigned _num_disarmed_set; static void task_main_trampoline(int argc, char *argv[]); @@ -218,7 +226,9 @@ PX4FMU::PX4FMU() : _armed(false), _pwm_on(false), _mixers(nullptr), + _failsafe_pwm({0}), _disarmed_pwm({0}), + _num_failsafe_set(0), _num_disarmed_set(0) { for (unsigned i = 0; i < _max_actuators; i++) { @@ -515,98 +525,103 @@ PX4FMU::task_main() /* sleep waiting for data, stopping to check for PPM * input at 100Hz */ - int ret = ::poll(&fds[0], 2, 10); + int ret = ::poll(&fds[0], 2, CONTROL_INPUT_DROP_LIMIT_MS); /* this would be bad... */ if (ret < 0) { log("poll error %d", errno); usleep(1000000); continue; - } + } else if (ret == 0) { + /* timeout: no control data, switch to failsafe values */ +// warnx("no PWM: failsafe"); - /* do we have a control update? */ - if (fds[0].revents & POLLIN) { + } else { - /* get controls - must always do this to avoid spinning */ - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); + /* do we have a control update? */ + if (fds[0].revents & POLLIN) { - /* can we mix? */ - if (_mixers != nullptr) { + /* get controls - must always do this to avoid spinning */ + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); - unsigned num_outputs; + /* can we mix? */ + if (_mixers != nullptr) { - 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; - } + unsigned num_outputs; - /* do mixing */ - outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); - outputs.timestamp = hrt_absolute_time(); - - /* iterate actuators */ - for (unsigned i = 0; i < num_outputs; i++) { - /* last resort: catch NaN, INF and out-of-band errors */ - if (i >= outputs.noutputs || - !isfinite(outputs.output[i]) || - outputs.output[i] < -1.0f || - outputs.output[i] > 1.0f) { - /* - * Value is NaN, INF or out of band - set to the minimum value. - * This will be clearly visible on the servo status and will limit the risk of accidentally - * spinning motors. It would be deadly in flight. - */ - outputs.output[i] = -1.0f; + 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(); + + /* iterate actuators */ + for (unsigned i = 0; i < num_outputs; i++) { + /* last resort: catch NaN, INF and out-of-band errors */ + if (i >= outputs.noutputs || + !isfinite(outputs.output[i]) || + outputs.output[i] < -1.0f || + outputs.output[i] > 1.0f) { + /* + * Value is NaN, INF or out of band - set to the minimum value. + * This will be clearly visible on the servo status and will limit the risk of accidentally + * spinning motors. It would be deadly in flight. + */ + outputs.output[i] = -1.0f; + } + } + + uint16_t pwm_limited[num_outputs]; + + pwm_limit_calc(_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit); + + /* output actual limited values */ + for (unsigned i = 0; i < num_outputs; i++) { + controls_effective.control_effective[i] = (float)pwm_limited[i]; + } + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective); + + /* output to the servos */ + for (unsigned i = 0; i < num_outputs; i++) { + up_pwm_servo_set(i, pwm_limited[i]); + } + + /* and publish for anyone that cares to see */ + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs); } - - uint16_t pwm_limited[num_outputs]; - - pwm_limit_calc(_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit); - - /* output actual limited values */ - for (unsigned i = 0; i < num_outputs; i++) { - controls_effective.control_effective[i] = (float)pwm_limited[i]; - } - orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective); - - /* output to the servos */ - for (unsigned i = 0; i < num_outputs; i++) { - up_pwm_servo_set(i, pwm_limited[i]); - } - - /* and publish for anyone that cares to see */ - orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs); } - } - /* how about an arming update? */ - if (fds[1].revents & POLLIN) { - actuator_armed_s aa; + /* how about an arming update? */ + if (fds[1].revents & POLLIN) { + actuator_armed_s aa; - /* get new value */ - orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa); + /* get new value */ + orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa); - /* update the armed status and check that we're not locked down */ - bool set_armed = aa.armed && !aa.lockdown; - if (_armed != set_armed) - _armed = set_armed; + /* update the armed status and check that we're not locked down */ + bool set_armed = aa.armed && !aa.lockdown; + if (_armed != set_armed) + _armed = set_armed; - /* update PWM status if armed or if disarmed PWM values are set */ - bool pwm_on = (aa.armed || _num_disarmed_set > 0); - if (_pwm_on != pwm_on) { - _pwm_on = pwm_on; - up_pwm_servo_arm(pwm_on); + /* update PWM status if armed or if disarmed PWM values are set */ + bool pwm_on = (aa.armed || _num_disarmed_set > 0); + if (_pwm_on != pwm_on) { + _pwm_on = pwm_on; + up_pwm_servo_arm(pwm_on); + } } } @@ -737,6 +752,45 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) *(uint32_t *)arg = _pwm_alt_rate_channels; break; + case PWM_SERVO_SET_FAILSAFE_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values*)arg; + /* discard if too many values are sent */ + if (pwm->channel_count > _max_actuators) { + ret = -EINVAL; + break; + } + for (unsigned i = 0; i < pwm->channel_count; i++) { + if (pwm->values[i] == 0) { + /* ignore 0 */ + } else if (pwm->values[i] > PWM_MAX) { + _failsafe_pwm[i] = PWM_MAX; + } else if (pwm->values[i] < PWM_MIN) { + _failsafe_pwm[i] = PWM_MIN; + } else { + _failsafe_pwm[i] = pwm->values[i]; + } + } + + /* + * update the counter + * this is needed to decide if disarmed PWM output should be turned on or not + */ + _num_failsafe_set = 0; + for (unsigned i = 0; i < _max_actuators; i++) { + if (_failsafe_pwm[i] > 0) + _num_failsafe_set++; + } + break; + } + case PWM_SERVO_GET_FAILSAFE_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values*)arg; + for (unsigned i = 0; i < _max_actuators; i++) { + pwm->values[i] = _failsafe_pwm[i]; + } + pwm->channel_count = _max_actuators; + break; + } + case PWM_SERVO_SET_DISARMED_PWM: { struct pwm_output_values *pwm = (struct pwm_output_values*)arg; /* discard if too many values are sent */ diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index d709f1cc89..b603ffc8d5 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -943,53 +943,6 @@ PX4IO::io_set_control_state() return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_controls); } -int -PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len) -{ - if (len > _max_actuators) - /* fail with error */ - return E2BIG; - - /* copy values to registers in IO */ - return io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, vals, len); -} - -int -PX4IO::set_min_values(const uint16_t *vals, unsigned len) -{ - - 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) -{ - - 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::set_idle_values(const uint16_t *vals, unsigned len) -{ - - if (len > _max_actuators) - /* fail with error */ - return E2BIG; - - /* copy values to registers in IO */ - return io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, vals, len); -} - int PX4IO::io_set_arming_state() @@ -1803,7 +1756,7 @@ PX4IO::print_status() printf("failsafe"); for (unsigned i = 0; i < _max_actuators; i++) printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); - printf("\nidle values"); + printf("\ndisarmed values"); for (unsigned i = 0; i < _max_actuators; i++) printf(" %u", io_reg_get(PX4IO_PAGE_DISARMED_PWM, i)); printf("\n"); @@ -1874,15 +1827,39 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) *(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES); break; + case PWM_SERVO_SET_FAILSAFE_PWM: { + struct pwm_output_values* pwm = (struct pwm_output_values*)arg; + if (pwm->channel_count > _max_actuators) + /* fail with error */ + return E2BIG; + + /* copy values to registers in IO */ + ret = io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, pwm->values, pwm->channel_count); + break; + } + + case PWM_SERVO_GET_FAILSAFE_PWM: + + ret = io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, 0, (uint16_t*)arg, _max_actuators); + if (ret != OK) { + ret = -EIO; + } + break; + case PWM_SERVO_SET_DISARMED_PWM: { struct pwm_output_values* pwm = (struct pwm_output_values*)arg; - set_idle_values(pwm->values, pwm->channel_count); + if (pwm->channel_count > _max_actuators) + /* fail with error */ + return E2BIG; + + /* copy values to registers in IO */ + ret = io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, pwm->values, pwm->channel_count); break; } case PWM_SERVO_GET_DISARMED_PWM: - ret = io_reg_get(PX4IO_PAGE_DISARMED_PWM, 0, (uint16_t*)arg, sizeof(struct pwm_output_values)/sizeof(uint16_t)); + ret = io_reg_get(PX4IO_PAGE_DISARMED_PWM, 0, (uint16_t*)arg, _max_actuators); if (ret != OK) { ret = -EIO; } @@ -1890,13 +1867,18 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) case PWM_SERVO_SET_MIN_PWM: { struct pwm_output_values* pwm = (struct pwm_output_values*)arg; - set_min_values(pwm->values, pwm->channel_count); + if (pwm->channel_count > _max_actuators) + /* fail with error */ + return E2BIG; + + /* copy values to registers in IO */ + ret = io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, pwm->values, pwm->channel_count); break; } case PWM_SERVO_GET_MIN_PWM: - ret = io_reg_get(PX4IO_PAGE_CONTROL_MIN_PWM, 0, (uint16_t*)arg, sizeof(struct pwm_output_values)/sizeof(uint16_t)); + ret = io_reg_get(PX4IO_PAGE_CONTROL_MIN_PWM, 0, (uint16_t*)arg, _max_actuators); if (ret != OK) { ret = -EIO; } @@ -1904,13 +1886,18 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) case PWM_SERVO_SET_MAX_PWM: { struct pwm_output_values* pwm = (struct pwm_output_values*)arg; - set_max_values(pwm->values, pwm->channel_count); + if (pwm->channel_count > _max_actuators) + /* fail with error */ + return E2BIG; + + /* copy values to registers in IO */ + ret = io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, pwm->values, pwm->channel_count); break; } case PWM_SERVO_GET_MAX_PWM: - ret = io_reg_get(PX4IO_PAGE_CONTROL_MAX_PWM, 0, (uint16_t*)arg, sizeof(struct pwm_output_values)/sizeof(uint16_t)); + ret = io_reg_get(PX4IO_PAGE_CONTROL_MAX_PWM, 0, (uint16_t*)arg, _max_actuators); if (ret != OK) { ret = -EIO; } @@ -2518,37 +2505,6 @@ px4io_main(int argc, char *argv[]) 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 (unsigned i = 0; i < sizeof(failsafe) / sizeof(failsafe[0]); i++) { - - /* set channel to commandline argument or to 900 for non-provided channels */ - if ((unsigned)argc > i + 2) { - failsafe[i] = atoi(argv[i+2]); - if (failsafe[i] < PWM_MIN || failsafe[i] > PWM_MAX) { - errx(1, "value out of range of %d < value < %d. Aborting.", PWM_MIN, PWM_MAX); - } - } 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")) { @@ -2616,5 +2572,5 @@ px4io_main(int argc, char *argv[]) bind(argc, argv); out: - errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'failsafe', 'bind' or 'update'"); + errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind' or 'update'"); } diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index a338afe161..6a0532beec 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -199,7 +199,7 @@ uint16_t r_page_rc_input_config[PX4IO_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRI * * Disable pulses as default. */ -uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0 }; +uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 }; /** * PAGE 106 @@ -276,8 +276,15 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* copy channel data */ while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { - /* XXX range-check value? */ - r_page_servo_failsafe[offset] = *values; + if (*values == 0) { + /* ignore 0 */ + } else if (*values < PWM_MIN) { + r_page_servo_failsafe[offset] = PWM_MIN; + } else if (*values > PWM_MAX) { + r_page_servo_failsafe[offset] = PWM_MAX; + } else { + r_page_servo_failsafe[offset] = *values; + } /* flag the failsafe values as custom */ r_setup_arming |= PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM; diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 25f8c4e753..3185e43714 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -72,20 +72,21 @@ usage(const char *reason) warnx("%s", reason); errx(1, "usage:\n" - "pwm arm|disarm|rate|min|max|disarmed|test|info ...\n" + "pwm arm|disarm|rate|failsafe|disarmed|min|max|test|info ...\n" "\n" " arm Arm output\n" " disarm Disarm output\n" "\n" - " rate Configure PWM rates\n" + " rate ... Configure PWM rates\n" " [-g ] Channel group that should update at the alternate rate\n" " [-m ] Directly supply channel mask\n" " [-a] Configure all outputs\n" " -r PWM rate (50 to 400 Hz)\n" "\n" + " failsafe ... Configure failsafe PWM values\n" + " disarmed ... Configure disarmed PWM values\n" " min ... Configure minimum PWM values\n" " max ... Configure maximum PWM values\n" - " disarmed ... Configure disarmed PWM values\n" " [-c ] Supply channels (e.g. 1234)\n" " [-m ] Directly supply channel mask (e.g. 0xF)\n" " [-a] Configure all outputs\n" @@ -357,6 +358,35 @@ pwm_main(int argc, char *argv[]) } exit(0); + } else if (!strcmp(argv[1], "failsafe")) { + + if (set_mask == 0) { + usage("no channels set"); + } + if (pwm_value == 0) + usage("no PWM value provided"); + + struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0}; + + for (unsigned i = 0; i < servo_count; i++) { + if (set_mask & 1< Date: Sat, 19 Oct 2013 10:44:38 +0200 Subject: [PATCH 798/872] Use new pwm cmds in rc.custom_io_esc --- ROMFS/px4fmu_common/init.d/rc.custom_io_esc | 28 ++++++++++----------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_io_esc b/ROMFS/px4fmu_common/init.d/rc.custom_io_esc index e645d9d549..999422767a 100644 --- a/ROMFS/px4fmu_common/init.d/rc.custom_io_esc +++ b/ROMFS/px4fmu_common/init.d/rc.custom_io_esc @@ -62,19 +62,6 @@ then usleep 5000 sh /etc/init.d/rc.io - - if [ $ESC_MAKER = afro ] - then - # Set PWM values for Afro ESCs - px4io idle 1050 1050 1050 1050 - px4io min 1080 1080 1080 1080 - px4io max 1860 1860 1860 1860 - else - # Set PWM values for typical ESCs - px4io idle 900 900 900 900 - px4io min 1110 1100 1100 1100 - px4io max 1800 1800 1800 1800 - fi else fmu mode_pwm # Start MAVLink (on UART1 / ttyS0) @@ -84,6 +71,19 @@ else set EXIT_ON_END yes fi +if [ $ESC_MAKER = afro ] +then + # Set PWM values for Afro ESCs + pwm disarmed -c 1234 -p 1050 + pwm min -c 1234 -p 1080 + pwm max -c 1234 -p 1860 +else + # Set PWM values for typical ESCs + pwm disarmed -c 1234 -p 900 + pwm min -c 1234 -p 980 + pwm max -c 1234 -p 1800 +fi + # # Load mixer # @@ -105,7 +105,7 @@ fi # # Set PWM output frequency # -pwm -u 400 -m 0xff +pwm rate -r 400 -c 1234 # # Start common for all multirotors apps From ba77836000eaa28041969577482e3e4074d11e1b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 19 Oct 2013 10:44:58 +0200 Subject: [PATCH 799/872] Small indentation fix --- ROMFS/px4fmu_common/init.d/rcS | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 93e76184d4..5fb62af481 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -235,13 +235,13 @@ then set MODE custom fi - # PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame - if param compare SYS_AUTOSTART 22 - then - set FRAME_GEOMETRY w - sh /etc/init.d/rc.custom_io_esc - set MODE custom - fi + # PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame + if param compare SYS_AUTOSTART 22 + then + set FRAME_GEOMETRY w + sh /etc/init.d/rc.custom_io_esc + set MODE custom + fi if param compare SYS_AUTOSTART 30 then From 6d406968eaba0924fe48945fd8a71b6f8a0adc1b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Oct 2013 11:11:15 +0200 Subject: [PATCH 800/872] Added hexrotor support --- ROMFS/px4fmu_common/init.d/12-13_hex | 95 ++++++++++++++++++++++++++++ ROMFS/px4fmu_common/init.d/rcS | 14 ++++ 2 files changed, 109 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/12-13_hex diff --git a/ROMFS/px4fmu_common/init.d/12-13_hex b/ROMFS/px4fmu_common/init.d/12-13_hex new file mode 100644 index 0000000000..0f0bb05ced --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/12-13_hex @@ -0,0 +1,95 @@ +#!nsh + +echo "[init] PX4FMU v1, v2 with or without IO on Hex" + +# +# 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.004 + param set MC_ATTRATE_I 0.0 + 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 7.0 + param set MC_YAWPOS_D 0.0 + 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 set NAV_TAKEOFF_ALT 3.0 + param set MPC_TILT_MAX 0.5 + param set MPC_THR_MAX 0.7 + param set MPC_THR_MIN 0.3 + param set MPC_XY_D 0 + param set MPC_XY_P 0.5 + param set MPC_XY_VEL_D 0 + param set MPC_XY_VEL_I 0 + param set MPC_XY_VEL_MAX 3 + param set MPC_XY_VEL_P 0.2 + param set MPC_Z_D 0 + param set MPC_Z_P 1 + param set MPC_Z_VEL_D 0 + param set MPC_Z_VEL_I 0.1 + param set MPC_Z_VEL_MAX 2 + param set MPC_Z_VEL_P 0.20 + + param save +fi + +# +# Force some key parameters to sane values +# MAV_TYPE list: https://pixhawk.ethz.ch/mavlink/ +# 13 = hexarotor +# +param set MAV_TYPE 13 + +set EXIT_ON_END no + +# +# Start and configure PX4IO or FMU interface +# +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start + usleep 5000 + + sh /etc/init.d/rc.io + # Set PWM values for DJI ESCs + px4io idle 900 900 900 900 900 900 + px4io min 1200 1200 1200 1200 1200 1200 + px4io max 1900 1900 1900 1900 1900 1900 +else + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + usleep 5000 + fmu mode_pwm + param set BAT_V_SCALING 0.004593 + set EXIT_ON_END yes +fi + +# +# Load mixer +# +mixer load /dev/pwm_output $MIXER + +# +# Set PWM output frequency to 400 Hz +# +pwm -u 400 -m 0xff + +# +# Start common for all multirotors apps +# +sh /etc/init.d/rc.multirotor + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 93e76184d4..ced8750015 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -177,6 +177,20 @@ then sh /etc/init.d/11_dji_f450 set MODE custom fi + + if param compare SYS_AUTOSTART 12 + then + set MIXER /etc/mixers/FMU_hex_x.mix + sh /etc/init.d/12-13_hex + set MODE custom + fi + + if param compare SYS_AUTOSTART 13 + then + set MIXER /etc/mixers/FMU_hex_+.mix + sh /etc/init.d/12-13_hex + set MODE custom + fi if param compare SYS_AUTOSTART 15 then From 233a068a7b6eb4851aa47b80a1852851bc851d73 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 15 Oct 2013 17:54:57 +0200 Subject: [PATCH 801/872] quad hil + rotor configuration startup script --- ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil | 110 ++++++++++++++++++ ROMFS/px4fmu_common/init.d/rcS | 40 ++++--- 2 files changed, 135 insertions(+), 15 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil diff --git a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil new file mode 100644 index 0000000000..1c85e04f2a --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil @@ -0,0 +1,110 @@ +#!nsh +# +# USB HIL start +# + +echo "[HIL] HILS quadrotor + starting.." + +# +# 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.0 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_P 0.05 + param set MC_ATT_D 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_P 3.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_P 2.1 + param set MC_YAWRATE_D 0.0 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_P 0.05 + param set NAV_TAKEOFF_ALT 3.0 + param set MPC_TILT_MAX 0.5 + param set MPC_THR_MAX 0.5 + param set MPC_THR_MIN 0.1 + param set MPC_XY_D 0 + param set MPC_XY_P 0.5 + param set MPC_XY_VEL_D 0 + param set MPC_XY_VEL_I 0 + param set MPC_XY_VEL_MAX 3 + param set MPC_XY_VEL_P 0.2 + param set MPC_Z_D 0 + param set MPC_Z_P 1 + param set MPC_Z_VEL_D 0 + param set MPC_Z_VEL_I 0.1 + param set MPC_Z_VEL_MAX 2 + param set MPC_Z_VEL_P 0.20 + + param save +fi + +# Allow USB some time to come up +sleep 1 +# Tell MAVLink that this link is "fast" +mavlink start -b 230400 -d /dev/ttyACM0 + +# Create a fake HIL /dev/pwm_output interface +hil mode_pwm + +# +# 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 commander (depends on orb, mavlink) +# +commander start + +# +# Check if we got an IO +# +if px4io start +then + echo "IO started" +else + fmu mode_serial + echo "FMU started" +fi + +# +# Start the sensors (depends on orb, px4io) +# +sh /etc/init.d/rc.sensors + +# +# Start the attitude estimator (depends on orb) +# +att_pos_estimator_ekf start + +# +# Load mixer and start controllers (depends on px4io) +# +mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix + +# +# Start position estimator +# +position_estimator_inav start + +# +# Start attitude control +# +multirotor_att_control start + +# +# Start position control +# +multirotor_pos_control start + +echo "[HIL] setup done, running" + diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index ced8750015..fd588017f9 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -110,23 +110,33 @@ then then sh /etc/init.d/1000_rc_fw.hil set MODE custom - else - if param compare SYS_AUTOSTART 1001 - then - sh /etc/init.d/1001_rc_quad.hil - set MODE custom - else - if param compare SYS_AUTOSTART 1002 - then - sh /etc/init.d/1002_rc_fw_state.hil - set MODE custom - else - # Try to get an USB console - nshterm /dev/ttyACM0 & - fi - fi fi + if param compare SYS_AUTOSTART 1001 + then + sh /etc/init.d/1001_rc_quad.hil + set MODE custom + fi + + if param compare SYS_AUTOSTART 1002 + then + sh /etc/init.d/1002_rc_fw_state.hil + set MODE custom + fi + + if param compare SYS_AUTOSTART 1003 + then + sh /etc/init.d/1003_rc_quad_+.hil + set MODE custom + fi + + if [ $MODE != custom ] + then + # Try to get an USB console + nshterm /dev/ttyACM0 & + fi + + # # Upgrade PX4IO firmware # From 5cd675d8cce6a3c35eab1970cfb8668b0a6b24e5 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 19 Oct 2013 11:42:06 +0200 Subject: [PATCH 802/872] The mavlink baudrate was too high in the custom_io_esc startup script --- ROMFS/px4fmu_common/init.d/rc.custom_io_esc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_io_esc b/ROMFS/px4fmu_common/init.d/rc.custom_io_esc index e645d9d549..2bfaed76cb 100644 --- a/ROMFS/px4fmu_common/init.d/rc.custom_io_esc +++ b/ROMFS/px4fmu_common/init.d/rc.custom_io_esc @@ -58,7 +58,7 @@ usleep 10000 if px4io detect then # Start MAVLink (depends on orb) - mavlink start -d /dev/ttyS1 -b 115200 + mavlink start -d /dev/ttyS1 -b 57600 usleep 5000 sh /etc/init.d/rc.io @@ -78,7 +78,7 @@ then else fmu mode_pwm # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS1 -b 115200 + mavlink start -d /dev/ttyS1 -b 57600 usleep 5000 param set BAT_V_SCALING 0.004593 set EXIT_ON_END yes From dbb49c035bb85876b234fc057d9f244b43453a44 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 19 Oct 2013 15:19:42 +1100 Subject: [PATCH 803/872] rgbled: fixed detection of device on PX4v1 There is a serial EEPROM on the PX4IOv1 board that answers on I2C address 0x55. We need some extra I2C transfers to ensure we are talking to a real RGBLED device. --- src/drivers/rgbled/rgbled.cpp | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index ea87b37d94..34a45c7393 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -172,7 +172,20 @@ RGBLED::probe() bool on, powersave; uint8_t r, g, b; - ret = get(on, powersave, r, g, b); + /** + this may look strange, but is needed. There is a serial + EEPROM (Microchip-24aa01) on the PX4FMU-v1 that responds to + a bunch of I2C addresses, including the 0x55 used by this + LED device. So we need to do enough operations to be sure + we are talking to the right device. These 3 operations seem + to be enough, as the 3rd one consistently fails if no + RGBLED is on the bus. + */ + if ((ret=get(on, powersave, r, g, b)) != OK || + (ret=send_led_enable(false) != OK) || + (ret=send_led_enable(false) != OK)) { + return ret; + } return ret; } From e3fe4437204b2aecdaa1131fcb4bb0c7751a43df Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 19 Oct 2013 15:21:02 +1100 Subject: [PATCH 804/872] rgbled: fixed getopt() handling this allows the -a option to be used, for example rgbled -a 0x55 start --- src/drivers/rgbled/rgbled.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 34a45c7393..d49211b7b8 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -574,7 +574,7 @@ rgbled_main(int argc, char *argv[]) int ch; /* jump over start/off/etc and look at options first */ - while ((ch = getopt(argc - 1, &argv[1], "a:b:")) != EOF) { + while ((ch = getopt(argc, argv, "a:b:")) != EOF) { switch (ch) { case 'a': rgbledadr = strtol(optarg, NULL, 0); @@ -590,7 +590,12 @@ rgbled_main(int argc, char *argv[]) } } - const char *verb = argv[1]; + if (optind >= argc) { + rgbled_usage(); + exit(1); + } + + const char *verb = argv[optind]; int fd; int ret; From 14e2464fabbae27f9655efdb3e5ee55479c469f7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 19 Oct 2013 15:21:46 +1100 Subject: [PATCH 805/872] rgbled: don't try the same bus twice on PX4v1 the external I2C bus is the same as the LED bus --- src/drivers/rgbled/rgbled.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index d49211b7b8..727c86e02a 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -616,6 +616,9 @@ rgbled_main(int argc, char *argv[]) if (g_rgbled == nullptr) { // fall back to default bus + if (PX4_I2C_BUS_LED == PX4_I2C_BUS_EXPANSION) { + errx(1, "init failed"); + } i2cdevice = PX4_I2C_BUS_LED; } } From 6a624ff753ad9ba6e7fb74783b6b8294a1c17957 Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Sat, 19 Oct 2013 23:04:36 +0200 Subject: [PATCH 806/872] Fix gyro calibration for rotated sensors. The calibration routine now uses the raw sensor values instead of the already rotated values. --- src/modules/commander/gyro_calibration.cpp | 194 +++++++++------------ 1 file changed, 86 insertions(+), 108 deletions(-) diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 369e6da624..b6de5141f6 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -60,17 +60,7 @@ int do_gyro_calibration(int mavlink_fd) { mavlink_log_info(mavlink_fd, "Gyro calibration starting, do not move unit."); - const unsigned calibration_count = 5000; - - int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); - struct sensor_combined_s raw; - - unsigned 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 = { + struct gyro_scale gyro_scale = { 0.0f, 1.0f, 0.0f, @@ -79,27 +69,40 @@ int do_gyro_calibration(int mavlink_fd) 1.0f, }; - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null)) - warn("WARNING: failed to set scale / offsets for gyro"); + /* subscribe to gyro sensor topic */ + int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro)); + struct gyro_report gyro_report; + + /* reset all offsets to zero and all scales to one */ + int fd = open(GYRO_DEVICE_PATH, 0); + + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale)) + warn("WARNING: failed to reset scale / offsets for gyro"); close(fd); + + /*** --- OFFSETS --- ***/ + + /* determine gyro mean values */ + const unsigned calibration_count = 5000; + unsigned calibration_counter = 0; unsigned poll_errcount = 0; while (calibration_counter < calibration_count) { /* wait blocking for new data */ struct pollfd fds[1]; - fds[0].fd = sub_sensor_combined; + fds[0].fd = sub_sensor_gyro; fds[0].events = POLLIN; int poll_ret = poll(fds, 1, 1000); 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]; + orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report); + gyro_scale.x_offset += gyro_report.x; + gyro_scale.y_offset += gyro_report.y; + gyro_scale.z_offset += gyro_report.z; calibration_counter++; if (calibration_counter % (calibration_count / 20) == 0) mavlink_log_info(mavlink_fd, "gyro cal progress <%u> percent", (calibration_counter * 100) / calibration_count); @@ -110,67 +113,49 @@ int do_gyro_calibration(int mavlink_fd) if (poll_errcount > 1000) { mavlink_log_info(mavlink_fd, "ERROR: Failed reading gyro sensor"); - close(sub_sensor_combined); + close(sub_sensor_gyro); return ERROR; } } - gyro_offset[0] = gyro_offset[0] / calibration_count; - gyro_offset[1] = gyro_offset[1] / calibration_count; - gyro_offset[2] = gyro_offset[2] / calibration_count; + gyro_scale.x_offset /= calibration_count; + gyro_scale.y_offset /= calibration_count; + gyro_scale.z_offset /= 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"); - close(sub_sensor_combined); - return ERROR; - } - - tune_neutral(); - /* third beep by cal end routine */ - - } else { - mavlink_log_info(mavlink_fd, "offset cal FAILED (NaN)"); - close(sub_sensor_combined); + if (!isfinite(gyro_scale.x_offset) || !isfinite(gyro_scale.y_offset) || !isfinite(gyro_scale.z_offset)) { + mavlink_log_info(mavlink_fd, "gyro offset calibration FAILED (NaN)"); + close(sub_sensor_gyro); return ERROR; } + /* beep on calibration end */ mavlink_log_info(mavlink_fd, "offset calibration done."); + tune_neutral(); + + /* set offset parameters to new values */ + if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_scale.x_offset)) + || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_scale.y_offset)) + || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_scale.z_offset))) { + mavlink_log_critical(mavlink_fd, "Setting gyro offset parameters failed!"); + } + -#if 0 /*** --- SCALING --- ***/ +#if 0 + /* this was only a proof of concept and is currently not working. scaling will be set to 1.0 for now. */ mavlink_log_info(mavlink_fd, "offset done. Rotate for scale 30x or wait 5s to skip."); warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip."); + /* apply new offsets */ + fd = open(GYRO_DEVICE_PATH, 0); + + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale)) + warn("WARNING: failed to apply new offsets for gyro"); + + close(fd); + + unsigned rotations_count = 30; float gyro_integral = 0.0f; float baseline_integral = 0.0f; @@ -246,52 +231,45 @@ int do_gyro_calibration(int mavlink_fd) warnx("gyro scale: yaw (z): %6.4f", (double)gyro_scale); mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", (double)gyro_scale); -#else - float gyro_scales[] = { 1.0f, 1.0f, 1.0f }; -#endif - - - if (isfinite(gyro_scales[0]) && isfinite(gyro_scales[1]) && isfinite(gyro_scales[2])) { - - 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], - gyro_scales[0], - gyro_offset[1], - gyro_scales[1], - gyro_offset[2], - gyro_scales[2], - }; - - 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"); - } - - mavlink_log_info(mavlink_fd, "gyro calibration done"); - - /* third beep by cal end routine */ - close(sub_sensor_combined); - return OK; - } else { - mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)"); - close(sub_sensor_combined); + if (!isfinite(gyro_scale.x_scale) || !isfinite(gyro_scale.y_scale) || !isfinite(gyro_scale.z_scale)) { + mavlink_log_info(mavlink_fd, "gyro scale calibration FAILED (NaN)"); + close(sub_sensor_gyro); return ERROR; } + + /* beep on calibration end */ + mavlink_log_info(mavlink_fd, "scale calibration done."); + tune_neutral(); + +#endif + + /* set scale parameters to new values */ + if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scale.x_scale)) + || param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scale.y_scale)) + || param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scale.z_scale))) { + mavlink_log_critical(mavlink_fd, "Setting gyro scale parameters failed!"); + } + + /* apply new scaling and offsets */ + fd = open(GYRO_DEVICE_PATH, 0); + + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale)) + warn("WARNING: failed to apply new scale 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"); + close(sub_sensor_gyro); + return ERROR; + } + + close(sub_sensor_gyro); + return OK; } From 8cffd2b8a33ccb4d556a181d5a6e55111c1b1f53 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 20 Oct 2013 12:16:00 +0200 Subject: [PATCH 807/872] fix scaling (unit) of airspeed in HIL src/modules/mavlink/mavlink_receiver.cpp --- src/modules/mavlink/mavlink_receiver.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 8243748dc8..c51a6de088 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -422,13 +422,13 @@ handle_message(mavlink_message_t *msg) hil_sensors.accelerometer_counter = hil_counter; /* differential pressure */ - hil_sensors.differential_pressure_pa = imu.diff_pressure; + hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa hil_sensors.differential_pressure_counter = hil_counter; /* airspeed from differential pressure, ambient pressure and temp */ struct airspeed_s airspeed; - float ias = calc_indicated_airspeed(imu.diff_pressure); + float ias = calc_indicated_airspeed(hil_sensors.differential_pressure_pa); // XXX need to fix this float tas = ias; From ef6f1f6f808e49ff3aca68aa08001e37093b89ec Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 20 Oct 2013 19:36:42 +0200 Subject: [PATCH 808/872] get_rot_matrix() moved to separate library, accel calibration of rotated board fixed --- makefiles/config_px4fmu-v1_default.mk | 1 + makefiles/config_px4fmu-v2_default.mk | 1 + src/lib/conversion/module.mk | 38 ++++++++ src/lib/conversion/rotation.cpp | 30 ++++++ src/lib/conversion/rotation.h | 89 ++++++++++++++++++ .../commander/accelerometer_calibration.cpp | 45 ++++++--- src/modules/commander/module.mk | 1 - src/modules/sensors/sensors.cpp | 93 +------------------ 8 files changed, 192 insertions(+), 106 deletions(-) create mode 100644 src/lib/conversion/module.mk create mode 100644 src/lib/conversion/rotation.cpp create mode 100644 src/lib/conversion/rotation.h diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 46640f3c51..862abde92b 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -115,6 +115,7 @@ MODULES += lib/mathlib/math/filter MODULES += lib/ecl MODULES += lib/external_lgpl MODULES += lib/geo +MODULES += lib/conversion # # Demo apps diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index bb589cb9f3..71986c3a34 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -111,6 +111,7 @@ MODULES += lib/mathlib/math/filter MODULES += lib/ecl MODULES += lib/external_lgpl MODULES += lib/geo +MODULES += lib/conversion # # Demo apps diff --git a/src/lib/conversion/module.mk b/src/lib/conversion/module.mk new file mode 100644 index 0000000000..102711aaf7 --- /dev/null +++ b/src/lib/conversion/module.mk @@ -0,0 +1,38 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Rotation library +# + +SRCS = rotation.cpp diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp new file mode 100644 index 0000000000..b65226cf1c --- /dev/null +++ b/src/lib/conversion/rotation.cpp @@ -0,0 +1,30 @@ +/* + * rotation.cpp + * + * Created on: 20.10.2013 + * Author: ton + */ + +#include "math.h" +#include "rotation.h" + +__EXPORT void +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); + } + } +} diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h new file mode 100644 index 0000000000..97c6a09076 --- /dev/null +++ b/src/lib/conversion/rotation.h @@ -0,0 +1,89 @@ +/* + * rotation.h + * + * Created on: 20.10.2013 + * Author: ton + */ + +#ifndef ROTATION_H_ +#define ROTATION_H_ + +#include +#include + +/** + * Enum for board and external compass rotations. + * This enum maps from board attitude to airframe attitude. + */ +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 } +}; + +/** + * Get the rotation matrix + */ +__EXPORT void +get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix); + +#endif /* ROTATION_H_ */ diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index cfa7d9e8a1..c9bfedbda0 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -112,11 +112,13 @@ #include #include #include +#include #include #include #include #include #include +#include #include #include #include @@ -144,24 +146,41 @@ int do_accel_calibration(int mavlink_fd) { int res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_scale); if (res == OK) { - /* measurements complete successfully, set parameters */ - if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs[0])) - || param_set(param_find("SENS_ACC_YOFF"), &(accel_offs[1])) - || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs[2])) - || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale[0])) - || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale[1])) - || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale[2]))) { + /* measurements complete successfully, rotate calibration values */ + param_t board_rotation_h = param_find("SENS_BOARD_ROT"); + enum Rotation board_rotation_id; + param_get(board_rotation_h, &(board_rotation_id)); + math::Matrix board_rotation(3, 3); + get_rot_matrix(board_rotation_id, &board_rotation); + board_rotation = board_rotation.transpose(); + math::Vector3 vect(3); + vect(0) = accel_offs[0]; + vect(1) = accel_offs[1]; + vect(2) = accel_offs[2]; + math::Vector3 accel_offs_rotated = board_rotation * vect; + vect(0) = accel_scale[0]; + vect(1) = accel_scale[1]; + vect(2) = accel_scale[2]; + math::Vector3 accel_scale_rotated = board_rotation * vect; + + /* set parameters */ + if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs_rotated(0))) + || param_set(param_find("SENS_ACC_YOFF"), &(accel_offs_rotated(1))) + || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs_rotated(2))) + || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale_rotated(0))) + || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale_rotated(1))) + || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale_rotated(2)))) { mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed"); } int fd = open(ACCEL_DEVICE_PATH, 0); struct accel_scale ascale = { - accel_offs[0], - accel_scale[0], - accel_offs[1], - accel_scale[1], - accel_offs[2], - accel_scale[2], + accel_offs_rotated(0), + accel_scale_rotated(0), + accel_offs_rotated(1), + accel_scale_rotated(1), + accel_offs_rotated(2), + accel_scale_rotated(2), }; if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index 91d75121eb..554dfcb080 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -47,4 +47,3 @@ SRCS = commander.cpp \ baro_calibration.cpp \ rc_calibration.cpp \ airspeed_calibration.cpp - diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 3fcacaf8f9..1b79de8fd2 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -67,6 +67,7 @@ #include #include #include +#include #include @@ -135,75 +136,6 @@ #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. - * This enum maps from board attitude to airframe attitude. - */ -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 * @@ -384,11 +316,6 @@ private: */ int parameters_update(); - /** - * Get the rotation matrices - */ - void get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix); - /** * Do accel-related initialisation. */ @@ -803,24 +730,6 @@ Sensors::parameters_update() 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() { From b75c8e672fb401d9b1673d53a1972b9dddfa6b15 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 20 Oct 2013 23:16:23 +0200 Subject: [PATCH 809/872] accelerometer calibration fix --- .../commander/accelerometer_calibration.cpp | 45 +++++++------------ 1 file changed, 17 insertions(+), 28 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index c9bfedbda0..b6aa64aa4d 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -129,7 +129,7 @@ #endif static const int ERROR = -1; -int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]); +int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][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); int mat_invert3(float src[3][3], float dst[3][3]); @@ -142,8 +142,8 @@ int do_accel_calibration(int mavlink_fd) { /* measure and calculate offsets & scales */ float accel_offs[3]; - float accel_scale[3]; - int res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_scale); + float accel_T[3][3]; + int res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T); if (res == OK) { /* measurements complete successfully, rotate calibration values */ @@ -153,34 +153,31 @@ int do_accel_calibration(int mavlink_fd) { math::Matrix board_rotation(3, 3); get_rot_matrix(board_rotation_id, &board_rotation); board_rotation = board_rotation.transpose(); - math::Vector3 vect(3); - vect(0) = accel_offs[0]; - vect(1) = accel_offs[1]; - vect(2) = accel_offs[2]; - math::Vector3 accel_offs_rotated = board_rotation * vect; - vect(0) = accel_scale[0]; - vect(1) = accel_scale[1]; - vect(2) = accel_scale[2]; - math::Vector3 accel_scale_rotated = board_rotation * vect; + math::Vector3 accel_offs_vec; + accel_offs_vec.set(&accel_offs[0]); + math::Vector3 accel_offs_rotated = board_rotation * accel_offs_vec; + math::Matrix accel_T_mat(3, 3); + accel_T_mat.set(&accel_T[0][0]); + math::Matrix accel_T_rotated = board_rotation.transpose() * accel_T_mat * board_rotation; /* set parameters */ if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs_rotated(0))) || param_set(param_find("SENS_ACC_YOFF"), &(accel_offs_rotated(1))) || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs_rotated(2))) - || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale_rotated(0))) - || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale_rotated(1))) - || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale_rotated(2)))) { + || param_set(param_find("SENS_ACC_XSCALE"), &(accel_T_rotated(0, 0))) + || param_set(param_find("SENS_ACC_YSCALE"), &(accel_T_rotated(1, 1))) + || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_T_rotated(2, 2)))) { mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed"); } int fd = open(ACCEL_DEVICE_PATH, 0); struct accel_scale ascale = { accel_offs_rotated(0), - accel_scale_rotated(0), + accel_T_rotated(0, 0), accel_offs_rotated(1), - accel_scale_rotated(1), + accel_T_rotated(1, 1), accel_offs_rotated(2), - accel_scale_rotated(2), + accel_T_rotated(2, 2), }; if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) @@ -206,7 +203,7 @@ int do_accel_calibration(int mavlink_fd) { /* exit accel calibration mode */ } -int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]) { +int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][3]) { const int samples_num = 2500; float accel_ref[6][3]; bool data_collected[6] = { false, false, false, false, false, false }; @@ -282,21 +279,13 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float } close(sensor_combined_sub); - /* calculate offsets and rotation+scale matrix */ - float accel_T[3][3]; + /* calculate offsets and transform matrix */ 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 calculation error"); return ERROR; } - /* convert accel transform matrix to scales, - * rotation part of transform matrix is not used by now - */ - for (int i = 0; i < 3; i++) { - accel_scale[i] = accel_T[i][i]; - } - return OK; } From 0dc9c9ac262c10866cbaf23ca98c8ce4582b5993 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 20 Oct 2013 23:28:09 +0200 Subject: [PATCH 810/872] accelerometer_calibration: code style fixed, lib/conversion copyright fix --- src/lib/conversion/module.mk | 2 +- src/lib/conversion/rotation.cpp | 40 +++++- src/lib/conversion/rotation.h | 40 +++++- .../commander/accelerometer_calibration.cpp | 122 +++++++++++------- 4 files changed, 149 insertions(+), 55 deletions(-) diff --git a/src/lib/conversion/module.mk b/src/lib/conversion/module.mk index 102711aaf7..f5f59a2dca 100644 --- a/src/lib/conversion/module.mk +++ b/src/lib/conversion/module.mk @@ -32,7 +32,7 @@ ############################################################################ # -# Rotation library +# Conversion library # SRCS = rotation.cpp diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index b65226cf1c..b078562c2d 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -1,8 +1,40 @@ -/* - * rotation.cpp +/**************************************************************************** * - * Created on: 20.10.2013 - * Author: ton + * 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 rotation.cpp + * + * Vector rotation library */ #include "math.h" diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index 97c6a09076..85c63c0fcb 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -1,8 +1,40 @@ -/* - * rotation.h +/**************************************************************************** * - * Created on: 20.10.2013 - * Author: ton + * 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 rotation.h + * + * Vector rotation library */ #ifndef ROTATION_H_ diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index b6aa64aa4d..4880af9071 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -135,7 +135,8 @@ 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); -int do_accel_calibration(int mavlink_fd) { +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"); @@ -162,11 +163,11 @@ int do_accel_calibration(int mavlink_fd) { /* set parameters */ if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs_rotated(0))) - || param_set(param_find("SENS_ACC_YOFF"), &(accel_offs_rotated(1))) - || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs_rotated(2))) - || param_set(param_find("SENS_ACC_XSCALE"), &(accel_T_rotated(0, 0))) - || param_set(param_find("SENS_ACC_YSCALE"), &(accel_T_rotated(1, 1))) - || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_T_rotated(2, 2)))) { + || param_set(param_find("SENS_ACC_YOFF"), &(accel_offs_rotated(1))) + || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs_rotated(2))) + || param_set(param_find("SENS_ACC_XSCALE"), &(accel_T_rotated(0, 0))) + || param_set(param_find("SENS_ACC_YSCALE"), &(accel_T_rotated(1, 1))) + || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_T_rotated(2, 2)))) { mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed"); } @@ -194,6 +195,7 @@ int do_accel_calibration(int mavlink_fd) { mavlink_log_info(mavlink_fd, "accel calibration done"); return OK; + } else { /* measurements error */ mavlink_log_info(mavlink_fd, "accel calibration aborted"); @@ -203,7 +205,8 @@ int do_accel_calibration(int mavlink_fd) { /* exit accel calibration mode */ } -int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][3]) { +int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][3]) +{ const int samples_num = 2500; float accel_ref[6][3]; bool data_collected[6] = { false, false, false, false, false, false }; @@ -243,12 +246,12 @@ 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[1]) ? "x- " : "", - (!data_collected[2]) ? "y+ " : "", - (!data_collected[3]) ? "y- " : "", - (!data_collected[4]) ? "z+ " : "", - (!data_collected[5]) ? "z- " : ""); + (!data_collected[0]) ? "x+ " : "", + (!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); @@ -257,6 +260,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float break; int orient = detect_orientation(mavlink_fd, sensor_combined_sub); + if (orient < 0) { close(sensor_combined_sub); return ERROR; @@ -270,17 +274,19 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float 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); 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]); + (double)accel_ref[orient][0], + (double)accel_ref[orient][1], + (double)accel_ref[orient][2]); data_collected[orient] = true; tune_neutral(); } + close(sensor_combined_sub); /* calculate offsets and transform matrix */ 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 calculation error"); return ERROR; @@ -295,7 +301,8 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float * @return 0..5 according to orientation when vehicle is still and ready for measurements, * ERROR if vehicle is not still after 30s or orientation error is more than 5m/s^2 */ -int detect_orientation(int mavlink_fd, int sub_sensor_combined) { +int detect_orientation(int mavlink_fd, int sub_sensor_combined) +{ struct sensor_combined_s sensor; /* exponential moving average of accel */ float accel_ema[3] = { 0.0f, 0.0f, 0.0f }; @@ -326,30 +333,35 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { while (true) { /* wait blocking for new data */ int poll_ret = poll(fds, 1, 1000); + if (poll_ret) { orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &sensor); t = hrt_absolute_time(); float dt = (t - t_prev) / 1000000.0f; t_prev = t; float w = dt / ema_len; + for (int i = 0; i < 3; i++) { accel_ema[i] = accel_ema[i] * (1.0f - w) + sensor.accelerometer_m_s2[i] * w; float d = (float) sensor.accelerometer_m_s2[i] - accel_ema[i]; d = d * d; accel_disp[i] = accel_disp[i] * (1.0f - w); + if (d > accel_disp[i]) accel_disp[i] = d; } + /* still detector with hysteresis */ - if ( accel_disp[0] < still_thr2 && - accel_disp[1] < still_thr2 && - accel_disp[2] < still_thr2 ) { + if (accel_disp[0] < still_thr2 && + accel_disp[1] < still_thr2 && + accel_disp[2] < still_thr2) { /* is still now */ if (t_still == 0) { /* first time */ mavlink_log_info(mavlink_fd, "detected rest position, waiting..."); t_still = t; t_timeout = t + timeout; + } else { /* still since t_still */ if (t > t_still + still_time) { @@ -357,18 +369,21 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { break; } } - } else if ( accel_disp[0] > still_thr2 * 2.0f || - accel_disp[1] > still_thr2 * 2.0f || - accel_disp[2] > still_thr2 * 2.0f) { + + } else if (accel_disp[0] > still_thr2 * 2.0f || + accel_disp[1] > still_thr2 * 2.0f || + accel_disp[2] > still_thr2 * 2.0f) { /* not still, reset still start time */ if (t_still != 0) { mavlink_log_info(mavlink_fd, "detected motion, please hold still..."); t_still = 0; } } + } else if (poll_ret == 0) { poll_errcount++; } + if (t > t_timeout) { poll_errcount++; } @@ -379,29 +394,34 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { } } - if ( fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr && - fabsf(accel_ema[1]) < accel_err_thr && - fabsf(accel_ema[2]) < accel_err_thr ) + 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 ( fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr && - fabsf(accel_ema[1]) < accel_err_thr && - fabsf(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 ( fabsf(accel_ema[0]) < accel_err_thr && - fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr && - fabsf(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 ( fabsf(accel_ema[0]) < accel_err_thr && - fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr && - fabsf(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 ( fabsf(accel_ema[0]) < accel_err_thr && - fabsf(accel_ema[1]) < accel_err_thr && - fabsf(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 ( fabsf(accel_ema[0]) < accel_err_thr && - fabsf(accel_ema[1]) < accel_err_thr && - fabsf(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"); @@ -412,7 +432,8 @@ 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) { +int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num) +{ struct pollfd fds[1]; fds[0].fd = sensor_combined_sub; fds[0].events = POLLIN; @@ -423,12 +444,16 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp while (count < samples_num) { 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); + for (int i = 0; i < 3; i++) accel_sum[i] += sensor.accelerometer_m_s2[i]; + count++; + } else { errcount++; continue; @@ -445,10 +470,12 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp return OK; } -int mat_invert3(float src[3][3], float dst[3][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]); + src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) + + src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]); + if (det == 0.0f) return ERROR; // Singular matrix @@ -465,7 +492,8 @@ int mat_invert3(float src[3][3], float dst[3][3]) { return OK; } -int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g) { +int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g) +{ /* calculate offsets */ for (int i = 0; i < 3; i++) { accel_offs[i] = (accel_ref[i * 2][i] + accel_ref[i * 2 + 1][i]) / 2; @@ -474,6 +502,7 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo /* 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_ref[i * 2][j] - accel_offs[j]; @@ -483,6 +512,7 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo /* calculate inverse matrix for A */ float mat_A_inv[3][3]; + if (mat_invert3(mat_A, mat_A_inv) != OK) return ERROR; From ed79b686c57f41d9d6bd3726fe0071e11740b3d7 Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Sun, 20 Oct 2013 00:08:36 +0200 Subject: [PATCH 811/872] Adjusted mavlink info messages during gyro calibration to not break QGroundControl. --- src/modules/commander/gyro_calibration.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index b6de5141f6..e1d6e8340d 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -129,7 +129,7 @@ int do_gyro_calibration(int mavlink_fd) } /* beep on calibration end */ - mavlink_log_info(mavlink_fd, "offset calibration done."); + mavlink_log_info(mavlink_fd, "gyro offset calibration done."); tune_neutral(); /* set offset parameters to new values */ @@ -240,7 +240,7 @@ int do_gyro_calibration(int mavlink_fd) } /* beep on calibration end */ - mavlink_log_info(mavlink_fd, "scale calibration done."); + mavlink_log_info(mavlink_fd, "gyro scale calibration done."); tune_neutral(); #endif @@ -270,6 +270,8 @@ int do_gyro_calibration(int mavlink_fd) return ERROR; } + mavlink_log_info(mavlink_fd, "gyro calibration done."); + close(sub_sensor_gyro); return OK; } From ea89f23c917733f8a682c82e24e1e4f223f79843 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 21 Oct 2013 20:07:47 +0200 Subject: [PATCH 812/872] calibration: bugs fixed, mavlink messages cleanup --- src/drivers/drv_accel.h | 2 +- .../commander/accelerometer_calibration.cpp | 201 +++++++++------- src/modules/commander/commander.cpp | 54 +++-- src/modules/commander/gyro_calibration.cpp | 219 ++++++++++-------- 4 files changed, 266 insertions(+), 210 deletions(-) diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h index eff5e73495..8a4f684284 100644 --- a/src/drivers/drv_accel.h +++ b/src/drivers/drv_accel.h @@ -66,7 +66,7 @@ struct accel_report { int16_t temperature_raw; }; -/** accel scaling factors; Vout = (Vin * Vscale) + Voffset */ +/** accel scaling factors; Vout = Vscale * (Vin + Voffset) */ struct accel_scale { float x_offset; float x_scale; diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 4880af9071..d11d7eb5d3 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -100,6 +100,24 @@ * accel_T = A^-1 * g * g = 9.80665 * + * ===== Rotation ===== + * + * Calibrating using model: + * accel_corr = accel_T_r * (rot * accel_raw - accel_offs_r) + * + * Actual correction: + * accel_corr = rot * accel_T * (accel_raw - accel_offs) + * + * Known: accel_T_r, accel_offs_r, rot + * Unknown: accel_T, accel_offs + * + * Solution: + * accel_T_r * (rot * accel_raw - accel_offs_r) = rot * accel_T * (accel_raw - accel_offs) + * rot^-1 * accel_T_r * (rot * accel_raw - accel_offs_r) = accel_T * (accel_raw - accel_offs) + * rot^-1 * accel_T_r * rot * accel_raw - rot^-1 * accel_T_r * accel_offs_r = accel_T * accel_raw - accel_T * accel_offs) + * => accel_T = rot^-1 * accel_T_r * rot + * => accel_offs = rot^-1 * accel_offs_r + * * @author Anton Babushkin */ @@ -137,72 +155,97 @@ 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"); + mavlink_log_info(mavlink_fd, "accel calibration: started"); + mavlink_log_info(mavlink_fd, "accel calibration: progress <0>"); + + struct accel_scale accel_scale = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + + int res = OK; + + /* reset all offsets to zero and all scales to one */ + int fd = open(ACCEL_DEVICE_PATH, 0); + res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); + close(fd); + + if (res != OK) { + mavlink_log_critical(mavlink_fd, "ERROR: failed to reset scale / offsets"); + } /* measure and calculate offsets & scales */ float accel_offs[3]; float accel_T[3][3]; - int res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T); + res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T); if (res == OK) { - /* measurements complete successfully, rotate calibration values */ + /* measurements completed successfully, rotate calibration values */ param_t board_rotation_h = param_find("SENS_BOARD_ROT"); - enum Rotation board_rotation_id; - param_get(board_rotation_h, &(board_rotation_id)); + int32_t board_rotation_int; + param_get(board_rotation_h, &(board_rotation_int)); + enum Rotation board_rotation_id = (enum Rotation)board_rotation_int; math::Matrix board_rotation(3, 3); get_rot_matrix(board_rotation_id, &board_rotation); - board_rotation = board_rotation.transpose(); + math::Matrix board_rotation_t = board_rotation.transpose(); math::Vector3 accel_offs_vec; accel_offs_vec.set(&accel_offs[0]); - math::Vector3 accel_offs_rotated = board_rotation * accel_offs_vec; + math::Vector3 accel_offs_rotated = board_rotation_t * accel_offs_vec; math::Matrix accel_T_mat(3, 3); accel_T_mat.set(&accel_T[0][0]); - math::Matrix accel_T_rotated = board_rotation.transpose() * accel_T_mat * board_rotation; + math::Matrix accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation; + + accel_scale.x_offset = accel_offs_rotated(0); + accel_scale.x_scale = accel_T_rotated(0, 0); + accel_scale.y_offset = accel_offs_rotated(1); + accel_scale.y_scale = accel_T_rotated(1, 1); + accel_scale.z_offset = accel_offs_rotated(2); + accel_scale.z_scale = accel_T_rotated(2, 2); /* set parameters */ - if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs_rotated(0))) - || param_set(param_find("SENS_ACC_YOFF"), &(accel_offs_rotated(1))) - || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs_rotated(2))) - || param_set(param_find("SENS_ACC_XSCALE"), &(accel_T_rotated(0, 0))) - || param_set(param_find("SENS_ACC_YSCALE"), &(accel_T_rotated(1, 1))) - || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_T_rotated(2, 2)))) { - mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed"); + if (param_set(param_find("SENS_ACC_XOFF"), &(accel_scale.x_offset)) + || param_set(param_find("SENS_ACC_YOFF"), &(accel_scale.y_offset)) + || param_set(param_find("SENS_ACC_ZOFF"), &(accel_scale.z_offset)) + || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale.x_scale)) + || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale.y_scale)) + || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale.z_scale))) { + mavlink_log_critical(mavlink_fd, "ERROR: setting accel params failed"); + res = ERROR; } - - int fd = open(ACCEL_DEVICE_PATH, 0); - struct accel_scale ascale = { - accel_offs_rotated(0), - accel_T_rotated(0, 0), - accel_offs_rotated(1), - accel_T_rotated(1, 1), - accel_offs_rotated(2), - accel_T_rotated(2, 2), - }; - - if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) - warn("WARNING: failed to set scale / offsets for accel"); - - 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"); - } - - mavlink_log_info(mavlink_fd, "accel calibration done"); - return OK; - - } else { - /* measurements error */ - mavlink_log_info(mavlink_fd, "accel calibration aborted"); - return ERROR; } - /* exit accel calibration mode */ + if (res == OK) { + /* apply new scaling and offsets */ + int fd = open(ACCEL_DEVICE_PATH, 0); + res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); + close(fd); + + if (res != OK) { + mavlink_log_critical(mavlink_fd, "ERROR: failed to apply new params for accel"); + } + } + + if (res == OK) { + /* auto-save to EEPROM */ + res = param_save_default(); + + if (res != OK) { + mavlink_log_critical(mavlink_fd, "ERROR: failed to save parameters"); + } + } + + if (res == OK) { + mavlink_log_info(mavlink_fd, "accel calibration: done"); + + } else { + mavlink_log_info(mavlink_fd, "accel calibration: failed"); + } + + return res; } int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][3]) @@ -212,27 +255,10 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float bool data_collected[6] = { false, false, false, false, false, false }; const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" }; - /* reset existing calibration */ - int fd = open(ACCEL_DEVICE_PATH, 0); - struct accel_scale ascale_null = { - 0.0f, - 1.0f, - 0.0f, - 1.0f, - 0.0f, - 1.0f, - }; - int ioctl_res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null); - close(fd); - - if (OK != ioctl_res) { - warn("ERROR: failed to set scale / offsets for accel"); - return ERROR; - } - int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); unsigned done_count = 0; + int res = OK; while (true) { bool done = true; @@ -245,6 +271,12 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float } } + if (old_done_count != done_count) + mavlink_log_info(mavlink_fd, "accel calibration: progress <%u>", 17 * done_count); + + if (done) + break; + mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s", (!data_collected[0]) ? "x+ " : "", (!data_collected[1]) ? "x- " : "", @@ -253,17 +285,11 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float (!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); - - if (done) - break; - int orient = detect_orientation(mavlink_fd, sensor_combined_sub); if (orient < 0) { - close(sensor_combined_sub); - return ERROR; + res = ERROR; + break; } if (data_collected[orient]) { @@ -284,15 +310,16 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float close(sensor_combined_sub); - /* calculate offsets and transform matrix */ - int res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); + if (res == OK) { + /* calculate offsets and transform matrix */ + res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); - if (res != 0) { - mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error"); - return ERROR; + if (res != OK) { + mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error"); + } } - return OK; + return res; } /* @@ -309,7 +336,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) /* max-hold dispersion of accel */ float accel_disp[3] = { 0.0f, 0.0f, 0.0f }; /* EMA time constant in seconds*/ - float ema_len = 0.2f; + float ema_len = 0.5f; /* set "still" threshold to 0.25 m/s^2 */ float still_thr2 = pow(0.25f, 2); /* set accel error threshold to 5m/s^2 */ @@ -342,8 +369,8 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) float w = dt / ema_len; for (int i = 0; i < 3; i++) { - accel_ema[i] = accel_ema[i] * (1.0f - w) + sensor.accelerometer_m_s2[i] * w; - float d = (float) sensor.accelerometer_m_s2[i] - accel_ema[i]; + float d = sensor.accelerometer_m_s2[i] - accel_ema[i]; + accel_ema[i] += d * w; d = d * d; accel_disp[i] = accel_disp[i] * (1.0f - w); @@ -389,8 +416,8 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) } if (poll_errcount > 1000) { - mavlink_log_info(mavlink_fd, "ERROR: Failed reading sensor"); - return -1; + mavlink_log_critical(mavlink_fd, "ERROR: failed reading sensor"); + return ERROR; } } @@ -424,9 +451,9 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr) return 5; // [ 0, 0, -g ] - mavlink_log_info(mavlink_fd, "ERROR: invalid orientation"); + mavlink_log_critical(mavlink_fd, "ERROR: invalid orientation"); - return -2; // Can't detect orientation + return ERROR; // Can't detect orientation } /* diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2ef509980c..9545ef171d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -369,8 +369,10 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe if (hil_ret == OK && control_mode->flag_system_hil_enabled) { /* reset the arming mode to disarmed */ arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_STANDBY, armed); + if (arming_res != TRANSITION_DENIED) { mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby"); + } else { mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state"); } @@ -481,27 +483,28 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe break; } - case VEHICLE_CMD_COMPONENT_ARM_DISARM: - { - transition_result_t arming_res = TRANSITION_NOT_CHANGED; - if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) { - if (safety->safety_switch_available && !safety->safety_off) { - print_reject_arm("NOT ARMING: Press safety switch first."); - arming_res = TRANSITION_DENIED; + case VEHICLE_CMD_COMPONENT_ARM_DISARM: { + transition_result_t arming_res = TRANSITION_NOT_CHANGED; - } else { - arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed); - } + if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) { + if (safety->safety_switch_available && !safety->safety_off) { + print_reject_arm("NOT ARMING: Press safety switch first."); + arming_res = TRANSITION_DENIED; - if (arming_res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] ARMED by component arm cmd"); - result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - mavlink_log_info(mavlink_fd, "[cmd] REJECTING component arm cmd"); - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } else { + arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed); + } + + if (arming_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] ARMED by component arm cmd"); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + mavlink_log_info(mavlink_fd, "[cmd] REJECTING component arm cmd"); + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } } } - } break; default: @@ -940,7 +943,7 @@ int commander_thread_main(int argc, char *argv[]) last_idle_time = system_load.tasks[0].total_runtime; /* check if board is connected via USB */ - struct stat statbuf; + //struct stat statbuf; //on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0); } @@ -970,6 +973,7 @@ int commander_thread_main(int argc, char *argv[]) if (armed.armed) { arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED_ERROR, &armed); + } else { arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed); } @@ -1244,12 +1248,14 @@ int commander_thread_main(int argc, char *argv[]) counter++; int blink_state = blink_msg_state(); + if (blink_state > 0) { /* blinking LED message, don't touch LEDs */ if (blink_state == 2) { /* blinking LED message completed, restore normal state */ control_status_leds(&status, &armed, true); } + } else { /* normal state */ control_status_leds(&status, &armed, status_changed); @@ -1264,7 +1270,7 @@ int commander_thread_main(int argc, char *argv[]) ret = pthread_join(commander_low_prio_thread, NULL); if (ret) { - warn("join failed", ret); + warn("join failed: %d", ret); } rgbled_set_mode(RGBLED_MODE_OFF); @@ -1308,6 +1314,7 @@ control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chan /* driving rgbled */ if (changed) { bool set_normal_color = false; + /* set mode */ if (status->arming_state == ARMING_STATE_ARMED) { rgbled_set_mode(RGBLED_MODE_ON); @@ -1332,6 +1339,7 @@ control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chan if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) { rgbled_set_color(RGBLED_COLOR_AMBER); } + /* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */ } else { @@ -1694,11 +1702,10 @@ void *commander_low_prio_loop(void *arg) fds[0].events = POLLIN; while (!thread_should_exit) { - - /* wait for up to 100ms for data */ + /* wait for up to 200ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 200); - /* timed out - periodic check for _task_should_exit, etc. */ + /* timed out - periodic check for thread_should_exit, etc. */ if (pret == 0) continue; @@ -1773,7 +1780,7 @@ void *commander_low_prio_loop(void *arg) } else if ((int)(cmd.param4) == 1) { /* RC calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); calib_ret = do_rc_calibration(mavlink_fd); } else if ((int)(cmd.param5) == 1) { @@ -1854,7 +1861,6 @@ void *commander_low_prio_loop(void *arg) /* send acknowledge command */ // XXX TODO } - } close(cmd_sub); diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index e1d6e8340d..219ae6cb2f 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, do not move unit."); + mavlink_log_info(mavlink_fd, "gyro calibration: started"); struct gyro_scale gyro_scale = { 0.0f, @@ -69,79 +69,85 @@ int do_gyro_calibration(int mavlink_fd) 1.0f, }; + int res = OK; + + /* reset all offsets to zero and all scales to one */ + int fd = open(GYRO_DEVICE_PATH, 0); + res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale); + close(fd); + + if (res != OK) { + mavlink_log_critical(mavlink_fd, "ERROR: failed to reset scale / offsets"); + } + /* subscribe to gyro sensor topic */ int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro)); struct gyro_report gyro_report; - /* reset all offsets to zero and all scales to one */ - int fd = open(GYRO_DEVICE_PATH, 0); + if (res == OK) { + /* determine gyro mean values */ + const unsigned calibration_count = 5000; + unsigned calibration_counter = 0; + unsigned poll_errcount = 0; - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale)) - warn("WARNING: failed to reset scale / offsets for gyro"); + while (calibration_counter < calibration_count) { + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = sub_sensor_gyro; + fds[0].events = POLLIN; - close(fd); + int poll_ret = poll(fds, 1, 1000); + if (poll_ret > 0) { + orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report); + gyro_scale.x_offset += gyro_report.x; + gyro_scale.y_offset += gyro_report.y; + gyro_scale.z_offset += gyro_report.z; + calibration_counter++; - /*** --- OFFSETS --- ***/ + if (calibration_counter % (calibration_count / 20) == 0) + mavlink_log_info(mavlink_fd, "gyro calibration: progress <%u>", (calibration_counter * 100) / calibration_count); - /* determine gyro mean values */ - const unsigned calibration_count = 5000; - unsigned calibration_counter = 0; - unsigned poll_errcount = 0; + } else { + poll_errcount++; + } - while (calibration_counter < calibration_count) { - - /* wait blocking for new data */ - struct pollfd fds[1]; - fds[0].fd = sub_sensor_gyro; - fds[0].events = POLLIN; - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret > 0) { - orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report); - gyro_scale.x_offset += gyro_report.x; - gyro_scale.y_offset += gyro_report.y; - gyro_scale.z_offset += gyro_report.z; - calibration_counter++; - if (calibration_counter % (calibration_count / 20) == 0) - mavlink_log_info(mavlink_fd, "gyro cal progress <%u> percent", (calibration_counter * 100) / calibration_count); - - } else { - poll_errcount++; + if (poll_errcount > 1000) { + mavlink_log_critical(mavlink_fd, "ERROR: failed reading gyro sensor"); + res = ERROR; + break; + } } - if (poll_errcount > 1000) { - mavlink_log_info(mavlink_fd, "ERROR: Failed reading gyro sensor"); - close(sub_sensor_gyro); - return ERROR; + gyro_scale.x_offset /= calibration_count; + gyro_scale.y_offset /= calibration_count; + gyro_scale.z_offset /= calibration_count; + } + + if (res == OK) { + /* check offsets */ + if (!isfinite(gyro_scale.x_offset) || !isfinite(gyro_scale.y_offset) || !isfinite(gyro_scale.z_offset)) { + mavlink_log_critical(mavlink_fd, "ERROR: offset is NaN"); + res = ERROR; } } - gyro_scale.x_offset /= calibration_count; - gyro_scale.y_offset /= calibration_count; - gyro_scale.z_offset /= calibration_count; - - if (!isfinite(gyro_scale.x_offset) || !isfinite(gyro_scale.y_offset) || !isfinite(gyro_scale.z_offset)) { - mavlink_log_info(mavlink_fd, "gyro offset calibration FAILED (NaN)"); - close(sub_sensor_gyro); - return ERROR; + if (res == OK) { + /* set offset parameters to new values */ + if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_scale.x_offset)) + || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_scale.y_offset)) + || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_scale.z_offset))) { + mavlink_log_critical(mavlink_fd, "ERROR: setting gyro offs params failed"); + res = ERROR; + } } - /* beep on calibration end */ - mavlink_log_info(mavlink_fd, "gyro offset calibration done."); +#if 0 + /* beep on offset calibration end */ + mavlink_log_info(mavlink_fd, "gyro offset calibration done"); tune_neutral(); - /* set offset parameters to new values */ - if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_scale.x_offset)) - || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_scale.y_offset)) - || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_scale.z_offset))) { - mavlink_log_critical(mavlink_fd, "Setting gyro offset parameters failed!"); - } - - - /*** --- SCALING --- ***/ -#if 0 + /* scale calibration */ /* this was only a proof of concept and is currently not working. scaling will be set to 1.0 for now. */ mavlink_log_info(mavlink_fd, "offset done. Rotate for scale 30x or wait 5s to skip."); @@ -163,9 +169,11 @@ int do_gyro_calibration(int mavlink_fd) // 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; + 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(); @@ -175,7 +183,7 @@ int 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)) { + && (hrt_absolute_time() - start_time > 5 * 1e6)) { mavlink_log_info(mavlink_fd, "scale skipped, gyro calibration done"); close(sub_sensor_combined); return OK; @@ -203,14 +211,17 @@ int do_gyro_calibration(int mavlink_fd) // 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 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; + 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; @@ -220,15 +231,15 @@ int do_gyro_calibration(int mavlink_fd) // 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 */ - // mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); - // return; + // } 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", (double)gyro_scale); mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", (double)gyro_scale); @@ -236,42 +247,54 @@ int do_gyro_calibration(int mavlink_fd) if (!isfinite(gyro_scale.x_scale) || !isfinite(gyro_scale.y_scale) || !isfinite(gyro_scale.z_scale)) { mavlink_log_info(mavlink_fd, "gyro scale calibration FAILED (NaN)"); close(sub_sensor_gyro); + mavlink_log_critical(mavlink_fd, "gyro calibration failed"); return ERROR; } /* beep on calibration end */ - mavlink_log_info(mavlink_fd, "gyro scale calibration done."); + mavlink_log_info(mavlink_fd, "gyro scale calibration done"); tune_neutral(); #endif - /* set scale parameters to new values */ - if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scale.x_scale)) - || param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scale.y_scale)) - || param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scale.z_scale))) { - mavlink_log_critical(mavlink_fd, "Setting gyro scale parameters failed!"); - } - - /* apply new scaling and offsets */ - fd = open(GYRO_DEVICE_PATH, 0); - - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale)) - warn("WARNING: failed to apply new scale 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"); - close(sub_sensor_gyro); - return ERROR; - } - - mavlink_log_info(mavlink_fd, "gyro calibration done."); - close(sub_sensor_gyro); - return OK; + + if (res == OK) { + /* set scale parameters to new values */ + if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scale.x_scale)) + || param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scale.y_scale)) + || param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scale.z_scale))) { + mavlink_log_critical(mavlink_fd, "ERROR: setting gyro scale params failed"); + res = ERROR; + } + } + + if (res == OK) { + /* apply new scaling and offsets */ + fd = open(GYRO_DEVICE_PATH, 0); + res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale); + close(fd); + + if (res != OK) { + mavlink_log_critical(mavlink_fd, "ERROR: failed to apply new params for gyro"); + } + } + + if (res == OK) { + /* auto-save to EEPROM */ + res = param_save_default(); + + if (res != OK) { + mavlink_log_critical(mavlink_fd, "ERROR: failed to save parameters"); + } + } + + if (res == OK) { + mavlink_log_info(mavlink_fd, "gyro calibration: done"); + + } else { + mavlink_log_info(mavlink_fd, "gyro calibration: failed"); + } + + return res; } From ef42ef15c6991800111b25374b0f6e3935c2a9a9 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 21 Oct 2013 22:24:59 +0200 Subject: [PATCH 813/872] accel/gyro/mag calibration: big cleanup, use common messages --- .../commander/accelerometer_calibration.cpp | 40 ++- src/modules/commander/calibration_messages.h | 57 ++++ src/modules/commander/gyro_calibration.cpp | 37 ++- src/modules/commander/mag_calibration.cpp | 302 +++++++++--------- 4 files changed, 252 insertions(+), 184 deletions(-) create mode 100644 src/modules/commander/calibration_messages.h diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index d11d7eb5d3..49a8d6b339 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -122,6 +122,7 @@ */ #include "accelerometer_calibration.h" +#include "calibration_messages.h" #include "commander_helper.h" #include @@ -147,6 +148,8 @@ #endif static const int ERROR = -1; +static const char *sensor_name = "accel"; + int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][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); @@ -155,8 +158,7 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo int do_accel_calibration(int mavlink_fd) { - mavlink_log_info(mavlink_fd, "accel calibration: started"); - mavlink_log_info(mavlink_fd, "accel calibration: progress <0>"); + mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); struct accel_scale accel_scale = { 0.0f, @@ -175,7 +177,7 @@ int do_accel_calibration(int mavlink_fd) close(fd); if (res != OK) { - mavlink_log_critical(mavlink_fd, "ERROR: failed to reset scale / offsets"); + mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG); } /* measure and calculate offsets & scales */ @@ -213,7 +215,7 @@ int do_accel_calibration(int mavlink_fd) || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale.x_scale)) || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale.y_scale)) || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale.z_scale))) { - mavlink_log_critical(mavlink_fd, "ERROR: setting accel params failed"); + mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); res = ERROR; } } @@ -225,7 +227,7 @@ int do_accel_calibration(int mavlink_fd) close(fd); if (res != OK) { - mavlink_log_critical(mavlink_fd, "ERROR: failed to apply new params for accel"); + mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG); } } @@ -234,15 +236,15 @@ int do_accel_calibration(int mavlink_fd) res = param_save_default(); if (res != OK) { - mavlink_log_critical(mavlink_fd, "ERROR: failed to save parameters"); + mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG); } } if (res == OK) { - mavlink_log_info(mavlink_fd, "accel calibration: done"); + mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); } else { - mavlink_log_info(mavlink_fd, "accel calibration: failed"); + mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); } return res; @@ -266,13 +268,16 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float done_count = 0; for (int i = 0; i < 6; i++) { - if (!data_collected[i]) { + if (data_collected[i]) { + done_count++; + + } else { done = false; } } if (old_done_count != done_count) - mavlink_log_info(mavlink_fd, "accel calibration: progress <%u>", 17 * done_count); + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 17 * done_count); if (done) break; @@ -293,7 +298,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float } if (data_collected[orient]) { - mavlink_log_info(mavlink_fd, "%s done, please rotate to a different axis", orientation_strs[orient]); + mavlink_log_info(mavlink_fd, "%s done, rotate to a different axis", orientation_strs[orient]); continue; } @@ -374,6 +379,9 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) d = d * d; accel_disp[i] = accel_disp[i] * (1.0f - w); + if (d > still_thr2 * 8.0f) + d = still_thr2 * 8.0f; + if (d > accel_disp[i]) accel_disp[i] = d; } @@ -397,12 +405,12 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) } } - } else if (accel_disp[0] > still_thr2 * 2.0f || - accel_disp[1] > still_thr2 * 2.0f || - accel_disp[2] > still_thr2 * 2.0f) { + } else if (accel_disp[0] > still_thr2 * 4.0f || + accel_disp[1] > still_thr2 * 4.0f || + accel_disp[2] > still_thr2 * 4.0f) { /* not still, reset still start time */ if (t_still != 0) { - mavlink_log_info(mavlink_fd, "detected motion, please hold still..."); + mavlink_log_info(mavlink_fd, "detected motion, hold still..."); t_still = 0; } } @@ -416,7 +424,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) } if (poll_errcount > 1000) { - mavlink_log_critical(mavlink_fd, "ERROR: failed reading sensor"); + mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); return ERROR; } } diff --git a/src/modules/commander/calibration_messages.h b/src/modules/commander/calibration_messages.h new file mode 100644 index 0000000000..fd8b8564df --- /dev/null +++ b/src/modules/commander/calibration_messages.h @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * 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 calibration_messages.h + * + * Common calibration messages. + * + * @author Anton Babushkin + */ + +#ifndef CALIBRATION_MESSAGES_H_ +#define CALIBRATION_MESSAGES_H_ + +#define CAL_STARTED_MSG "%s calibration: started" +#define CAL_DONE_MSG "%s calibration: done" +#define CAL_FAILED_MSG "%s calibration: failed" +#define CAL_PROGRESS_MSG "%s calibration: progress <%u>" + +#define CAL_FAILED_SENSOR_MSG "ERROR: failed reading sensor" +#define CAL_FAILED_RESET_CAL_MSG "ERROR: failed to reset calibration" +#define CAL_FAILED_APPLY_CAL_MSG "ERROR: failed to apply calibration" +#define CAL_FAILED_SET_PARAMS_MSG "ERROR: failed to set parameters" +#define CAL_FAILED_SAVE_PARAMS_MSG "ERROR: failed to save parameters" + +#endif /* CALIBRATION_MESSAGES_H_ */ diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 219ae6cb2f..30cd0d48d4 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -33,10 +33,12 @@ /** * @file gyro_calibration.cpp + * * Gyroscope calibration routine */ #include "gyro_calibration.h" +#include "calibration_messages.h" #include "commander_helper.h" #include @@ -56,9 +58,12 @@ #endif static const int ERROR = -1; +static const char *sensor_name = "gyro"; + int do_gyro_calibration(int mavlink_fd) { - mavlink_log_info(mavlink_fd, "gyro calibration: started"); + mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); + mavlink_log_info(mavlink_fd, "don't move system"); struct gyro_scale gyro_scale = { 0.0f, @@ -77,19 +82,19 @@ int do_gyro_calibration(int mavlink_fd) close(fd); if (res != OK) { - mavlink_log_critical(mavlink_fd, "ERROR: failed to reset scale / offsets"); + mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG); } - /* subscribe to gyro sensor topic */ - int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro)); - struct gyro_report gyro_report; - if (res == OK) { /* determine gyro mean values */ const unsigned calibration_count = 5000; unsigned calibration_counter = 0; unsigned poll_errcount = 0; + /* subscribe to gyro sensor topic */ + int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro)); + struct gyro_report gyro_report; + while (calibration_counter < calibration_count) { /* wait blocking for new data */ struct pollfd fds[1]; @@ -106,19 +111,21 @@ int do_gyro_calibration(int mavlink_fd) calibration_counter++; if (calibration_counter % (calibration_count / 20) == 0) - mavlink_log_info(mavlink_fd, "gyro calibration: progress <%u>", (calibration_counter * 100) / calibration_count); + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count); } else { poll_errcount++; } if (poll_errcount > 1000) { - mavlink_log_critical(mavlink_fd, "ERROR: failed reading gyro sensor"); + mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); res = ERROR; break; } } + close(sub_sensor_gyro); + gyro_scale.x_offset /= calibration_count; gyro_scale.y_offset /= calibration_count; gyro_scale.z_offset /= calibration_count; @@ -137,7 +144,7 @@ int do_gyro_calibration(int mavlink_fd) if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_scale.x_offset)) || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_scale.y_offset)) || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_scale.z_offset))) { - mavlink_log_critical(mavlink_fd, "ERROR: setting gyro offs params failed"); + mavlink_log_critical(mavlink_fd, "ERROR: failed to set offset params"); res = ERROR; } } @@ -257,14 +264,12 @@ int do_gyro_calibration(int mavlink_fd) #endif - close(sub_sensor_gyro); - if (res == OK) { /* set scale parameters to new values */ if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scale.x_scale)) || param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scale.y_scale)) || param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scale.z_scale))) { - mavlink_log_critical(mavlink_fd, "ERROR: setting gyro scale params failed"); + mavlink_log_critical(mavlink_fd, "ERROR: failed to set scale params"); res = ERROR; } } @@ -276,7 +281,7 @@ int do_gyro_calibration(int mavlink_fd) close(fd); if (res != OK) { - mavlink_log_critical(mavlink_fd, "ERROR: failed to apply new params for gyro"); + mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG); } } @@ -285,15 +290,15 @@ int do_gyro_calibration(int mavlink_fd) res = param_save_default(); if (res != OK) { - mavlink_log_critical(mavlink_fd, "ERROR: failed to save parameters"); + mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG); } } if (res == OK) { - mavlink_log_info(mavlink_fd, "gyro calibration: done"); + mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); } else { - mavlink_log_info(mavlink_fd, "gyro calibration: failed"); + mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); } return res; diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index b0d4266be5..09f4104f89 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -33,12 +33,14 @@ /** * @file mag_calibration.cpp + * * Magnetometer calibration routine */ #include "mag_calibration.h" #include "commander_helper.h" #include "calibration_routines.h" +#include "calibration_messages.h" #include #include @@ -59,26 +61,20 @@ #endif static const int ERROR = -1; +static const char *sensor_name = "mag"; + int do_mag_calibration(int mavlink_fd) { - 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; + mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); + mavlink_log_info(mavlink_fd, "don't move system"); /* 45 seconds */ uint64_t calibration_interval = 45 * 1000 * 1000; - /* maximum 2000 values */ + /* maximum 500 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, @@ -88,97 +84,92 @@ int do_mag_calibration(int mavlink_fd) 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"); + int res = OK; + + /* erase old calibration */ + int fd = open(MAG_DEVICE_PATH, O_RDONLY); + res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null); + + if (res != OK) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG); } - /* calibrate range */ - if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) { - warnx("failed to calibrate scale"); + if (res == OK) { + /* calibrate range */ + res = ioctl(fd, MAGIOCCALIBRATE, fd); + + if (res != OK) { + mavlink_log_critical(mavlink_fd, "ERROR: failed to calibrate scale"); + } } close(fd); - mavlink_log_info(mavlink_fd, "mag cal progress <20> percent"); + float *x; + float *y; + float *z; - /* calibrate offsets */ + if (res == OK) { + /* allocate memory */ + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20); - // uint64_t calibration_start = hrt_absolute_time(); + x = (float *)malloc(sizeof(float) * calibration_maxcount); + y = (float *)malloc(sizeof(float) * calibration_maxcount); + z = (float *)malloc(sizeof(float) * calibration_maxcount); - 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 ERROR; + if (x == NULL || y == NULL || z == NULL) { + mavlink_log_critical(mavlink_fd, "ERROR: out of memory"); + res = ERROR; + } } - mavlink_log_info(mavlink_fd, "scale calibration completed, dynamic calibration starting.."); + if (res == OK) { + int sub_mag = orb_subscribe(ORB_ID(sensor_mag)); + struct mag_report mag; - unsigned poll_errcount = 0; + /* limit update rate to get equally spaced measurements over time (in ms) */ + orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); - while (hrt_absolute_time() < calibration_deadline && - calibration_counter < calibration_maxcount) { + /* calibrate offsets */ + uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; + unsigned poll_errcount = 0; - /* wait blocking for new data */ - struct pollfd fds[1]; - fds[0].fd = sub_mag; - fds[0].events = POLLIN; + mavlink_log_info(mavlink_fd, "rotate in a figure 8 around all axis"); - /* user guidance */ - if (hrt_absolute_time() >= axis_deadline && - axis_index < 3) { + while (hrt_absolute_time() < calibration_deadline && + calibration_counter < calibration_maxcount) { - axis_index++; + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = sub_mag; + fds[0].events = POLLIN; - mavlink_log_info(mavlink_fd, "please rotate in a figure 8 or around %c axis.", axislabels[axis_index]); - tune_neutral(); + int poll_ret = poll(fds, 1, 1000); - axis_deadline += calibration_interval / 3; + if (poll_ret > 0) { + orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); + + x[calibration_counter] = mag.x; + y[calibration_counter] = mag.y; + z[calibration_counter] = mag.z; + + calibration_counter++; + + if (calibration_counter % (calibration_maxcount / 20) == 0) + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount); + + } else { + poll_errcount++; + } + + if (poll_errcount > 1000) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); + res = ERROR; + break; + } } - if (!(axis_index < 3)) { - break; - } - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret > 0) { - orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); - - x[calibration_counter] = mag.x; - y[calibration_counter] = mag.y; - z[calibration_counter] = mag.z; - - calibration_counter++; - if (calibration_counter % (calibration_maxcount / 20) == 0) - mavlink_log_info(mavlink_fd, "mag cal progress <%u> percent", 20 + (calibration_counter * 50) / calibration_maxcount); - - - } else { - poll_errcount++; - } - - if (poll_errcount > 1000) { - mavlink_log_info(mavlink_fd, "ERROR: Failed reading mag sensor"); - close(sub_mag); - free(x); - free(y); - free(z); - return ERROR; - } - - + close(sub_mag); } float sphere_x; @@ -186,93 +177,100 @@ int do_mag_calibration(int mavlink_fd) float sphere_z; float sphere_radius; - mavlink_log_info(mavlink_fd, "mag cal progress <70> percent"); - sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); - mavlink_log_info(mavlink_fd, "mag cal progress <80> percent"); + if (res == OK) { + /* sphere fit */ + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 70); + sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 80); - free(x); - free(y); - free(z); + if (!isfinite(sphere_x) || !isfinite(sphere_y) || !isfinite(sphere_z)) { + mavlink_log_critical(mavlink_fd, "ERROR: NaN in sphere fit"); + res = ERROR; + } + } - if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) { + if (x != NULL) + free(x); - fd = open(MAG_DEVICE_PATH, 0); + if (y != NULL) + free(y); + if (z != NULL) + free(z); + + if (res == OK) { + /* apply calibration and set parameters */ struct mag_scale mscale; - if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale)) - warn("WARNING: failed to get scale / offsets for mag"); + fd = open(MAG_DEVICE_PATH, 0); + res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale); - mscale.x_offset = sphere_x; - mscale.y_offset = sphere_y; - mscale.z_offset = sphere_z; + if (res != OK) { + mavlink_log_critical(mavlink_fd, "ERROR: failed to get current calibration"); + } - if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) - warn("WARNING: failed to set scale / offsets for mag"); + if (res == OK) { + mscale.x_offset = sphere_x; + mscale.y_offset = sphere_y; + mscale.z_offset = sphere_z; + + res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale); + + if (res != OK) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG); + } + } close(fd); - /* announce and set new offset */ + if (res == OK) { + /* set parameters */ + if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) + res = ERROR; - 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))) + res = ERROR; + + if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) + res = ERROR; + + if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) + res = ERROR; + + if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) + res = ERROR; + + if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) + res = ERROR; + + if (res != OK) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); + } + + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 90); } - if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) { - warnx("Setting Y mag offset failed!\n"); + if (res == OK) { + /* auto-save to EEPROM */ + res = param_save_default(); + + if (res != OK) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG); + } } - if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) { - warnx("Setting Z mag offset failed!\n"); + mavlink_log_info(mavlink_fd, "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, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale, + (double)mscale.y_scale, (double)mscale.z_scale); + + if (res == OK) { + mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); + + } else { + mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); } - 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"); - } - - mavlink_log_info(mavlink_fd, "mag cal progress <90> percent"); - - /* 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"); - close(sub_mag); - return ERROR; - } - - 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, "magnetometer calibration completed"); - mavlink_log_info(mavlink_fd, "mag cal progress <100> percent"); - - close(sub_mag); - return OK; - /* third beep by cal end routine */ - - } else { - mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)"); - close(sub_mag); - return ERROR; + return res; } } From 495073935e428d99833135bb227f3085d2def1cf Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 21 Oct 2013 23:33:01 +0200 Subject: [PATCH 814/872] accelerometer_calibration: stability fix --- src/modules/commander/accelerometer_calibration.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 49a8d6b339..5eeca5a1a3 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -180,10 +180,13 @@ int do_accel_calibration(int mavlink_fd) mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG); } - /* measure and calculate offsets & scales */ float accel_offs[3]; float accel_T[3][3]; - res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T); + + if (res == OK) { + /* measure and calculate offsets & scales */ + res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T); + } if (res == OK) { /* measurements completed successfully, rotate calibration values */ From 7f0ced968e5d518776930296ed870a47cc8c1756 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Mon, 21 Oct 2013 21:28:26 -0400 Subject: [PATCH 815/872] Working on roboclaw driver. --- makefiles/config_px4fmu-v1_default.mk | 2 +- makefiles/config_px4fmu-v2_default.mk | 1 + src/drivers/roboclaw/RoboClaw.cpp | 250 +++++++++++++++++++++++++ src/drivers/roboclaw/RoboClaw.hpp | 187 ++++++++++++++++++ src/drivers/roboclaw/module.mk | 41 ++++ src/drivers/roboclaw/roboclaw_main.cpp | 181 ++++++++++++++++++ 6 files changed, 661 insertions(+), 1 deletion(-) create mode 100644 src/drivers/roboclaw/RoboClaw.cpp create mode 100644 src/drivers/roboclaw/RoboClaw.hpp create mode 100644 src/drivers/roboclaw/module.mk create mode 100644 src/drivers/roboclaw/roboclaw_main.cpp diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 46640f3c51..3cccc8447a 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -33,7 +33,7 @@ MODULES += drivers/hott/hott_sensors MODULES += drivers/blinkm MODULES += drivers/rgbled MODULES += drivers/mkblctrl -MODULES += drivers/md25 +MODULES += drivers/roboclaw MODULES += drivers/airspeed MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index bb589cb9f3..a2027ded9b 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -31,6 +31,7 @@ MODULES += drivers/hil MODULES += drivers/hott/hott_telemetry MODULES += drivers/hott/hott_sensors MODULES += drivers/blinkm +MODULES += drivers/roboclaw MODULES += drivers/airspeed MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp new file mode 100644 index 0000000000..88b22e72aa --- /dev/null +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -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 RoboClaw.cpp + * + * RoboClaw Motor Driver + * + * references: + * http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf + * + */ + +#include "RoboClaw.hpp" +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +uint8_t RoboClaw::checksum_mask = 0x7f; + +RoboClaw::RoboClaw(const char *deviceName, uint16_t address): + _address(address), + _uart(0), + _controlPoll(), + _actuators(NULL, ORB_ID(actuator_controls_0), 20), + _motor1Position(0), + _motor1Speed(0), + _motor2Position(0), + _motor2Speed(0) +{ + // setup control polling + _controlPoll.fd = _actuators.getHandle(); + _controlPoll.events = POLLIN; + + // start serial port + _uart = open(deviceName, O_RDWR | O_NOCTTY); + struct termios uart_config; + tcgetattr(_uart, &uart_config); + uart_config.c_oflag &= ~ONLCR; // no CR for every LF + cfsetispeed(&uart_config, B38400); + cfsetospeed(&uart_config, B38400); + tcsetattr(_uart, TCSANOW, &uart_config); + + // setup default settings, reset encoders + resetEncoders(); +} + +RoboClaw::~RoboClaw() +{ + setMotorDutyCycle(MOTOR_1, 0.0); + setMotorDutyCycle(MOTOR_2, 0.0); + close(_uart); +} + +int RoboClaw::readEncoder(e_motor motor) +{ + uint16_t sum = 0; + if (motor == MOTOR_1) { + _sendCommand(CMD_READ_ENCODER_1, nullptr, 0, sum); + } else if (motor == MOTOR_2) { + _sendCommand(CMD_READ_ENCODER_2, nullptr, 0, sum); + } + uint8_t rbuf[6]; + int ret = read(_uart, rbuf, 6); + uint32_t count = 0; + uint8_t * countBytes = (uint8_t *)(&count); + countBytes[3] = rbuf[0]; + countBytes[2] = rbuf[1]; + countBytes[1] = rbuf[2]; + countBytes[0] = rbuf[3]; + uint8_t status = rbuf[4]; + if ((sum + _sumBytes(rbuf,5)) & + checksum_mask == rbuf[5]) return -1; + int overFlow = 0; + if (status & STATUS_UNDERFLOW) { + overFlow = -1; + } else if (status & STATUS_OVERFLOW) { + overFlow = +1; + } + if (motor == MOTOR_1) { + _motor1Overflow += overFlow; + } else if (motor == MOTOR_2) { + _motor2Overflow += overFlow; + } + return ret; +} + +void RoboClaw::status(char *string, size_t n) +{ + snprintf(string, n, + "motor 1 position:\t%10.2f\tspeed:\t%10.2f\n" + "motor 2 position:\t%10.2f\tspeed:\t%10.2f\n", + double(getMotorPosition(MOTOR_1)), + double(getMotorSpeed(MOTOR_1)), + double(getMotorPosition(MOTOR_2)), + double(getMotorSpeed(MOTOR_2))); +} + +float RoboClaw::getMotorPosition(e_motor motor) +{ + if (motor == MOTOR_1) { + return _motor1Position; + } else if (motor == MOTOR_2) { + return _motor2Position; + } +} + +float RoboClaw::getMotorSpeed(e_motor motor) +{ + if (motor == MOTOR_1) { + return _motor1Speed; + } else if (motor == MOTOR_2) { + return _motor2Speed; + } +} + +int RoboClaw::setMotorSpeed(e_motor motor, float value) +{ + uint16_t sum = 0; + // bound + if (value > 1) value = 1; + if (value < -1) value = -1; + uint8_t speed = fabs(value)*127; + // send command + if (motor == MOTOR_1) { + if (value > 0) { + _sendCommand(CMD_DRIVE_FWD_1, &speed, 1, sum); + } else { + _sendCommand(CMD_DRIVE_REV_1, &speed, 1, sum); + } + } else if (motor == MOTOR_2) { + if (value > 0) { + _sendCommand(CMD_DRIVE_FWD_2, &speed, 1, sum); + } else { + _sendCommand(CMD_DRIVE_REV_2, &speed, 1, sum); + } + } +} + +int RoboClaw::setMotorDutyCycle(e_motor motor, float value) +{ + uint16_t sum = 0; + // bound + if (value > 1) value = 1; + if (value < -1) value = -1; + int16_t duty = 1500*value; + // send command + if (motor == MOTOR_1) { + _sendCommand(CMD_SIGNED_DUTYCYCLE_1, + (uint8_t *)(&duty), 2, sum); + } else if (motor == MOTOR_2) { + _sendCommand(CMD_SIGNED_DUTYCYCLE_2, + (uint8_t *)(&duty), 2, sum); + } +} + +void RoboClaw::update() +{ + // wait for an actuator publication, + // check for exit condition every second + // note "::poll" is required to distinguish global + // poll from member function for driver + if (::poll(&_controlPoll, 1, 1000) < 0) return; // poll error + + // if new data, send to motors + if (_actuators.updated()) { + _actuators.update(); + setMotorSpeed(MOTOR_1,_actuators.control[CH_VOLTAGE_LEFT]); + setMotorSpeed(MOTOR_2,_actuators.control[CH_VOLTAGE_RIGHT]); + } +} + +uint16_t RoboClaw::_sumBytes(uint8_t * buf, size_t n) +{ + uint16_t sum = 0; + for (size_t i=0;i +#include +#include +#include +#include + +/** + * This is a driver for the RoboClaw motor controller + */ +class RoboClaw +{ +public: + + /** control channels */ + enum e_channel { + CH_VOLTAGE_LEFT = 0, + CH_VOLTAGE_RIGHT + }; + + /** motors */ + enum e_motor { + MOTOR_1 = 0, + MOTOR_2 + }; + + /** + * constructor + * @param deviceName the name of the + * serial port e.g. "/dev/ttyS2" + * @param address the adddress of the motor + * (selectable on roboclaw) + */ + RoboClaw(const char *deviceName, uint16_t address); + + /** + * deconstructor + */ + virtual ~RoboClaw(); + + /** + * @return position of a motor, rev + */ + float getMotorPosition(e_motor motor); + + /** + * @return speed of a motor, rev/sec + */ + float getMotorSpeed(e_motor motor); + + /** + * set the speed of a motor, rev/sec + */ + int setMotorSpeed(e_motor motor, float value); + + /** + * set the duty cycle of a motor, rev/sec + */ + int setMotorDutyCycle(e_motor motor, float value); + + /** + * reset the encoders + * @return status + */ + int resetEncoders(); + + /** + * main update loop that updates RoboClaw motor + * dutycycle based on actuator publication + */ + void update(); + + /** + * read data from serial + */ + int readEncoder(e_motor motor); + + /** + * print status + */ + void status(char *string, size_t n); + +private: + + // Quadrature status flags + enum e_quadrature_status_flags { + STATUS_UNDERFLOW = 1 << 0, /**< encoder went below 0 **/ + STATUS_REVERSE = 1 << 1, /**< motor doing in reverse dir **/ + STATUS_OVERFLOW = 1 << 2, /**< encoder went above 2^32 **/ + }; + + // commands + // We just list the commands we want from the manual here. + enum e_command { + + // simple + CMD_DRIVE_FWD_1 = 0, + CMD_DRIVE_REV_1 = 1, + CMD_DRIVE_FWD_2 = 4, + CMD_DRIVE_REV_2 = 5, + + // encoder commands + CMD_READ_ENCODER_1 = 16, + CMD_READ_ENCODER_2 = 17, + CMD_RESET_ENCODERS = 20, + + // advanced motor control + CMD_READ_SPEED_HIRES_1 = 30, + CMD_READ_SPEED_HIRES_2 = 31, + CMD_SIGNED_DUTYCYCLE_1 = 32, + CMD_SIGNED_DUTYCYCLE_2 = 33, + }; + + static uint8_t checksum_mask; + + uint16_t _address; + + int _uart; + + /** poll structure for control packets */ + struct pollfd _controlPoll; + + /** actuator controls subscription */ + control::UOrbSubscription _actuators; + + // private data + float _motor1Position; + float _motor1Speed; + int16_t _motor1Overflow; + + float _motor2Position; + float _motor2Speed; + int16_t _motor2Overflow; + + // private methods + uint16_t _sumBytes(uint8_t * buf, size_t n); + int _sendCommand(e_command cmd, uint8_t * data, size_t n_data, uint16_t & prev_sum); +}; + +// unit testing +int roboclawTest(const char *deviceName, uint8_t address); + +// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78 diff --git a/src/drivers/roboclaw/module.mk b/src/drivers/roboclaw/module.mk new file mode 100644 index 0000000000..1abecf198b --- /dev/null +++ b/src/drivers/roboclaw/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# RoboClaw Motor Controller +# + +MODULE_COMMAND = roboclaw + +SRCS = roboclaw_main.cpp \ + RoboClaw.cpp diff --git a/src/drivers/roboclaw/roboclaw_main.cpp b/src/drivers/roboclaw/roboclaw_main.cpp new file mode 100644 index 0000000000..a61c646fcf --- /dev/null +++ b/src/drivers/roboclaw/roboclaw_main.cpp @@ -0,0 +1,181 @@ +/**************************************************************************** + * + * 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 roboclaw_main.cpp + * + * RoboClaw Motor Driver + * + * references: + * http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf + * + */ + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include "RoboClaw.hpp" + +static bool thread_should_exit = false; /**< Deamon exit flag */ +static bool thread_running = false; /**< Deamon status flag */ +static int deamon_task; /**< Handle of deamon task / thread */ + +/** + * Deamon management function. + */ +extern "C" __EXPORT int roboclaw_main(int argc, char *argv[]); + +/** + * Mainloop of deamon. + */ +int roboclaw_thread_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +static void usage(); + +static void usage() +{ + fprintf(stderr, "usage: roboclaw " + "{start|stop|status|test}\n\n"); +} + +/** + * The deamon app only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ +int roboclaw_main(int argc, char *argv[]) +{ + + if (argc < 1) + usage(); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + printf("roboclaw already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + deamon_task = task_spawn_cmd("roboclaw", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 10, + 2048, + roboclaw_thread_main, + (const char **)argv); + exit(0); + + } else if (!strcmp(argv[1], "test")) { + + if (argc < 4) { + printf("usage: roboclaw test device address\n"); + exit(-1); + } + + const char *deviceName = argv[1]; + uint8_t address = strtoul(argv[2], nullptr, 0); + + roboclawTest(deviceName, address); + thread_should_exit = true; + exit(0); + + } else if (!strcmp(argv[1], "stop")) { + + thread_should_exit = true; + exit(0); + + } else if (!strcmp(argv[1], "status")) { + + if (thread_running) { + printf("\troboclaw app is running\n"); + + } else { + printf("\troboclaw app not started\n"); + } + exit(0); + } + + usage(); + exit(1); +} + +int roboclaw_thread_main(int argc, char *argv[]) +{ + printf("[roboclaw] starting\n"); + + // skip parent process args + argc -=2; + argv +=2; + + if (argc < 3) { + printf("usage: roboclaw start device address\n"); + return -1; + } + + const char *deviceName = argv[1]; + uint8_t address = strtoul(argv[2], nullptr, 0); + + // start + RoboClaw roboclaw(deviceName, address); + + thread_running = true; + + // loop + while (!thread_should_exit) { + roboclaw.update(); + } + + // exit + printf("[roboclaw] exiting.\n"); + thread_running = false; + return 0; +} + +// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78 From ce68f93867abfd3ea528809144f7c045d9bce544 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Mon, 21 Oct 2013 23:40:36 -0400 Subject: [PATCH 816/872] Debugging roboclaw comm. --- src/drivers/roboclaw/RoboClaw.cpp | 52 ++++++++++++++++++++++---- src/drivers/roboclaw/RoboClaw.hpp | 2 +- src/drivers/roboclaw/roboclaw_main.cpp | 8 +++- 3 files changed, 52 insertions(+), 10 deletions(-) diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index 88b22e72aa..aecc511aed 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -75,12 +75,18 @@ RoboClaw::RoboClaw(const char *deviceName, uint16_t address): // start serial port _uart = open(deviceName, O_RDWR | O_NOCTTY); + if (_uart < 0) err(1, "could not open %s", deviceName); + int ret = 0; struct termios uart_config; - tcgetattr(_uart, &uart_config); + ret = tcgetattr(_uart, &uart_config); + if (ret < 0) err (1, "failed to get attr"); uart_config.c_oflag &= ~ONLCR; // no CR for every LF - cfsetispeed(&uart_config, B38400); - cfsetospeed(&uart_config, B38400); - tcsetattr(_uart, TCSANOW, &uart_config); + ret = cfsetispeed(&uart_config, B38400); + if (ret < 0) err (1, "failed to set input speed"); + ret = cfsetospeed(&uart_config, B38400); + if (ret < 0) err (1, "failed to set output speed"); + ret = tcsetattr(_uart, TCSANOW, &uart_config); + if (ret < 0) err (1, "failed to set attr"); // setup default settings, reset encoders resetEncoders(); @@ -110,23 +116,30 @@ int RoboClaw::readEncoder(e_motor motor) countBytes[1] = rbuf[2]; countBytes[0] = rbuf[3]; uint8_t status = rbuf[4]; - if ((sum + _sumBytes(rbuf,5)) & - checksum_mask == rbuf[5]) return -1; + if (((sum + _sumBytes(rbuf,5)) & checksum_mask) + == rbuf[5]) return -1; int overFlow = 0; if (status & STATUS_UNDERFLOW) { + printf("roboclaw: underflow\n"); overFlow = -1; } else if (status & STATUS_OVERFLOW) { + printf("roboclaw: overflow\n"); overFlow = +1; } + static int64_t overflowAmount = 0x100000000LL; if (motor == MOTOR_1) { _motor1Overflow += overFlow; + _motor1Position = count + + _motor1Overflow*_motor1Position; } else if (motor == MOTOR_2) { _motor2Overflow += overFlow; + _motor2Position = count + + _motor2Overflow*_motor2Position; } return ret; } -void RoboClaw::status(char *string, size_t n) +void RoboClaw::printStatus(char *string, size_t n) { snprintf(string, n, "motor 1 position:\t%10.2f\tspeed:\t%10.2f\n" @@ -195,6 +208,13 @@ int RoboClaw::setMotorDutyCycle(e_motor motor, float value) } } +int RoboClaw::resetEncoders() +{ + uint16_t sum = 0; + return _sendCommand(CMD_RESET_ENCODERS, + nullptr, 0, sum); +} + void RoboClaw::update() { // wait for an actuator publication, @@ -214,9 +234,13 @@ void RoboClaw::update() uint16_t RoboClaw::_sumBytes(uint8_t * buf, size_t n) { uint16_t sum = 0; + printf("sum\n"); for (size_t i=0;i Date: Tue, 22 Oct 2013 05:04:13 -0400 Subject: [PATCH 817/872] Roboclaw encoders/ dutycycledrive complete. --- ROMFS/px4fmu_common/init.d/40_io_segway | 16 +-- ROMFS/px4fmu_common/init.d/rcS | 6 ++ src/drivers/roboclaw/RoboClaw.cpp | 131 ++++++++++++++++-------- src/drivers/roboclaw/RoboClaw.hpp | 11 +- src/drivers/roboclaw/roboclaw_main.cpp | 28 +++-- 5 files changed, 130 insertions(+), 62 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/40_io_segway b/ROMFS/px4fmu_common/init.d/40_io_segway index 4b7ed52869..ffe7f695b1 100644 --- a/ROMFS/px4fmu_common/init.d/40_io_segway +++ b/ROMFS/px4fmu_common/init.d/40_io_segway @@ -21,8 +21,8 @@ param set MAV_TYPE 10 # # Start MAVLink (depends on orb) # -mavlink start -d /dev/ttyS1 -b 57600 -usleep 5000 +#mavlink start -d /dev/ttyS1 -b 57600 +#usleep 5000 # # Start and configure PX4IO interface @@ -32,25 +32,25 @@ sh /etc/init.d/rc.io # # Start the commander (depends on orb, mavlink) # -commander start +#commander start # # Start the sensors (depends on orb, px4io) # -sh /etc/init.d/rc.sensors +#sh /etc/init.d/rc.sensors # # Start GPS interface (depends on orb) # -gps start +#gps start # # Start the attitude estimator (depends on orb) # -attitude_estimator_ekf start +#attitude_estimator_ekf start # # Load mixer and start controllers (depends on px4io) # -md25 start 3 0x58 -segway start +roboclaw test /dev/ttyS2 128 +#segway start diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index fd588017f9..3a458286ed 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -272,6 +272,12 @@ then sh /etc/init.d/30_io_camflyer set MODE custom fi + + if param compare SYS_AUTOSTART 40 + then + sh /etc/init.d/40_io_segway + set MODE custom + fi if param compare SYS_AUTOSTART 31 then diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index aecc511aed..3bbd7f48f5 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -59,15 +59,19 @@ uint8_t RoboClaw::checksum_mask = 0x7f; -RoboClaw::RoboClaw(const char *deviceName, uint16_t address): +RoboClaw::RoboClaw(const char *deviceName, uint16_t address, + uint16_t pulsesPerRev): _address(address), + _pulsesPerRev(pulsesPerRev), _uart(0), _controlPoll(), _actuators(NULL, ORB_ID(actuator_controls_0), 20), _motor1Position(0), _motor1Speed(0), + _motor1Overflow(0), _motor2Position(0), - _motor2Speed(0) + _motor2Speed(0), + _motor2Overflow(0) { // setup control polling _controlPoll.fd = _actuators.getHandle(); @@ -90,6 +94,13 @@ RoboClaw::RoboClaw(const char *deviceName, uint16_t address): // setup default settings, reset encoders resetEncoders(); + + // XXX roboclaw gives 128 as first several csums + // have to read a couple of messages first + for (int i=0;i<5;i++) { + if (readEncoder(MOTOR_1) > 0) break; + usleep(3000); + } } RoboClaw::~RoboClaw() @@ -107,8 +118,18 @@ int RoboClaw::readEncoder(e_motor motor) } else if (motor == MOTOR_2) { _sendCommand(CMD_READ_ENCODER_2, nullptr, 0, sum); } - uint8_t rbuf[6]; - int ret = read(_uart, rbuf, 6); + uint8_t rbuf[50]; + usleep(5000); + int nread = read(_uart, rbuf, 50); + if (nread < 6) { + printf("failed to read\n"); + return -1; + } + //printf("received: \n"); + //for (int i=0;i 0) { - _sendCommand(CMD_DRIVE_FWD_1, &speed, 1, sum); + return _sendCommand(CMD_DRIVE_FWD_1, &speed, 1, sum); } else { - _sendCommand(CMD_DRIVE_REV_1, &speed, 1, sum); + return _sendCommand(CMD_DRIVE_REV_1, &speed, 1, sum); } } else if (motor == MOTOR_2) { if (value > 0) { - _sendCommand(CMD_DRIVE_FWD_2, &speed, 1, sum); + return _sendCommand(CMD_DRIVE_FWD_2, &speed, 1, sum); } else { - _sendCommand(CMD_DRIVE_REV_2, &speed, 1, sum); + return _sendCommand(CMD_DRIVE_REV_2, &speed, 1, sum); } } + return -1; } int RoboClaw::setMotorDutyCycle(e_motor motor, float value) @@ -200,12 +234,13 @@ int RoboClaw::setMotorDutyCycle(e_motor motor, float value) int16_t duty = 1500*value; // send command if (motor == MOTOR_1) { - _sendCommand(CMD_SIGNED_DUTYCYCLE_1, + return _sendCommand(CMD_SIGNED_DUTYCYCLE_1, (uint8_t *)(&duty), 2, sum); } else if (motor == MOTOR_2) { - _sendCommand(CMD_SIGNED_DUTYCYCLE_2, + return _sendCommand(CMD_SIGNED_DUTYCYCLE_2, (uint8_t *)(&duty), 2, sum); } + return -1; } int RoboClaw::resetEncoders() @@ -215,13 +250,13 @@ int RoboClaw::resetEncoders() nullptr, 0, sum); } -void RoboClaw::update() +int RoboClaw::update() { // wait for an actuator publication, // check for exit condition every second // note "::poll" is required to distinguish global // poll from member function for driver - if (::poll(&_controlPoll, 1, 1000) < 0) return; // poll error + if (::poll(&_controlPoll, 1, 1000) < 0) return -1; // poll error // if new data, send to motors if (_actuators.updated()) { @@ -229,56 +264,68 @@ void RoboClaw::update() setMotorSpeed(MOTOR_1,_actuators.control[CH_VOLTAGE_LEFT]); setMotorSpeed(MOTOR_2,_actuators.control[CH_VOLTAGE_RIGHT]); } + return 0; } uint16_t RoboClaw::_sumBytes(uint8_t * buf, size_t n) { uint16_t sum = 0; - printf("sum\n"); + //printf("sum\n"); for (size_t i=0;i Date: Tue, 22 Oct 2013 05:08:20 -0400 Subject: [PATCH 818/872] Updated script. --- ROMFS/px4fmu_common/init.d/40_io_segway | 16 ++++++++-------- ROMFS/px4fmu_common/init.d/rcS | 12 ++++++------ 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/40_io_segway b/ROMFS/px4fmu_common/init.d/40_io_segway index ffe7f695b1..fb9dec0437 100644 --- a/ROMFS/px4fmu_common/init.d/40_io_segway +++ b/ROMFS/px4fmu_common/init.d/40_io_segway @@ -21,8 +21,8 @@ param set MAV_TYPE 10 # # Start MAVLink (depends on orb) # -#mavlink start -d /dev/ttyS1 -b 57600 -#usleep 5000 +mavlink start -d /dev/ttyS1 -b 57600 +usleep 5000 # # Start and configure PX4IO interface @@ -32,25 +32,25 @@ sh /etc/init.d/rc.io # # Start the commander (depends on orb, mavlink) # -#commander start +commander start # # Start the sensors (depends on orb, px4io) # -#sh /etc/init.d/rc.sensors +sh /etc/init.d/rc.sensors # # Start GPS interface (depends on orb) # -#gps start +gps start # # Start the attitude estimator (depends on orb) # -#attitude_estimator_ekf start +attitude_estimator_ekf start # # Load mixer and start controllers (depends on px4io) # -roboclaw test /dev/ttyS2 128 -#segway start +roboclaw start /dev/ttyS2 128 1200 +segway start diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 3a458286ed..424787d54a 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -273,18 +273,18 @@ then set MODE custom fi - if param compare SYS_AUTOSTART 40 - then - sh /etc/init.d/40_io_segway - set MODE custom - fi - if param compare SYS_AUTOSTART 31 then sh /etc/init.d/31_io_phantom set MODE custom fi + if param compare SYS_AUTOSTART 40 + then + sh /etc/init.d/40_io_segway + set MODE custom + fi + if param compare SYS_AUTOSTART 100 then sh /etc/init.d/100_mpx_easystar From 108d723a4902086f883136c7de6a75d65256500b Mon Sep 17 00:00:00 2001 From: James Goppert Date: Tue, 22 Oct 2013 05:10:26 -0400 Subject: [PATCH 819/872] Removed old timing hack. --- src/drivers/roboclaw/RoboClaw.cpp | 7 ------- 1 file changed, 7 deletions(-) diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index 3bbd7f48f5..73bd5dadac 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -94,13 +94,6 @@ RoboClaw::RoboClaw(const char *deviceName, uint16_t address, // setup default settings, reset encoders resetEncoders(); - - // XXX roboclaw gives 128 as first several csums - // have to read a couple of messages first - for (int i=0;i<5;i++) { - if (readEncoder(MOTOR_1) > 0) break; - usleep(3000); - } } RoboClaw::~RoboClaw() From d143e827dcd774548bca6d6b4cb6fb69ef35a826 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Tue, 22 Oct 2013 05:43:10 -0400 Subject: [PATCH 820/872] Updated segway controller for new state machine. --- makefiles/config_px4fmu-v1_default.mk | 2 +- makefiles/config_px4fmu-v2_default.mk | 2 +- src/modules/segway/BlockSegwayController.cpp | 13 +++++++------ 3 files changed, 9 insertions(+), 8 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 3cccc8447a..85ac3f546f 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -79,7 +79,7 @@ MODULES += examples/flow_position_estimator # # Vehicle Control # -#MODULES += modules/segway # XXX needs state machine update +MODULES += modules/segway MODULES += modules/fw_pos_control_l1 MODULES += modules/fw_att_control MODULES += modules/multirotor_att_control diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index a2027ded9b..c0972be9eb 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -78,7 +78,7 @@ MODULES += examples/flow_position_estimator # # Vehicle Control # -#MODULES += modules/segway # XXX needs state machine update +MODULES += modules/segway MODULES += modules/fw_pos_control_l1 MODULES += modules/fw_att_control MODULES += modules/multirotor_att_control diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp index b1dc39445f..96a443c6ea 100644 --- a/src/modules/segway/BlockSegwayController.cpp +++ b/src/modules/segway/BlockSegwayController.cpp @@ -26,7 +26,7 @@ void BlockSegwayController::update() { _actuators.control[i] = 0.0f; // only update guidance in auto mode - if (_status.state_machine == SYSTEM_STATE_AUTO) { + if (_status.main_state == MAIN_STATE_AUTO) { // update guidance } @@ -34,17 +34,18 @@ void BlockSegwayController::update() { float spdCmd = -th2v.update(_att.pitch) - q2v.update(_att.pitchspeed); // handle autopilot modes - if (_status.state_machine == SYSTEM_STATE_AUTO || - _status.state_machine == SYSTEM_STATE_STABILIZED) { + if (_status.main_state == MAIN_STATE_AUTO || + _status.main_state == MAIN_STATE_SEATBELT || + _status.main_state == MAIN_STATE_EASY) { _actuators.control[0] = spdCmd; _actuators.control[1] = spdCmd; - } else if (_status.state_machine == SYSTEM_STATE_MANUAL) { - if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { + } else if (_status.main_state == MAIN_STATE_MANUAL) { + if (_status.navigation_state == NAVIGATION_STATE_DIRECT) { _actuators.control[CH_LEFT] = _manual.throttle; _actuators.control[CH_RIGHT] = _manual.pitch; - } else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { + } else if (_status.navigation_state == NAVIGATION_STATE_STABILIZE) { _actuators.control[0] = spdCmd; _actuators.control[1] = spdCmd; } From c4a1a338ff4378c908ec34c5a5f408c1b96d0735 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Tue, 22 Oct 2013 05:43:27 -0400 Subject: [PATCH 821/872] Changed driver to control motor duty cycle. --- src/drivers/roboclaw/RoboClaw.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index 73bd5dadac..d65a9be361 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -254,8 +254,8 @@ int RoboClaw::update() // if new data, send to motors if (_actuators.updated()) { _actuators.update(); - setMotorSpeed(MOTOR_1,_actuators.control[CH_VOLTAGE_LEFT]); - setMotorSpeed(MOTOR_2,_actuators.control[CH_VOLTAGE_RIGHT]); + setMotorDutyCycle(MOTOR_1,_actuators.control[CH_VOLTAGE_LEFT]); + setMotorDutyCycle(MOTOR_2,_actuators.control[CH_VOLTAGE_RIGHT]); } return 0; } From 28b4e978534e164d08125a9b0cf1fe428d9ad122 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 22 Oct 2013 21:01:30 +0200 Subject: [PATCH 822/872] Fixed bug with fd leak in rc_calibration_check --- src/modules/commander/commander.cpp | 4 ++-- src/modules/systemlib/rc_check.c | 4 +--- src/modules/systemlib/rc_check.h | 2 +- src/systemcmds/preflight_check/preflight_check.c | 4 ++-- 4 files changed, 6 insertions(+), 8 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2ef509980c..db758c3865 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -687,7 +687,7 @@ int commander_thread_main(int argc, char *argv[]) bool updated = false; - bool rc_calibration_ok = (OK == rc_calibration_check()); + bool rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd)); /* Subscribe to safety topic */ int safety_sub = orb_subscribe(ORB_ID(safety)); @@ -802,7 +802,7 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; /* re-check RC calibration */ - rc_calibration_ok = (OK == rc_calibration_check()); + rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd)); /* navigation parameters */ param_get(_param_takeoff_alt, &takeoff_alt); diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c index 60d6473b8c..b4350cc243 100644 --- a/src/modules/systemlib/rc_check.c +++ b/src/modules/systemlib/rc_check.c @@ -47,14 +47,12 @@ #include #include -int rc_calibration_check(void) { +int rc_calibration_check(int mavlink_fd) { 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 */ diff --git a/src/modules/systemlib/rc_check.h b/src/modules/systemlib/rc_check.h index e2238d1516..e70b83ccee 100644 --- a/src/modules/systemlib/rc_check.h +++ b/src/modules/systemlib/rc_check.h @@ -47,6 +47,6 @@ * @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); +__EXPORT int rc_calibration_check(int mavlink_fd); __END_DECLS diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index e9c5f1a2cf..1c58a2db69 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -140,7 +140,7 @@ int preflight_check_main(int argc, char *argv[]) /* ---- RC CALIBRATION ---- */ - bool rc_ok = (OK == rc_calibration_check()); + bool rc_ok = (OK == rc_calibration_check(mavlink_fd)); /* warn */ if (!rc_ok) @@ -227,4 +227,4 @@ static int led_off(int leds, int led) static int led_on(int leds, int led) { return ioctl(leds, LED_ON, led); -} \ No newline at end of file +} From 2f66a8894f1f8035bfc076306aa0d83197be108a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 22 Oct 2013 21:02:29 +0200 Subject: [PATCH 823/872] param_save_default() rewritten: don't try 10 times to do every operation but do it safe using temp file --- src/modules/systemlib/param/param.c | 95 ++++++++++++++--------------- 1 file changed, 47 insertions(+), 48 deletions(-) diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index ccdb2ea38b..398657dd79 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -508,64 +508,63 @@ param_get_default_file(void) int param_save_default(void) { - int result; - unsigned retries = 0; - - /* delete the file in case it exists */ - struct stat buffer; - if (stat(param_get_default_file(), &buffer) == 0) { - - do { - result = unlink(param_get_default_file()); - if (result != 0) { - retries++; - usleep(1000 * retries); - } - } while (result != OK && retries < 10); - - if (result != OK) - warnx("unlinking file %s failed.", param_get_default_file()); - } - - /* create the file */ + int res; int fd; - do { - /* do another attempt in case the unlink call is not synced yet */ - fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL); + const char *filename = param_get_default_file(); + const char *filename_tmp = malloc(strlen(filename) + 5); + sprintf(filename_tmp, "%s.tmp", filename); + + /* delete temp file if exist */ + res = unlink(filename_tmp); + + if (res != OK && errno == ENOENT) + res = OK; + + if (res != OK) + warn("failed to delete temp file: %s", filename_tmp); + + if (res == OK) { + /* write parameters to temp file */ + fd = open(filename_tmp, O_WRONLY | O_CREAT | O_EXCL); + if (fd < 0) { - retries++; - usleep(1000 * retries); + warn("failed to open temp file: %s", filename_tmp); + res = ERROR; } - } while (fd < 0 && retries < 10); + if (res == OK) { + res = param_export(fd, false); - if (fd < 0) { - - warn("opening '%s' for writing failed", param_get_default_file()); - return fd; - } - - do { - result = param_export(fd, false); - - if (result != OK) { - retries++; - usleep(1000 * retries); + if (res != OK) + warnx("failed to write parameters to file: %s", filename_tmp); } - } while (result != 0 && retries < 10); - - - close(fd); - - if (result != OK) { - warn("error exporting parameters to '%s'", param_get_default_file()); - (void)unlink(param_get_default_file()); - return result; + close(fd); } - return 0; + if (res == OK) { + /* delete parameters file */ + res = unlink(filename); + + if (res != OK && errno == ENOENT) + res = OK; + + if (res != OK) + warn("failed to delete parameters file: %s", filename); + } + + if (res == OK) { + /* rename temp file to parameters */ + res = rename(filename_tmp, filename); + + if (res != OK) + warn("failed to rename %s to %s", filename_tmp, filename); + } + + free(filename_tmp); + + return res; } /** From 342a7bf55b815241b98e775e16833ce5e9a48974 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 22 Oct 2013 22:21:10 +0200 Subject: [PATCH 824/872] esc_calib: get disarmed/max values from PWM device, more informative output --- src/systemcmds/esc_calib/esc_calib.c | 115 +++++++++++++++++---------- 1 file changed, 72 insertions(+), 43 deletions(-) diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c index 608c9fff17..1ea02d81e5 100644 --- a/src/systemcmds/esc_calib/esc_calib.c +++ b/src/systemcmds/esc_calib/esc_calib.c @@ -72,12 +72,13 @@ 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" - ); + + 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" + ); } @@ -98,12 +99,12 @@ esc_calib_main(int argc, char *argv[]) if (argc < 2) usage(NULL); - while ((ch = getopt(argc-1, argv, "d:")) != EOF) { + while ((ch = getopt(argc - 1, argv, "d:")) != EOF) { switch (ch) { - + case 'd': dev = optarg; - argc-=2; + argc -= 2; break; default: @@ -111,7 +112,7 @@ esc_calib_main(int argc, char *argv[]) } } - if(argc < 2) { + if (argc < 2) { usage("no channels provided"); } @@ -122,121 +123,149 @@ esc_calib_main(int argc, char *argv[]) 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; + channels_selected[channel_number - 1] = true; } } } printf("\nATTENTION, please remove or fix propellers before starting calibration!\n" - "\n" - "Make sure\n" - "\t - that the ESCs are not powered\n" - "\t - that safety is off (two short blinks)\n" - "\t - that the controllers are stopped\n" - "\n" - "Do you want to start calibration now: y or n?\n"); + "\n" + "Make sure\n" + "\t - that the ESCs are not powered\n" + "\t - that safety is off (two short blinks)\n" + "\t - that the controllers are stopped\n" + "\n" + "Do you want to start calibration now: y or n?\n"); /* wait for user input */ while (1) { - + ret = poll(&fds, 1, 0); + if (ret > 0) { read(0, &c, 1); + if (c == 'y' || c == 'Y') { - + break; + } else if (c == 0x03 || c == 0x63 || c == 'q') { printf("ESC calibration exited\n"); exit(0); + } else if (c == 'n' || c == 'N') { printf("ESC calibration aborted\n"); exit(0); + } else { printf("Unknown input, ESC calibration aborted\n"); 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); - - /* Wait for user confirmation */ - printf("\nHigh PWM set\n" - "\n" - "Connect battery now and hit ENTER after the ESCs confirm the first calibration step\n" - "\n"); + /* get max PWM value setting */ + uint16_t pwm_max = 0; + ret = ioctl(fd, PWM_SERVO_GET_MAX_PWM, &pwm_max); + + if (ret != OK) + err(1, "PWM_SERVO_GET_MAX_PWM"); + + /* get disarmed PWM value setting */ + uint16_t pwm_disarmed = 0; + ret = ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, &pwm_disarmed); + + if (ret != OK) + err(1, "PWM_SERVO_GET_DISARMED_PWM"); + + /* wait for user confirmation */ + printf("\nHigh PWM set: %d\n" + "\n" + "Connect battery now and hit ENTER after the ESCs confirm the first calibration step\n" + "\n", pwm_max); fflush(stdout); while (1) { + /* set max PWM */ + for (unsigned i = 0; i < MAX_CHANNELS; i++) { + if (channels_selected[i]) { + ret = ioctl(fd, PWM_SERVO_SET(i), pwm_max); - /* First set high PWM */ - for (unsigned i = 0; i 0) { read(0, &c, 1); + if (c == 13) { - + break; + } else if (c == 0x03 || c == 0x63 || c == 'q') { warnx("ESC calibration exited"); exit(0); } } + /* rate limit to ~ 20 Hz */ usleep(50000); } - /* we don't need any more user input */ - - - printf("Low PWM set, hit ENTER when finished\n" - "\n"); + printf("Low PWM set: %d\n" + "\n" + "Hit ENTER when finished\n" + "\n", pwm_disarmed); while (1) { - /* Then set low PWM */ - for (unsigned i = 0; i 0) { read(0, &c, 1); + if (c == 13) { - + break; + } else if (c == 0x03 || c == 0x63 || c == 'q') { printf("ESC calibration exited\n"); exit(0); } } + /* rate limit to ~ 20 Hz */ usleep(50000); } - printf("ESC calibration finished\n"); exit(0); From 3c6f43869178719abef90e5ef73e02dee952b1ce Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 23 Oct 2013 18:57:06 +0200 Subject: [PATCH 825/872] sdlog2: parameters logging implemented (APM-compatible) --- src/modules/sdlog2/sdlog2.c | 122 +++++++++++++++++++-------- src/modules/sdlog2/sdlog2_format.h | 8 +- src/modules/sdlog2/sdlog2_messages.h | 27 ++++-- 3 files changed, 110 insertions(+), 47 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 61a54170a3..f94875d5b8 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -58,6 +58,7 @@ #include #include #include +#include #include #include @@ -84,6 +85,7 @@ #include #include +#include #include @@ -181,12 +183,17 @@ static void sdlog2_stop_log(void); /** * Write a header to log file: list of message formats. */ -static void write_formats(int fd); +static int write_formats(int fd); /** * Write version message to log file. */ -static void write_version(int fd); +static int write_version(int fd); + +/** + * Write parameters to log file. + */ +static int write_parameters(int fd); static bool file_exist(const char *filename); @@ -242,11 +249,11 @@ int sdlog2_main(int argc, char *argv[]) main_thread_should_exit = false; deamon_task = task_spawn_cmd("sdlog2", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT - 30, - 3000, - sdlog2_thread_main, - (const char **)argv); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT - 30, + 3000, + sdlog2_thread_main, + (const char **)argv); exit(0); } @@ -359,13 +366,13 @@ static void *logwriter_thread(void *arg) struct logbuffer_s *logbuf = (struct logbuffer_s *)arg; - int log_file = open_logfile(); + int log_fd = open_logfile(); - /* write log messages formats */ - write_formats(log_file); - - /* write version */ - write_version(log_file); + /* write log messages formats, version and parameters */ + log_bytes_written += write_formats(log_fd); + log_bytes_written += write_version(log_fd); + log_bytes_written += write_parameters(log_fd); + fsync(log_fd); int poll_count = 0; @@ -404,7 +411,7 @@ static void *logwriter_thread(void *arg) n = available; } - n = write(log_file, read_ptr, n); + n = write(log_fd, read_ptr, n); should_wait = (n == available) && !is_part; @@ -419,21 +426,23 @@ static void *logwriter_thread(void *arg) } else { n = 0; + /* exit only with empty buffer */ if (main_thread_should_exit || logwriter_should_exit) { break; } + should_wait = true; } if (++poll_count == 10) { - fsync(log_file); + fsync(log_fd); poll_count = 0; } } - fsync(log_file); - close(log_file); + fsync(log_fd); + close(log_fd); return OK; } @@ -487,15 +496,17 @@ void sdlog2_stop_log() /* wait for write thread to return */ int ret; + if ((ret = pthread_join(logwriter_pthread, NULL)) != 0) { warnx("error joining logwriter thread: %i", ret); } + logwriter_pthread = 0; sdlog2_status(); } -void write_formats(int fd) +int write_formats(int fd) { /* construct message format packet */ struct { @@ -505,35 +516,72 @@ void write_formats(int fd) LOG_PACKET_HEADER_INIT(LOG_FORMAT_MSG), }; - /* fill message format packet for each format and write to log */ - int i; + int written = 0; - for (i = 0; i < log_formats_num; i++) { + /* fill message format packet for each format and write it */ + for (int i = 0; i < log_formats_num; i++) { log_msg_format.body = log_formats[i]; - log_bytes_written += write(fd, &log_msg_format, sizeof(log_msg_format)); + written += write(fd, &log_msg_format, sizeof(log_msg_format)); } - fsync(fd); + return written; } -void write_version(int fd) +int write_version(int fd) { /* construct version message */ struct { LOG_PACKET_HEADER; struct log_VER_s body; } log_msg_VER = { - LOG_PACKET_HEADER_INIT(127), + LOG_PACKET_HEADER_INIT(LOG_VER_MSG), }; - /* fill message format packet for each format and write to log */ - int i; - + /* fill version message and write it */ strncpy(log_msg_VER.body.fw_git, FW_GIT, sizeof(log_msg_VER.body.fw_git)); strncpy(log_msg_VER.body.arch, HW_ARCH, sizeof(log_msg_VER.body.arch)); - log_bytes_written += write(fd, &log_msg_VER, sizeof(log_msg_VER)); + return write(fd, &log_msg_VER, sizeof(log_msg_VER)); +} - fsync(fd); +int write_parameters(int fd) +{ + /* construct parameter message */ + struct { + LOG_PACKET_HEADER; + struct log_PARM_s body; + } log_msg_PARM = { + LOG_PACKET_HEADER_INIT(LOG_PARM_MSG), + }; + + int written = 0; + param_t params_cnt = param_count(); + + for (param_t param = 0; param < params_cnt; param++) { + /* fill parameter message and write it */ + strncpy(log_msg_PARM.body.name, param_name(param), sizeof(log_msg_PARM.body.name)); + float value = NAN; + + switch (param_type(param)) { + case PARAM_TYPE_INT32: { + int32_t i; + param_get(param, &i); + value = i; // cast integer to float + break; + } + + case PARAM_TYPE_FLOAT: + param_get(param, &value); + break; + + default: + break; + } + + log_msg_PARM.body.value = value; + written += write(fd, &log_msg_PARM, sizeof(log_msg_PARM)); + } + + return written; } int sdlog2_thread_main(int argc, char *argv[]) @@ -611,12 +659,13 @@ int sdlog2_thread_main(int argc, char *argv[]) } const char *converter_in = "/etc/logging/conv.zip"; - char* converter_out = malloc(120); + char *converter_out = malloc(120); sprintf(converter_out, "%s/conv.zip", folder_path); if (file_copy(converter_in, converter_out)) { errx(1, "unable to copy conversion scripts, exiting."); } + free(converter_out); /* only print logging path, important to find log file later */ @@ -630,6 +679,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } struct vehicle_status_s buf_status; + memset(&buf_status, 0, sizeof(buf_status)); /* warning! using union here to save memory, elements should be used separately! */ @@ -655,6 +705,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct esc_status_s esc; struct vehicle_global_velocity_setpoint_s global_vel_sp; } buf; + memset(&buf, 0, sizeof(buf)); struct { @@ -825,7 +876,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); fds[fdsc_count].fd = subs.airspeed_sub; fds[fdsc_count].events = POLLIN; - fdsc_count++; + fdsc_count++; /* --- ESCs --- */ subs.esc_sub = orb_subscribe(ORB_ID(esc_status)); @@ -913,7 +964,7 @@ int sdlog2_thread_main(int argc, char *argv[]) continue; } - ifds = 1; // Begin from fds[1] again + ifds = 1; // begin from fds[1] again pthread_mutex_lock(&logbuffer_mutex); @@ -1172,8 +1223,8 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- ESCs --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(esc_status), subs.esc_sub, &buf.esc); - for (uint8_t i=0; iarming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR; + if (armed != flag_system_armed) { flag_system_armed = armed; diff --git a/src/modules/sdlog2/sdlog2_format.h b/src/modules/sdlog2/sdlog2_format.h index 5c175ef7ef..dc5e6c8bd4 100644 --- a/src/modules/sdlog2/sdlog2_format.h +++ b/src/modules/sdlog2/sdlog2_format.h @@ -85,10 +85,10 @@ struct log_format_s { #define LOG_FORMAT(_name, _format, _labels) { \ .type = LOG_##_name##_MSG, \ - .length = sizeof(struct log_##_name##_s) + LOG_PACKET_HEADER_LEN, \ - .name = #_name, \ - .format = _format, \ - .labels = _labels \ + .length = sizeof(struct log_##_name##_s) + LOG_PACKET_HEADER_LEN, \ + .name = #_name, \ + .format = _format, \ + .labels = _labels \ } #define LOG_FORMAT_MSG 0x80 diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 9f709e4591..90093a407c 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -48,12 +48,6 @@ /* define message formats */ #pragma pack(push, 1) -/* --- TIME - TIME STAMP --- */ -#define LOG_TIME_MSG 1 -struct log_TIME_s { - uint64_t t; -}; - /* --- ATT - ATTITUDE --- */ #define LOG_ATT_MSG 2 struct log_ATT_s { @@ -253,18 +247,31 @@ struct log_GVSP_s { float vz; }; +/* --- TIME - TIME STAMP --- */ +#define LOG_TIME_MSG 129 +struct log_TIME_s { + uint64_t t; +}; + /* --- VER - VERSION --- */ -#define LOG_VER_MSG 127 +#define LOG_VER_MSG 130 struct log_VER_s { char arch[16]; char fw_git[64]; }; +/* --- PARM - PARAMETER --- */ +#define LOG_PARM_MSG 131 +struct log_PARM_s { + char name[16]; + float value; +}; + #pragma pack(pop) /* construct list of all message formats */ static const struct log_format_s log_formats[] = { - LOG_FORMAT(TIME, "Q", "StartTime"), + /* business-level messages, ID < 0x80 */ LOG_FORMAT(ATT, "ffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate"), LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), @@ -283,7 +290,11 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"), LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), + /* system-level messages, ID >= 0x80 */ + // FMT: don't write format of format message, it's useless + LOG_FORMAT(TIME, "Q", "StartTime"), LOG_FORMAT(VER, "NZ", "Arch,FwGit"), + LOG_FORMAT(PARM, "Nf", "Name,Value"), }; static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); From bb8a2c3631a518f822d1e7e3d40c28a281901a3f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 23 Oct 2013 18:57:44 +0200 Subject: [PATCH 826/872] sdlog2_dump.py: C strings parsing fixed --- Tools/sdlog2_dump.py | 25 ++++++++++++------------- 1 file changed, 12 insertions(+), 13 deletions(-) diff --git a/Tools/sdlog2_dump.py b/Tools/sdlog2_dump.py index 7fefc5908f..5b1e55e22a 100644 --- a/Tools/sdlog2_dump.py +++ b/Tools/sdlog2_dump.py @@ -25,8 +25,12 @@ import struct, sys if sys.hexversion >= 0x030000F0: runningPython3 = True + def _parseCString(cstr): + return str(cstr, 'ascii').split('\0')[0] else: runningPython3 = False + def _parseCString(cstr): + return str(cstr).split('\0')[0] class SDLog2Parser: BLOCK_SIZE = 8192 @@ -175,9 +179,9 @@ class SDLog2Parser: self.__csv_columns.append(full_label) self.__csv_data[full_label] = None if self.__file != None: - print(self.__csv_delim.join(self.__csv_columns), file=self.__file) + print(self.__csv_delim.join(self.__csv_columns), file=self.__file) else: - print(self.__csv_delim.join(self.__csv_columns)) + print(self.__csv_delim.join(self.__csv_columns)) def __printCSVRow(self): s = [] @@ -190,9 +194,9 @@ class SDLog2Parser: s.append(v) if self.__file != None: - print(self.__csv_delim.join(s), file=self.__file) + print(self.__csv_delim.join(s), file=self.__file) else: - print(self.__csv_delim.join(s)) + print(self.__csv_delim.join(s)) def __parseMsgDescr(self): if runningPython3: @@ -202,14 +206,9 @@ class SDLog2Parser: msg_type = data[0] if msg_type != self.MSG_TYPE_FORMAT: msg_length = data[1] - if runningPython3: - msg_name = str(data[2], 'ascii').strip("\0") - msg_format = str(data[3], 'ascii').strip("\0") - msg_labels = str(data[4], 'ascii').strip("\0").split(",") - else: - msg_name = str(data[2]).strip("\0") - msg_format = str(data[3]).strip("\0") - msg_labels = str(data[4]).strip("\0").split(",") + msg_name = _parseCString(data[2]) + msg_format = _parseCString(data[3]) + msg_labels = _parseCString(data[4]).split(",") # Convert msg_format to struct.unpack format string msg_struct = "" msg_mults = [] @@ -243,7 +242,7 @@ class SDLog2Parser: data = list(struct.unpack(msg_struct, str(self.__buffer[self.__ptr+self.MSG_HEADER_LEN:self.__ptr+msg_length]))) for i in range(len(data)): if type(data[i]) is str: - data[i] = data[i].strip("\0") + data[i] = _parseCString(data[i]) m = msg_mults[i] if m != None: data[i] = data[i] * m From 28845c4846e4f225e686f6643b5d2e0851392b2b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 19 Oct 2013 09:45:34 +0200 Subject: [PATCH 827/872] Rascal (AERT) hil startup script --- ...1000_rc_fw.hil => 1000_rc_fw_easystar.hil} | 0 .../init.d/1004_rc_fw_Rascal110.hil | 103 ++++++++++++++++++ ROMFS/px4fmu_common/init.d/rcS | 8 +- 3 files changed, 110 insertions(+), 1 deletion(-) rename ROMFS/px4fmu_common/init.d/{1000_rc_fw.hil => 1000_rc_fw_easystar.hil} (100%) create mode 100644 ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil diff --git a/ROMFS/px4fmu_common/init.d/1000_rc_fw.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil similarity index 100% rename from ROMFS/px4fmu_common/init.d/1000_rc_fw.hil rename to ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil diff --git a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil new file mode 100644 index 0000000000..8c5e4b6a8f --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil @@ -0,0 +1,103 @@ +#!nsh +# +# USB HIL start +# + +echo "[HIL] HILStar starting.." + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 15 + param set FW_P_LIM_MAX 50 + param set FW_P_LIM_MIN -50 + param set FW_P_P 60 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 1.1 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 100 + param set FW_R_RMAX 100 + param set FW_THR_CRUISE 0.65 + param set FW_THR_MAX 1 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5.0 + param set FW_T_SINK_MIN 4.0 + param set FW_Y_ROLLFF 1.1 + param set FW_L1_PERIOD 16 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 + + param set SYS_AUTOCONFIG 0 + param save +fi + +# Allow USB some time to come up +sleep 1 +# Tell MAVLink that this link is "fast" +mavlink start -b 230400 -d /dev/ttyACM0 + +# Create a fake HIL /dev/pwm_output interface +hil mode_pwm + +# +# 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 commander (depends on orb, mavlink) +# +commander start + +# +# Check if we got an IO +# +if px4io start +then + echo "IO started" +else + fmu mode_serial + echo "FMU started" +fi + +# +# Start the sensors (depends on orb, px4io) +# +sh /etc/init.d/rc.sensors + +# +# Start the attitude estimator (depends on orb) +# +att_pos_estimator_ekf start + +# +# Load mixer and start controllers (depends on px4io) +# +set MODE autostart +mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix +if [ -f /fs/microsd/etc/mixers/FMU_AERT.mix ] +then + echo "Using /fs/microsd/etc/mixers/FMU_AERT.mix" + mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_AERT.mix +else + echo "Using /etc/mixers/FMU_AERT.mix" + mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix +fi + + +fw_pos_control_l1 start +fw_att_control start + +echo "[HIL] setup done, running" + diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 424787d54a..94c01419e3 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -108,7 +108,7 @@ then if param compare SYS_AUTOSTART 1000 then - sh /etc/init.d/1000_rc_fw.hil + sh /etc/init.d/1000_rc_fw_easystar.hil set MODE custom fi @@ -129,6 +129,12 @@ then sh /etc/init.d/1003_rc_quad_+.hil set MODE custom fi + + if param compare SYS_AUTOSTART 1004 + then + sh /etc/init.d/1004_rc_fw_Rascal110.hil + set MODE custom + fi if [ $MODE != custom ] then From 2cd9ad97eaf20c43a4c519413b258712d844f1d1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 24 Oct 2013 09:26:03 +0200 Subject: [PATCH 828/872] Removed unnecessary return statements --- src/drivers/ets_airspeed/ets_airspeed.cpp | 3 --- src/drivers/meas_airspeed/meas_airspeed.cpp | 3 --- 2 files changed, 6 deletions(-) diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index 084671ae20..9d8ad084e5 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -132,11 +132,8 @@ ETSAirspeed::measure() if (OK != ret) { perf_count(_comms_errors); log("i2c::transfer returned %d", ret); - return ret; } - ret = OK; - return ret; } diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index ee45d46acb..b3003fc1b0 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -138,11 +138,8 @@ MEASAirspeed::measure() if (OK != ret) { perf_count(_comms_errors); - return ret; } - ret = OK; - return ret; } From cc324f2624cc3a4347d736fd5634672d5a2716e9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 24 Oct 2013 09:28:14 +0200 Subject: [PATCH 829/872] Ignoring docs output --- Tools/px4params/.gitignore | 2 ++ 1 file changed, 2 insertions(+) create mode 100644 Tools/px4params/.gitignore diff --git a/Tools/px4params/.gitignore b/Tools/px4params/.gitignore new file mode 100644 index 0000000000..73cf39575d --- /dev/null +++ b/Tools/px4params/.gitignore @@ -0,0 +1,2 @@ +parameters.wiki +parameters.xml \ No newline at end of file From 1cb73687f7acd4ae3263c40940afe057b1b7d368 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 20 Oct 2013 21:51:45 +0200 Subject: [PATCH 830/872] added parameter for maximal roll angle --- src/lib/ecl/l1/ecl_l1_pos_controller.cpp | 2 +- src/lib/ecl/l1/ecl_l1_pos_controller.h | 11 +++++++++++ .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 5 +++++ .../fw_pos_control_l1/fw_pos_control_l1_params.c | 3 +++ 4 files changed, 20 insertions(+), 1 deletion(-) diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp index daf136d495..196ded26ca 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp @@ -43,7 +43,7 @@ float ECL_L1_Pos_Controller::nav_roll() { float ret = atanf(_lateral_accel * 1.0f / CONSTANTS_ONE_G); - ret = math::constrain(ret, (-M_PI_F) / 2.0f, M_PI_F / 2.0f); + ret = math::constrain(ret, -_roll_lim_rad, _roll_lim_rad); return ret; } diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.h b/src/lib/ecl/l1/ecl_l1_pos_controller.h index 5a17346cb1..7a3c42a925 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.h +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.h @@ -222,6 +222,15 @@ public: _K_L1 = 4.0f * _L1_damping * _L1_damping; } + + /** + * Set the maximum roll angle output in radians + * + */ + void set_l1_roll_limit(float roll_lim_rad) { + _roll_lim_rad = roll_lim_rad; + } + private: float _lateral_accel; ///< Lateral acceleration setpoint in m/s^2 @@ -238,6 +247,8 @@ private: float _K_L1; ///< L1 control gain for _L1_damping float _heading_omega; ///< Normalized frequency + float _roll_lim_rad; /// Date: Sun, 29 Sep 2013 19:00:12 +0200 Subject: [PATCH 831/872] adding skywalker x5 startup script --- ROMFS/px4fmu_common/init.d/32_skywalker_x5 | 86 ++++++++++++++++++++++ ROMFS/px4fmu_common/init.d/rcS | 6 ++ 2 files changed, 92 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/32_skywalker_x5 diff --git a/ROMFS/px4fmu_common/init.d/32_skywalker_x5 b/ROMFS/px4fmu_common/init.d/32_skywalker_x5 new file mode 100644 index 0000000000..432cbb14e2 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/32_skywalker_x5 @@ -0,0 +1,86 @@ +#!nsh + +echo "[init] PX4FMU v1, v2 with or without IO on Skywalker X5" + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + # TODO + + param set SYS_AUTOCONFIG 0 + param save +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing +# +param set MAV_TYPE 1 + +set EXIT_ON_END no + +# +# Start and configure PX4IO or FMU interface +# +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start + + commander start + + sh /etc/init.d/rc.io + # Limit to 100 Hz updates and (implicit) 50 Hz PWM + px4io limit 100 +else + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + + commander start + + fmu mode_pwm + param set BAT_V_SCALING 0.004593 + set EXIT_ON_END yes +fi + +# +# Start the sensors and test them. +# +sh /etc/init.d/rc.sensors + +# +# Start logging (depends on sensors) +# +sh /etc/init.d/rc.logging + +# +# Start GPS interface (depends on orb) +# +gps start + +# +# Start the attitude and position estimator +# +att_pos_estimator_ekf start + +# +# Load mixer and start controllers (depends on px4io) +# +if [ -f /fs/microsd/etc/mixers/FMU_delta.mix ] +then + echo "Using FMU_delta mixer from sd card" + mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_delta.mix +else + echo "Using standard FMU_delta mixer" + mixer load /dev/pwm_output /etc/mixers/FMU_delta.mix +fi +fw_att_control start +fw_pos_control_l1 start + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 94c01419e3..9d0800eb17 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -284,6 +284,12 @@ then sh /etc/init.d/31_io_phantom set MODE custom fi + + if param compare SYS_AUTOSTART 32 + then + sh /etc/init.d/32_skywalker_x5 + set MODE custom + fi if param compare SYS_AUTOSTART 40 then From 8193382ec245ff82ffb30e2d9038c253bc93a099 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Oct 2013 17:25:14 +0200 Subject: [PATCH 832/872] change default mixer for skywalker x5 --- ROMFS/px4fmu_common/init.d/32_skywalker_x5 | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/32_skywalker_x5 b/ROMFS/px4fmu_common/init.d/32_skywalker_x5 index 432cbb14e2..cd7677112d 100644 --- a/ROMFS/px4fmu_common/init.d/32_skywalker_x5 +++ b/ROMFS/px4fmu_common/init.d/32_skywalker_x5 @@ -69,13 +69,13 @@ att_pos_estimator_ekf start # # Load mixer and start controllers (depends on px4io) # -if [ -f /fs/microsd/etc/mixers/FMU_delta.mix ] +if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ] then - echo "Using FMU_delta mixer from sd card" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_delta.mix + echo "Using /fs/microsd/etc/mixers/FMU_Q.mix" + mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix else - echo "Using standard FMU_delta mixer" - mixer load /dev/pwm_output /etc/mixers/FMU_delta.mix + echo "Using /etc/mixers/FMU_Q.mix" + mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix fi fw_att_control start fw_pos_control_l1 start From 2733a54fe201e060ad08aac8dfd02caeed12b08c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 26 Oct 2013 14:43:34 +0400 Subject: [PATCH 833/872] missionlib: waypoint yaw fix --- src/modules/mavlink/missionlib.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c index e8d707948a..d37b18a195 100644 --- a/src/modules/mavlink/missionlib.c +++ b/src/modules/mavlink/missionlib.c @@ -220,7 +220,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, sp.lon = param6_lon_y * 1e7f; sp.altitude = param7_alt_z; sp.altitude_is_relative = false; - sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; + sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F); set_special_fields(param1, param2, param3, param4, command, &sp); /* Initialize setpoint publication if necessary */ From 4b63c5488582241c7e3af45dce74c112dbfa60bd Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 27 Oct 2013 14:48:12 +0100 Subject: [PATCH 834/872] l1: fix constrain of sine_eta1 --- src/lib/ecl/l1/ecl_l1_pos_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp index 196ded26ca..27d76f959c 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp @@ -190,7 +190,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c float xtrackErr = vector_A_to_airplane % vector_AB; float sine_eta1 = xtrackErr / math::max(_L1_distance , 0.1f); /* limit output to 45 degrees */ - sine_eta1 = math::constrain(sine_eta1, -M_PI_F / 4.0f, +M_PI_F / 4.0f); + sine_eta1 = math::constrain(sine_eta1, -0.7071f, 0.7071f); //sin(pi/4) = 0.7071 float eta1 = asinf(sine_eta1); eta = eta1 + eta2; /* bearing from current position to L1 point */ From 9064f8bf09dd91388b9fd3e66568d086bf1be69b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 28 Oct 2013 15:49:26 +1100 Subject: [PATCH 835/872] px4io: fixed the io_reg_{set,get} errors this fixes the PX4IO state machine to avoid the io errors we were seeing. There are still some open questions with this code, but it now seems to give zero errors, which is an improvement! --- src/modules/px4iofirmware/i2c.c | 6 ------ src/modules/px4iofirmware/px4io.c | 10 ++-------- src/modules/px4iofirmware/serial.c | 20 +++----------------- 3 files changed, 5 insertions(+), 31 deletions(-) diff --git a/src/modules/px4iofirmware/i2c.c b/src/modules/px4iofirmware/i2c.c index 10aeb5c9fa..79b6546b39 100644 --- a/src/modules/px4iofirmware/i2c.c +++ b/src/modules/px4iofirmware/i2c.c @@ -149,12 +149,6 @@ interface_init(void) #endif } -void -interface_tick() -{ -} - - /* reset the I2C bus used to recover from lockups diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index e70b3fe880..e28106971a 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -159,9 +159,6 @@ user_start(int argc, char *argv[]) /* 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,11 +200,6 @@ 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(); @@ -218,6 +210,7 @@ user_start(int argc, char *argv[]) controls_tick(); perf_end(controls_perf); +#if 0 /* check for debug activity */ show_debug_messages(); @@ -234,6 +227,7 @@ user_start(int argc, char *argv[]) (unsigned)minfo.mxordblk); last_debug_time = hrt_absolute_time(); } +#endif } } diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index 94d7407dff..e9adc8cd69 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -74,9 +74,6 @@ 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; - static struct IOPacket dma_packet; /* serial register accessors */ @@ -150,16 +147,6 @@ interface_init(void) debug("serial init"); } -void -interface_tick() -{ - /* XXX look for stuck/damaged DMA and reset? */ - if (idle_ticks++ > 100) { - dma_reset(); - idle_ticks = 0; - } -} - static void rx_handle_packet(void) { @@ -230,9 +217,6 @@ 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(); @@ -308,6 +292,7 @@ serial_interrupt(int irq, void *context) /* it was too short - possibly truncated */ perf_count(pc_badidle); + dma_reset(); return 0; } @@ -343,7 +328,8 @@ dma_reset(void) sizeof(dma_packet), DMA_CCR_MINC | DMA_CCR_PSIZE_8BITS | - DMA_CCR_MSIZE_8BITS); + DMA_CCR_MSIZE_8BITS | + DMA_CCR_PRIVERYHI); /* start receive DMA ready for the next packet */ stm32_dmastart(rx_dma, rx_dma_callback, NULL, false); From 75a0c18a9e56ac64e043cce00223ea8a627d24a5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 28 Oct 2013 15:49:57 +1100 Subject: [PATCH 836/872] px4io: FMU half of px4io error fixes --- src/drivers/px4io/px4io_serial.cpp | 28 +++++++++++++++++----------- 1 file changed, 17 insertions(+), 11 deletions(-) diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp index 236cb918dc..43318ca849 100644 --- a/src/drivers/px4io/px4io_serial.cpp +++ b/src/drivers/px4io/px4io_serial.cpp @@ -48,6 +48,7 @@ #include #include #include +#include #include @@ -262,7 +263,8 @@ PX4IO_serial::init() up_enable_irq(PX4IO_SERIAL_VECTOR); /* enable UART in DMA mode, enable error and line idle interrupts */ - /* rCR3 = USART_CR3_EIE;*/ + rCR3 = USART_CR3_EIE; + rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE; /* create semaphores */ @@ -497,22 +499,20 @@ PX4IO_serial::_wait_complete() DMA_SCR_PBURST_SINGLE | DMA_SCR_MBURST_SINGLE); stm32_dmastart(_tx_dma, nullptr, nullptr, false); + //rCR1 &= ~USART_CR1_TE; + //rCR1 |= USART_CR1_TE; rCR3 |= USART_CR3_DMAT; perf_end(_pc_dmasetup); - /* compute the deadline for a 5ms timeout */ + /* compute the deadline for a 10ms timeout */ struct timespec abstime; clock_gettime(CLOCK_REALTIME, &abstime); -#if 0 - abstime.tv_sec++; /* long timeout for testing */ -#else - abstime.tv_nsec += 10000000; /* 0ms timeout */ - if (abstime.tv_nsec > 1000000000) { + abstime.tv_nsec += 10*1000*1000; + if (abstime.tv_nsec >= 1000*1000*1000) { abstime.tv_sec++; - abstime.tv_nsec -= 1000000000; + abstime.tv_nsec -= 1000*1000*1000; } -#endif /* wait for the transaction to complete - 64 bytes @ 1.5Mbps ~426µs */ int ret; @@ -619,8 +619,8 @@ PX4IO_serial::_do_interrupt() */ if (_rx_dma_status == _dma_status_waiting) { _abort_dma(); - perf_count(_pc_uerrs); + perf_count(_pc_uerrs); /* complete DMA as though in error */ _do_rx_dma_callback(DMA_STATUS_TEIF); @@ -642,6 +642,12 @@ PX4IO_serial::_do_interrupt() unsigned length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma); if ((length < 1) || (length < PKT_SIZE(_dma_buffer))) { perf_count(_pc_badidle); + + /* stop the receive DMA */ + stm32_dmastop(_rx_dma); + + /* complete the short reception */ + _do_rx_dma_callback(DMA_STATUS_TEIF); return; } @@ -670,4 +676,4 @@ PX4IO_serial::_abort_dma() stm32_dmastop(_rx_dma); } -#endif /* PX4IO_SERIAL_BASE */ \ No newline at end of file +#endif /* PX4IO_SERIAL_BASE */ From 52ee477137a293d40fc552f4100c52d19b142fc1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 28 Oct 2013 15:50:33 +1100 Subject: [PATCH 837/872] lsm303d: try to reset the lsm303d if it goes bad in flight this is based on earlier work by Julian Oes --- src/drivers/lsm303d/lsm303d.cpp | 32 +++++++++++++++++++++++++++++--- 1 file changed, 29 insertions(+), 3 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 8bed8a8df0..60601e22c8 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -241,11 +241,18 @@ private: perf_counter_t _accel_sample_perf; perf_counter_t _mag_sample_perf; + perf_counter_t _reg7_resets; + perf_counter_t _reg1_resets; math::LowPassFilter2p _accel_filter_x; math::LowPassFilter2p _accel_filter_y; math::LowPassFilter2p _accel_filter_z; + // expceted values of reg1 and reg7 to catch in-flight + // brownouts of the sensor + uint8_t _reg7_expected; + uint8_t _reg1_expected; + /** * Start automatic measurement. */ @@ -434,9 +441,13 @@ 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")), + _reg1_resets(perf_alloc(PC_COUNT, "lsm303d_reg1_resets")), + _reg7_resets(perf_alloc(PC_COUNT, "lsm303d_reg7_resets")), _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) + _accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _reg1_expected(0), + _reg7_expected(0) { // enable debug() calls _debug_enabled = true; @@ -526,10 +537,12 @@ 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); + _reg1_expected = REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A; + write_reg(ADDR_CTRL_REG1, _reg1_expected); /* enable mag */ - write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); + _reg7_expected = REG7_CONT_MODE_M; + write_reg(ADDR_CTRL_REG7, _reg7_expected); write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); @@ -1133,6 +1146,7 @@ LSM303D::accel_set_samplerate(unsigned frequency) } modify_reg(ADDR_CTRL_REG1, clearbits, setbits); + _reg1_expected = (_reg1_expected & ~clearbits) | setbits; return OK; } @@ -1210,6 +1224,12 @@ LSM303D::mag_measure_trampoline(void *arg) void LSM303D::measure() { + if (read_reg(ADDR_CTRL_REG1) != _reg1_expected) { + perf_count(_reg1_resets); + reset(); + return; + } + /* status register and data as read back from the device */ #pragma pack(push, 1) @@ -1282,6 +1302,12 @@ LSM303D::measure() void LSM303D::mag_measure() { + if (read_reg(ADDR_CTRL_REG7) != _reg7_expected) { + perf_count(_reg7_resets); + reset(); + return; + } + /* status register and data as read back from the device */ #pragma pack(push, 1) struct { From 1336d625a83ba3f1afc207b64ec248dd1e15848a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Oct 2013 14:47:37 +0100 Subject: [PATCH 838/872] Hotfix: Announcing important messages via audio --- src/modules/commander/commander.cpp | 58 ++++++++++++++--------------- 1 file changed, 29 insertions(+), 29 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 9814a7bccf..44a1442968 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -379,7 +379,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } // TODO remove debug code - //mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode); + //mavlink_log_critical(mavlink_fd, "#audio: command setmode: %d %d", base_mode, custom_main_mode); /* set arming state */ arming_res = TRANSITION_NOT_CHANGED; @@ -496,11 +496,11 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } if (arming_res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] ARMED by component arm cmd"); + mavlink_log_critical(mavlink_fd, "#audio: ARMED by component arm cmd"); result = VEHICLE_CMD_RESULT_ACCEPTED; } else { - mavlink_log_info(mavlink_fd, "[cmd] REJECTING component arm cmd"); + mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd"); result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } } @@ -521,13 +521,13 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe tune_negative(); if (result == VEHICLE_CMD_RESULT_DENIED) { - mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd->command); + mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd->command); } else if (result == VEHICLE_CMD_RESULT_FAILED) { - mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd->command); + mavlink_log_critical(mavlink_fd, "#audio: 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); + mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd->command); } } @@ -874,10 +874,10 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; if (status.condition_landed) { - mavlink_log_critical(mavlink_fd, "[cmd] LANDED"); + mavlink_log_critical(mavlink_fd, "#audio: LANDED"); } else { - mavlink_log_critical(mavlink_fd, "[cmd] IN AIR"); + mavlink_log_critical(mavlink_fd, "#audio: IN AIR"); } } } @@ -955,7 +955,7 @@ int commander_thread_main(int argc, char *argv[]) //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, "#audio: WARNING: LOW BATTERY"); status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; status_changed = true; battery_tune_played = false; @@ -967,7 +967,7 @@ int commander_thread_main(int argc, char *argv[]) /* 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, "#audio: EMERGENCY: CRITICAL BATTERY"); status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; battery_tune_played = false; @@ -1069,12 +1069,12 @@ int commander_thread_main(int argc, char *argv[]) /* 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"); + mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time"); status_changed = true; } else { if (status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "[cmd] RC signal regained"); + mavlink_log_critical(mavlink_fd, "#audio: RC signal regained"); status_changed = true; } } @@ -1143,7 +1143,7 @@ int commander_thread_main(int argc, char *argv[]) } else if (res == TRANSITION_DENIED) { 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); + mavlink_log_critical(mavlink_fd, "#audio: 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 */ @@ -1159,12 +1159,12 @@ int commander_thread_main(int argc, char *argv[]) } 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); + mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); } } else { if (!status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: RC SIGNAL LOST"); + mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); status.rc_signal_lost = true; status_changed = true; } @@ -1189,7 +1189,7 @@ int commander_thread_main(int argc, char *argv[]) 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); + mavlink_log_critical(mavlink_fd, "#audio: 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 */ @@ -1506,7 +1506,7 @@ print_reject_mode(const char *msg) 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); + sprintf(s, "#audio: warning: reject %s", msg); mavlink_log_critical(mavlink_fd, s); tune_negative(); } @@ -1520,7 +1520,7 @@ print_reject_arm(const char *msg) 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); + sprintf(s, "#audio: %s", msg); mavlink_log_critical(mavlink_fd, s); tune_negative(); } @@ -1617,14 +1617,14 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c if (res == TRANSITION_CHANGED) { if (control_mode->flag_control_position_enabled) { - mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: POS HOLD"); + mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: POS HOLD"); } else { if (status->condition_landed) { - mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: ALT HOLD (LAND)"); + mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: ALT HOLD (LAND)"); } else { - mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: ALT HOLD"); + mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: ALT HOLD"); } } } @@ -1660,22 +1660,22 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul break; case VEHICLE_CMD_RESULT_DENIED: - mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command); + mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd.command); tune_negative(); break; case VEHICLE_CMD_RESULT_FAILED: - mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command); + mavlink_log_critical(mavlink_fd, "#audio: 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); + mavlink_log_critical(mavlink_fd, "#audio: 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); + mavlink_log_critical(mavlink_fd, "#audio: command unsupported: %u", cmd.command); tune_negative(); break; @@ -1814,14 +1814,14 @@ void *commander_low_prio_loop(void *arg) answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); } else { - mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); + mavlink_log_critical(mavlink_fd, "#audio: parameters load ERROR"); /* convenience as many parts of NuttX use negative errno */ if (ret < 0) ret = -ret; if (ret < 1000) - mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret)); + mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret)); answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); } @@ -1834,14 +1834,14 @@ void *commander_low_prio_loop(void *arg) answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); } else { - mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); + mavlink_log_critical(mavlink_fd, "#audio: parameters save error"); /* convenience as many parts of NuttX use negative errno */ if (ret < 0) ret = -ret; if (ret < 1000) - mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret)); + mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret)); answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); } From 0fa03e65ab3ab0e173e487b3e5f5321780f3afff Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Oct 2013 15:21:50 +0100 Subject: [PATCH 839/872] Cleanup of Doxygen tags --- src/drivers/drv_accel.h | 4 +++- src/drivers/drv_airspeed.h | 5 ++++- src/drivers/drv_baro.h | 4 +++- src/drivers/drv_gps.h | 4 +++- src/drivers/drv_gyro.h | 4 +++- src/drivers/drv_rc_input.h | 3 ++- src/drivers/drv_sensor.h | 4 +++- src/drivers/drv_tone_alarm.h | 4 +++- 8 files changed, 24 insertions(+), 8 deletions(-) diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h index 8a4f684284..7d93ed9383 100644 --- a/src/drivers/drv_accel.h +++ b/src/drivers/drv_accel.h @@ -32,7 +32,9 @@ ****************************************************************************/ /** - * @file Accelerometer driver interface. + * @file drv_accel.h + * + * Accelerometer driver interface. */ #ifndef _DRV_ACCEL_H diff --git a/src/drivers/drv_airspeed.h b/src/drivers/drv_airspeed.h index 7bb9ee2af1..78f31495d9 100644 --- a/src/drivers/drv_airspeed.h +++ b/src/drivers/drv_airspeed.h @@ -32,7 +32,10 @@ ****************************************************************************/ /** - * @file Airspeed driver interface. + * @file drv_airspeed.h + * + * Airspeed driver interface. + * * @author Simon Wilks */ diff --git a/src/drivers/drv_baro.h b/src/drivers/drv_baro.h index aa251889f7..e2f0137ae2 100644 --- a/src/drivers/drv_baro.h +++ b/src/drivers/drv_baro.h @@ -32,7 +32,9 @@ ****************************************************************************/ /** - * @file Barometric pressure sensor driver interface. + * @file drv_baro.h + * + * Barometric pressure sensor driver interface. */ #ifndef _DRV_BARO_H diff --git a/src/drivers/drv_gps.h b/src/drivers/drv_gps.h index 398cf48701..06e3535b30 100644 --- a/src/drivers/drv_gps.h +++ b/src/drivers/drv_gps.h @@ -32,7 +32,9 @@ ****************************************************************************/ /** - * @file GPS driver interface. + * @file drv_gps.h + * + * GPS driver interface. */ #ifndef _DRV_GPS_H diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h index fefcae50b9..2ae8c70582 100644 --- a/src/drivers/drv_gyro.h +++ b/src/drivers/drv_gyro.h @@ -32,7 +32,9 @@ ****************************************************************************/ /** - * @file Gyroscope driver interface. + * @file drv_gyro.h + * + * Gyroscope driver interface. */ #ifndef _DRV_GYRO_H diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index 4decc324ef..78ffad9d7b 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -32,8 +32,9 @@ ****************************************************************************/ /** - * @file R/C input interface. + * @file drv_rc_input.h * + * R/C input interface. */ #ifndef _DRV_RC_INPUT_H diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 3a89cab9dd..8e8b2c1b95 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -32,7 +32,9 @@ ****************************************************************************/ /** - * @file Common sensor API and ioctl definitions. + * @file drv_sensor.h + * + * Common sensor API and ioctl definitions. */ #ifndef _DRV_SENSOR_H diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h index 2fab37dd21..cb5fd53a5f 100644 --- a/src/drivers/drv_tone_alarm.h +++ b/src/drivers/drv_tone_alarm.h @@ -31,7 +31,9 @@ * ****************************************************************************/ -/* +/** + * @file drv_tone_alarm.h + * * Driver for the PX4 audio alarm port, /dev/tone_alarm. * * The tone_alarm driver supports a set of predefined "alarm" From a06b3e50ab4d4c73fd400ec2891b1ab7a9eb8451 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 29 Oct 2013 13:38:44 +0100 Subject: [PATCH 840/872] Only read 5 values, then return --- src/examples/px4_simple_app/px4_simple_app.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/examples/px4_simple_app/px4_simple_app.c b/src/examples/px4_simple_app/px4_simple_app.c index 7f655964d9..44e6dc7f35 100644 --- a/src/examples/px4_simple_app/px4_simple_app.c +++ b/src/examples/px4_simple_app/px4_simple_app.c @@ -71,7 +71,7 @@ int px4_simple_app_main(int argc, char *argv[]) int error_counter = 0; - while (true) { + for (int i = 0; i < 5; i++) { /* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */ int poll_ret = poll(fds, 1, 1000); From 2293aa4e0ae809e31ba1aa71492253460a5e3aab Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Oct 2013 21:22:05 +0100 Subject: [PATCH 841/872] Fixed min value check, works for fixed wing now --- src/modules/systemlib/pwm_limit/pwm_limit.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c index 3187a4fa2e..4cc618dddd 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.c +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -100,7 +100,7 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_ temp_pwm = output[i] * (max_pwm[i] - min_pwm[i])/2 + (max_pwm[i] + min_pwm[i])/2; /* already follow user/controller input if higher than min_pwm */ - effective_pwm[i] = (disarmed_pwm[i]*(10000-progress) + (temp_pwm > min_pwm[i] ? temp_pwm : min_pwm[i])*progress)/10000; + effective_pwm[i] = (temp_pwm > min_pwm[i]) ? temp_pwm : ((disarmed_pwm[i]*(10000-progress) + min_pwm[i])*progress) / 10000; output[i] = (float)progress/10000.0f * output[i]; } break; From 44f88bf0a741fe530dea1c8b3137f30117a7b4b5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Oct 2013 09:03:19 +0100 Subject: [PATCH 842/872] Fix to allow setting again zero disarmed PWM values after boot --- src/modules/px4iofirmware/registers.c | 45 +++++++++++++++++---------- 1 file changed, 29 insertions(+), 16 deletions(-) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 6a0532beec..40597adf1f 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -338,26 +338,39 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num break; case PX4IO_PAGE_DISARMED_PWM: + { + /* flag for all outputs */ + bool all_disarmed_off = true; - /* copy channel data */ - while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { + /* copy channel data */ + while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { - if (*values == 0) { - /* ignore 0 */ - } else if (*values < PWM_MIN) { - r_page_servo_disarmed[offset] = PWM_MIN; - } else if (*values > PWM_MAX) { - r_page_servo_disarmed[offset] = PWM_MAX; - } else { - r_page_servo_disarmed[offset] = *values; + if (*values == 0) { + /* 0 means disabling always PWM */ + r_page_servo_disarmed[offset] = 0; + } else if (*values < PWM_MIN) { + r_page_servo_disarmed[offset] = PWM_MIN; + all_disarmed_off = false; + } else if (*values > PWM_MAX) { + r_page_servo_disarmed[offset] = PWM_MAX; + all_disarmed_off = false; + } else { + r_page_servo_disarmed[offset] = *values; + all_disarmed_off = false; + } + + offset++; + num_values--; + values++; } - /* flag the failsafe values as custom */ - r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE; - - offset++; - num_values--; - values++; + if (all_disarmed_off) { + /* disable PWM output if disarmed */ + r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE); + } else { + /* enable PWM output always */ + r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE; + } } break; From f0466143de18aedb5ada6f01b3c6435c9a0dc82a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Oct 2013 09:04:03 +0100 Subject: [PATCH 843/872] Minor warning and no error in case of zero value for disarmed --- src/systemcmds/pwm/pwm.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 3185e43714..09a9344008 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -335,7 +335,7 @@ pwm_main(int argc, char *argv[]) usage("no channels set"); } if (pwm_value == 0) - usage("no PWM value provided"); + warnx("reading disarmed value of zero, disabling disarmed PWM"); struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0}; @@ -343,7 +343,7 @@ pwm_main(int argc, char *argv[]) if (set_mask & 1< Date: Wed, 30 Oct 2013 09:14:17 +0100 Subject: [PATCH 844/872] Fixed pwm limit to apply the proper limits / scaling --- src/modules/systemlib/pwm_limit/pwm_limit.c | 22 ++++++++++++++++++--- 1 file changed, 19 insertions(+), 3 deletions(-) diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c index 4cc618dddd..cac3dc82a3 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.c +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -97,10 +97,26 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_ progress = (hrt_absolute_time() - INIT_TIME_US - limit->time_armed)*10000 / RAMP_TIME_US; for (unsigned i=0; i 0) { - temp_pwm = output[i] * (max_pwm[i] - min_pwm[i])/2 + (max_pwm[i] + min_pwm[i])/2; - /* already follow user/controller input if higher than min_pwm */ - effective_pwm[i] = (temp_pwm > min_pwm[i]) ? temp_pwm : ((disarmed_pwm[i]*(10000-progress) + min_pwm[i])*progress) / 10000; + /* safeguard against overflows */ + uint16_t disarmed = disarmed_pwm[i]; + if (disarmed > min_pwm[i]) + disarmed = min_pwm[i]; + + uint16_t disarmed_min_diff = min_pwm[i] - disarmed; + ramp_min_pwm = disarmed + (disarmed_min_diff * progress) / 10000; + } else { + + /* no disarmed pwm value set, choose min pwm */ + ramp_min_pwm = min_pwm[i]; + } + + effective_pwm[i] = output[i] * (max_pwm[i] - ramp_min_pwm)/2 + (max_pwm[i] + ramp_min_pwm)/2; output[i] = (float)progress/10000.0f * output[i]; } break; From 34d2f318ac8a72cce63e3e14e004daee45001011 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Oct 2013 09:14:57 +0100 Subject: [PATCH 845/872] Fixed commander and IO startup sequence, in-air restart is operational in this shape --- ROMFS/px4fmu_common/init.d/10_dji_f330 | 2 ++ ROMFS/px4fmu_common/init.d/11_dji_f450 | 2 ++ ROMFS/px4fmu_common/init.d/15_tbs_discovery | 2 ++ ROMFS/px4fmu_common/init.d/16_3dr_iris | 2 ++ ROMFS/px4fmu_common/init.d/rc.custom_io_esc | 2 ++ ROMFS/px4fmu_common/init.d/rc.multirotor | 5 ----- 6 files changed, 10 insertions(+), 5 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10_dji_f330 b/ROMFS/px4fmu_common/init.d/10_dji_f330 index 764a88a24e..81ea292aae 100644 --- a/ROMFS/px4fmu_common/init.d/10_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/10_dji_f330 @@ -59,6 +59,8 @@ then mavlink start usleep 5000 + commander start + sh /etc/init.d/rc.io # Set PWM values for DJI ESCs else diff --git a/ROMFS/px4fmu_common/init.d/11_dji_f450 b/ROMFS/px4fmu_common/init.d/11_dji_f450 index 91847b06bf..4dbf76cee7 100644 --- a/ROMFS/px4fmu_common/init.d/11_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/11_dji_f450 @@ -43,6 +43,8 @@ then mavlink start usleep 5000 + commander start + sh /etc/init.d/rc.io else # Start MAVLink (on UART1 / ttyS0) diff --git a/ROMFS/px4fmu_common/init.d/15_tbs_discovery b/ROMFS/px4fmu_common/init.d/15_tbs_discovery index 65be56df8c..bd6189a6d3 100644 --- a/ROMFS/px4fmu_common/init.d/15_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/15_tbs_discovery @@ -43,6 +43,8 @@ then mavlink start usleep 5000 + commander start + sh /etc/init.d/rc.io else # Start MAVLink (on UART1 / ttyS0) diff --git a/ROMFS/px4fmu_common/init.d/16_3dr_iris b/ROMFS/px4fmu_common/init.d/16_3dr_iris index d36699b9a5..d8cc0e9132 100644 --- a/ROMFS/px4fmu_common/init.d/16_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/16_3dr_iris @@ -43,6 +43,8 @@ then mavlink start usleep 5000 + commander start + sh /etc/init.d/rc.io else # Start MAVLink (on UART1 / ttyS0) diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_io_esc b/ROMFS/px4fmu_common/init.d/rc.custom_io_esc index 999422767a..d750cc87ad 100644 --- a/ROMFS/px4fmu_common/init.d/rc.custom_io_esc +++ b/ROMFS/px4fmu_common/init.d/rc.custom_io_esc @@ -61,6 +61,8 @@ then mavlink start -d /dev/ttyS1 -b 115200 usleep 5000 + commander start + sh /etc/init.d/rc.io else fmu mode_pwm diff --git a/ROMFS/px4fmu_common/init.d/rc.multirotor b/ROMFS/px4fmu_common/init.d/rc.multirotor index e3638e3d1f..dfff07198a 100644 --- a/ROMFS/px4fmu_common/init.d/rc.multirotor +++ b/ROMFS/px4fmu_common/init.d/rc.multirotor @@ -13,11 +13,6 @@ sh /etc/init.d/rc.sensors # sh /etc/init.d/rc.logging -# -# Start the commander. -# -commander start - # # Start GPS interface (depends on orb) # From 727342a5168bb23501c50287acb8edfe6d80e157 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Oct 2013 22:34:51 +0100 Subject: [PATCH 846/872] Teached the FMU driver that stopping is also an option --- src/drivers/px4fmu/fmu.cpp | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 6d4019f249..dd475bb6ca 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1093,6 +1093,20 @@ fmu_start(void) return ret; } +int +fmu_stop(void) +{ + int ret = OK; + + if (g_fmu != nullptr) { + + delete g_fmu; + g_fmu = nullptr; + } + + return ret; +} + void test(void) { @@ -1224,6 +1238,12 @@ fmu_main(int argc, char *argv[]) PortMode new_mode = PORT_MODE_UNSET; const char *verb = argv[1]; + if (!strcmp(verb, "stop")) { + fmu_stop(); + errx(0, "FMU driver stopped"); + } + + if (fmu_start() != OK) errx(1, "failed to start the FMU driver"); From 9820ed9de36418a4ff518c8833b199bb4c074a4d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 31 Oct 2013 08:23:32 +0100 Subject: [PATCH 847/872] Actually allow full range in FMU driver --- src/drivers/px4fmu/fmu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 3920b5fb8b..4bd27f9076 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -908,7 +908,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) /* FALLTHROUGH */ case PWM_SERVO_SET(1): case PWM_SERVO_SET(0): - if (arg < 2100) { + if (arg <= 2100) { up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg); } else { ret = -EINVAL; From 3c8c091e76c264fd803c066bc3a23cfdfc9738b2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 31 Oct 2013 08:23:44 +0100 Subject: [PATCH 848/872] esc_calib on steroids --- src/systemcmds/esc_calib/esc_calib.c | 91 +++++++++++++++++++++++++--- 1 file changed, 84 insertions(+), 7 deletions(-) diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c index 1ea02d81e5..0ac9e9e17b 100644 --- a/src/systemcmds/esc_calib/esc_calib.c +++ b/src/systemcmds/esc_calib/esc_calib.c @@ -62,6 +62,8 @@ #include "systemlib/err.h" #include "drivers/drv_pwm_output.h" +#include + static void usage(const char *reason); __EXPORT int esc_calib_main(int argc, char *argv[]); @@ -92,6 +94,9 @@ esc_calib_main(int argc, char *argv[]) int ret; char c; + int low = -1; + int high = -1; + struct pollfd fds; fds.fd = 0; /* stdin */ fds.events = POLLIN; @@ -107,6 +112,16 @@ esc_calib_main(int argc, char *argv[]) argc -= 2; break; + case 'l': + low = strtol(optarg, NULL, 0); + argc -= 2; + break; + + case 'h': + high = strtol(optarg, NULL, 0); + argc -= 2; + break; + default: usage(NULL); } @@ -131,6 +146,26 @@ esc_calib_main(int argc, char *argv[]) } } + /* make sure no other source is publishing control values now */ + struct actuator_controls_s actuators; + int act_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + + /* clear changed flag */ + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, act_sub, &actuators); + + /* wait 50 ms */ + usleep(50000); + + /* now expect nothing changed on that topic */ + bool orb_updated; + orb_check(act_sub, &orb_updated); + + if (orb_updated) { + errx(1, "ABORTING! Attitude control still active. Please ensure to shut down all controllers:\n" + "\tmultirotor_att_control stop\n" + "\tfw_att_control stop\n"); + } + printf("\nATTENTION, please remove or fix propellers before starting calibration!\n" "\n" "Make sure\n" @@ -179,17 +214,59 @@ esc_calib_main(int argc, char *argv[]) /* get max PWM value setting */ uint16_t pwm_max = 0; - ret = ioctl(fd, PWM_SERVO_GET_MAX_PWM, &pwm_max); - if (ret != OK) - err(1, "PWM_SERVO_GET_MAX_PWM"); + if (high > 0 && high > low && high < 2200) { + pwm_max = high; + } else { + ret = ioctl(fd, PWM_SERVO_GET_MAX_PWM, &pwm_max); + + if (ret != OK) + err(1, "PWM_SERVO_GET_MAX_PWM"); + } + + /* bound to sane values */ + if (pwm_max > 2200) + pwm_max = 2200; + + if (pwm_max < 1700) + pwm_max = 1700; /* get disarmed PWM value setting */ uint16_t pwm_disarmed = 0; - ret = ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, &pwm_disarmed); + if (low > 0 && low < high && low > 800) { + pwm_disarmed = low; + } else { + ret = ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, &pwm_disarmed); + + if (ret != OK) + err(1, "PWM_SERVO_GET_DISARMED_PWM"); + + if (pwm_disarmed == 0) { + ret = ioctl(fd, PWM_SERVO_GET_MIN_PWM, &pwm_disarmed); + + if (ret != OK) + err(1, "PWM_SERVO_GET_MIN_PWM"); + } + } + + /* bound to sane values */ + if (pwm_disarmed > 1300) + pwm_disarmed = 1300; + + if (pwm_disarmed < 800) + pwm_disarmed = 800; + + /* 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_GET_DISARMED_PWM"); + err(1, "PWM_SERVO_SET_ARM_OK"); + /* tell IO that the system is armed (it will output values if safety is off) */ + ret = ioctl(fd, PWM_SERVO_ARM, 0); + if (ret != OK) + err(1, "PWM_SERVO_ARM"); + + warnx("Outputs armed"); /* wait for user confirmation */ printf("\nHigh PWM set: %d\n" @@ -205,7 +282,7 @@ esc_calib_main(int argc, char *argv[]) ret = ioctl(fd, PWM_SERVO_SET(i), pwm_max); if (ret != OK) - err(1, "PWM_SERVO_SET(%d)", i); + err(1, "PWM_SERVO_SET(%d), value: %d", i, pwm_max); } } @@ -242,7 +319,7 @@ esc_calib_main(int argc, char *argv[]) ret = ioctl(fd, PWM_SERVO_SET(i), pwm_disarmed); if (ret != OK) - err(1, "PWM_SERVO_SET(%d)", i); + err(1, "PWM_SERVO_SET(%d), value: %d", i, pwm_disarmed); } } From 7d443eb3325cfff469c88864fdc96b68612d36c0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 31 Oct 2013 09:03:37 +0100 Subject: [PATCH 849/872] Commandline parsing fixes --- src/systemcmds/esc_calib/esc_calib.c | 44 ++++++++++++++++++---------- 1 file changed, 28 insertions(+), 16 deletions(-) diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c index 0ac9e9e17b..d524ab23be 100644 --- a/src/systemcmds/esc_calib/esc_calib.c +++ b/src/systemcmds/esc_calib/esc_calib.c @@ -77,7 +77,7 @@ usage(const char *reason) errx(1, "usage:\n" - "esc_calib [-d ] \n" + "esc_calib [-l ] [-h ] [-d ] \n" " PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n" " Provide channels (e.g.: 1 2 3 4)\n" ); @@ -101,25 +101,32 @@ esc_calib_main(int argc, char *argv[]) fds.fd = 0; /* stdin */ fds.events = POLLIN; - if (argc < 2) - usage(NULL); + if (argc < 2) { + usage("no channels provided"); + } - while ((ch = getopt(argc - 1, argv, "d:")) != EOF) { + int arg_consumed = 0; + + while ((ch = getopt(argc, &argv[0], "l:h:d:")) != -1) { switch (ch) { case 'd': dev = optarg; - argc -= 2; + arg_consumed += 2; break; case 'l': - low = strtol(optarg, NULL, 0); - argc -= 2; + low = strtoul(optarg, &ep, 0); + if (*ep != '\0') + usage("bad low pwm value"); + arg_consumed += 2; break; case 'h': - high = strtol(optarg, NULL, 0); - argc -= 2; + high = strtoul(optarg, &ep, 0); + if (*ep != '\0') + usage("bad high pwm value"); + arg_consumed += 2; break; default: @@ -127,14 +134,12 @@ esc_calib_main(int argc, char *argv[]) } } - if (argc < 2) { - usage("no channels provided"); - } - - while (--argc) { + while ((--argc - arg_consumed) > 0) { const char *arg = argv[argc]; unsigned channel_number = strtol(arg, &ep, 0); + warnx("adding channel #%d", channel_number); + if (*ep == '\0') { if (channel_number > MAX_CHANNELS || channel_number <= 0) { err(1, "invalid channel number: %d", channel_number); @@ -257,11 +262,11 @@ esc_calib_main(int argc, char *argv[]) if (pwm_disarmed < 800) pwm_disarmed = 800; - /* tell IO that its ok to disable its safety with the switch */ + /* tell IO/FMU 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"); - /* tell IO that the system is armed (it will output values if safety is off) */ + /* tell IO/FMU that the system is armed (it will output values if safety is off) */ ret = ioctl(fd, PWM_SERVO_ARM, 0); if (ret != OK) err(1, "PWM_SERVO_ARM"); @@ -343,6 +348,13 @@ esc_calib_main(int argc, char *argv[]) usleep(50000); } + /* disarm */ + ret = ioctl(fd, PWM_SERVO_DISARM, 0); + if (ret != OK) + err(1, "PWM_SERVO_DISARM"); + + warnx("Outputs disarmed"); + printf("ESC calibration finished\n"); exit(0); From 88351f3da178be1c73dad47557d894943e484e34 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 31 Oct 2013 09:20:44 +0100 Subject: [PATCH 850/872] esc_calib: Changed cmdline interface (now same as for the pwm systecmd), read in the number of channels available, don't make the esc_calib dependant on min/max/disarmed values --- src/systemcmds/esc_calib/esc_calib.c | 160 ++++++++++++--------------- 1 file changed, 68 insertions(+), 92 deletions(-) diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c index d524ab23be..c402061971 100644 --- a/src/systemcmds/esc_calib/esc_calib.c +++ b/src/systemcmds/esc_calib/esc_calib.c @@ -67,8 +67,6 @@ static void usage(const char *reason); __EXPORT int esc_calib_main(int argc, char *argv[]); -#define MAX_CHANNELS 14 - static void usage(const char *reason) { @@ -76,12 +74,15 @@ usage(const char *reason) warnx("%s", reason); errx(1, - "usage:\n" - "esc_calib [-l ] [-h ] [-d ] \n" - " PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n" - " Provide channels (e.g.: 1 2 3 4)\n" - ); - + "usage:\n" + "esc_calib\n" + " [-d PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n" + " [-l Low PWM value in us (default: %dus)\n" + " [-h High PWM value in us (default: %dus)\n" + " [-c ] Supply channels (e.g. 1234)\n" + " [-m ] Directly supply channel mask (e.g. 0xF)\n" + " [-a] Use all outputs\n" + , PWM_MIN, PWM_MAX); } int @@ -89,13 +90,18 @@ esc_calib_main(int argc, char *argv[]) { char *dev = PWM_OUTPUT_DEVICE_PATH; char *ep; - bool channels_selected[MAX_CHANNELS] = {false}; int ch; int ret; char c; - int low = -1; - int high = -1; + unsigned max_channels = 0; + + uint32_t set_mask = 0; + unsigned long channels; + unsigned single_ch = 0; + + uint16_t pwm_high = PWM_MAX; + uint16_t pwm_low = PWM_MIN; struct pollfd fds; fds.fd = 0; /* stdin */ @@ -107,7 +113,7 @@ esc_calib_main(int argc, char *argv[]) int arg_consumed = 0; - while ((ch = getopt(argc, &argv[0], "l:h:d:")) != -1) { + while ((ch = getopt(argc, argv, "d:c:m:al:h:")) != EOF) { switch (ch) { case 'd': @@ -115,41 +121,49 @@ esc_calib_main(int argc, char *argv[]) arg_consumed += 2; break; + case 'c': + /* Read in channels supplied as one int and convert to mask: 1234 -> 0xF */ + channels = strtoul(optarg, &ep, 0); + + while ((single_ch = channels % 10)) { + + set_mask |= 1<<(single_ch-1); + channels /= 10; + } + break; + + case 'm': + /* Read in mask directly */ + set_mask = strtoul(optarg, &ep, 0); + if (*ep != '\0') + usage("bad set_mask value"); + break; + + case 'a': + /* Choose all channels */ + for (unsigned i = 0; i PWM_HIGHEST_MIN) + usage("low PWM invalid"); break; - case 'h': - high = strtoul(optarg, &ep, 0); - if (*ep != '\0') - usage("bad high pwm value"); - arg_consumed += 2; + /* Read in custom high value */ + pwm_high = strtoul(optarg, &ep, 0); + if (*ep != '\0' || pwm_high > PWM_MAX || pwm_high < PWM_LOWEST_MAX) + usage("high PWM invalid"); break; - default: usage(NULL); } } - while ((--argc - arg_consumed) > 0) { - const char *arg = argv[argc]; - unsigned channel_number = strtol(arg, &ep, 0); - - warnx("adding channel #%d", channel_number); - - 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; - - } - } - } + if (set_mask == 0) + usage("no channels chosen"); /* make sure no other source is publishing control values now */ struct actuator_controls_s actuators; @@ -217,50 +231,10 @@ esc_calib_main(int argc, char *argv[]) if (fd < 0) err(1, "can't open %s", dev); - /* get max PWM value setting */ - uint16_t pwm_max = 0; - - if (high > 0 && high > low && high < 2200) { - pwm_max = high; - } else { - ret = ioctl(fd, PWM_SERVO_GET_MAX_PWM, &pwm_max); - - if (ret != OK) - err(1, "PWM_SERVO_GET_MAX_PWM"); - } - - /* bound to sane values */ - if (pwm_max > 2200) - pwm_max = 2200; - - if (pwm_max < 1700) - pwm_max = 1700; - - /* get disarmed PWM value setting */ - uint16_t pwm_disarmed = 0; - - if (low > 0 && low < high && low > 800) { - pwm_disarmed = low; - } else { - ret = ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, &pwm_disarmed); - - if (ret != OK) - err(1, "PWM_SERVO_GET_DISARMED_PWM"); - - if (pwm_disarmed == 0) { - ret = ioctl(fd, PWM_SERVO_GET_MIN_PWM, &pwm_disarmed); - - if (ret != OK) - err(1, "PWM_SERVO_GET_MIN_PWM"); - } - } - - /* bound to sane values */ - if (pwm_disarmed > 1300) - pwm_disarmed = 1300; - - if (pwm_disarmed < 800) - pwm_disarmed = 800; + /* get number of channels available on the device */ + ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&max_channels); + if (ret != OK) + err(1, "PWM_SERVO_GET_COUNT"); /* tell IO/FMU that its ok to disable its safety with the switch */ ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); @@ -273,21 +247,23 @@ esc_calib_main(int argc, char *argv[]) warnx("Outputs armed"); + /* wait for user confirmation */ printf("\nHigh PWM set: %d\n" "\n" "Connect battery now and hit ENTER after the ESCs confirm the first calibration step\n" - "\n", pwm_max); + "\n", pwm_high); fflush(stdout); while (1) { /* set max PWM */ - for (unsigned i = 0; i < MAX_CHANNELS; i++) { - if (channels_selected[i]) { - ret = ioctl(fd, PWM_SERVO_SET(i), pwm_max); + for (unsigned i = 0; i < max_channels; i++) { + + if (set_mask & 1< Date: Tue, 20 Aug 2013 14:19:22 +0200 Subject: [PATCH 851/872] Fixed small typo --- src/systemcmds/config/config.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index 188dafa4ef..80689f20c8 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -272,7 +272,7 @@ do_accel(int argc, char *argv[]) } } else { - 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"); + 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 4' to set measurement range to 4 G\n\t"); } int srate = ioctl(fd, ACCELIOCGSAMPLERATE, 0); From eac640739b8eb63343e01566d4c093179e3b657f Mon Sep 17 00:00:00 2001 From: runepx4 Date: Thu, 31 Oct 2013 10:23:37 +0100 Subject: [PATCH 852/872] Added 8 rotor Coaxial Rotor mixer --- ROMFS/px4fmu_common/mixers/FMU_octo_cox.mix | 7 +++++++ src/modules/systemlib/mixer/mixer.h | 1 + src/modules/systemlib/mixer/mixer_multirotor.cpp | 15 +++++++++++++++ src/modules/systemlib/mixer/multi_tables | 13 ++++++++++++- 4 files changed, 35 insertions(+), 1 deletion(-) create mode 100644 ROMFS/px4fmu_common/mixers/FMU_octo_cox.mix diff --git a/ROMFS/px4fmu_common/mixers/FMU_octo_cox.mix b/ROMFS/px4fmu_common/mixers/FMU_octo_cox.mix new file mode 100644 index 0000000000..51cebb7858 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_octo_cox.mix @@ -0,0 +1,7 @@ +Multirotor mixer for PX4FMU +=========================== + +This file defines a single mixer for a Coaxial Octocopter in the X configuration. All controls +are mixed 100%. + +R: 8c 10000 10000 10000 0 diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index 723bf9f3b7..1c889a8119 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -449,6 +449,7 @@ public: HEX_PLUS, /**< hex in + configuration */ OCTA_X, OCTA_PLUS, + OCTA_COX, MAX_GEOMETRY }; diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index b89f341b60..bf77795d5c 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -130,6 +130,16 @@ const MultirotorMixer::Rotor _config_octa_plus[] = { { 1.000000, 0.000000, -1.00 }, { -1.000000, 0.000000, -1.00 }, }; +const MultirotorMixer::Rotor _config_octa_cox[] = { + { -0.707107, 0.707107, 1.00 }, + { 0.707107, 0.707107, -1.00 }, + { 0.707107, -0.707107, 1.00 }, + { -0.707107, -0.707107, -1.00 }, + { 0.707107, 0.707107, 1.00 }, + { -0.707107, 0.707107, -1.00 }, + { -0.707107, -0.707107, 1.00 }, + { 0.707107, -0.707107, -1.00 }, +}; const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = { &_config_quad_x[0], &_config_quad_plus[0], @@ -139,6 +149,7 @@ const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = { &_config_hex_plus[0], &_config_octa_x[0], &_config_octa_plus[0], + &_config_octa_cox[0], }; const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = { 4, /* quad_x */ @@ -149,6 +160,7 @@ const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = { 6, /* hex_plus */ 8, /* octa_x */ 8, /* octa_plus */ + 8, /* octa_cox */ }; } @@ -240,6 +252,9 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl } else if (!strcmp(geomname, "8x")) { geometry = MultirotorMixer::OCTA_X; + + } else if (!strcmp(geomname, "8c")) { + geometry = MultirotorMixer::OCTA_COX; } else { debug("unrecognised geometry '%s'", geomname); diff --git a/src/modules/systemlib/mixer/multi_tables b/src/modules/systemlib/mixer/multi_tables index 683c630409..050bf2f47f 100755 --- a/src/modules/systemlib/mixer/multi_tables +++ b/src/modules/systemlib/mixer/multi_tables @@ -74,7 +74,18 @@ set octa_plus { 90 CW } -set tables {quad_x quad_plus quad_v quad_wide hex_x hex_plus octa_x octa_plus} +set octa_cox { + 45 CCW + -45 CW + -135 CCW + 135 CW + -45 CCW + 45 CW + 135 CCW + -135 CW +} + +set tables {quad_x quad_plus quad_v quad_wide hex_x hex_plus octa_x octa_plus octa_cox} proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %5.2f }," [rcos [expr $a + 90]] [rcos $a] [expr -$d]]} From 25bf1abecffeb0b4c29386ef6a019b7a60c23899 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 31 Oct 2013 10:29:06 +0100 Subject: [PATCH 853/872] pwm_output: Allow PWM values from 900us to 2100us but use a default of 1000us to 2000us --- src/drivers/drv_pwm_output.h | 18 +++++++++++++---- src/drivers/px4fmu/fmu.cpp | 28 +++++++++++++-------------- src/drivers/px4io/px4io.cpp | 2 +- src/modules/px4iofirmware/registers.c | 28 +++++++++++++-------------- src/systemcmds/esc_calib/esc_calib.c | 10 +++++----- 5 files changed, 48 insertions(+), 38 deletions(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index efd6afb4bd..51f916f37b 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -65,9 +65,14 @@ __BEGIN_DECLS #define PWM_OUTPUT_MAX_CHANNELS 16 /** - * Minimum PWM in us + * Lowest minimum PWM in us */ -#define PWM_MIN 900 +#define PWM_LOWEST_MIN 900 + +/** + * Default minimum PWM in us + */ +#define PWM_DEFAULT_MIN 1000 /** * Highest PWM allowed as the minimum PWM @@ -75,9 +80,14 @@ __BEGIN_DECLS #define PWM_HIGHEST_MIN 1300 /** - * Maximum PWM in us + * Highest maximum PWM in us */ -#define PWM_MAX 2100 +#define PWM_HIGHEST_MAX 2100 + +/** + * Default maximum PWM in us + */ +#define PWM_DEFAULT_MAX 2000 /** * Lowest PWM allowed as the maximum PWM diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 4bd27f9076..0441566e98 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -232,8 +232,8 @@ PX4FMU::PX4FMU() : _num_disarmed_set(0) { for (unsigned i = 0; i < _max_actuators; i++) { - _min_pwm[i] = PWM_MIN; - _max_pwm[i] = PWM_MAX; + _min_pwm[i] = PWM_DEFAULT_MIN; + _max_pwm[i] = PWM_DEFAULT_MAX; } _debug_enabled = true; @@ -762,10 +762,10 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) for (unsigned i = 0; i < pwm->channel_count; i++) { if (pwm->values[i] == 0) { /* ignore 0 */ - } else if (pwm->values[i] > PWM_MAX) { - _failsafe_pwm[i] = PWM_MAX; - } else if (pwm->values[i] < PWM_MIN) { - _failsafe_pwm[i] = PWM_MIN; + } else if (pwm->values[i] > PWM_HIGHEST_MAX) { + _failsafe_pwm[i] = PWM_HIGHEST_MAX; + } else if (pwm->values[i] < PWM_LOWEST_MIN) { + _failsafe_pwm[i] = PWM_LOWEST_MIN; } else { _failsafe_pwm[i] = pwm->values[i]; } @@ -801,10 +801,10 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) for (unsigned i = 0; i < pwm->channel_count; i++) { if (pwm->values[i] == 0) { /* ignore 0 */ - } else if (pwm->values[i] > PWM_MAX) { - _disarmed_pwm[i] = PWM_MAX; - } else if (pwm->values[i] < PWM_MIN) { - _disarmed_pwm[i] = PWM_MIN; + } else if (pwm->values[i] > PWM_HIGHEST_MAX) { + _disarmed_pwm[i] = PWM_HIGHEST_MAX; + } else if (pwm->values[i] < PWM_LOWEST_MIN) { + _disarmed_pwm[i] = PWM_LOWEST_MIN; } else { _disarmed_pwm[i] = pwm->values[i]; } @@ -842,8 +842,8 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) /* ignore 0 */ } else if (pwm->values[i] > PWM_HIGHEST_MIN) { _min_pwm[i] = PWM_HIGHEST_MIN; - } else if (pwm->values[i] < PWM_MIN) { - _min_pwm[i] = PWM_MIN; + } else if (pwm->values[i] < PWM_LOWEST_MIN) { + _min_pwm[i] = PWM_LOWEST_MIN; } else { _min_pwm[i] = pwm->values[i]; } @@ -872,8 +872,8 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) /* ignore 0 */ } else if (pwm->values[i] < PWM_LOWEST_MAX) { _max_pwm[i] = PWM_LOWEST_MAX; - } else if (pwm->values[i] > PWM_MAX) { - _max_pwm[i] = PWM_MAX; + } else if (pwm->values[i] > PWM_HIGHEST_MAX) { + _max_pwm[i] = PWM_HIGHEST_MAX; } else { _max_pwm[i] = pwm->values[i]; } diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 63e775857c..08e697b9f8 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1985,7 +1985,7 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) /* TODO: we could go lower for e.g. TurboPWM */ unsigned channel = cmd - PWM_SERVO_SET(0); - if ((channel >= _max_actuators) || (arg < PWM_MIN) || (arg > PWM_MAX)) { + if ((channel >= _max_actuators) || (arg < PWM_LOWEST_MIN) || (arg > PWM_HIGHEST_MAX)) { ret = -EINVAL; } else { diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 40597adf1f..86a40bc228 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -207,7 +207,7 @@ uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 }; * minimum PWM values when armed * */ -uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT] = { PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN }; +uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT] = { PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN }; /** * PAGE 107 @@ -215,7 +215,7 @@ uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT] = { PWM_MIN, PWM_MIN, PWM_ * maximum PWM values when armed * */ -uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT] = { PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX }; +uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT] = { PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX }; /** * PAGE 108 @@ -278,10 +278,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num if (*values == 0) { /* ignore 0 */ - } else if (*values < PWM_MIN) { - r_page_servo_failsafe[offset] = PWM_MIN; - } else if (*values > PWM_MAX) { - r_page_servo_failsafe[offset] = PWM_MAX; + } else if (*values < PWM_LOWEST_MIN) { + r_page_servo_failsafe[offset] = PWM_LOWEST_MIN; + } else if (*values > PWM_HIGHEST_MAX) { + r_page_servo_failsafe[offset] = PWM_HIGHEST_MAX; } else { r_page_servo_failsafe[offset] = *values; } @@ -304,8 +304,8 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* ignore 0 */ } else if (*values > PWM_HIGHEST_MIN) { r_page_servo_control_min[offset] = PWM_HIGHEST_MIN; - } else if (*values < PWM_MIN) { - r_page_servo_control_min[offset] = PWM_MIN; + } else if (*values < PWM_LOWEST_MIN) { + r_page_servo_control_min[offset] = PWM_LOWEST_MIN; } else { r_page_servo_control_min[offset] = *values; } @@ -323,8 +323,8 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num if (*values == 0) { /* ignore 0 */ - } else if (*values > PWM_MAX) { - r_page_servo_control_max[offset] = PWM_MAX; + } else if (*values > PWM_HIGHEST_MAX) { + r_page_servo_control_max[offset] = PWM_HIGHEST_MAX; } else if (*values < PWM_LOWEST_MAX) { r_page_servo_control_max[offset] = PWM_LOWEST_MAX; } else { @@ -348,11 +348,11 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num if (*values == 0) { /* 0 means disabling always PWM */ r_page_servo_disarmed[offset] = 0; - } else if (*values < PWM_MIN) { - r_page_servo_disarmed[offset] = PWM_MIN; + } else if (*values < PWM_LOWEST_MIN) { + r_page_servo_disarmed[offset] = PWM_LOWEST_MIN; all_disarmed_off = false; - } else if (*values > PWM_MAX) { - r_page_servo_disarmed[offset] = PWM_MAX; + } else if (*values > PWM_HIGHEST_MAX) { + r_page_servo_disarmed[offset] = PWM_HIGHEST_MAX; all_disarmed_off = false; } else { r_page_servo_disarmed[offset] = *values; diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c index c402061971..b237b31be3 100644 --- a/src/systemcmds/esc_calib/esc_calib.c +++ b/src/systemcmds/esc_calib/esc_calib.c @@ -82,7 +82,7 @@ usage(const char *reason) " [-c ] Supply channels (e.g. 1234)\n" " [-m ] Directly supply channel mask (e.g. 0xF)\n" " [-a] Use all outputs\n" - , PWM_MIN, PWM_MAX); + , PWM_DEFAULT_MIN, PWM_DEFAULT_MAX); } int @@ -100,8 +100,8 @@ esc_calib_main(int argc, char *argv[]) unsigned long channels; unsigned single_ch = 0; - uint16_t pwm_high = PWM_MAX; - uint16_t pwm_low = PWM_MIN; + uint16_t pwm_high = PWM_DEFAULT_MAX; + uint16_t pwm_low = PWM_DEFAULT_MIN; struct pollfd fds; fds.fd = 0; /* stdin */ @@ -148,13 +148,13 @@ esc_calib_main(int argc, char *argv[]) case 'l': /* Read in custom low value */ - if (*ep != '\0' || pwm_low < PWM_MIN || pwm_low > PWM_HIGHEST_MIN) + if (*ep != '\0' || pwm_low < PWM_LOWEST_MIN || pwm_low > PWM_HIGHEST_MIN) usage("low PWM invalid"); break; case 'h': /* Read in custom high value */ pwm_high = strtoul(optarg, &ep, 0); - if (*ep != '\0' || pwm_high > PWM_MAX || pwm_high < PWM_LOWEST_MAX) + if (*ep != '\0' || pwm_high > PWM_HIGHEST_MAX || pwm_high < PWM_LOWEST_MAX) usage("high PWM invalid"); break; default: From 5781b58640a7bb3937f9eff979f99535ab1169e3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Nov 2013 09:05:28 +0100 Subject: [PATCH 854/872] Minor bugfix to commander, emits arming sound now on the right occasions. Fixes an annoying issue where the arming sound would go off constantly if the safety was re-engaged in arming mode, something that we consider to be ok operationally --- src/modules/commander/commander.cpp | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 44a1442968..ed090271ca 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -199,7 +199,7 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode */ int commander_thread_main(int argc, char *argv[]); -void control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed); +void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_armed, bool changed); void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed); @@ -843,6 +843,12 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(safety), safety_sub, &safety); + + // XXX this would be the right approach to do it, but do we *WANT* this? + // /* disarm if safety is now on and still armed */ + // if (safety.safety_switch_available && !safety.safety_off) { + // (void)arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed); + // } } /* update global position estimate */ @@ -1219,7 +1225,7 @@ int commander_thread_main(int argc, char *argv[]) } /* play arming and battery warning tunes */ - if (!arm_tune_played && armed.armed) { + if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_switch_available && safety.safety_off))) { /* play tune when armed */ if (tune_arm() == OK) arm_tune_played = true; @@ -1240,7 +1246,7 @@ int commander_thread_main(int argc, char *argv[]) } /* reset arm_tune_played when disarmed */ - if (!(armed.armed && (!safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available)))) { + if (status.arming_state != ARMING_STATE_ARMED || (safety.safety_switch_available && !safety.safety_off)) { arm_tune_played = false; } @@ -1309,7 +1315,7 @@ check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *val } void -control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed) +control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_armed, bool changed) { /* driving rgbled */ if (changed) { @@ -1356,11 +1362,11 @@ control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chan #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 /* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */ - if (armed->armed) { + if (actuator_armed->armed) { /* armed, solid */ led_on(LED_BLUE); - } else if (armed->ready_to_arm) { + } else if (actuator_armed->ready_to_arm) { /* ready to arm, blink at 1Hz */ if (leds_counter % 20 == 0) led_toggle(LED_BLUE); From 2fd1aed6be03de42d09c062871838ee7e852aa4a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Nov 2013 12:02:45 +0100 Subject: [PATCH 855/872] Disable the segway app, as its GCC 4.7 incompatible and not generally used --- makefiles/config_px4fmu-v1_default.mk | 2 +- makefiles/config_px4fmu-v2_default.mk | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index f9061c1100..3068270865 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -79,7 +79,7 @@ MODULES += examples/flow_position_estimator # # Vehicle Control # -MODULES += modules/segway +#MODULES += modules/segway # XXX Needs GCC 4.7 fix MODULES += modules/fw_pos_control_l1 MODULES += modules/fw_att_control MODULES += modules/multirotor_att_control diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index ed2f2da5ed..761fb8d9d6 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -78,7 +78,7 @@ MODULES += examples/flow_position_estimator # # Vehicle Control # -MODULES += modules/segway +#MODULES += modules/segway # XXX Needs GCC 4.7 fix MODULES += modules/fw_pos_control_l1 MODULES += modules/fw_att_control MODULES += modules/multirotor_att_control @@ -120,7 +120,7 @@ MODULES += lib/conversion #MODULES += examples/math_demo # Tutorial code from # https://pixhawk.ethz.ch/px4/dev/hello_sky -#MODULES += examples/px4_simple_app +MODULES += examples/px4_simple_app # Tutorial code from # https://pixhawk.ethz.ch/px4/dev/daemon From 3eac9ce1596f90acf2e86f807b1b4efd3904a80b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 2 Nov 2013 16:16:49 +0100 Subject: [PATCH 856/872] fix usage of wrong value for max airspeed parameter --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index ffa7915a7d..a6653bfc7c 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -451,7 +451,7 @@ FixedwingPositionControl::parameters_update() _tecs.set_speed_weight(_parameters.speed_weight); _tecs.set_pitch_damping(_parameters.pitch_damping); _tecs.set_indicated_airspeed_min(_parameters.airspeed_min); - _tecs.set_indicated_airspeed_max(_parameters.airspeed_min); + _tecs.set_indicated_airspeed_max(_parameters.airspeed_max); _tecs.set_max_climb_rate(_parameters.max_climb_rate); /* sanity check parameters */ From ef7a425a45397fed510920b98a4ad08e62170f4c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 2 Nov 2013 17:33:45 +0100 Subject: [PATCH 857/872] fix vehicle_airspeed_poll logic: _tecs.enable_airspeed was not called before on valid airspeed --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index ffa7915a7d..28ca9776a0 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -501,7 +501,6 @@ FixedwingPositionControl::vehicle_airspeed_poll() orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); _airspeed_valid = true; _airspeed_last_valid = hrt_absolute_time(); - return true; } else { @@ -514,7 +513,7 @@ FixedwingPositionControl::vehicle_airspeed_poll() /* update TECS state */ _tecs.enable_airspeed(_airspeed_valid); - return false; + return airspeed_updated; } void From a4c99225c02e719d7900a533b777fd682eb5bd5c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 3 Nov 2013 16:49:02 +0100 Subject: [PATCH 858/872] initialize _vel_dot and _STEdotErrLast --- src/lib/external_lgpl/tecs/tecs.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 4a98c8e974..f8f832ed76 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -54,7 +54,9 @@ public: _SPE_est(0.0f), _SKE_est(0.0f), _SPEdot(0.0f), - _SKEdot(0.0f) { + _SKEdot(0.0f), + _vel_dot(0.0f), + _STEdotErrLast(0.0f) { } From 98f5a77574fde4b2c41d28cc0d694f6ca250fba1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 3 Nov 2013 17:52:27 +0100 Subject: [PATCH 859/872] Fix to cancel pending callbacks for closing ORB topics --- src/modules/uORB/uORB.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index 7abbf42aed..50e433ec3c 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -249,9 +249,10 @@ ORBDevNode::close(struct file *filp) } else { SubscriberData *sd = filp_to_sd(filp); - if (sd != nullptr) + if (sd != nullptr) { + hrt_cancel(&sd->update_call); delete sd; - } + } return CDev::close(filp); } From 4865814f92c4a085972e317204c37042b609fdf8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 3 Nov 2013 17:58:28 +0100 Subject: [PATCH 860/872] Fixed typo, added testing - previous corner case now cleanly prevented --- src/modules/uORB/uORB.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index 50e433ec3c..149b8f6d24 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -253,6 +253,7 @@ ORBDevNode::close(struct file *filp) hrt_cancel(&sd->update_call); delete sd; } + } return CDev::close(filp); } From ba0687bc5e471809ae311ef98f3ddda3c29713b4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 3 Nov 2013 18:06:58 +0100 Subject: [PATCH 861/872] Matrix and Vector printing cleanup --- src/lib/mathlib/math/arm/Matrix.hpp | 6 +++--- src/lib/mathlib/math/arm/Vector.hpp | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/lib/mathlib/math/arm/Matrix.hpp b/src/lib/mathlib/math/arm/Matrix.hpp index 715fd3a5e8..1945bb02db 100644 --- a/src/lib/mathlib/math/arm/Matrix.hpp +++ b/src/lib/mathlib/math/arm/Matrix.hpp @@ -121,10 +121,10 @@ public: for (size_t i = 0; i < getRows(); i++) { for (size_t j = 0; j < getCols(); j++) { float sig; - int exp; + int exponent; float num = (*this)(i, j); - float2SigExp(num, sig, exp); - printf("%6.3fe%03.3d,", (double)sig, exp); + float2SigExp(num, sig, exponent); + printf("%6.3fe%03d ", (double)sig, exponent); } printf("\n"); diff --git a/src/lib/mathlib/math/arm/Vector.hpp b/src/lib/mathlib/math/arm/Vector.hpp index 4155800e86..52220fc152 100644 --- a/src/lib/mathlib/math/arm/Vector.hpp +++ b/src/lib/mathlib/math/arm/Vector.hpp @@ -109,10 +109,10 @@ public: inline void print() const { for (size_t i = 0; i < getRows(); i++) { float sig; - int exp; + int exponent; float num = (*this)(i); - float2SigExp(num, sig, exp); - printf("%6.3fe%03.3d,", (double)sig, exp); + float2SigExp(num, sig, exponent); + printf("%6.3fe%03d ", (double)sig, exponent); } printf("\n"); From b53d86ed681eb6c9f979bb10d5f487fa9c94d81b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 3 Nov 2013 18:26:02 +0100 Subject: [PATCH 862/872] Hotfix for mag calibration --- src/modules/commander/mag_calibration.cpp | 29 +++++++++++++++-------- 1 file changed, 19 insertions(+), 10 deletions(-) diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 09f4104f89..4ebf266f47 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -73,7 +73,7 @@ int do_mag_calibration(int mavlink_fd) /* maximum 500 values */ const unsigned int calibration_maxcount = 500; - unsigned int calibration_counter = 0; + unsigned int calibration_counter; struct mag_scale mscale_null = { 0.0f, @@ -99,28 +99,34 @@ int do_mag_calibration(int mavlink_fd) res = ioctl(fd, MAGIOCCALIBRATE, fd); if (res != OK) { - mavlink_log_critical(mavlink_fd, "ERROR: failed to calibrate scale"); + mavlink_log_critical(mavlink_fd, "Skipped scale calibration"); + /* this is non-fatal - mark it accordingly */ + res = OK; } } close(fd); - float *x; - float *y; - float *z; + float *x = NULL; + float *y = NULL; + float *z = NULL; if (res == OK) { /* allocate memory */ mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20); - x = (float *)malloc(sizeof(float) * calibration_maxcount); - y = (float *)malloc(sizeof(float) * calibration_maxcount); - z = (float *)malloc(sizeof(float) * calibration_maxcount); + x = reinterpret_cast(malloc(sizeof(float) * calibration_maxcount)); + y = reinterpret_cast(malloc(sizeof(float) * calibration_maxcount)); + z = reinterpret_cast(malloc(sizeof(float) * calibration_maxcount)); if (x == NULL || y == NULL || z == NULL) { mavlink_log_critical(mavlink_fd, "ERROR: out of memory"); res = ERROR; + return res; } + } else { + /* exit */ + return ERROR; } if (res == OK) { @@ -136,6 +142,8 @@ int do_mag_calibration(int mavlink_fd) mavlink_log_info(mavlink_fd, "rotate in a figure 8 around all axis"); + calibration_counter = 0; + while (hrt_absolute_time() < calibration_deadline && calibration_counter < calibration_maxcount) { @@ -178,6 +186,7 @@ int do_mag_calibration(int mavlink_fd) float sphere_radius; if (res == OK) { + /* sphere fit */ mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 70); sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); @@ -270,7 +279,7 @@ int do_mag_calibration(int mavlink_fd) } else { mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); } - - return res; } + + return res; } From 791695ccd008859f6abe1a12d86b7be2ba811fec Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 3 Nov 2013 18:26:39 +0100 Subject: [PATCH 863/872] Hotfix: Check for out of range accel values --- .../preflight_check/preflight_check.c | 28 +++++++++++++++++-- 1 file changed, 26 insertions(+), 2 deletions(-) diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index 1c58a2db69..07581459b4 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -109,7 +109,7 @@ int preflight_check_main(int argc, char *argv[]) /* ---- ACCEL ---- */ close(fd); - fd = open(ACCEL_DEVICE_PATH, 0); + fd = open(ACCEL_DEVICE_PATH, O_RDONLY); ret = ioctl(fd, ACCELIOCSELFTEST, 0); if (ret != OK) { @@ -119,6 +119,29 @@ int preflight_check_main(int argc, char *argv[]) goto system_eval; } + /* check measurement result range */ + struct accel_report acc; + ret = read(fd, &acc, sizeof(acc)); + + if (ret == sizeof(acc)) { + /* evaluate values */ + if (sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z) > 30.0f /* m/s^2 */) { + warnx("accel with spurious values"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: |ACCEL| > 30 m/s^2"); + /* this is frickin' fatal */ + fail_on_error = true; + system_ok = false; + goto system_eval; + } + } else { + warnx("accel read failed"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL READ"); + /* this is frickin' fatal */ + fail_on_error = true; + system_ok = false; + goto system_eval; + } + /* ---- GYRO ---- */ close(fd); @@ -193,9 +216,10 @@ system_eval: /* stop alarm */ ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE); - /* switch on leds */ + /* switch off leds */ led_on(leds, LED_BLUE); led_on(leds, LED_AMBER); + close(leds); if (fail_on_error) { /* exit with error message */ From 3042731d26e94b3c126e61e422b98fcd7df4694c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 3 Nov 2013 18:27:26 +0100 Subject: [PATCH 864/872] Smaller hotfixes for att pos estimator --- src/modules/att_pos_estimator_ekf/KalmanNav.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index 33879892ee..18fb6dcbc9 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -34,7 +34,7 @@ /** * @file KalmanNav.cpp * - * kalman filter navigation code + * Kalman filter navigation code */ #include @@ -228,10 +228,7 @@ void KalmanNav::update() updateSubscriptions(); // initialize attitude when sensors online - if (!_attitudeInitialized && sensorsUpdate && - _sensors.accelerometer_counter > 10 && - _sensors.gyro_counter > 10 && - _sensors.magnetometer_counter > 10) { + if (!_attitudeInitialized && sensorsUpdate) { if (correctAtt() == ret_ok) _attitudeInitCounter++; if (_attitudeInitCounter > 100) { @@ -643,7 +640,7 @@ int KalmanNav::correctAtt() if (beta > _faultAtt.get()) { warnx("fault in attitude: beta = %8.4f", (double)beta); - warnx("y:\n"); y.print(); + warnx("y:"); y.print(); } // update quaternions from euler From ed60dc50fcf2ab4dde76e937244bfb6eae81c709 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 5 Nov 2013 07:43:08 +0100 Subject: [PATCH 865/872] Hotfix: forbid integrator to accumulate NaN values if they ever would occur --- .../fw_att_control/fw_att_control_main.cpp | 61 ++++++++++--------- 1 file changed, 32 insertions(+), 29 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index eb67874db4..b3973084f3 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -635,43 +635,46 @@ FixedwingAttitudeControl::task_main() } } - float roll_rad = _roll_ctrl.control(roll_sp, _att.roll, _att.rollspeed, - airspeed_scaling, lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed); - _actuators.control[0] = (isfinite(roll_rad)) ? roll_rad * actuator_scaling : 0.0f; + if (isfinite(roll_sp) && isfinite(pitch_sp)) { - float pitch_rad = _pitch_ctrl.control(pitch_sp, _att.pitch, _att.pitchspeed, _att.roll, airspeed_scaling, - lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed); - _actuators.control[1] = (isfinite(pitch_rad)) ? pitch_rad * actuator_scaling : 0.0f; + float roll_rad = _roll_ctrl.control(roll_sp, _att.roll, _att.rollspeed, + airspeed_scaling, lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed); + _actuators.control[0] = (isfinite(roll_rad)) ? roll_rad * actuator_scaling : 0.0f; - float yaw_rad = _yaw_ctrl.control(_att.roll, _att.yawspeed, _accel.y, airspeed_scaling, lock_integrator, - _parameters.airspeed_min, _parameters.airspeed_max, airspeed); - _actuators.control[2] = (isfinite(yaw_rad)) ? yaw_rad * actuator_scaling : 0.0f; + float pitch_rad = _pitch_ctrl.control(pitch_sp, _att.pitch, _att.pitchspeed, _att.roll, airspeed_scaling, + lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed); + _actuators.control[1] = (isfinite(pitch_rad)) ? pitch_rad * actuator_scaling : 0.0f; - /* throttle passed through */ - _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; + float yaw_rad = _yaw_ctrl.control(_att.roll, _att.yawspeed, _accel.y, airspeed_scaling, lock_integrator, + _parameters.airspeed_min, _parameters.airspeed_max, airspeed); + _actuators.control[2] = (isfinite(yaw_rad)) ? yaw_rad * actuator_scaling : 0.0f; - // warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown", - // airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1], - // _actuators.control[2], _actuators.control[3]); + /* throttle passed through */ + _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; - /* - * Lazily publish the rate setpoint (for analysis, the actuators are published below) - * only once available - */ - vehicle_rates_setpoint_s rates_sp; - rates_sp.roll = _roll_ctrl.get_desired_rate(); - rates_sp.pitch = _pitch_ctrl.get_desired_rate(); - rates_sp.yaw = 0.0f; // XXX not yet implemented + // warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown", + // airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1], + // _actuators.control[2], _actuators.control[3]); - rates_sp.timestamp = hrt_absolute_time(); + /* + * Lazily publish the rate setpoint (for analysis, the actuators are published below) + * only once available + */ + vehicle_rates_setpoint_s rates_sp; + rates_sp.roll = _roll_ctrl.get_desired_rate(); + rates_sp.pitch = _pitch_ctrl.get_desired_rate(); + rates_sp.yaw = 0.0f; // XXX not yet implemented - if (_rate_sp_pub > 0) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp); + rates_sp.timestamp = hrt_absolute_time(); - } else { - /* advertise and publish */ - _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); + if (_rate_sp_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp); + + } else { + /* advertise and publish */ + _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); + } } } else { From 1358d4cb88e70370014410353faf3e51afb1ceb6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 5 Nov 2013 07:44:16 +0100 Subject: [PATCH 866/872] Hotfix: Fix integrator parameters --- src/modules/fw_att_control/fw_att_control_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index b3973084f3..00a0dcd619 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -279,20 +279,20 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.p_i = param_find("FW_P_I"); _parameter_handles.p_rmax_pos = param_find("FW_P_RMAX_POS"); _parameter_handles.p_rmax_neg = param_find("FW_P_RMAX_NEG"); - _parameter_handles.p_integrator_max = param_find("FW_P_integrator_max"); + _parameter_handles.p_integrator_max = param_find("FW_P_IMAX"); _parameter_handles.p_roll_feedforward = param_find("FW_P_ROLLFF"); _parameter_handles.r_p = param_find("FW_R_P"); _parameter_handles.r_d = param_find("FW_R_D"); _parameter_handles.r_i = param_find("FW_R_I"); - _parameter_handles.r_integrator_max = param_find("FW_R_integrator_max"); + _parameter_handles.r_integrator_max = param_find("FW_R_IMAX"); _parameter_handles.r_rmax = param_find("FW_R_RMAX"); _parameter_handles.y_p = param_find("FW_Y_P"); _parameter_handles.y_i = param_find("FW_Y_I"); _parameter_handles.y_d = param_find("FW_Y_D"); _parameter_handles.y_roll_feedforward = param_find("FW_Y_ROLLFF"); - _parameter_handles.y_integrator_max = param_find("FW_Y_integrator_max"); + _parameter_handles.y_integrator_max = param_find("FW_Y_IMAX"); _parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN"); _parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM"); From d3b267c06e53aaf119b66717ac33e78834ea0d69 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 5 Nov 2013 09:20:07 +0100 Subject: [PATCH 867/872] Integral fixes, last parts --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 5 ++--- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 7 +++++-- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index d876f1a39b..2eb58abd66 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -62,8 +62,7 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc /* get the usual dt estimate */ uint64_t dt_micros = ecl_elapsed_time(&_last_run); _last_run = ecl_absolute_time(); - - float dt = dt_micros / 1000000; + float dt = (float)dt_micros * 1e-6f; /* lock integral for long intervals */ if (dt_micros > 500000) @@ -115,7 +114,7 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc _rate_error = _rate_setpoint - pitch_rate; - float ilimit_scaled = 0.0f; + float ilimit_scaled = _integrator_max * scaler; if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) { diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index b9a73fc029..0b1ffa7a4d 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -64,8 +64,11 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra /* get the usual dt estimate */ uint64_t dt_micros = ecl_elapsed_time(&_last_run); _last_run = ecl_absolute_time(); + float dt = (float)dt_micros * 1e-6f; - float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000; + /* lock integral for long intervals */ + if (dt_micros > 500000) + lock_integrator = true; float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); float k_i_rate = _k_i * _tc; @@ -90,7 +93,7 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra _rate_error = _rate_setpoint - roll_rate; - float ilimit_scaled = 0.0f; + float ilimit_scaled = _integrator_max * scaler; if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) { From 857c3d2efd49085dfd28827b06d96776340e5a09 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 5 Nov 2013 16:59:34 +0100 Subject: [PATCH 868/872] Startup scripts: Corrected cases where commander was not started, updated several outdated scripts --- ROMFS/px4fmu_common/init.d/100_mpx_easystar | 35 +++------------ ROMFS/px4fmu_common/init.d/101_hk_bixler | 40 ++++++----------- ROMFS/px4fmu_common/init.d/10_dji_f330 | 5 +-- ROMFS/px4fmu_common/init.d/11_dji_f450 | 2 +- ROMFS/px4fmu_common/init.d/12-13_hex | 15 ++++--- ROMFS/px4fmu_common/init.d/15_tbs_discovery | 10 ++--- ROMFS/px4fmu_common/init.d/16_3dr_iris | 4 +- ROMFS/px4fmu_common/init.d/30_io_camflyer | 42 ++++++------------ ROMFS/px4fmu_common/init.d/31_io_phantom | 42 ++++++------------ ROMFS/px4fmu_common/init.d/32_skywalker_x5 | 31 +++---------- ROMFS/px4fmu_common/init.d/666_fmu_q_x550 | 43 +++++++++++++------ .../init.d/rc.custom_dji_f330_mkblctrl | 2 +- ROMFS/px4fmu_common/init.d/rc.custom_io_esc | 4 +- ROMFS/px4fmu_common/init.d/rc.fixedwing | 39 +++++++++++++++++ ROMFS/px4fmu_common/init.d/rc.multirotor | 7 ++- 15 files changed, 147 insertions(+), 174 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/rc.fixedwing diff --git a/ROMFS/px4fmu_common/init.d/100_mpx_easystar b/ROMFS/px4fmu_common/init.d/100_mpx_easystar index 4f843e9aa1..b797ceebc8 100644 --- a/ROMFS/px4fmu_common/init.d/100_mpx_easystar +++ b/ROMFS/px4fmu_common/init.d/100_mpx_easystar @@ -52,8 +52,6 @@ then # Start MAVLink (depends on orb) mavlink start - commander start - sh /etc/init.d/rc.io # Limit to 100 Hz updates and (implicit) 50 Hz PWM px4io limit 100 @@ -61,46 +59,27 @@ else # Start MAVLink (on UART1 / ttyS0) mavlink start -d /dev/ttyS0 - commander start - fmu mode_pwm param set BAT_V_SCALING 0.004593 set EXIT_ON_END yes fi -# -# Start the sensors and test them. -# -sh /etc/init.d/rc.sensors - -# -# Start logging (depends on sensors) -# -sh /etc/init.d/rc.logging - -# -# Start GPS interface (depends on orb) -# -gps start - -# -# Start the attitude and position estimator -# -att_pos_estimator_ekf start - # # Load mixer and start controllers (depends on px4io) # if [ -f /fs/microsd/etc/mixers/FMU_RET.mix ] then - echo "Using FMU_RET mixer from sd card" + echo "Using /fs/microsd/etc/mixers/FMU_RET.mix" mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_RET.mix else - echo "Using standard FMU_RET mixer" + echo "Using /etc/mixers/FMU_RET.mix" mixer load /dev/pwm_output /etc/mixers/FMU_RET.mix fi -fw_att_control start -fw_pos_control_l1 start + +# +Start common fixedwing apps +# +sh /etc/init.d/rc.fixedwing if [ $EXIT_ON_END == yes ] then diff --git a/ROMFS/px4fmu_common/init.d/101_hk_bixler b/ROMFS/px4fmu_common/init.d/101_hk_bixler index cef86c34d0..920a24e2f6 100644 --- a/ROMFS/px4fmu_common/init.d/101_hk_bixler +++ b/ROMFS/px4fmu_common/init.d/101_hk_bixler @@ -52,8 +52,6 @@ then # Start MAVLink (depends on orb) mavlink start - commander start - sh /etc/init.d/rc.io # Limit to 100 Hz updates and (implicit) 50 Hz PWM px4io limit 100 @@ -61,39 +59,27 @@ else # Start MAVLink (on UART1 / ttyS0) mavlink start -d /dev/ttyS0 - commander start - fmu mode_pwm param set BAT_V_SCALING 0.004593 set EXIT_ON_END yes fi - -# -# Start the sensors and test them. -# -sh /etc/init.d/rc.sensors -# -# Start logging (depends on sensors) -# -sh /etc/init.d/rc.logging - -# -# Start GPS interface (depends on orb) -# -gps start - -# -# Start the attitude and position estimator -# -att_pos_estimator_ekf start - # # Load mixer and start controllers (depends on px4io) # -mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix -fw_att_control start -fw_pos_control_l1 start +if [ -f /fs/microsd/etc/mixers/FMU_AERT.mix ] +then + echo "Using /fs/microsd/etc/mixers/FMU_AERT.mix" + mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_AERT.mix +else + echo "Using /etc/mixers/FMU_Q.mix" + mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix +fi + +# +Start common fixedwing apps +# +sh /etc/init.d/rc.fixedwing if [ $EXIT_ON_END == yes ] then diff --git a/ROMFS/px4fmu_common/init.d/10_dji_f330 b/ROMFS/px4fmu_common/init.d/10_dji_f330 index 81ea292aae..467b56bbf9 100644 --- a/ROMFS/px4fmu_common/init.d/10_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/10_dji_f330 @@ -59,10 +59,7 @@ then mavlink start usleep 5000 - commander start - sh /etc/init.d/rc.io - # Set PWM values for DJI ESCs else # Start MAVLink (on UART1 / ttyS0) mavlink start -d /dev/ttyS0 @@ -83,7 +80,7 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix pwm rate -c 1234 -r 400 # -# Set disarmed, min and max PWM signals +# Set disarmed, min and max PWM signals (for DJI ESCs) # pwm disarmed -c 1234 -p 900 pwm min -c 1234 -p 1200 diff --git a/ROMFS/px4fmu_common/init.d/11_dji_f450 b/ROMFS/px4fmu_common/init.d/11_dji_f450 index 4dbf76cee7..818f9375e4 100644 --- a/ROMFS/px4fmu_common/init.d/11_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/11_dji_f450 @@ -73,7 +73,7 @@ pwm min -c 1234 -p 1200 pwm max -c 1234 -p 1800 # -# Start common for all multirotors apps +# Start common multirotor apps # sh /etc/init.d/rc.multirotor diff --git a/ROMFS/px4fmu_common/init.d/12-13_hex b/ROMFS/px4fmu_common/init.d/12-13_hex index 0f0bb05ced..f83f6cfd05 100644 --- a/ROMFS/px4fmu_common/init.d/12-13_hex +++ b/ROMFS/px4fmu_common/init.d/12-13_hex @@ -61,10 +61,6 @@ then usleep 5000 sh /etc/init.d/rc.io - # Set PWM values for DJI ESCs - px4io idle 900 900 900 900 900 900 - px4io min 1200 1200 1200 1200 1200 1200 - px4io max 1900 1900 1900 1900 1900 1900 else # Start MAVLink (on UART1 / ttyS0) mavlink start -d /dev/ttyS0 @@ -77,12 +73,19 @@ fi # # Load mixer # -mixer load /dev/pwm_output $MIXER +mixer load /dev/pwm_output /etc/mixers/FMU_hex_x.mix # # Set PWM output frequency to 400 Hz # -pwm -u 400 -m 0xff +pwm rate -c 123456 -r 400 + +# +# Set disarmed, min and max PWM signals +# +pwm disarmed -c 123456 -p 900 +pwm min -c 123456 -p 1100 +pwm max -c 123456 -p 1900 # # Start common for all multirotors apps diff --git a/ROMFS/px4fmu_common/init.d/15_tbs_discovery b/ROMFS/px4fmu_common/init.d/15_tbs_discovery index bd6189a6d3..c79e9d2830 100644 --- a/ROMFS/px4fmu_common/init.d/15_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/15_tbs_discovery @@ -31,7 +31,7 @@ fi # MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 - + set EXIT_ON_END no # @@ -43,8 +43,6 @@ then mavlink start usleep 5000 - commander start - sh /etc/init.d/rc.io else # Start MAVLink (on UART1 / ttyS0) @@ -66,11 +64,11 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix pwm rate -c 1234 -r 400 # -# Set disarmed, min and max PWM signals (for DJI ESCs) +# Set disarmed, min and max PWM signals # pwm disarmed -c 1234 -p 900 -pwm min -c 1234 -p 1200 -pwm max -c 1234 -p 1800 +pwm min -c 1234 -p 1100 +pwm max -c 1234 -p 1900 # # Start common for all multirotors apps diff --git a/ROMFS/px4fmu_common/init.d/16_3dr_iris b/ROMFS/px4fmu_common/init.d/16_3dr_iris index d8cc0e9132..6be9178786 100644 --- a/ROMFS/px4fmu_common/init.d/16_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/16_3dr_iris @@ -31,7 +31,7 @@ fi # MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 - + set EXIT_ON_END no # @@ -43,8 +43,6 @@ then mavlink start usleep 5000 - commander start - sh /etc/init.d/rc.io else # Start MAVLink (on UART1 / ttyS0) diff --git a/ROMFS/px4fmu_common/init.d/30_io_camflyer b/ROMFS/px4fmu_common/init.d/30_io_camflyer index 191d8cd95f..ffab26c384 100644 --- a/ROMFS/px4fmu_common/init.d/30_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/30_io_camflyer @@ -30,8 +30,6 @@ then # Start MAVLink (depends on orb) mavlink start - commander start - sh /etc/init.d/rc.io # Limit to 100 Hz updates and (implicit) 50 Hz PWM px4io limit 100 @@ -39,41 +37,29 @@ else # Start MAVLink (on UART1 / ttyS0) mavlink start -d /dev/ttyS0 - commander start - fmu mode_pwm param set BAT_V_SCALING 0.004593 set EXIT_ON_END yes fi - -# -# Start the sensors and test them. -# -sh /etc/init.d/rc.sensors -# -# Start logging (depends on sensors) -# -sh /etc/init.d/rc.logging - -# -# Start GPS interface (depends on orb) -# -gps start - -# -# Start the attitude and position estimator -# -att_pos_estimator_ekf start - # # Load mixer and start controllers (depends on px4io) # -mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix -fw_att_control start -fw_pos_control_l1 start +if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ] +then + echo "Using /fs/microsd/etc/mixers/FMU_Q.mix" + mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix +else + echo "Using /etc/mixers/FMU_Q.mix" + mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix +fi + +# +Start common fixedwing apps +# +sh /etc/init.d/rc.fixedwing if [ $EXIT_ON_END == yes ] then exit -fi +fi \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/31_io_phantom b/ROMFS/px4fmu_common/init.d/31_io_phantom index 6528337454..14607e2dfc 100644 --- a/ROMFS/px4fmu_common/init.d/31_io_phantom +++ b/ROMFS/px4fmu_common/init.d/31_io_phantom @@ -52,8 +52,6 @@ then # Start MAVLink (depends on orb) mavlink start - commander start - sh /etc/init.d/rc.io # Limit to 100 Hz updates and (implicit) 50 Hz PWM px4io limit 100 @@ -61,41 +59,29 @@ else # Start MAVLink (on UART1 / ttyS0) mavlink start -d /dev/ttyS0 - commander start - fmu mode_pwm param set BAT_V_SCALING 0.004593 set EXIT_ON_END yes fi - -# -# Start the sensors and test them. -# -sh /etc/init.d/rc.sensors -# -# Start logging (depends on sensors) -# -sh /etc/init.d/rc.logging - -# -# Start GPS interface (depends on orb) -# -gps start - -# -# Start the attitude and position estimator -# -att_pos_estimator_ekf start - # # Load mixer and start controllers (depends on px4io) # -mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix -fw_att_control start -fw_pos_control_l1 start +if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ] +then + echo "Using /fs/microsd/etc/mixers/FMU_Q.mix" + mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix +else + echo "Using /etc/mixers/FMU_Q.mix" + mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix +fi + +# +Start common fixedwing apps +# +sh /etc/init.d/rc.fixedwing if [ $EXIT_ON_END == yes ] then exit -fi +fi \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/32_skywalker_x5 b/ROMFS/px4fmu_common/init.d/32_skywalker_x5 index cd7677112d..11071613c4 100644 --- a/ROMFS/px4fmu_common/init.d/32_skywalker_x5 +++ b/ROMFS/px4fmu_common/init.d/32_skywalker_x5 @@ -30,8 +30,6 @@ then # Start MAVLink (depends on orb) mavlink start - commander start - sh /etc/init.d/rc.io # Limit to 100 Hz updates and (implicit) 50 Hz PWM px4io limit 100 @@ -39,32 +37,10 @@ else # Start MAVLink (on UART1 / ttyS0) mavlink start -d /dev/ttyS0 - commander start - fmu mode_pwm param set BAT_V_SCALING 0.004593 set EXIT_ON_END yes fi - -# -# Start the sensors and test them. -# -sh /etc/init.d/rc.sensors - -# -# Start logging (depends on sensors) -# -sh /etc/init.d/rc.logging - -# -# Start GPS interface (depends on orb) -# -gps start - -# -# Start the attitude and position estimator -# -att_pos_estimator_ekf start # # Load mixer and start controllers (depends on px4io) @@ -77,8 +53,11 @@ else echo "Using /etc/mixers/FMU_Q.mix" mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix fi -fw_att_control start -fw_pos_control_l1 start + +# +Start common fixedwing apps +# +sh /etc/init.d/rc.fixedwing if [ $EXIT_ON_END == yes ] then diff --git a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 index eae37098bd..1ee84b9b01 100644 --- a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 +++ b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 @@ -1,6 +1,6 @@ #!nsh -echo "[init] 666_fmu_q_x550: PX4FMU Quad X550 with PWM outputs" +echo "[init] 666_fmu_q_x550: PX4FMU Quad X550 with or without IO" # # Load default params for this platform @@ -33,17 +33,27 @@ fi # MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 - -# -# Start MAVLink -# -mavlink start -d /dev/ttyS0 -b 57600 -usleep 5000 + +set EXIT_ON_END no # -# Start PWM output +# Start and configure PX4IO or FMU interface # -fmu mode_pwm +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start + usleep 5000 + + sh /etc/init.d/rc.io +else + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + usleep 5000 + fmu mode_pwm + param set BAT_V_SCALING 0.004593 + set EXIT_ON_END yes +fi # # Load mixer @@ -55,10 +65,19 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix # pwm rate -c 1234 -r 400 +# +# Set disarmed, min and max PWM signals +# +pwm disarmed -c 1234 -p 900 +pwm min -c 1234 -p 1100 +pwm max -c 1234 -p 1900 + # # Start common for all multirotors apps # sh /etc/init.d/rc.multirotor - -# Exit, because /dev/ttyS0 is needed for MAVLink -exit + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl index a63d0e5f1d..4686382ca6 100644 --- a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl +++ b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl @@ -84,10 +84,10 @@ then sh /etc/init.d/rc.io else - fmu mode_pwm # Start MAVLink (on UART1 / ttyS0) mavlink start -d /dev/ttyS0 usleep 5000 + fmu mode_pwm param set BAT_V_SCALING 0.004593 set EXIT_ON_END yes fi diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_io_esc b/ROMFS/px4fmu_common/init.d/rc.custom_io_esc index 0c0cfa53d3..045e41e529 100644 --- a/ROMFS/px4fmu_common/init.d/rc.custom_io_esc +++ b/ROMFS/px4fmu_common/init.d/rc.custom_io_esc @@ -60,9 +60,7 @@ then # Start MAVLink (depends on orb) mavlink start -d /dev/ttyS1 -b 57600 usleep 5000 - - commander start - + sh /etc/init.d/rc.io else fmu mode_pwm diff --git a/ROMFS/px4fmu_common/init.d/rc.fixedwing b/ROMFS/px4fmu_common/init.d/rc.fixedwing new file mode 100644 index 0000000000..79e34a3b97 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.fixedwing @@ -0,0 +1,39 @@ +#!nsh +# +# Standard everything needed for fixedwing except mixer, actuator output and mavlink +# + +# +# Start the Commander +# +commander start + +# +# Start the sensors and test them. +# +sh /etc/init.d/rc.sensors + +# +# Start logging (depends on sensors) +# +sh /etc/init.d/rc.logging + +# +# Start GPS interface (depends on orb) +# +gps start + +# +# Start the attitude and position estimator +# +att_pos_estimator_ekf start + +# +# Start attitude controller +# +fw_att_control start + +# +# Start the position controller +# +fw_pos_control_l1 start diff --git a/ROMFS/px4fmu_common/init.d/rc.multirotor b/ROMFS/px4fmu_common/init.d/rc.multirotor index dfff07198a..6bae991754 100644 --- a/ROMFS/px4fmu_common/init.d/rc.multirotor +++ b/ROMFS/px4fmu_common/init.d/rc.multirotor @@ -1,8 +1,13 @@ #!nsh # -# Standard everything needed for multirotors except mixer, output and mavlink +# Standard everything needed for multirotors except mixer, actuator output and mavlink # +# +# Start the Commander +# +commander start + # # Start the sensors and test them. # From 4502c285eb8b284b7c08666b6d0e3e81035bace3 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 5 Nov 2013 19:56:33 +0100 Subject: [PATCH 869/872] Startup scripts: Start the commander early and let it try to open the mavlink_fd with 20Hz --- ROMFS/px4fmu_common/init.d/rc.fixedwing | 5 ----- ROMFS/px4fmu_common/init.d/rc.multirotor | 5 ----- ROMFS/px4fmu_common/init.d/rcS | 5 +++++ src/modules/commander/commander.cpp | 17 +++++++---------- 4 files changed, 12 insertions(+), 20 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.fixedwing b/ROMFS/px4fmu_common/init.d/rc.fixedwing index 79e34a3b97..f028510064 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fixedwing +++ b/ROMFS/px4fmu_common/init.d/rc.fixedwing @@ -3,11 +3,6 @@ # Standard everything needed for fixedwing except mixer, actuator output and mavlink # -# -# Start the Commander -# -commander start - # # Start the sensors and test them. # diff --git a/ROMFS/px4fmu_common/init.d/rc.multirotor b/ROMFS/px4fmu_common/init.d/rc.multirotor index 6bae991754..bc550ac5a1 100644 --- a/ROMFS/px4fmu_common/init.d/rc.multirotor +++ b/ROMFS/px4fmu_common/init.d/rc.multirotor @@ -3,11 +3,6 @@ # Standard everything needed for multirotors except mixer, actuator output and mavlink # -# -# Start the Commander -# -commander start - # # Start the sensors and test them. # diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index cff8446a6b..d8b5cb6087 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -105,6 +105,11 @@ then blinkm systemstate fi fi + + # + # Start the Commander (needs to be this early for in-air-restarts) + # + commander start if param compare SYS_AUTOSTART 1000 then diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ed090271ca..ace13bb784 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -116,6 +116,8 @@ extern struct system_load_s system_load; #define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) #define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) +#define MAVLINK_OPEN_INTERVAL 50000 + #define STICK_ON_OFF_LIMIT 0.75f #define STICK_THRUST_RANGE 1.0f #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 @@ -582,16 +584,6 @@ int commander_thread_main(int argc, char *argv[]) mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - if (mavlink_fd < 0) { - /* 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 */ /* make sure we are in preflight state */ memset(&status, 0, sizeof(status)); @@ -770,6 +762,11 @@ int commander_thread_main(int argc, char *argv[]) while (!thread_should_exit) { + if (mavlink_fd < 0 && counter % (1000000 / MAVLINK_OPEN_INTERVAL) == 0) { + /* try to open the mavlink log device every once in a while */ + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + } + /* update parameters */ orb_check(param_changed_sub, &updated); From 9b08923cd2422117d8c1e05fb0ccddc4bf00528f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 6 Nov 2013 17:01:29 +0100 Subject: [PATCH 870/872] remove commander from hil startup scripts --- ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil | 5 ----- ROMFS/px4fmu_common/init.d/1001_rc_quad.hil | 5 ----- ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil | 5 ----- ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil | 5 ----- ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil | 5 ----- 5 files changed, 25 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil index 5e4028bbb8..40a13b5d17 100644 --- a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil @@ -55,11 +55,6 @@ hil mode_pwm # param set MAV_TYPE 1 -# -# Start the commander (depends on orb, mavlink) -# -commander start - # # Check if we got an IO # diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil index bbe3b7e288..9b664d63ed 100644 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil @@ -60,11 +60,6 @@ hil mode_pwm # param set MAV_TYPE 2 -# -# Start the commander (depends on orb, mavlink) -# -commander start - # # Check if we got an IO # diff --git a/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil b/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil index d5782a89b8..7b9f41bf6d 100644 --- a/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil +++ b/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil @@ -52,11 +52,6 @@ hil mode_pwm # param set MAV_TYPE 1 -# -# Start the commander (depends on orb, mavlink) -# -commander start - # # Check if we got an IO # diff --git a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil index 1c85e04f2a..0cc07ad343 100644 --- a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil +++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil @@ -60,11 +60,6 @@ hil mode_pwm # param set MAV_TYPE 2 -# -# Start the commander (depends on orb, mavlink) -# -commander start - # # Check if we got an IO # diff --git a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil index 8c5e4b6a8f..344d784224 100644 --- a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil +++ b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil @@ -55,11 +55,6 @@ hil mode_pwm # param set MAV_TYPE 1 -# -# Start the commander (depends on orb, mavlink) -# -commander start - # # Check if we got an IO # From 2af2bacc4fe081852da40a329b275d17629a705c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 6 Nov 2013 23:41:15 +0100 Subject: [PATCH 871/872] Startup scripts: fixed stupid typo --- ROMFS/px4fmu_common/init.d/100_mpx_easystar | 2 +- ROMFS/px4fmu_common/init.d/101_hk_bixler | 2 +- ROMFS/px4fmu_common/init.d/30_io_camflyer | 2 +- ROMFS/px4fmu_common/init.d/31_io_phantom | 2 +- ROMFS/px4fmu_common/init.d/32_skywalker_x5 | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/100_mpx_easystar b/ROMFS/px4fmu_common/init.d/100_mpx_easystar index b797ceebc8..abe378b22f 100644 --- a/ROMFS/px4fmu_common/init.d/100_mpx_easystar +++ b/ROMFS/px4fmu_common/init.d/100_mpx_easystar @@ -77,7 +77,7 @@ else fi # -Start common fixedwing apps +# Start common fixedwing apps # sh /etc/init.d/rc.fixedwing diff --git a/ROMFS/px4fmu_common/init.d/101_hk_bixler b/ROMFS/px4fmu_common/init.d/101_hk_bixler index 920a24e2f6..c616da9886 100644 --- a/ROMFS/px4fmu_common/init.d/101_hk_bixler +++ b/ROMFS/px4fmu_common/init.d/101_hk_bixler @@ -77,7 +77,7 @@ else fi # -Start common fixedwing apps +# Start common fixedwing apps # sh /etc/init.d/rc.fixedwing diff --git a/ROMFS/px4fmu_common/init.d/30_io_camflyer b/ROMFS/px4fmu_common/init.d/30_io_camflyer index ffab26c384..8a8bc15903 100644 --- a/ROMFS/px4fmu_common/init.d/30_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/30_io_camflyer @@ -55,7 +55,7 @@ else fi # -Start common fixedwing apps +# Start common fixedwing apps # sh /etc/init.d/rc.fixedwing diff --git a/ROMFS/px4fmu_common/init.d/31_io_phantom b/ROMFS/px4fmu_common/init.d/31_io_phantom index 14607e2dfc..63cd7f9b2e 100644 --- a/ROMFS/px4fmu_common/init.d/31_io_phantom +++ b/ROMFS/px4fmu_common/init.d/31_io_phantom @@ -77,7 +77,7 @@ else fi # -Start common fixedwing apps +# Start common fixedwing apps # sh /etc/init.d/rc.fixedwing diff --git a/ROMFS/px4fmu_common/init.d/32_skywalker_x5 b/ROMFS/px4fmu_common/init.d/32_skywalker_x5 index 11071613c4..1e752f13a2 100644 --- a/ROMFS/px4fmu_common/init.d/32_skywalker_x5 +++ b/ROMFS/px4fmu_common/init.d/32_skywalker_x5 @@ -55,7 +55,7 @@ else fi # -Start common fixedwing apps +# Start common fixedwing apps # sh /etc/init.d/rc.fixedwing From c63995e91c188b476aa2608b42a366f68dced423 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 8 Nov 2013 14:22:27 +0100 Subject: [PATCH 872/872] Hotfix: Be more aggressive about SPI2 init on v1 boards --- src/drivers/boards/px4fmu-v1/px4fmu_init.c | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_init.c b/src/drivers/boards/px4fmu-v1/px4fmu_init.c index 964f5069c6..4b12b75f9e 100644 --- a/src/drivers/boards/px4fmu-v1/px4fmu_init.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_init.c @@ -222,14 +222,9 @@ __EXPORT int nsh_archinitialize(void) * 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] 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 { + #ifdef CONFIG_STM32_SPI2 + spi2 = up_spiinitialize(2); /* Default SPI2 to 1MHz and de-assert the known chip selects. */ SPI_SETFREQUENCY(spi2, 10000000); SPI_SETBITS(spi2, 8); @@ -238,7 +233,13 @@ __EXPORT int nsh_archinitialize(void) SPI_SELECT(spi2, PX4_SPIDEV_ACCEL_MAG, false); message("[boot] Initialized SPI port2 (ADC IN12/13 blocked)\n"); - } + #else + spi2 = NULL; + 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 + #endif /* Get the SPI port for the microSD slot */

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 420/872] 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 e943488e9f85b7e8982bf137dbbe7f8183da21bf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 20 Aug 2013 10:07:37 +0200 Subject: [PATCH 421/872] Show values when selftest fails --- src/systemcmds/config/config.c | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index 42814f2b2f..5a02fd6206 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -135,7 +135,11 @@ do_gyro(int argc, char *argv[]) int ret = ioctl(fd, GYROIOCSELFTEST, 0); if (ret) { - warnx("gyro self test FAILED! Check calibration."); + 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"); } @@ -177,6 +181,10 @@ do_mag(int argc, char *argv[]) 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"); } @@ -237,6 +245,10 @@ do_accel(int argc, char *argv[]) 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"); } From 6dcad01d03251093da2ee4ee0026672b224aae72 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 20 Aug 2013 10:35:23 +0200 Subject: [PATCH 422/872] 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 ae3a549d5780c58d54a9b54186fc309720bbfde6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 20 Aug 2013 10:35:23 +0200 Subject: [PATCH 423/872] 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 ce8fe9feec..4f70756006 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 424/872] 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 75a6e095676a7951c7077f757b7a10ea1399729f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 20 Aug 2013 13:20:34 +0200 Subject: [PATCH 425/872] Ignore files for ctags (used with SublimeText3) --- .gitignore | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index 3fdc1bf2a2..76f45281ce 100644 --- a/.gitignore +++ b/.gitignore @@ -27,4 +27,6 @@ mavlink/include/mavlink/v0.9/ /NuttX /Documentation/doxy.log /Documentation/html/ -/Documentation/doxygen*objdb*tmp \ No newline at end of file +/Documentation/doxygen*objdb*tmp +.tags +.tags_sorted_by_file \ No newline at end of file From d2d59aa39278de9461ff72061cbd8a89d7e81f4b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 20 Aug 2013 13:04:57 +0200 Subject: [PATCH 426/872] 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 427/872] 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 428/872] 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 429/872] 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 430/872] 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 431/872] 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 432/872] 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 433/872] 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 434/872] 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 435/872] 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 436/872] 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 437/872] 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 438/872] 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 439/872] 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 440/872] 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 441/872] 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 442/872] 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 443/872] 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 444/872] 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 445/872] 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 446/872] 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 447/872] 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 448/872] 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 449/872] 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 450/872] 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 966cee66dfaf63abfe3b96c6066d92d204483820 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 22 Aug 2013 15:32:58 +0200 Subject: [PATCH 451/872] Add navigator - not enabled for compilation, WIP --- src/modules/navigator/module.mk | 41 ++ src/modules/navigator/navigator_main.cpp | 604 +++++++++++++++++++++++ src/modules/navigator/navigator_params.c | 53 ++ 3 files changed, 698 insertions(+) create mode 100644 src/modules/navigator/module.mk create mode 100644 src/modules/navigator/navigator_main.cpp create mode 100644 src/modules/navigator/navigator_params.c diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk new file mode 100644 index 0000000000..0404b06c78 --- /dev/null +++ b/src/modules/navigator/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Main Navigation Controller +# + +MODULE_COMMAND = navigator + +SRCS = navigator_main.cpp \ + navigator_params.c diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp new file mode 100644 index 0000000000..f6c44444aa --- /dev/null +++ b/src/modules/navigator/navigator_main.cpp @@ -0,0 +1,604 @@ +/**************************************************************************** + * + * 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 navigator_main.c + * Implementation of the main navigation state machine. + * + * Handles missions, geo fencing and failsafe navigation behavior. + */ + +#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 +#include +#include +#include + +/** + * navigator app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int navigator_main(int argc, char *argv[]); + +class Navigator +{ +public: + /** + * Constructor + */ + Navigator(); + + /** + * Destructor, also kills the sensors task. + */ + ~Navigator(); + + /** + * Start the sensors task. + * + * @return OK on success. + */ + int start(); + +private: + + bool _task_should_exit; /**< if true, sensor task should exit */ + int _navigator_task; /**< task handle for sensor task */ + + int _global_pos_sub; + int _att_sub; /**< vehicle attitude subscription */ + int _attitude_sub; /**< raw rc channels data subscription */ + int _airspeed_sub; /**< airspeed subscription */ + int _vstatus_sub; /**< vehicle status subscription */ + int _params_sub; /**< notification of parameter updates */ + int _manual_control_sub; /**< notification of manual control updates */ + int _mission_sub; + + orb_advert_t _triplet_pub; /**< position setpoint */ + + struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ + struct manual_control_setpoint_s _manual; /**< r/c channel data */ + struct airspeed_s _airspeed; /**< airspeed */ + struct vehicle_status_s _vstatus; /**< vehicle status */ + struct vehicle_global_position_s _global_pos; /**< global vehicle position */ + struct vehicle_global_position_set_triplet_s _global_triplet; /**< triplet of global setpoints */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + unsigned _mission_items_maxcount; /**< maximum number of mission items supported */ + struct mission_item_s * _mission_items; /**< storage for mission items */ + bool _mission_valid; /**< flag if mission is valid */ + + /** manual control states */ + float _seatbelt_hold_heading; /**< heading the system should hold in seatbelt mode */ + float _loiter_hold_lat; + float _loiter_hold_lon; + float _loiter_hold_alt; + bool _loiter_hold; + + struct { + float throttle_cruise; + } _parameters; /**< local copies of interesting parameters */ + + struct { + param_t throttle_cruise; + + } _parameter_handles; /**< handles for interesting parameters */ + + + /** + * Update our local parameter cache. + */ + int parameters_update(); + + /** + * Update control outputs + * + */ + void control_update(); + + /** + * Check for changes in vehicle status. + */ + void vehicle_status_poll(); + + /** + * Check for position updates. + */ + void vehicle_attitude_poll(); + + /** + * Check for set triplet updates. + */ + void mission_poll(); + + /** + * Control throttle. + */ + float control_throttle(float energy_error); + + /** + * Control pitch. + */ + float control_pitch(float altitude_error); + + void calculate_airspeed_errors(); + void calculate_gndspeed_undershoot(); + void calculate_altitude_error(); + + /** + * Shim for calling task_main from task_create. + */ + static void task_main_trampoline(int argc, char *argv[]); + + /** + * Main sensor collection task. + */ + void task_main() __attribute__((noreturn)); +}; + +namespace navigator +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +Navigator *g_navigator; +} + +Navigator::Navigator() : + + _task_should_exit(false), + _navigator_task(-1), + +/* subscriptions */ + _global_pos_sub(-1), + _att_sub(-1), + _airspeed_sub(-1), + _vstatus_sub(-1), + _params_sub(-1), + _manual_control_sub(-1), + +/* publications */ + _triplet_pub(-1), + +/* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), +/* states */ + _mission_items_maxcount(20), + _mission_valid(false), + _loiter_hold(false) +{ + _mission_items = (mission_item_s*)malloc(sizeof(mission_item_s) * _mission_items_maxcount); + if (!_mission_items) { + _mission_items_maxcount = 0; + warnx("no free RAM to allocate mission, rejecting any waypoints"); + } + + _parameter_handles.throttle_cruise = param_find("NAV_DUMMY"); + + /* fetch initial parameter values */ + parameters_update(); +} + +Navigator::~Navigator() +{ + if (_navigator_task != -1) { + + /* task wakes up every 100ms or so at the longest */ + _task_should_exit = true; + + /* wait for a second for the task to quit at our request */ + unsigned i = 0; + + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + task_delete(_navigator_task); + break; + } + } while (_navigator_task != -1); + } + + navigator::g_navigator = nullptr; +} + +int +Navigator::parameters_update() +{ + + //param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise)); + + return OK; +} + +void +Navigator::vehicle_status_poll() +{ + bool vstatus_updated; + + /* Check HIL state if vehicle status has changed */ + orb_check(_vstatus_sub, &vstatus_updated); + + if (vstatus_updated) { + + orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus); + } +} + +void +Navigator::vehicle_attitude_poll() +{ + /* check if there is a new position */ + bool att_updated; + orb_check(_att_sub, &att_updated); + + if (att_updated) { + orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); + } +} + +void +Navigator::mission_poll() +{ + /* check if there is a new setpoint */ + bool mission_updated; + orb_check(_mission_sub, &mission_updated); + + if (mission_updated) { + + struct mission_s mission; + orb_copy(ORB_ID(mission), _mission_sub, &mission); + + // XXX this is not optimal yet, but a first prototype / + // test implementation + + if (mission.count <= _mission_items_maxcount) { + /* + * Perform an atomic copy & state update + */ + irqstate_t flags = irqsave(); + + memcpy(_mission_items, mission.items, mission.count * sizeof(struct mission_item_s)); + _mission_valid = true; + + irqrestore(flags); + } else { + warnx("mission larger than storage space"); + } + } +} + +void +Navigator::task_main_trampoline(int argc, char *argv[]) +{ + navigator::g_navigator->task_main(); +} + +void +Navigator::task_main() +{ + + /* inform about start */ + warnx("Initializing.."); + fflush(stdout); + + /* + * do subscriptions + */ + _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + _mission_sub = orb_subscribe(ORB_ID(mission)); + _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + _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); + /* rate limit position updates to 50 Hz */ + orb_set_interval(_global_pos_sub, 20); + + parameters_update(); + + /* wakeup source(s) */ + struct pollfd fds[2]; + + /* Setup of loop */ + fds[0].fd = _params_sub; + fds[0].events = POLLIN; + fds[1].fd = _global_pos_sub; + fds[1].events = POLLIN; + + while (!_task_should_exit) { + + /* wait for up to 500ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + + /* timed out - periodic check for _task_should_exit, etc. */ + if (pret == 0) + continue; + + /* 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; + } + + perf_begin(_loop_perf); + + /* check vehicle status for changes to publication state */ + vehicle_status_poll(); + + /* only update parameters if they changed */ + if (fds[0].revents & POLLIN) { + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), _params_sub, &update); + + /* update parameters from storage */ + parameters_update(); + } + + /* only run controller if position changed */ + if (fds[1].revents & POLLIN) { + + + static uint64_t last_run = 0; + float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; + last_run = hrt_absolute_time(); + + /* guard against too large deltaT's */ + if (deltaT > 1.0f) + deltaT = 0.01f; + + /* load local copies */ + orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); + + vehicle_attitude_poll(); + + mission_poll(); + + math::Vector2f ground_speed(_global_pos.vx, _global_pos.vy); + // Current waypoint + math::Vector2f next_wp(_global_triplet.current.lat / 1e7f, _global_triplet.current.lon / 1e7f); + // Global position + math::Vector2f current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f); + + /* AUTONOMOUS FLIGHT */ + + if (1 /* autonomous flight */) { + + /* execute navigation once we have a setpoint */ + if (_mission_valid) { + + // Next waypoint + math::Vector2f prev_wp; + + if (_global_triplet.previous_valid) { + prev_wp.setX(_global_triplet.previous.lat / 1e7f); + prev_wp.setY(_global_triplet.previous.lon / 1e7f); + + } else { + /* + * No valid next waypoint, go for heading hold. + * This is automatically handled by the L1 library. + */ + prev_wp.setX(_global_triplet.current.lat / 1e7f); + prev_wp.setY(_global_triplet.current.lon / 1e7f); + + } + + + + /******** MAIN NAVIGATION STATE MACHINE ********/ + + // XXX to be put in its own class + + if (_global_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) { + /* waypoint is a plain navigation waypoint */ + + + } else if (_global_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + _global_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _global_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { + + /* waypoint is a loiter waypoint */ + + } + + // XXX at this point we always want no loiter hold if a + // mission is active + _loiter_hold = false; + + } else { + + if (!_loiter_hold) { + _loiter_hold_lat = _global_pos.lat / 1e7f; + _loiter_hold_lon = _global_pos.lon / 1e7f; + _loiter_hold_alt = _global_pos.alt; + _loiter_hold = true; + } + + //_parameters.loiter_hold_radius + } + + } else if (0/* seatbelt mode enabled */) { + + /** SEATBELT FLIGHT **/ + continue; + + } else { + + /** MANUAL FLIGHT **/ + + /* no flight mode applies, do not publish an attitude setpoint */ + continue; + } + + /******** MAIN NAVIGATION STATE MACHINE ********/ + + if (_global_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { + // XXX define launch position and return + + } else if (_global_triplet.current.nav_cmd == NAV_CMD_LAND) { + // XXX flared descent on final approach + + } else if (_global_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { + + /* apply minimum pitch if altitude has not yet been reached */ + if (_global_pos.alt < _global_triplet.current.altitude) { + _att_sp.pitch_body = math::max(_att_sp.pitch_body, _global_triplet.current.param1); + } + } + + /* lazily publish the setpoint only once available */ + if (_triplet_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_global_position_set_triplet), _triplet_pub, &_global_triplet); + + } else { + /* advertise and publish */ + _triplet_pub = orb_advertise(ORB_ID(vehicle_global_position_set_triplet), &_global_triplet); + } + + } + + perf_end(_loop_perf); + } + + warnx("exiting.\n"); + + _navigator_task = -1; + _exit(0); +} + +int +Navigator::start() +{ + ASSERT(_navigator_task == -1); + + /* start the task */ + _navigator_task = task_spawn_cmd("navigator", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2048, + (main_t)&Navigator::task_main_trampoline, + nullptr); + + if (_navigator_task < 0) { + warn("task start failed"); + return -errno; + } + + return OK; +} + +int navigator_main(int argc, char *argv[]) +{ + if (argc < 1) + errx(1, "usage: navigator {start|stop|status}"); + + if (!strcmp(argv[1], "start")) { + + if (navigator::g_navigator != nullptr) + errx(1, "already running"); + + navigator::g_navigator = new Navigator; + + if (navigator::g_navigator == nullptr) + errx(1, "alloc failed"); + + if (OK != navigator::g_navigator->start()) { + delete navigator::g_navigator; + navigator::g_navigator = nullptr; + err(1, "start failed"); + } + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + if (navigator::g_navigator == nullptr) + errx(1, "not running"); + + delete navigator::g_navigator; + navigator::g_navigator = nullptr; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (navigator::g_navigator) { + errx(0, "running"); + + } else { + errx(1, "not running"); + } + } + + warnx("unrecognized command"); + return 1; +} diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c new file mode 100644 index 0000000000..06df9a4526 --- /dev/null +++ b/src/modules/navigator/navigator_params.c @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * 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 navigator_params.c + * + * Parameters defined by the navigator task. + * + * @author Lorenz Meier + */ + +#include + +#include + +/* + * Navigator parameters, accessible via MAVLink + * + */ + +PARAM_DEFINE_FLOAT(NAV_DUMMY, 0.0f); + From ab5ec0da0b4afcb3f68eaf70efebe691513f74ce Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 22 Aug 2013 15:53:19 +0200 Subject: [PATCH 452/872] 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 453/872] 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 454/872] 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 455/872] 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 456/872] 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 457/872] 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 458/872] 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 459/872] 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 460/872] 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 461/872] 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 462/872] 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 463/872] 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 464/872] 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 1fed9ef1b1ec2d0bf7c2cba7d60be77e37faaf40 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 23 Aug 2013 13:37:58 +0200 Subject: [PATCH 465/872] Make px_mkfw.py and px_upload.py compatible with both python 2.7 and 3.3 --- Tools/px_mkfw.py | 8 ++++---- Tools/px_uploader.py | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/Tools/px_mkfw.py b/Tools/px_mkfw.py index faef9ca335..b598a65a1a 100755 --- a/Tools/px_mkfw.py +++ b/Tools/px_mkfw.py @@ -1,7 +1,7 @@ #!/usr/bin/env python ############################################################################ # -# 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 @@ -60,7 +60,7 @@ def mkdesc(): proto['description'] = "" proto['git_identity'] = "" proto['build_time'] = 0 - proto['image'] = base64.b64encode(bytearray()) + proto['image'] = bytes() proto['image_size'] = 0 return proto @@ -99,12 +99,12 @@ if args.description != None: 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() + desc['git_identity'] = str(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)) + desc['image'] = base64.b64encode(zlib.compress(bytes,9)).decode('utf-8') print(json.dumps(desc, indent=4)) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index d2ebf16987..a5277ed21a 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -1,7 +1,7 @@ #!/usr/bin/env python ############################################################################ # -# 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 @@ -102,7 +102,7 @@ class firmware(object): 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') + crcpad = bytearray(b'\xff\xff\xff\xff') def __init__(self, path): From a897b3d88e5d1c3e8c005d0d5fd7b0dacd434ed0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 Aug 2013 16:28:53 +0200 Subject: [PATCH 466/872] Added complete attitude control framework --- makefiles/config_px4fmu-v1_default.mk | 4 +- makefiles/config_px4fmu-v2_default.mk | 5 +- .../ecl/attitude_fw/ecl_pitch_controller.cpp | 60 ++ .../ecl/attitude_fw/ecl_pitch_controller.h | 106 +++ .../ecl/attitude_fw/ecl_roll_controller.cpp | 62 ++ src/lib/ecl/attitude_fw/ecl_roll_controller.h | 97 +++ .../ecl/attitude_fw/ecl_yaw_controller.cpp | 63 ++ src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 89 ++ src/lib/ecl/module.mk | 40 + .../fw_att_control/fw_att_control_main.cpp | 770 ++++++++++++++++++ .../fw_att_control/fw_att_control_params.c | 136 ++++ src/modules/fw_att_control/module.mk | 41 + 12 files changed, 1469 insertions(+), 4 deletions(-) create mode 100644 src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp create mode 100644 src/lib/ecl/attitude_fw/ecl_pitch_controller.h create mode 100644 src/lib/ecl/attitude_fw/ecl_roll_controller.cpp create mode 100644 src/lib/ecl/attitude_fw/ecl_roll_controller.h create mode 100644 src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp create mode 100644 src/lib/ecl/attitude_fw/ecl_yaw_controller.h create mode 100644 src/lib/ecl/module.mk create mode 100644 src/modules/fw_att_control/fw_att_control_main.cpp create mode 100644 src/modules/fw_att_control/fw_att_control_params.c create mode 100644 src/modules/fw_att_control/module.mk diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index a556d0b073..452ab8a924 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -79,7 +79,7 @@ MODULES += examples/flow_position_estimator # #MODULES += modules/segway # XXX needs state machine update #MODULES += modules/fw_pos_control_l1 -#MODULES += modules/fw_att_control +MODULES += modules/fw_att_control MODULES += modules/multirotor_att_control MODULES += modules/multirotor_pos_control MODULES += examples/flow_position_control @@ -104,7 +104,7 @@ MODULES += modules/uORB LIBRARIES += lib/mathlib/CMSIS MODULES += lib/mathlib MODULES += lib/mathlib/math/filter -#MODULES += lib/ecl +MODULES += lib/ecl MODULES += lib/geo # diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 20dbe717fd..bd18a25cd5 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -73,8 +73,9 @@ MODULES += examples/flow_position_estimator # # Vehicle Control # +#MODULES += modules/segway # XXX needs state machine update #MODULES += modules/fw_pos_control_l1 -#MODULES += modules/fw_att_control +MODULES += modules/fw_att_control MODULES += modules/multirotor_att_control MODULES += modules/multirotor_pos_control @@ -97,7 +98,7 @@ MODULES += modules/uORB LIBRARIES += lib/mathlib/CMSIS MODULES += lib/mathlib MODULES += lib/mathlib/math/filter -#MODULES += lib/ecl +MODULES += lib/ecl MODULES += lib/geo # diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp new file mode 100644 index 0000000000..0faba287d8 --- /dev/null +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). 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 APL 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 ecl_pitch_controller.cpp + * Implementation of a simple orthogonal pitch PID controller. + * + */ + +#include "ecl_pitch_controller.h" + +ECL_PitchController::ECL_PitchController() : + _last_run(0), + _last_output(0.0f), + _integrator(0.0f), + _rate_error(0.0f), + _desired_rate(0.0f) +{ +} + +float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler, + bool lock_integrator, float airspeed_min, float airspeed_max, float aspeed) +{ + return 0.0f; +} + +void ECL_PitchController::reset_integrator() +{ + _integrator = 0.0f; +} diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h new file mode 100644 index 0000000000..391f90b9df --- /dev/null +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h @@ -0,0 +1,106 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). 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 APL 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 ecl_pitch_controller.h + * Definition of a simple orthogonal pitch PID controller. + * + */ + +#ifndef ECL_PITCH_CONTROLLER_H +#define ECL_PITCH_CONTROLLER_H + +#include +#include + +class __EXPORT ECL_PitchController +{ +public: + ECL_PitchController(); + + float control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler = 1.0f, + bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float aspeed = (0.0f / 0.0f)); + + void reset_integrator(); + + void set_time_constant(float time_constant) { + _tc = time_constant; + } + void set_k_p(float k_p) { + _k_p = k_p; + } + void set_k_i(float k_i) { + _k_i = k_i; + } + void set_k_d(float k_d) { + _k_d = k_d; + } + void set_integrator_max(float max) { + _integrator_max = max; + } + void set_max_rate_pos(float max_rate_pos) { + _max_rate_pos = max_rate_pos; + } + void set_max_rate_neg(float max_rate_neg) { + _max_rate_neg = max_rate_neg; + } + void set_roll_ff(float roll_ff) { + _roll_ff = roll_ff; + } + + float get_rate_error() { + return _rate_error; + } + + float get_desired_rate() { + return _desired_rate; + } + +private: + + uint64_t _last_run; + float _tc; + float _k_p; + float _k_i; + float _k_d; + float _integrator_max; + float _max_rate_pos; + float _max_rate_neg; + float _roll_ff; + float _last_output; + float _integrator; + float _rate_error; + float _desired_rate; +}; + +#endif // ECL_PITCH_CONTROLLER_H diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp new file mode 100644 index 0000000000..2f8129e829 --- /dev/null +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). 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 APL 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 ecl_roll_controller.cpp + * Implementation of a simple orthogonal roll PID controller. + * + */ + +#include "ecl_roll_controller.h" + +ECL_RollController::ECL_RollController() : + _last_run(0), + _last_output(0.0f), + _integrator(0.0f), + _rate_error(0.0f), + _desired_rate(0.0f) +{ + +} + +float ECL_RollController::control(float roll_setpoint, float roll, float roll_rate, + float scaler, bool lock_integrator, float airspeed_min, float airspeed_max, float aspeed) +{ + return 0.0f; +} + +void ECL_RollController::reset_integrator() +{ + _integrator = 0.0f; +} + diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h new file mode 100644 index 0000000000..d2b7961316 --- /dev/null +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). 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 APL 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 ecl_roll_controller.h + * Definition of a simple orthogonal roll PID controller. + * + */ + +#ifndef ECL_ROLL_CONTROLLER_H +#define ECL_ROLL_CONTROLLER_H + +#include +#include + +class __EXPORT ECL_RollController +{ +public: + ECL_RollController(); + + float control(float roll_setpoint, float roll, float roll_rate, + float scaler = 1.0f, bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float aspeed = (0.0f / 0.0f)); + + void reset_integrator(); + + void set_time_constant(float time_constant) { + _tc = time_constant; + } + void set_k_p(float k_p) { + _k_p = k_p; + } + void set_k_i(float k_i) { + _k_i = k_i; + } + void set_k_d(float k_d) { + _k_d = k_d; + } + void set_integrator_max(float max) { + _integrator_max = max; + } + void set_max_rate(float max_rate) { + _max_rate = max_rate; + } + + float get_rate_error() { + return _rate_error; + } + + float get_desired_rate() { + return _desired_rate; + } + +private: + uint64_t _last_run; + float _tc; + float _k_p; + float _k_i; + float _k_d; + float _integrator_max; + float _max_rate; + float _last_output; + float _integrator; + float _rate_error; + float _desired_rate; +}; + +#endif // ECL_ROLL_CONTROLLER_H diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp new file mode 100644 index 0000000000..0d8a0513f0 --- /dev/null +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -0,0 +1,63 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). 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 APL 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 ecl_yaw_controller.cpp + * Implementation of a simple orthogonal coordinated turn yaw PID controller. + * + */ + +#include "ecl_yaw_controller.h" + +ECL_YawController::ECL_YawController() : + _last_run(0), + _last_error(0.0f), + _last_output(0.0f), + _last_rate_hp_out(0.0f), + _last_rate_hp_in(0.0f), + _k_d_last(0.0f), + _integrator(0.0f) +{ + +} + +float ECL_YawController::control(float roll, float yaw_rate, float accel_y, float scaler, bool lock_integrator, + float airspeed_min, float airspeed_max, float aspeed) +{ + return 0.0f; +} + +void ECL_YawController::reset_integrator() +{ + _integrator = 0.0f; +} diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h new file mode 100644 index 0000000000..cfaa6c2331 --- /dev/null +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -0,0 +1,89 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). 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 APL 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 ecl_yaw_controller.h + * Definition of a simple orthogonal coordinated turn yaw PID controller. + * + */ +#ifndef ECL_YAW_CONTROLLER_H +#define ECL_YAW_CONTROLLER_H + +#include +#include + +class __EXPORT ECL_YawController +{ +public: + ECL_YawController(); + + float control(float roll, float yaw_rate, float accel_y, float scaler = 1.0f, bool lock_integrator = false, + float airspeed_min = 0, float airspeed_max = 0, float aspeed = (0.0f / 0.0f)); + + void reset_integrator(); + + void set_k_side(float k_a) { + _k_side = k_a; + } + void set_k_i(float k_i) { + _k_i = k_i; + } + void set_k_d(float k_d) { + _k_d = k_d; + } + void set_k_roll_ff(float k_roll_ff) { + _k_roll_ff = k_roll_ff; + } + void set_integrator_max(float max) { + _integrator_max = max; + } + +private: + uint64_t _last_run; + + float _k_side; + float _k_i; + float _k_d; + float _k_roll_ff; + float _integrator_max; + + float _last_error; + float _last_output; + float _last_rate_hp_out; + float _last_rate_hp_in; + float _k_d_last; + float _integrator; + +}; + +#endif // ECL_YAW_CONTROLLER_H diff --git a/src/lib/ecl/module.mk b/src/lib/ecl/module.mk new file mode 100644 index 0000000000..19610872e5 --- /dev/null +++ b/src/lib/ecl/module.mk @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2013 Estimation and Control Library (ECL). 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. +# +############################################################################ + +# +# Estimation and Control Library +# + +SRCS = attitude_fw/ecl_pitch_controller.cpp \ + attitude_fw/ecl_roll_controller.cpp \ + attitude_fw/ecl_yaw_controller.cpp diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp new file mode 100644 index 0000000000..6b98003fd2 --- /dev/null +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -0,0 +1,770 @@ +/**************************************************************************** + * + * 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 fw_att_control_main.c + * Implementation of a generic attitude controller based on classic orthogonal PIDs. + * + * @author Lorenz Meier + * + */ + +#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 +#include +#include + +#include +#include +#include + +/** + * Fixedwing attitude control app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int fw_att_control_main(int argc, char *argv[]); + +class FixedwingAttitudeControl +{ +public: + /** + * Constructor + */ + FixedwingAttitudeControl(); + + /** + * Destructor, also kills the sensors task. + */ + ~FixedwingAttitudeControl(); + + /** + * Start the sensors task. + * + * @return OK on success. + */ + int start(); + +private: + + bool _task_should_exit; /**< if true, sensor task should exit */ + int _control_task; /**< task handle for sensor task */ + + int _att_sub; /**< vehicle attitude subscription */ + int _accel_sub; /**< accelerometer subscription */ + int _att_sp_sub; /**< vehicle attitude setpoint */ + int _attitude_sub; /**< raw rc channels data subscription */ + int _airspeed_sub; /**< airspeed subscription */ + int _vcontrol_mode_sub; /**< vehicle status subscription */ + int _params_sub; /**< notification of parameter updates */ + int _manual_sub; /**< notification of manual control updates */ + + orb_advert_t _rate_sp_pub; /**< rate setpoint publication */ + orb_advert_t _attitude_sp_pub; /**< attitude setpoint point */ + orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */ + + struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct accel_report _accel; /**< body frame accelerations */ + struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ + struct manual_control_setpoint_s _manual; /**< r/c channel data */ + struct airspeed_s _airspeed; /**< airspeed */ + struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */ + struct actuator_controls_s _actuators; /**< actuator control inputs */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + bool _setpoint_valid; /**< flag if the position control setpoint is valid */ + bool _airspeed_valid; /**< flag if the airspeed measurement is valid */ + + struct { + float tconst; + float p_p; + float p_d; + float p_i; + float p_rmax_pos; + float p_rmax_neg; + float p_integrator_max; + float p_roll_feedforward; + float r_p; + float r_d; + float r_i; + float r_integrator_max; + float r_rmax; + float y_p; + float y_i; + float y_d; + float y_roll_feedforward; + float y_integrator_max; + + float airspeed_min; + float airspeed_trim; + float airspeed_max; + } _parameters; /**< local copies of interesting parameters */ + + struct { + + param_t tconst; + param_t p_p; + param_t p_d; + param_t p_i; + param_t p_rmax_pos; + param_t p_rmax_neg; + param_t p_integrator_max; + param_t p_roll_feedforward; + param_t r_p; + param_t r_d; + param_t r_i; + param_t r_integrator_max; + param_t r_rmax; + param_t y_p; + param_t y_i; + param_t y_d; + param_t y_roll_feedforward; + param_t y_integrator_max; + + param_t airspeed_min; + param_t airspeed_trim; + param_t airspeed_max; + } _parameter_handles; /**< handles for interesting parameters */ + + + ECL_RollController _roll_ctrl; + ECL_PitchController _pitch_ctrl; + ECL_YawController _yaw_ctrl; + + + /** + * Update our local parameter cache. + */ + int parameters_update(); + + /** + * Update control outputs + * + */ + void control_update(); + + /** + * Check for changes in vehicle control mode. + */ + void vehicle_control_mode_poll(); + + /** + * Check for changes in manual inputs. + */ + void vehicle_manual_poll(); + + + /** + * Check for airspeed updates. + */ + bool vehicle_airspeed_poll(); + + /** + * Check for accel updates. + */ + void vehicle_accel_poll(); + + /** + * Check for set triplet updates. + */ + void vehicle_setpoint_poll(); + + /** + * Shim for calling task_main from task_create. + */ + static void task_main_trampoline(int argc, char *argv[]); + + /** + * Main sensor collection task. + */ + void task_main() __attribute__((noreturn)); +}; + +namespace att_control +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +FixedwingAttitudeControl *g_control; +} + +FixedwingAttitudeControl::FixedwingAttitudeControl() : + + _task_should_exit(false), + _control_task(-1), + +/* subscriptions */ + _att_sub(-1), + _accel_sub(-1), + _airspeed_sub(-1), + _vcontrol_mode_sub(-1), + _params_sub(-1), + _manual_sub(-1), + +/* publications */ + _rate_sp_pub(-1), + _actuators_0_pub(-1), + _attitude_sp_pub(-1), + +/* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), +/* states */ + _setpoint_valid(false), + _airspeed_valid(false) +{ + _parameter_handles.tconst = param_find("FW_ATT_TC"); + _parameter_handles.p_p = param_find("FW_P_P"); + _parameter_handles.p_d = param_find("FW_P_D"); + _parameter_handles.p_i = param_find("FW_P_I"); + _parameter_handles.p_rmax_pos = param_find("FW_P_RMAX_POS"); + _parameter_handles.p_rmax_neg = param_find("FW_P_RMAX_NEG"); + _parameter_handles.p_integrator_max = param_find("FW_P_integrator_max"); + _parameter_handles.p_roll_feedforward = param_find("FW_P_ROLLFF"); + + _parameter_handles.r_p = param_find("FW_R_P"); + _parameter_handles.r_d = param_find("FW_R_D"); + _parameter_handles.r_i = param_find("FW_R_I"); + _parameter_handles.r_integrator_max = param_find("FW_R_integrator_max"); + _parameter_handles.r_rmax = param_find("FW_R_RMAX"); + + _parameter_handles.y_p = param_find("FW_Y_P"); + _parameter_handles.y_i = param_find("FW_Y_I"); + _parameter_handles.y_d = param_find("FW_Y_D"); + _parameter_handles.y_roll_feedforward = param_find("FW_Y_ROLLFF"); + _parameter_handles.y_integrator_max = param_find("FW_Y_integrator_max"); + + _parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN"); + _parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM"); + _parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX"); + + /* fetch initial parameter values */ + parameters_update(); +} + +FixedwingAttitudeControl::~FixedwingAttitudeControl() +{ + if (_control_task != -1) { + + /* task wakes up every 100ms or so at the longest */ + _task_should_exit = true; + + /* wait for a second for the task to quit at our request */ + unsigned i = 0; + + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + task_delete(_control_task); + break; + } + } while (_control_task != -1); + } + + att_control::g_control = nullptr; +} + +int +FixedwingAttitudeControl::parameters_update() +{ + + param_get(_parameter_handles.tconst, &(_parameters.tconst)); + param_get(_parameter_handles.p_p, &(_parameters.p_p)); + param_get(_parameter_handles.p_d, &(_parameters.p_d)); + param_get(_parameter_handles.p_i, &(_parameters.p_i)); + param_get(_parameter_handles.p_rmax_pos, &(_parameters.p_rmax_pos)); + param_get(_parameter_handles.p_rmax_neg, &(_parameters.p_rmax_neg)); + param_get(_parameter_handles.p_integrator_max, &(_parameters.p_integrator_max)); + param_get(_parameter_handles.p_roll_feedforward, &(_parameters.p_roll_feedforward)); + + param_get(_parameter_handles.r_p, &(_parameters.r_p)); + param_get(_parameter_handles.r_d, &(_parameters.r_d)); + param_get(_parameter_handles.r_i, &(_parameters.r_i)); + param_get(_parameter_handles.r_integrator_max, &(_parameters.r_integrator_max)); + param_get(_parameter_handles.r_rmax, &(_parameters.r_rmax)); + + param_get(_parameter_handles.y_p, &(_parameters.y_p)); + param_get(_parameter_handles.y_i, &(_parameters.y_i)); + param_get(_parameter_handles.y_d, &(_parameters.y_d)); + param_get(_parameter_handles.y_roll_feedforward, &(_parameters.y_roll_feedforward)); + param_get(_parameter_handles.y_integrator_max, &(_parameters.y_integrator_max)); + + param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min)); + param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim)); + param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max)); + + /* pitch control parameters */ + _pitch_ctrl.set_time_constant(_parameters.tconst); + _pitch_ctrl.set_k_p(math::radians(_parameters.p_p)); + _pitch_ctrl.set_k_i(math::radians(_parameters.p_i)); + _pitch_ctrl.set_k_d(math::radians(_parameters.p_d)); + _pitch_ctrl.set_integrator_max(math::radians(_parameters.p_integrator_max)); + _pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos)); + _pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg)); + _pitch_ctrl.set_roll_ff(math::radians(_parameters.p_roll_feedforward)); + + /* roll control parameters */ + _roll_ctrl.set_time_constant(_parameters.tconst); + _roll_ctrl.set_k_p(math::radians(_parameters.r_p)); + _roll_ctrl.set_k_i(math::radians(_parameters.r_i)); + _roll_ctrl.set_k_d(math::radians(_parameters.r_d)); + _roll_ctrl.set_integrator_max(math::radians(_parameters.r_integrator_max)); + _roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax)); + + /* yaw control parameters */ + _yaw_ctrl.set_k_side(math::radians(_parameters.y_p)); + _yaw_ctrl.set_k_i(math::radians(_parameters.y_i)); + _yaw_ctrl.set_k_d(math::radians(_parameters.y_d)); + _yaw_ctrl.set_k_roll_ff(math::radians(_parameters.y_roll_feedforward)); + _yaw_ctrl.set_integrator_max(math::radians(_parameters.y_integrator_max)); + + return OK; +} + +void +FixedwingAttitudeControl::vehicle_control_mode_poll() +{ + bool vcontrol_mode_updated; + + /* Check HIL state if vehicle status has changed */ + orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated); + + if (vcontrol_mode_updated) { + + orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &_vcontrol_mode); + } +} + +void +FixedwingAttitudeControl::vehicle_manual_poll() +{ + bool manual_updated; + + /* get pilots inputs */ + orb_check(_manual_sub, &manual_updated); + + if (manual_updated) { + + orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual); + } +} + +bool +FixedwingAttitudeControl::vehicle_airspeed_poll() +{ + /* check if there is a new position */ + bool airspeed_updated; + orb_check(_airspeed_sub, &airspeed_updated); + + if (airspeed_updated) { + orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); + return true; + } + + return false; +} + +void +FixedwingAttitudeControl::vehicle_accel_poll() +{ + /* check if there is a new position */ + bool accel_updated; + orb_check(_accel_sub, &accel_updated); + + if (accel_updated) { + orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); + } +} + +void +FixedwingAttitudeControl::vehicle_setpoint_poll() +{ + /* check if there is a new setpoint */ + bool att_sp_updated; + orb_check(_att_sp_sub, &att_sp_updated); + + if (att_sp_updated) { + orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp); + _setpoint_valid = true; + } +} + +void +FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[]) +{ + att_control::g_control->task_main(); +} + +void +FixedwingAttitudeControl::task_main() +{ + + /* inform about start */ + warnx("Initializing.."); + fflush(stdout); + + /* + * do subscriptions + */ + _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); + _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + + /* rate limit vehicle status updates to 5Hz */ + orb_set_interval(_vcontrol_mode_sub, 200); + orb_set_interval(_att_sub, 100); + + parameters_update(); + + /* get an initial update for all sensor and status data */ + (void)vehicle_airspeed_poll(); + vehicle_setpoint_poll(); + vehicle_accel_poll(); + vehicle_control_mode_poll(); + vehicle_manual_poll(); + + /* wakeup source(s) */ + struct pollfd fds[2]; + + /* Setup of loop */ + fds[0].fd = _params_sub; + fds[0].events = POLLIN; + fds[1].fd = _att_sub; + fds[1].events = POLLIN; + + while (!_task_should_exit) { + + /* wait for up to 500ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + + /* timed out - periodic check for _task_should_exit, etc. */ + if (pret == 0) + continue; + + /* 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; + } + + perf_begin(_loop_perf); + + /* only update parameters if they changed */ + if (fds[0].revents & POLLIN) { + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), _params_sub, &update); + + /* update parameters from storage */ + parameters_update(); + } + + /* only run controller if attitude changed */ + if (fds[1].revents & POLLIN) { + + + static uint64_t last_run = 0; + float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; + last_run = hrt_absolute_time(); + + /* guard against too large deltaT's */ + if (deltaT > 1.0f) + deltaT = 0.01f; + + /* load local copies */ + orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); + + _airspeed_valid = vehicle_airspeed_poll(); + + vehicle_setpoint_poll(); + + vehicle_accel_poll(); + + vehicle_control_mode_poll(); + + vehicle_manual_poll(); + + /* lock integrator until control is started */ + bool lock_integrator; + + if (_vcontrol_mode.flag_control_attitude_enabled) { + lock_integrator = false; + + } else { + lock_integrator = true; + } + + /* decide if in stabilized or full manual control */ + + if (_vcontrol_mode.flag_control_attitude_enabled) { + + /* scale from radians to normalized -1 .. 1 range */ + const float actuator_scaling = 1.0f / (M_PI_F / 4.0f); + + /* scale around tuning airspeed */ + + float airspeed; + + /* if airspeed is smaller than min, the sensor is not giving good readings */ + if (!_airspeed_valid || + (_airspeed.indicated_airspeed_m_s < _parameters.airspeed_min) || + !isfinite(_airspeed.indicated_airspeed_m_s)) { + airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) / 2.0f; + + } else { + airspeed = _airspeed.indicated_airspeed_m_s; + } + + float airspeed_scaling = _parameters.airspeed_trim / airspeed; + //warnx("aspd scale: %6.2f act scale: %6.2f", airspeed_scaling, actuator_scaling); + + float roll_sp, pitch_sp; + float throttle_sp = 0.0f; + + if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) { + roll_sp = _att_sp.roll_body; + pitch_sp = _att_sp.pitch_body; + throttle_sp = _att_sp.thrust; + + } else { + /* + * Scale down roll and pitch as the setpoints are radians + * and a typical remote can only do 45 degrees, the mapping is + * -1..+1 to -45..+45 degrees or -0.75..+0.75 radians. + * + * With this mapping the stick angle is a 1:1 representation of + * the commanded attitude. If more than 45 degrees are desired, + * a scaling parameter can be applied to the remote. + */ + roll_sp = _manual.roll * 0.75f; + pitch_sp = _manual.pitch * 0.75f; + throttle_sp = _manual.throttle; + _actuators.control[4] = _manual.flaps; + + /* + * in manual mode no external source should / does emit attitude setpoints. + * emit the manual setpoint here to allow attitude controller tuning + * in attitude control mode. + */ + struct vehicle_attitude_setpoint_s att_sp; + att_sp.timestamp = hrt_absolute_time(); + att_sp.roll_body = roll_sp; + att_sp.pitch_body = pitch_sp; + att_sp.yaw_body = 0.0f; + att_sp.thrust = throttle_sp; + + /* lazily publish the setpoint only once available */ + if (_attitude_sp_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp); + + } else { + /* advertise and publish */ + _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + } + } + + float roll_rad = _roll_ctrl.control(roll_sp, _att.roll, _att.rollspeed, + airspeed_scaling, lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed); + _actuators.control[0] = (isfinite(roll_rad)) ? roll_rad * actuator_scaling : 0.0f; + + float pitch_rad = _pitch_ctrl.control(pitch_sp, _att.pitch, _att.pitchspeed, _att.roll, airspeed_scaling, + lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed); + _actuators.control[1] = (isfinite(pitch_rad)) ? pitch_rad * actuator_scaling : 0.0f; + + float yaw_rad = _yaw_ctrl.control(_att.roll, _att.yawspeed, _accel.y, airspeed_scaling, lock_integrator, + _parameters.airspeed_min, _parameters.airspeed_max, airspeed); + _actuators.control[2] = (isfinite(yaw_rad)) ? yaw_rad * actuator_scaling : 0.0f; + + /* throttle passed through */ + _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; + + // warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown", + // airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1], + // _actuators.control[2], _actuators.control[3]); + + /* + * Lazily publish the rate setpoint (for analysis, the actuators are published below) + * only once available + */ + vehicle_rates_setpoint_s rates_sp; + rates_sp.roll = _roll_ctrl.get_desired_rate(); + rates_sp.pitch = _pitch_ctrl.get_desired_rate(); + rates_sp.yaw = 0.0f; // XXX not yet implemented + + rates_sp.timestamp = hrt_absolute_time(); + + if (_rate_sp_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp); + + } else { + /* advertise and publish */ + _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); + } + + } else { + /* manual/direct control */ + _actuators.control[0] = _manual.roll; + _actuators.control[1] = _manual.pitch; + _actuators.control[2] = _manual.yaw; + _actuators.control[3] = _manual.throttle; + _actuators.control[4] = _manual.flaps; + } + + /* lazily publish the setpoint only once available */ + _actuators.timestamp = hrt_absolute_time(); + + if (_actuators_0_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); + + } else { + /* advertise and publish */ + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); + } + + } + + perf_end(_loop_perf); + } + + warnx("exiting.\n"); + + _control_task = -1; + _exit(0); +} + +int +FixedwingAttitudeControl::start() +{ + ASSERT(_control_task == -1); + + /* start the task */ + _control_task = task_spawn_cmd("fw_att_control", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2048, + (main_t)&FixedwingAttitudeControl::task_main_trampoline, + nullptr); + + if (_control_task < 0) { + warn("task start failed"); + return -errno; + } + + return OK; +} + +int fw_att_control_main(int argc, char *argv[]) +{ + if (argc < 1) + errx(1, "usage: fw_att_control {start|stop|status}"); + + if (!strcmp(argv[1], "start")) { + + if (att_control::g_control != nullptr) + errx(1, "already running"); + + att_control::g_control = new FixedwingAttitudeControl; + + if (att_control::g_control == nullptr) + errx(1, "alloc failed"); + + if (OK != att_control::g_control->start()) { + delete att_control::g_control; + att_control::g_control = nullptr; + err(1, "start failed"); + } + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + if (att_control::g_control == nullptr) + errx(1, "not running"); + + delete att_control::g_control; + att_control::g_control = nullptr; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (att_control::g_control) { + errx(0, "running"); + + } else { + errx(1, "not running"); + } + } + + warnx("unrecognized command"); + return 1; +} diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c new file mode 100644 index 0000000000..97aa275de2 --- /dev/null +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -0,0 +1,136 @@ +/**************************************************************************** + * + * 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 fw_pos_control_l1_params.c + * + * Parameters defined by the L1 position control task + * + * @author Lorenz Meier + */ + +#include + +#include + +/* + * Controller parameters, accessible via MAVLink + * + */ + +// @DisplayName Attitude Time Constant +// @Description This defines the latency between a step input and the achieved setpoint. Half a second is a good start value and fits for most average systems. Smaller systems may require smaller values, but as this will wear out servos faster, the value should only be decreased as needed. +// @Range 0.4 to 1.0 seconds, in tens of seconds +PARAM_DEFINE_FLOAT(FW_ATT_TC, 0.5f); + +// @DisplayName Proportional gain. +// @Description This defines how much the elevator input will be commanded dependend on the current pitch error. +// @Range 10 to 200, 1 increments +PARAM_DEFINE_FLOAT(FW_P_P, 40.0f); + +// @DisplayName Damping gain. +// @Description This gain damps the airframe pitch rate. In particular relevant for flying wings. +// @Range 0.0 to 10.0, 0.1 increments +PARAM_DEFINE_FLOAT(FW_P_D, 0.0f); + +// @DisplayName Integrator gain. +// @Description This gain defines how much control response will result out of a steady state error. It trims any constant error. +// @Range 0 to 50.0 +PARAM_DEFINE_FLOAT(FW_P_I, 0.0f); + +// @DisplayName Maximum positive / up pitch rate. +// @Description This limits the maximum pitch up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit. +// @Range 0 to 90.0 degrees per seconds, in 1 increments +PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 0.0f); + +// @DisplayName Maximum negative / down pitch rate. +// @Description This limits the maximum pitch down up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit. +// @Range 0 to 90.0 degrees per seconds, in 1 increments +PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 0.0f); + +// @DisplayName Pitch Integrator Anti-Windup +// @Description This limits the range in degrees the integrator can wind up to. +// @Range 0.0 to 45.0 +// @Increment 1.0 +PARAM_DEFINE_FLOAT(FW_P_IMAX, 15.0f); + +// @DisplayName Roll feedforward gain. +// @Description This compensates during turns and ensures the nose stays level. +// @Range 0.5 2.0 +// @Increment 0.05 +// @User User +PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 1.0f); + +// @DisplayName Proportional Gain. +// @Description This gain controls the roll angle to roll actuator output. +// @Range 10.0 200.0 +// @Increment 10.0 +// @User User +PARAM_DEFINE_FLOAT(FW_R_P, 40.0f); + +// @DisplayName Damping Gain +// @Description Controls the roll rate to roll actuator output. It helps to reduce motions in turbulence. +// @Range 0.0 10.0 +// @Increment 1.0 +// @User User +PARAM_DEFINE_FLOAT(FW_R_D, 0.0f); + +// @DisplayName Integrator Gain +// @Description This gain controls the contribution of the integral to roll actuator outputs. It trims out steady state errors. +// @Range 0.0 100.0 +// @Increment 5.0 +// @User User +PARAM_DEFINE_FLOAT(FW_R_I, 0.0f); + +// @DisplayName Roll Integrator Anti-Windup +// @Description This limits the range in degrees the integrator can wind up to. +// @Range 0.0 to 45.0 +// @Increment 1.0 +PARAM_DEFINE_FLOAT(FW_R_IMAX, 15.0f); + +// @DisplayName Maximum Roll Rate +// @Description This limits the maximum roll rate the controller will output (in degrees per second). Setting a value of zero disables the limit. +// @Range 0 to 90.0 degrees per seconds +// @Increment 1.0 +PARAM_DEFINE_FLOAT(FW_R_RMAX, 60); + + +PARAM_DEFINE_FLOAT(FW_Y_P, 0); +PARAM_DEFINE_FLOAT(FW_Y_I, 0); +PARAM_DEFINE_FLOAT(FW_Y_IMAX, 15.0f); +PARAM_DEFINE_FLOAT(FW_Y_D, 0); +PARAM_DEFINE_FLOAT(FW_Y_ROLLFF, 1); +PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 9.0f); +PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 12.0f); +PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 18.0f); diff --git a/src/modules/fw_att_control/module.mk b/src/modules/fw_att_control/module.mk new file mode 100644 index 0000000000..1e600757ea --- /dev/null +++ b/src/modules/fw_att_control/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Fixedwing attitude control application +# + +MODULE_COMMAND = fw_att_control + +SRCS = fw_att_control_main.cpp \ + fw_att_control_params.c From d1fd1bbbf7c7fd6bc76137257d6ecb91e8b6f3d4 Mon Sep 17 00:00:00 2001 From: tstellanova Date: Fri, 23 Aug 2013 13:27:16 -0700 Subject: [PATCH 467/872] 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 468/872] 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 469/872] 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 470/872] 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 471/872] 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 472/872] 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 473/872] 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 2075cba636eff6bb59aeefe0eee7eaa13276b16e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Aug 2013 22:13:49 +0200 Subject: [PATCH 474/872] Added H frame to docs --- Documentation/control_flow.graffle | 1962 ++++++++++++++++++++++++++-- 1 file changed, 1873 insertions(+), 89 deletions(-) diff --git a/Documentation/control_flow.graffle b/Documentation/control_flow.graffle index 2535231108..6188c44308 100644 --- a/Documentation/control_flow.graffle +++ b/Documentation/control_flow.graffle @@ -5,7 +5,7 @@ ApplicationVersion com.omnigroup.OmniGraffle - 139.16.0.171715 + 139.18.0.187838 CreationDate 2013-02-22 17:51:02 +0000 @@ -26,9 +26,9 @@ MasterSheets ModificationDate - 2013-04-20 15:38:47 +0000 + 2013-08-25 05:58:40 +0000 Modifier - Lorenz Meier + Lorenz NotesVisible NO OriginVisible @@ -55,7 +55,7 @@ NSPaperName string - iso-a3 + BADB3137-012D-43BB-AFDE-2C0069A874AD NSPaperSize @@ -123,7 +123,7 @@ Bounds - {{320.41170773935767, 646.55758982070449}, {100, 24}} + {{320.41170773792788, 646.55758667263535}, {100, 24}} Class ShapedGraphic FitText @@ -171,7 +171,7 @@ Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -193,8 +193,8 @@ 45 Points - {283.47949897905681, 658.66217570036315} - {459.99996984645378, 658.44980851233595} + {283.47949897630235, 658.66217426345122} + {459.99996984638915, 658.4498036008282} Style @@ -243,7 +243,7 @@ Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -289,7 +289,7 @@ Pad 0 Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;\red68\green147\blue53;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural @@ -340,7 +340,7 @@ Pad 0 Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;\red68\green147\blue53;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural @@ -389,7 +389,7 @@ Pad 0 Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;\red68\green147\blue53;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -437,7 +437,7 @@ Pad 0 Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;\red68\green147\blue53;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -461,8 +461,8 @@ 38 Points - {584.52245687751122, 125.49999965650382} - {584.56069426576869, 157.00000090441219} + {584.50076858662646, 125.50000000000021} + {584.50207726115275, 157.00000000001324} Style @@ -511,7 +511,7 @@ Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -532,8 +532,8 @@ VEHICLE_LOCAL_POSITION_SETPOINT} 34 Points - {210.47275259888471, 495.87499981747118} - {210.43193555768113, 543.06250166479344} + {210.49312053369275, 495.87499999999562} + {210.48281498396668, 543.06249999958038} Style @@ -582,7 +582,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -602,8 +602,8 @@ VEHICLE_LOCAL_POSITION_SETPOINT} 32 Points - {210.46968123779743, 580.06249992316259} - {210.44442316539605, 627.25000030102035} + {210.47945786724097, 580.0625} + {210.47913680605853, 627.24999999999989} Style @@ -637,8 +637,8 @@ VEHICLE_LOCAL_POSITION_SETPOINT} 30 Points - {210.48620562018775, 690.25} - {210.49612530146715, 737.4375} + {210.48620103078346, 690.25} + {210.49611383706232, 737.4375} Style @@ -694,7 +694,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -731,7 +731,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -789,7 +789,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -801,7 +801,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT} Bounds - {{152.49994329565658, 397.00120984204113}, {116, 24}} + {{152.49998995675264, 397.00120984204113}, {116, 24}} Class ShapedGraphic FitText @@ -849,7 +849,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -877,8 +877,8 @@ VEHICLE_LOCAL_POSITION_SETPOINT} -1 Points - {584.49994992834695, 220} - {584.4997453697481, 348.6875000000008} + {584.49999370649277, 220} + {584.49999370649277, 348.6875} Style @@ -971,7 +971,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -991,8 +991,8 @@ VEHICLE_LOCAL_POSITION_SETPOINT} 17 Points - {210.49997491180866, 385.6875} - {210.49991091996927, 432.875} + {210.49998995675264, 385.6875} + {210.49998995675264, 432.875} Style @@ -1029,7 +1029,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT} Points {283.49999999999511, 270.00001907176534} - {339.50000000000028, 270.00001907176534} + {339.49999999999994, 270.00001907176534} Style @@ -1098,8 +1098,8 @@ VEHICLE_LOCAL_POSITION_SETPOINT} 13 Points - {210.49171354592596, 207} - {210.49498558851923, 238.4999999999998} + {210.49209551674531, 207} + {210.49601794208507, 238.5} Style @@ -1133,8 +1133,8 @@ VEHICLE_LOCAL_POSITION_SETPOINT} 12 Points - {210.49498558851246, 138.5000000000002} - {210.48997117702493, 170.00000000001342} + {210.49601794208485, 138.5} + {210.49203588416967, 170} Style @@ -1190,7 +1190,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -1234,7 +1234,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -1278,7 +1278,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -1322,7 +1322,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -1360,7 +1360,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -1395,7 +1395,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -1430,7 +1430,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -1675,7 +1675,7 @@ PX4IO or PX4FMU} Pad 0 Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -1774,7 +1774,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -2145,7 +2145,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -2536,7 +2536,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -2927,7 +2927,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -3361,7 +3361,7 @@ PX4IO or PX4FMU} Pad 0 Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -3404,7 +3404,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -3791,7 +3791,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -4178,7 +4178,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -4565,7 +4565,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -4963,7 +4963,7 @@ PX4IO or PX4FMU} Pad 0 Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -5062,7 +5062,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -5421,7 +5421,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -5780,7 +5780,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -6175,7 +6175,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -6660,7 +6660,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -7019,7 +7019,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -7406,7 +7406,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -7793,7 +7793,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -8300,7 +8300,7 @@ PX4IO or PX4FMU} Pad 0 Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -8404,7 +8404,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -8795,7 +8795,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -9166,7 +9166,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -9537,7 +9537,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -9928,7 +9928,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -10367,7 +10367,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -10779,7 +10779,7 @@ PX4IO or PX4FMU} Pad 0 Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -10883,7 +10883,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -11270,7 +11270,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -11629,7 +11629,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -11988,7 +11988,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -12375,7 +12375,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -12810,7 +12810,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -13257,6 +13257,1697 @@ PX4IO or PX4FMU} 1 0/72 in = 1.0000 in GraphicsList + + Class + Group + Graphics + + + Bounds + {{281.4621559838219, 891.87422762618871}, {48, 14}} + Class + ShapedGraphic + FitText + YES + Flow + Resize + ID + 1534 + Shape + Rectangle + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + stroke + + Draws + NO + + + Text + + Pad + 0 + Text + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 +\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} +{\colortbl;\red255\green255\blue255;} +\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc + +\f0\b\fs24 \cf0 QUAD +\b0 H} + VerticalPad + 0 + + Wrap + NO + + + Class + Group + Graphics + + + Bounds + {{282.10284531231378, 729.4992324185539}, {50.666667938232422, 36}} + Class + ShapedGraphic + ID + 1536 + Rotation + 270 + Shape + HorizontalTriangle + Style + + fill + + Color + + b + 0.13195 + g + 0.165669 + r + 1 + + + + Text + + VerticalPad + 0 + + + + Bounds + {{271.43617794923421, 713.47324050519865}, {72, 72}} + Class + ShapedGraphic + ID + 1537 + Shape + Rectangle + Style + + stroke + + CornerRadius + 9 + + + + + ID + 1535 + Rotation + 315 + + + Class + Group + Graphics + + + Bounds + {{387.95096684220994, 833.99511602139876}, {24, 24}} + Class + ShapedGraphic + ID + 1539 + Shape + Circle + Style + + shadow + + Draws + NO + + + Text + + Text + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 +\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} +{\colortbl;\red255\green255\blue255;} +\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc + +\f0\fs24 \cf0 4} + VerticalPad + 0 + + + + Class + Group + Graphics + + + Class + LineGraphic + ID + 1541 + Points + + {447.56635253821332, 822.0678021003348} + {445.35664384700522, 823.21685061976291} + + Style + + stroke + + HeadArrow + 0 + Legacy + + TailArrow + 0 + + + + + Bounds + {{359.66328626413321, 875.75030562593793}, {16, 18.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1542 + Rotation + 45 + Shape + HorizontalTriangle + Style + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + + + Bounds + {{349.2634215034181, 795.53620935597417}, {101.5, 101.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1543 + Rotation + 135 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + + + Bounds + {{346.5134215034181, 792.78620935597417}, {107, 107}} + Class + ShapedGraphic + HFlip + YES + ID + 1544 + Rotation + 135 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + + + ID + 1540 + Rotation + 315 + + + Class + Group + Graphics + + + Class + LineGraphic + ID + 1546 + Points + + {352.51941963025371, 870.09214125245364} + {354.7291283214617, 868.94309273302531} + + Style + + stroke + + HeadArrow + 0 + Legacy + + TailArrow + 0 + + + + + Bounds + {{424.42248590433371, 797.90963772685075}, {16, 18.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1547 + Rotation + 225 + Shape + HorizontalTriangle + Style + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + + + Bounds + {{349.32235066504887, 795.12373399681428}, {101.5, 101.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1548 + Rotation + 315 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + + + Bounds + {{346.57235066504887, 792.37373399681428}, {107, 107}} + Class + ShapedGraphic + HFlip + YES + ID + 1549 + Rotation + 315 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + + + ID + 1545 + Rotation + 315 + + + Bounds + {{349.03928365032027, 795.08342417943641}, {101.82337649086284, 101.82337649086284}} + Class + ShapedGraphic + ID + 1550 + Rotation + 315 + Shape + Circle + Style + + fill + + Color + + a + 0.78 + b + 0.234512 + g + 0.673128 + r + 0.148947 + + + shadow + + Draws + NO + + stroke + + Draws + NO + + + Text + + VerticalPad + 0 + + + + ID + 1538 + Rotation + 315 + + + Class + Group + Graphics + + + Bounds + {{202.98738428569231, 833.92908910567212}, {24, 24}} + Class + ShapedGraphic + ID + 1552 + Shape + Circle + Style + + shadow + + Draws + NO + + + Text + + Text + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 +\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} +{\colortbl;\red255\green255\blue255;} +\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc + +\f0\fs24 \cf0 2} + VerticalPad + 0 + + VFlip + YES + + + Class + Group + Graphics + + + Class + LineGraphic + ID + 1554 + Points + + {167.72554658547227, 821.64822718882488} + {169.93525527668044, 822.79727570825287} + + Style + + stroke + + HeadArrow + 0 + Legacy + + TailArrow + 0 + + + + + Bounds + {{239.62861285955245, 875.33073071442777}, {16, 18.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1555 + Rotation + 135 + Shape + HorizontalTriangle + Style + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + VFlip + YES + + + Bounds + {{164.52847762026795, 795.1166344444639}, {101.5, 101.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1556 + Rotation + 45 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + VFlip + YES + + + Bounds + {{161.77847762026795, 792.3666344444639}, {107, 107}} + Class + ShapedGraphic + HFlip + YES + ID + 1557 + Rotation + 45 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + VFlip + YES + + + ID + 1553 + Rotation + 225 + VFlip + YES + + + Class + Group + Graphics + + + Class + LineGraphic + ID + 1559 + Points + + {262.41893329590295, 870.02611253847215} + {260.20922460469507, 868.87706401904404} + + Style + + stroke + + HeadArrow + 0 + Legacy + + TailArrow + 0 + + + + + Bounds + {{174.51586702182317, 797.84360901286959}, {16, 18.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1560 + Rotation + 315 + Shape + HorizontalTriangle + Style + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + VFlip + YES + + + Bounds + {{164.11600226110755, 795.05770528283301}, {101.5, 101.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1561 + Rotation + 225 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + VFlip + YES + + + Bounds + {{161.36600226110755, 792.30770528283301}, {107, 107}} + Class + ShapedGraphic + HFlip + YES + ID + 1562 + Rotation + 225 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + VFlip + YES + + + ID + 1558 + Rotation + 225 + VFlip + YES + + + Bounds + {{164.07569244372991, 795.01739580669891}, {101.82337649086284, 101.82337649086284}} + Class + ShapedGraphic + ID + 1563 + Rotation + 225 + Shape + Circle + Style + + fill + + Color + + a + 0.78 + b + 0.673128 + g + 0.534129 + r + 0.217952 + + + shadow + + Draws + NO + + stroke + + Draws + NO + + + Text + + VerticalPad + 0 + + VFlip + YES + + + ID + 1551 + Rotation + 315 + + + Class + Group + Graphics + + + Bounds + {{424.02444829866442, 666.20122319568361}, {24, 24}} + Class + ShapedGraphic + ID + 1565 + Shape + Circle + Style + + shadow + + Draws + NO + + + Text + + Text + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 +\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} +{\colortbl;\red255\green255\blue255;} +\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc + +\f0\fs24 \cf0 1} + VerticalPad + 0 + + VFlip + YES + + + Class + Group + Graphics + + + Class + LineGraphic + ID + 1567 + Points + + {388.76261059844421, 653.92036127883637} + {390.97231928965209, 655.06940979826459} + + Style + + stroke + + HeadArrow + 0 + Legacy + + TailArrow + 0 + + + + + Bounds + {{460.66567687252439, 707.60286480443915}, {16, 18.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1568 + Rotation + 135 + Shape + HorizontalTriangle + Style + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + VFlip + YES + + + Bounds + {{385.56554163323983, 627.3887685344755}, {101.5, 101.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1569 + Rotation + 45 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + VFlip + YES + + + Bounds + {{382.81554163323983, 624.6387685344755}, {107, 107}} + Class + ShapedGraphic + HFlip + YES + ID + 1570 + Rotation + 45 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + VFlip + YES + + + ID + 1566 + Rotation + 225 + VFlip + YES + + + Class + Group + Graphics + + + Class + LineGraphic + ID + 1572 + Points + + {483.455997308875, 702.29824662848364} + {481.24628861766723, 701.14919810905553} + + Style + + stroke + + HeadArrow + 0 + Legacy + + TailArrow + 0 + + + + + Bounds + {{395.55293103479517, 630.11574310288108}, {16, 18.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1573 + Rotation + 315 + Shape + HorizontalTriangle + Style + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + VFlip + YES + + + Bounds + {{385.15306627407972, 627.32983937284462}, {101.5, 101.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1574 + Rotation + 225 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + VFlip + YES + + + Bounds + {{382.40306627407972, 624.57983937284462}, {107, 107}} + Class + ShapedGraphic + HFlip + YES + ID + 1575 + Rotation + 225 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + VFlip + YES + + + ID + 1571 + Rotation + 225 + VFlip + YES + + + Bounds + {{385.11275645670196, 627.28952989671052}, {101.82337649086284, 101.82337649086284}} + Class + ShapedGraphic + ID + 1576 + Rotation + 225 + Shape + Circle + Style + + fill + + Color + + a + 0.78 + b + 0.673128 + g + 0.534129 + r + 0.217952 + + + shadow + + Draws + NO + + stroke + + Draws + NO + + + Text + + VerticalPad + 0 + + VFlip + YES + + + ID + 1564 + Rotation + 315 + + + Class + Group + Graphics + + + Bounds + {{170.01738471163631, 670.02445557113811}, {24, 24}} + Class + ShapedGraphic + ID + 1578 + Shape + Circle + Style + + shadow + + Draws + NO + + + Text + + Text + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 +\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} +{\colortbl;\red255\green255\blue255;} +\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc + +\f0\fs24 \cf0 3} + VerticalPad + 0 + + + + Class + Group + Graphics + + + Class + LineGraphic + ID + 1580 + Points + + {229.63277040763995, 658.09714165007426} + {227.42306171643185, 659.24619016950237} + + Style + + stroke + + HeadArrow + 0 + Legacy + + TailArrow + 0 + + + + + Bounds + {{141.72970413355978, 711.77964517567739}, {16, 18.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1581 + Rotation + 45 + Shape + HorizontalTriangle + Style + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + + + Bounds + {{131.32983937284456, 631.5655489057134}, {101.5, 101.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1582 + Rotation + 135 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + + + Bounds + {{128.57983937284456, 628.8155489057134}, {107, 107}} + Class + ShapedGraphic + HFlip + YES + ID + 1583 + Rotation + 135 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + + + ID + 1579 + Rotation + 315 + + + Class + Group + Graphics + + + Class + LineGraphic + ID + 1585 + Points + + {134.58583749968011, 706.12148080219276} + {136.79554619088805, 704.97243228276466} + + Style + + stroke + + HeadArrow + 0 + Legacy + + TailArrow + 0 + + + + + Bounds + {{206.48890377375986, 633.93897727658998}, {16, 18.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1586 + Rotation + 225 + Shape + HorizontalTriangle + Style + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + + + Bounds + {{131.38876853447516, 631.15307354655363}, {101.5, 101.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1587 + Rotation + 315 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + + + Bounds + {{128.63876853447516, 628.40307354655363}, {107, 107}} + Class + ShapedGraphic + HFlip + YES + ID + 1588 + Rotation + 315 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + + + ID + 1584 + Rotation + 315 + + + Bounds + {{131.1057015197467, 631.11276372917564}, {101.82337649086284, 101.82337649086284}} + Class + ShapedGraphic + ID + 1589 + Rotation + 315 + Shape + Circle + Style + + fill + + Color + + a + 0.78 + b + 0.234512 + g + 0.673128 + r + 0.148947 + + + shadow + + Draws + NO + + stroke + + Draws + NO + + + Text + + VerticalPad + 0 + + + + ID + 1577 + Rotation + 315 + + + Bounds + {{253.2315878349163, 736.67782196944336}, {8, 134}} + Class + ShapedGraphic + ID + 1590 + Rotation + 225 + Shape + Rectangle + + + Bounds + {{371.51084102985902, 644.26864825135988}, {8, 134}} + Class + ShapedGraphic + ID + 1591 + Rotation + 240 + Shape + Rectangle + + + Bounds + {{353.64074716687509, 736.67780398678804}, {8, 134}} + Class + ShapedGraphic + ID + 1592 + Rotation + 315 + Shape + Rectangle + + + Bounds + {{234.00404905414581, 645.26862880510475}, {8, 134}} + Class + ShapedGraphic + ID + 1593 + Rotation + 300 + Shape + Rectangle + + + ID + 1533 + Class Group @@ -13353,7 +15044,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -13744,7 +15435,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -14135,7 +15826,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -14526,7 +16217,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -14928,7 +16619,7 @@ PX4IO or PX4FMU} Pad 0 Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -14966,7 +16657,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -15337,7 +17028,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -15708,7 +17399,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -16115,7 +17806,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -16581,6 +18272,94 @@ PX4IO or PX4FMU} VPages 1 + + ActiveLayerIndex + 0 + AutoAdjust + + BackgroundGraphic + + Bounds + {{0, 0}, {806, 1132}} + Class + SolidGraphic + ID + 2 + Style + + shadow + + Draws + NO + + stroke + + Draws + NO + + + + BaseZoom + 0 + CanvasOrigin + {0, 0} + ColumnAlign + 1 + ColumnSpacing + 36 + DisplayScale + 1 0/72 in = 1.0000 in + GraphicsList + + GridInfo + + HPages + 1 + KeepToScale + + Layers + + + Lock + NO + Name + Ebene 1 + Print + YES + View + YES + + + LayoutInfo + + Animate + NO + circoMinDist + 18 + circoSeparation + 0.0 + layoutEngine + dot + neatoSeparation + 0.0 + twopiSeparation + 0.0 + + Orientation + 2 + PrintOnePage + + RowAlign + 1 + RowSpacing + 36 + SheetTitle + Arbeitsfläche 6 + UniqueID + 6 + VPages + 1 + SmartAlignmentGuidesActive YES @@ -16600,7 +18379,7 @@ PX4IO or PX4FMU} Frame - {{96, 56}, {1043, 822}} + {{170, 4}, {1043, 774}} ListView OutlineWidth @@ -16614,7 +18393,7 @@ PX4IO or PX4FMU} SidebarWidth 120 VisibleRegion - {{-51, -33}, {908, 683}} + {{-51, 366}, {908, 635}} Zoom 1 ZoomValues @@ -16644,6 +18423,11 @@ PX4IO or PX4FMU} 1 1 + + Arbeitsfläche 6 + 1 + 1 + From 548f322493fae95c41f3769192fa0c9b28d44d26 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sun, 25 Aug 2013 22:43:01 +0200 Subject: [PATCH 475/872] 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 476/872] 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 477/872] 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 478/872] 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 479/872] 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 480/872] 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 481/872] 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 482/872] 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 483/872] 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 484/872] 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 485/872] 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 486/872] 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 487/872] 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 488/872] 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 489/872] 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 490/872] 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 491/872] 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 492/872] 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 493/872] 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 494/872] 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 495/872] 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 496/872] 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; From d28f5ac03fc9e73cf26c8afa1ed701ca5af0df08 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 28 Aug 2013 09:14:38 +0200 Subject: [PATCH 497/872] Updated IO firmware upgrade strategy and locations --- ROMFS/px4fmu_common/init.d/rcS | 22 +++++---------- src/drivers/px4io/px4io.cpp | 8 +++--- src/drivers/px4io/px4io_uploader.cpp | 41 ++++++++++++++-------------- src/drivers/px4io/uploader.h | 2 +- 4 files changed, 33 insertions(+), 40 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index c4abd07d2e..6da03402e9 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -108,25 +108,17 @@ then nshterm /dev/ttyACM0 & # - # Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) + # Upgrade PX4IO firmware # - if [ -f /fs/microsd/px4io.bin ] + if px4io update then - echo "PX4IO Firmware found. Checking Upgrade.." - if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.cur + if [ -d /fs/microsd ] then - echo "No newer version, skipping upgrade." - else - 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 + echo "Flashed PX4IO Firmware OK" > /fs/microsd/px4io.log fi + + # Allow IO to safely kick back to app + usleep 200000 fi # diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 15873f79ba..392bc9f0ae 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2096,10 +2096,10 @@ px4io_main(int argc, char *argv[]) fn[1] = nullptr; } else { - fn[0] = "/fs/microsd/px4io.bin"; - fn[1] = "/etc/px4io.bin"; - fn[2] = "/fs/microsd/px4io2.bin"; - fn[3] = "/etc/px4io2.bin"; + fn[0] = "/etc/extras/px4io-v2_default.bin"; + fn[1] = "/etc/extras/px4io-v1_default.bin"; + fn[2] = "/fs/microsd/px4io.bin"; + fn[3] = "/fs/microsd/px4io2.bin"; fn[4] = nullptr; } diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index c7ce60b5ee..7db28ecad0 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -82,6 +82,27 @@ PX4IO_Uploader::upload(const char *filenames[]) #error Must define PX4IO_SERIAL_DEVICE in board configuration to support firmware upload #endif + /* allow an early abort and look for file first */ + for (unsigned i = 0; filenames[i] != nullptr; i++) { + _fw_fd = open(filenames[i], O_RDONLY); + + if (_fw_fd < 0) { + log("failed to open %s", filenames[i]); + continue; + } + + log("using firmware from %s", filenames[i]); + filename = filenames[i]; + break; + } + + if (filename == NULL) { + log("no firmware found"); + close(_io_fd); + _io_fd = -1; + return -ENOENT; + } + _io_fd = open(PX4IO_SERIAL_DEVICE, O_RDWR); if (_io_fd < 0) { @@ -106,26 +127,6 @@ PX4IO_Uploader::upload(const char *filenames[]) return -EIO; } - for (unsigned i = 0; filenames[i] != nullptr; i++) { - _fw_fd = open(filenames[i], O_RDONLY); - - if (_fw_fd < 0) { - log("failed to open %s", filenames[i]); - continue; - } - - log("using firmware from %s", filenames[i]); - filename = filenames[i]; - break; - } - - if (filename == NULL) { - log("no firmware found"); - close(_io_fd); - _io_fd = -1; - return -ENOENT; - } - struct stat st; if (stat(filename, &st) != 0) { log("Failed to stat %s - %d\n", filename, (int)errno); diff --git a/src/drivers/px4io/uploader.h b/src/drivers/px4io/uploader.h index 135202dd1b..a4a8a88fe6 100644 --- a/src/drivers/px4io/uploader.h +++ b/src/drivers/px4io/uploader.h @@ -91,7 +91,7 @@ private: void drain(); int send(uint8_t c); int send(uint8_t *p, unsigned count); - int get_sync(unsigned timeout = 1000); + int get_sync(unsigned timeout = 100); int sync(); int get_info(int param, uint32_t &val); int erase(); From d0c59ffe54dfffdef750684f5d8de09b83135862 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 28 Aug 2013 11:14:22 +0200 Subject: [PATCH 498/872] First stab at actual controller --- .../ecl/attitude_fw/ecl_roll_controller.cpp | 11 +++++ src/lib/ecl/attitude_fw/ecl_roll_controller.h | 4 +- src/lib/ecl/ecl.h | 43 +++++++++++++++++++ 3 files changed, 57 insertions(+), 1 deletion(-) create mode 100644 src/lib/ecl/ecl.h diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 2f8129e829..b28ecdabee 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -37,10 +37,12 @@ * */ +#include "../ecl.h" #include "ecl_roll_controller.h" ECL_RollController::ECL_RollController() : _last_run(0), + _tc(0.1f), _last_output(0.0f), _integrator(0.0f), _rate_error(0.0f), @@ -52,6 +54,15 @@ ECL_RollController::ECL_RollController() : float ECL_RollController::control(float roll_setpoint, float roll, float roll_rate, float scaler, bool lock_integrator, float airspeed_min, float airspeed_max, float aspeed) { + /* get the usual dt estimate */ + uint64_t dt_micros = ecl_elapsed_time(&_last_run); + _last_run = ecl_absolute_time(); + + float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000; + + + + return 0.0f; } diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h index d2b7961316..bba9ae1634 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h @@ -54,7 +54,9 @@ public: void reset_integrator(); void set_time_constant(float time_constant) { - _tc = time_constant; + if (time_constant > 0.1f && time_constant < 3.0f) { + _tc = time_constant; + } } void set_k_p(float k_p) { _k_p = k_p; diff --git a/src/lib/ecl/ecl.h b/src/lib/ecl/ecl.h new file mode 100644 index 0000000000..2bd76ce97a --- /dev/null +++ b/src/lib/ecl/ecl.h @@ -0,0 +1,43 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). 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 APL 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 ecl.h + * Adapter / shim layer for system calls needed by ECL + * + */ + +#include + +#define ecl_absolute_time hrt_absolute_time +#define ecl_elapsed_time hrt_elapsed_time \ No newline at end of file From ad732ee3a146b40c2b600eb78f804086105a4c57 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 28 Aug 2013 18:00:32 +1000 Subject: [PATCH 499/872] free perf counters in driver destructor this prevents drivers that probe on one bus then instantiate on another from leaving behind stale/duplicate perf counters --- src/drivers/airspeed/airspeed.cpp | 5 +++++ src/drivers/hmc5883/hmc5883.cpp | 5 +++++ src/drivers/ms5611/ms5611.cpp | 6 ++++++ 3 files changed, 16 insertions(+) diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 277d8249a3..e54f5b3abe 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -106,6 +106,11 @@ Airspeed::~Airspeed() /* free any existing reports */ if (_reports != nullptr) delete[] _reports; + + // free perf counters + perf_free(_sample_perf); + perf_free(_comms_errors); + perf_free(_buffer_overflows); } int diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index d77f03bb72..1a337ca3a8 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -359,6 +359,11 @@ HMC5883::~HMC5883() /* free any existing reports */ if (_reports != nullptr) delete[] _reports; + + // free perf counters + perf_free(_sample_perf); + perf_free(_comms_errors); + perf_free(_buffer_overflows); } int diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index d9268c0b30..b572e042c6 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -225,6 +225,12 @@ MS5611::~MS5611() if (_reports != nullptr) delete[] _reports; + // free perf counters + perf_free(_sample_perf); + perf_free(_measure_perf); + perf_free(_comms_errors); + perf_free(_buffer_overflows); + delete _interface; } From fdbc09e2a53281b8dda7c48676dcf695a79ba373 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 28 Aug 2013 18:31:27 +1000 Subject: [PATCH 500/872] avoid counters going above limit in INCREMENT() when using INCREMENT() the counter would temporarily read equal to limit, which could cause an issue if the task is preempted. (this macro should be in a common header, though which header?) --- src/drivers/airspeed/airspeed.h | 2 +- src/drivers/bma180/bma180.cpp | 2 +- src/drivers/hmc5883/hmc5883.cpp | 2 +- src/drivers/l3gd20/l3gd20.cpp | 2 +- src/drivers/lsm303d/lsm303d.cpp | 2 +- src/drivers/mb12xx/mb12xx.cpp | 2 +- src/drivers/ms5611/ms5611.cpp | 2 +- 7 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h index 89dfb22d70..b87494b40f 100644 --- a/src/drivers/airspeed/airspeed.h +++ b/src/drivers/airspeed/airspeed.h @@ -165,5 +165,5 @@ protected: }; /* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) +#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) diff --git a/src/drivers/bma180/bma180.cpp b/src/drivers/bma180/bma180.cpp index cfb6256704..079b5d21cb 100644 --- a/src/drivers/bma180/bma180.cpp +++ b/src/drivers/bma180/bma180.cpp @@ -234,7 +234,7 @@ private: }; /* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) +#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) BMA180::BMA180(int bus, spi_dev_e device) : diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 1a337ca3a8..a5229b2371 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -311,7 +311,7 @@ private: }; /* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) +#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) /* * Driver 'main' command. diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 5e0a2119a1..e6d765e13a 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -300,7 +300,7 @@ private: }; /* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) +#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index cf5f8d94ce..05d6f1881d 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -421,7 +421,7 @@ private: /* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) +#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index c5f49fb36e..f834169935 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -184,7 +184,7 @@ private: }; /* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) +#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) /* * Driver 'main' command. diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index b572e042c6..4e43f19c5d 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -77,7 +77,7 @@ static const int ERROR = -1; #endif /* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) +#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) /* helper macro for arithmetic - returns the square of the argument */ #define POW2(_x) ((_x) * (_x)) From 935ed2fe49370e5eafe3b5445eda2c5714162216 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 28 Aug 2013 18:03:43 +1000 Subject: [PATCH 501/872] meas_airspeed: don't use stale/bad data in airspeed reading also fixed handling of perf counters on error --- src/drivers/meas_airspeed/meas_airspeed.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 666bd30e64..b1cb2b3d84 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -162,6 +162,8 @@ MEASAirspeed::collect() if (ret < 0) { log("error reading from sensor: %d", ret); + perf_count(_comms_errors); + perf_end(_sample_perf); return ret; } @@ -169,9 +171,14 @@ MEASAirspeed::collect() if (status == 2) { log("err: stale data"); - + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; } else if (status == 3) { log("err: fault"); + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; } //uint16_t diff_pres_pa = (val[1]) | ((val[0] & ~(0xC0)) << 8); From 76a9e34e08573ff67ad34eb0251e3a7bdefd0406 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 28 Aug 2013 18:26:21 +1000 Subject: [PATCH 502/872] I2C airspeed driver needs 2 retries this prevents I2C transfer errors every few seconds with the meas_airspeed driver --- src/drivers/airspeed/airspeed.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index e54f5b3abe..1ec61eb60b 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -157,7 +157,7 @@ Airspeed::probe() */ _retries = 4; int ret = measure(); - _retries = 0; + _retries = 2; return ret; } From 0fb3be64eace32c9be8a17dab3d1cc3ee0459f9b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 28 Aug 2013 11:18:10 +0200 Subject: [PATCH 503/872] More verbosity on RC cal fail in sensors app --- src/modules/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index e857b1c2f7..a9c49c4415 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -673,7 +673,7 @@ Sensors::parameters_update() if (!isfinite(_parameters.scaling_factor[i]) || _parameters.scaling_factor[i] * _parameters.rev[i] < 0.000001f || _parameters.scaling_factor[i] * _parameters.rev[i] > 0.2f) { - + warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, _parameters.scaling_factor[i], (int)(_parameters.rev[i])); /* scaling factors do not make sense, lock them down */ _parameters.scaling_factor[i] = 0; rc_valid = false; From 4c3c09990262bd0f9e9574dd950da62bb7b432a4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 14 Aug 2013 15:53:45 +1000 Subject: [PATCH 504/872] USB: set attributes for bus power, no remote wakeup this may help the USB bus providing the full 500mA on some systems --- nuttx-configs/px4fmu-v1/nsh/defconfig | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index a6f914a647..68ab4da32d 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -538,9 +538,10 @@ CONFIG_USBDEV=y # # CONFIG_USBDEV_ISOCHRONOUS is not set # CONFIG_USBDEV_DUALSPEED is not set -CONFIG_USBDEV_SELFPOWERED=y -# CONFIG_USBDEV_BUSPOWERED is not set +# 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 b7ee1d34294b536c6a92c8e72455f24684a5db4f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 28 Aug 2013 14:34:49 +0200 Subject: [PATCH 505/872] Prevented an analog airspeed corner case from happening --- src/modules/sensors/sensor_params.c | 3 ++- src/modules/sensors/sensors.cpp | 9 +++++++-- 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index fd2a6318eb..16bfc8094f 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -68,7 +68,8 @@ 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, 0); +PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f); +PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0); PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index a9c49c4415..88a6bebb81 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -301,6 +301,7 @@ private: float accel_offset[3]; float accel_scale[3]; float diff_pres_offset_pa; + float diff_pres_analog_enabled; int board_rotation; int external_mag_rotation; @@ -348,6 +349,7 @@ private: param_t mag_offset[3]; param_t mag_scale[3]; param_t diff_pres_offset_pa; + param_t diff_pres_analog_enabled; param_t rc_map_roll; param_t rc_map_pitch; @@ -617,6 +619,7 @@ Sensors::Sensors() : /* Differential pressure offset */ _parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF"); + _parameter_handles.diff_pres_analog_enabled = param_find("SENS_DPRES_ANA"); _parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING"); @@ -784,6 +787,7 @@ Sensors::parameters_update() /* Airspeed offset */ param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa)); + param_get(_parameter_handles.diff_pres_analog_enabled, &(_parameters.diff_pres_analog_enabled)); /* scaling of ADC ticks to battery voltage */ if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) { @@ -1266,9 +1270,10 @@ Sensors::adc_poll(struct sensor_combined_s &raw) /** * The voltage divider pulls the signal down, only act on - * a valid voltage from a connected sensor + * a valid voltage from a connected sensor. Also assume a non- + * zero offset from the sensor if its connected. */ - if (voltage > 0.4f) { + if (voltage > 0.4f && _parameters.diff_pres_analog_enabled) { float diff_pres_pa = voltage * 1000.0f - _parameters.diff_pres_offset_pa; //for MPXV7002DP sensor From 49ef30b834888b91e7238662b23d651addf3b4b4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 28 Aug 2013 14:58:29 +0200 Subject: [PATCH 506/872] Reworked how start scripts work, relying on io detect now --- ROMFS/px4fmu_common/init.d/01_fmu_quad_x | 2 +- ROMFS/px4fmu_common/init.d/08_ardrone | 2 +- ROMFS/px4fmu_common/init.d/09_ardrone_flow | 2 +- ROMFS/px4fmu_common/init.d/10_io_f330 | 43 ++++++++------ ROMFS/px4fmu_common/init.d/15_io_tbs | 2 +- ROMFS/px4fmu_common/init.d/30_io_camflyer | 2 +- ROMFS/px4fmu_common/init.d/31_io_phantom | 2 +- ROMFS/px4fmu_common/init.d/40_io_segway | 2 +- ROMFS/px4fmu_common/init.d/666_fmu_q_x550 | 2 +- ROMFS/px4fmu_common/init.d/rc.boarddetect | 66 ---------------------- ROMFS/px4fmu_common/init.d/rc.hil | 10 ---- 11 files changed, 35 insertions(+), 100 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d/rc.boarddetect diff --git a/ROMFS/px4fmu_common/init.d/01_fmu_quad_x b/ROMFS/px4fmu_common/init.d/01_fmu_quad_x index f57e4bd68d..b106a974f8 100644 --- a/ROMFS/px4fmu_common/init.d/01_fmu_quad_x +++ b/ROMFS/px4fmu_common/init.d/01_fmu_quad_x @@ -25,7 +25,7 @@ then param set RC_SCALE_YAW 3 param set SYS_AUTOCONFIG 0 - param save /fs/microsd/params + param save fi # diff --git a/ROMFS/px4fmu_common/init.d/08_ardrone b/ROMFS/px4fmu_common/init.d/08_ardrone index eb9f82f771..f6d82a5ec4 100644 --- a/ROMFS/px4fmu_common/init.d/08_ardrone +++ b/ROMFS/px4fmu_common/init.d/08_ardrone @@ -11,7 +11,7 @@ then # TODO param set SYS_AUTOCONFIG 0 - param save /fs/microsd/params + param save fi # diff --git a/ROMFS/px4fmu_common/init.d/09_ardrone_flow b/ROMFS/px4fmu_common/init.d/09_ardrone_flow index 44fbb79b7b..794342a0b1 100644 --- a/ROMFS/px4fmu_common/init.d/09_ardrone_flow +++ b/ROMFS/px4fmu_common/init.d/09_ardrone_flow @@ -11,7 +11,7 @@ then # TODO param set SYS_AUTOCONFIG 0 - param save /fs/microsd/params + param save fi # diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_io_f330 index 7b6509bf88..5af4f97b5a 100644 --- a/ROMFS/px4fmu_common/init.d/10_io_f330 +++ b/ROMFS/px4fmu_common/init.d/10_io_f330 @@ -1,6 +1,6 @@ #!nsh -echo "[init] 10_io_f330: PX4FMU+PX4IO on a DJI F330 quad frame" +echo "[init] PX4FMU v1, v2 with or without IO on DJI F330" # # Load default params for this platform @@ -24,7 +24,7 @@ then param set MC_YAWRATE_I 0.2 param set MC_YAWRATE_P 0.3 - param save /fs/microsd/params + param save fi # @@ -32,24 +32,30 @@ fi # MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 - -# -# Start MAVLink (depends on orb) -# -mavlink start -usleep 5000 + +set EXIT_ON_END no # -# Start and configure PX4IO interface +# Start and configure PX4IO or FMU interface # -sh /etc/init.d/rc.io +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start + usleep 5000 -# -# Set PWM values for DJI ESCs -# -px4io idle 900 900 900 900 -px4io min 1200 1200 1200 1200 -px4io max 1800 1800 1800 1800 + sh /etc/init.d/rc.io + # Set PWM values for DJI ESCs + px4io idle 900 900 900 900 + px4io min 1200 1200 1200 1200 + px4io max 1800 1800 1800 1800 +else + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + usleep 5000 + fmu mode_pwm + set EXIT_ON_END yes +fi # # Load mixer @@ -65,3 +71,8 @@ pwm -u 400 -m 0xff # Start common for all multirotors apps # sh /etc/init.d/rc.multirotor + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/15_io_tbs b/ROMFS/px4fmu_common/init.d/15_io_tbs index b4f063e524..3fce394035 100644 --- a/ROMFS/px4fmu_common/init.d/15_io_tbs +++ b/ROMFS/px4fmu_common/init.d/15_io_tbs @@ -24,7 +24,7 @@ then param set MC_YAWRATE_I 0.0 param set MC_YAWRATE_P 0.2 - param save /fs/microsd/params + param save fi # diff --git a/ROMFS/px4fmu_common/init.d/30_io_camflyer b/ROMFS/px4fmu_common/init.d/30_io_camflyer index 6a0bd4da82..390649f898 100644 --- a/ROMFS/px4fmu_common/init.d/30_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/30_io_camflyer @@ -11,7 +11,7 @@ then # TODO param set SYS_AUTOCONFIG 0 - param save /fs/microsd/params + param save fi # diff --git a/ROMFS/px4fmu_common/init.d/31_io_phantom b/ROMFS/px4fmu_common/init.d/31_io_phantom index 7183138622..45563310ce 100644 --- a/ROMFS/px4fmu_common/init.d/31_io_phantom +++ b/ROMFS/px4fmu_common/init.d/31_io_phantom @@ -11,7 +11,7 @@ then # TODO param set SYS_AUTOCONFIG 0 - param save /fs/microsd/params + param save fi # diff --git a/ROMFS/px4fmu_common/init.d/40_io_segway b/ROMFS/px4fmu_common/init.d/40_io_segway index 2890f43bec..4b7ed52869 100644 --- a/ROMFS/px4fmu_common/init.d/40_io_segway +++ b/ROMFS/px4fmu_common/init.d/40_io_segway @@ -9,7 +9,7 @@ then # TODO param set SYS_AUTOCONFIG 0 - param save /fs/microsd/params + param save fi # diff --git a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 index 2c82180131..938663f0c2 100644 --- a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 +++ b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 @@ -25,7 +25,7 @@ then param set RC_SCALE_YAW 3 param set SYS_AUTOCONFIG 0 - param save /fs/microsd/params + param save fi # diff --git a/ROMFS/px4fmu_common/init.d/rc.boarddetect b/ROMFS/px4fmu_common/init.d/rc.boarddetect deleted file mode 100644 index f233e51df4..0000000000 --- a/ROMFS/px4fmu_common/init.d/rc.boarddetect +++ /dev/null @@ -1,66 +0,0 @@ -#!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.hil b/ROMFS/px4fmu_common/init.d/rc.hil index a843b7ffbc..eccb2b7678 100644 --- a/ROMFS/px4fmu_common/init.d/rc.hil +++ b/ROMFS/px4fmu_common/init.d/rc.hil @@ -13,16 +13,6 @@ mavlink start -b 230400 -d /dev/ttyS1 # Create a fake HIL /dev/pwm_output interface hil mode_pwm -# -# 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 - # # Force some key parameters to sane values # MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor From a48be0446bf78f73d7550864150c1cf3de4dafa3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 28 Aug 2013 14:58:53 +0200 Subject: [PATCH 507/872] Added IO detect command to be smart about what to start before actually doing it --- src/drivers/px4io/px4io.cpp | 65 ++++++++++++++++++++++++++++++++++++- 1 file changed, 64 insertions(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 392bc9f0ae..dda1b825cb 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -119,10 +119,17 @@ public: /** * Initialize the PX4IO class. * - * Initialize the physical I2C interface to PX4IO. Retrieve relevant initial system parameters. Initialize PX4IO registers. + * Retrieve relevant initial system parameters. Initialize PX4IO registers. */ virtual int init(); + /** + * Detect if a PX4IO is connected. + * + * Only validate if there is a PX4IO to talk to. + */ + virtual int detect(); + /** * IO Control handler. * @@ -475,6 +482,29 @@ PX4IO::~PX4IO() g_dev = nullptr; } +int +PX4IO::detect() +{ + int ret; + + ASSERT(_task == -1); + + /* do regular cdev init */ + ret = CDev::init(); + if (ret != OK) + 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; + } + + return 0; +} + int PX4IO::init() { @@ -1894,6 +1924,7 @@ start(int argc, char *argv[]) if (OK != g_dev->init()) { delete g_dev; + g_dev = nullptr; errx(1, "driver init failed"); } @@ -1920,6 +1951,35 @@ start(int argc, char *argv[]) exit(0); } +void +detect(int argc, char *argv[]) +{ + if (g_dev != nullptr) + errx(0, "already loaded"); + + /* allocate the interface */ + device::Device *interface = get_interface(); + + /* create the driver - it will set g_dev */ + (void)new PX4IO(interface); + + if (g_dev == nullptr) + errx(1, "driver alloc failed"); + + if (OK != g_dev->detect()) { + delete g_dev; + g_dev = nullptr; + errx(1, "driver detect did not succeed"); + } + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + exit(0); +} + void bind(int argc, char *argv[]) { @@ -2079,6 +2139,9 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) start(argc - 1, argv + 1); + if (!strcmp(argv[1], "detect")) + detect(argc - 1, argv + 1); + if (!strcmp(argv[1], "update")) { if (g_dev != nullptr) { From feb4dad9e1c8e766cac93b55437c8234493ed71b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 28 Aug 2013 15:22:24 +0200 Subject: [PATCH 508/872] Fixed IO detect message --- src/drivers/px4io/px4io.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index dda1b825cb..beeb019748 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -497,10 +497,10 @@ PX4IO::detect() /* 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."); + log("IO not installed"); return -1; } + log("IO found"); return 0; } From 0731d331bfbee88f91fa4737ec77c0a66ce171f6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 28 Aug 2013 15:50:06 +0200 Subject: [PATCH 509/872] ROMFS reshuffling / cleanup, in sync with QGC --- ROMFS/px4fmu_common/init.d/01_fmu_quad_x | 64 --------------- ROMFS/px4fmu_common/init.d/02_io_quad_x | 47 ----------- ROMFS/px4fmu_common/init.d/100_mpx_easystar | 68 ++++++++++++++++ .../init.d/{10_io_f330 => 10_dji_f330} | 0 ROMFS/px4fmu_common/init.d/11_dji_f450 | 78 +++++++++++++++++++ ROMFS/px4fmu_common/init.d/15_tbs_discovery | 78 +++++++++++++++++++ .../init.d/{15_io_tbs => 16_3dr_iris} | 39 ++++++---- ROMFS/px4fmu_common/init.d/30_io_camflyer | 5 +- ROMFS/px4fmu_common/init.d/31_io_phantom | 5 +- ROMFS/px4fmu_common/init.d/rcS | 27 +++---- 10 files changed, 269 insertions(+), 142 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d/01_fmu_quad_x delete mode 100644 ROMFS/px4fmu_common/init.d/02_io_quad_x create mode 100644 ROMFS/px4fmu_common/init.d/100_mpx_easystar rename ROMFS/px4fmu_common/init.d/{10_io_f330 => 10_dji_f330} (100%) create mode 100644 ROMFS/px4fmu_common/init.d/11_dji_f450 create mode 100644 ROMFS/px4fmu_common/init.d/15_tbs_discovery rename ROMFS/px4fmu_common/init.d/{15_io_tbs => 16_3dr_iris} (67%) diff --git a/ROMFS/px4fmu_common/init.d/01_fmu_quad_x b/ROMFS/px4fmu_common/init.d/01_fmu_quad_x deleted file mode 100644 index b106a974f8..0000000000 --- a/ROMFS/px4fmu_common/init.d/01_fmu_quad_x +++ /dev/null @@ -1,64 +0,0 @@ -#!nsh - -echo "[init] 01_fmu_quad_x: PX4FMU Quad X with PWM outputs" - -# -# 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 -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 - -# -# Start MAVLink -# -mavlink start -d /dev/ttyS0 -b 57600 -usleep 5000 - -# -# Start 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 common for all multirotors apps -# -sh /etc/init.d/rc.multirotor - -# 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 deleted file mode 100644 index a37c26ad1d..0000000000 --- a/ROMFS/px4fmu_common/init.d/02_io_quad_x +++ /dev/null @@ -1,47 +0,0 @@ -#!nsh - -echo "[init] 02_io_quad_x: PX4FMU+PX4IO Quad X with PWM outputs" - -# -# Load default params for this platform -# -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 2 = quadrotor -# -param set MAV_TYPE 2 - -# -# Start MAVLink -# -mavlink start -d /dev/ttyS1 -b 57600 -usleep 5000 - -# -# Start and configure PX4IO interface -# -sh /etc/init.d/rc.io - -# -# 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 diff --git a/ROMFS/px4fmu_common/init.d/100_mpx_easystar b/ROMFS/px4fmu_common/init.d/100_mpx_easystar new file mode 100644 index 0000000000..e1cefdb993 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/100_mpx_easystar @@ -0,0 +1,68 @@ +#!nsh + +echo "[init] Multiplex Easystar" + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + # TODO + + param set SYS_AUTOCONFIG 0 + param save +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing +# +param set MAV_TYPE 1 + +# +# 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 + +# +# Set actuator limit to 100 Hz update (50 Hz PWM) +px4io limit 100 + +# +# Start the commander +# +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 +# +gps start + +# +# Start the attitude and position estimator +# +att_pos_estimator_ekf start + +# +# Load mixer and start controllers (depends on px4io) +# +mixer load /dev/pwm_output /etc/mixers/FMU_RET.mix +fw_att_control start +fw_pos_control_l1 start diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_dji_f330 similarity index 100% rename from ROMFS/px4fmu_common/init.d/10_io_f330 rename to ROMFS/px4fmu_common/init.d/10_dji_f330 diff --git a/ROMFS/px4fmu_common/init.d/11_dji_f450 b/ROMFS/px4fmu_common/init.d/11_dji_f450 new file mode 100644 index 0000000000..5af4f97b5a --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/11_dji_f450 @@ -0,0 +1,78 @@ +#!nsh + +echo "[init] PX4FMU v1, v2 with or without IO on DJI F330" + +# +# 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.004 + param set MC_ATTRATE_I 0.0 + 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 7.0 + param set MC_RCLOSS_THR 0.0 + param set MC_YAWPOS_D 0.0 + 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 +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 2 = quadrotor +# +param set MAV_TYPE 2 + +set EXIT_ON_END no + +# +# Start and configure PX4IO or FMU interface +# +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start + usleep 5000 + + sh /etc/init.d/rc.io + # Set PWM values for DJI ESCs + px4io idle 900 900 900 900 + px4io min 1200 1200 1200 1200 + px4io max 1800 1800 1800 1800 +else + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + usleep 5000 + fmu mode_pwm + set EXIT_ON_END yes +fi + +# +# 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 + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/15_tbs_discovery b/ROMFS/px4fmu_common/init.d/15_tbs_discovery new file mode 100644 index 0000000000..4e7406b66d --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/15_tbs_discovery @@ -0,0 +1,78 @@ +#!nsh + +echo "[init] Team Blacksheep Discovery Quad" + +# +# 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 +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 2 = quadrotor +# +param set MAV_TYPE 2 + +set EXIT_ON_END no + +# +# Start and configure PX4IO or FMU interface +# +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start + usleep 5000 + + sh /etc/init.d/rc.io + # Set PWM values for DJI ESCs + px4io idle 900 900 900 900 + px4io min 1200 1200 1200 1200 + px4io max 1800 1800 1800 1800 +else + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + usleep 5000 + fmu mode_pwm + set EXIT_ON_END yes +fi + +# +# Load the mixer for a quad with wide arms +# +mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix + +# +# Set PWM output frequency +# +pwm -u 400 -m 0xff + +# +# Start common for all multirotors apps +# +sh /etc/init.d/rc.multirotor + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/15_io_tbs b/ROMFS/px4fmu_common/init.d/16_3dr_iris similarity index 67% rename from ROMFS/px4fmu_common/init.d/15_io_tbs rename to ROMFS/px4fmu_common/init.d/16_3dr_iris index 3fce394035..5bcaaaac14 100644 --- a/ROMFS/px4fmu_common/init.d/15_io_tbs +++ b/ROMFS/px4fmu_common/init.d/16_3dr_iris @@ -1,6 +1,6 @@ #!nsh -echo "[init] 15_io_tbs: PX4FMU+PX4IO on a Team Blacksheep Discovery quad" +echo "[init] 3DR Iris Quad" # # Load default params for this platform @@ -33,23 +33,29 @@ fi # param set MAV_TYPE 2 -# -# Start MAVLink (depends on orb) -# -mavlink start -usleep 5000 +set EXIT_ON_END no # -# Start and configure PX4IO interface +# Start and configure PX4IO or FMU interface # -sh /etc/init.d/rc.io +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start + usleep 5000 -# -# Set PWM values for DJI ESCs -# -px4io idle 900 900 900 900 -px4io min 1180 1180 1180 1180 -px4io max 1800 1800 1800 1800 + sh /etc/init.d/rc.io + # Set PWM values for DJI ESCs + px4io idle 900 900 900 900 + px4io min 1200 1200 1200 1200 + px4io max 1800 1800 1800 1800 +else + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + usleep 5000 + fmu mode_pwm + set EXIT_ON_END yes +fi # # Load the mixer for a quad with wide arms @@ -65,3 +71,8 @@ pwm -u 400 -m 0xff # Start common for all multirotors apps # sh /etc/init.d/rc.multirotor + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/30_io_camflyer b/ROMFS/px4fmu_common/init.d/30_io_camflyer index 390649f898..f7e653362f 100644 --- a/ROMFS/px4fmu_common/init.d/30_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/30_io_camflyer @@ -61,12 +61,13 @@ sh /etc/init.d/rc.logging gps start # -# Start the attitude estimator (depends on orb) +# Start the attitude and position estimator # -kalman_demo start +att_pos_estimator_ekf start # # Load mixer and start controllers (depends on px4io) # mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix fw_att_control start +fw_pos_control_l1 start diff --git a/ROMFS/px4fmu_common/init.d/31_io_phantom b/ROMFS/px4fmu_common/init.d/31_io_phantom index 45563310ce..e1e6099270 100644 --- a/ROMFS/px4fmu_common/init.d/31_io_phantom +++ b/ROMFS/px4fmu_common/init.d/31_io_phantom @@ -56,12 +56,13 @@ sh /etc/init.d/rc.logging gps start # -# Start the attitude estimator +# Start the attitude and position estimator # -kalman_demo start +att_pos_estimator_ekf start # # Load mixer and start controllers (depends on px4io) # mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix fw_att_control start +fw_pos_control_l1 start diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 6da03402e9..3ef6eb75ef 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -125,17 +125,6 @@ then # 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 @@ -151,13 +140,25 @@ then if param compare SYS_AUTOSTART 10 then - sh /etc/init.d/10_io_f330 + sh /etc/init.d/10_dji_f330 + set MODE custom + fi + + if param compare SYS_AUTOSTART 11 + then + sh /etc/init.d/11_dji_f450 set MODE custom fi if param compare SYS_AUTOSTART 15 then - sh /etc/init.d/15_io_tbs + sh /etc/init.d/15_tbs_discovery + set MODE custom + fi + + if param compare SYS_AUTOSTART 16 + then + sh /etc/init.d/16_3dr_iris set MODE custom fi From 756d67f2de759cb8d1e4be26deb108fc4e32899d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 28 Aug 2013 16:38:09 +0200 Subject: [PATCH 510/872] Removed non-helpful verbosity --- 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 beeb019748..1fa3fbbfbe 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1969,7 +1969,7 @@ detect(int argc, char *argv[]) if (OK != g_dev->detect()) { delete g_dev; g_dev = nullptr; - errx(1, "driver detect did not succeed"); + exit(1); } if (g_dev != nullptr) { From 5ef7e71bb0ca81551622fb5089b34c46770c0706 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 28 Aug 2013 16:42:43 +0200 Subject: [PATCH 511/872] More graceful startup messages --- ROMFS/px4fmu_common/init.d/rcS | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 3ef6eb75ef..56b068ccac 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -191,13 +191,13 @@ then commander start # Start px4io if present - if px4io start + if px4io detect then - echo "PX4IO driver started" + px4io start else if fmu mode_serial then - echo "FMU driver started" + echo "FMU driver (no PWM) started" fi fi From 007f4d79cbd7edec6fb69e4556f35bac8dce72f2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 28 Aug 2013 16:51:02 +0200 Subject: [PATCH 512/872] param MC_RCLOSS_THR removed from scripts --- ROMFS/px4fmu_common/init.d/10_dji_f330 | 1 - ROMFS/px4fmu_common/init.d/11_dji_f450 | 1 - ROMFS/px4fmu_common/init.d/15_tbs_discovery | 1 - ROMFS/px4fmu_common/init.d/16_3dr_iris | 1 - 4 files changed, 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10_dji_f330 b/ROMFS/px4fmu_common/init.d/10_dji_f330 index 5af4f97b5a..fb141e8a73 100644 --- a/ROMFS/px4fmu_common/init.d/10_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/10_dji_f330 @@ -16,7 +16,6 @@ then param set MC_ATT_D 0.0 param set MC_ATT_I 0.0 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.0 param set MC_YAWPOS_P 2.0 diff --git a/ROMFS/px4fmu_common/init.d/11_dji_f450 b/ROMFS/px4fmu_common/init.d/11_dji_f450 index 5af4f97b5a..fb141e8a73 100644 --- a/ROMFS/px4fmu_common/init.d/11_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/11_dji_f450 @@ -16,7 +16,6 @@ then param set MC_ATT_D 0.0 param set MC_ATT_I 0.0 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.0 param set MC_YAWPOS_P 2.0 diff --git a/ROMFS/px4fmu_common/init.d/15_tbs_discovery b/ROMFS/px4fmu_common/init.d/15_tbs_discovery index 4e7406b66d..1fcde3e27f 100644 --- a/ROMFS/px4fmu_common/init.d/15_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/15_tbs_discovery @@ -16,7 +16,6 @@ then 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 diff --git a/ROMFS/px4fmu_common/init.d/16_3dr_iris b/ROMFS/px4fmu_common/init.d/16_3dr_iris index 5bcaaaac14..fa2cb1feb0 100644 --- a/ROMFS/px4fmu_common/init.d/16_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/16_3dr_iris @@ -16,7 +16,6 @@ then 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 From f2f66432d76dad2587d3e8089d0c728eb8fdf2ae Mon Sep 17 00:00:00 2001 From: tstellanova Date: Wed, 28 Aug 2013 08:49:21 -0700 Subject: [PATCH 513/872] Grab UTC time from GPS --- src/drivers/gps/ubx.cpp | 24 +++++++++++++++++------- 1 file changed, 17 insertions(+), 7 deletions(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index b579db7150..ba5d14cc49 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -42,13 +42,14 @@ * * @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf */ - -#include -#include -#include -#include -#include #include +#include +#include +#include +#include +#include +#include + #include #include #include @@ -452,7 +453,16 @@ UBX::handle_message() timeinfo.tm_min = packet->min; timeinfo.tm_sec = packet->sec; time_t epoch = mktime(&timeinfo); - + +#ifndef CONFIG_RTC + //Since we lack a hardware RTC, set the system time clock based on GPS UTC + //TODO generalize this by moving into gps.cpp? + timespec ts; + ts.tv_sec = epoch; + ts.tv_nsec = packet->time_nanoseconds; + clock_settime(CLOCK_REALTIME,&ts); +#endif + _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this _gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f); _gps_position->timestamp_time = hrt_absolute_time(); From 53813d44a0c6322e46e72c0439e0265ddea76e9e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 28 Aug 2013 21:41:52 +0200 Subject: [PATCH 514/872] sdlog2: "landed" flag logging --- src/modules/sdlog2/sdlog2.c | 9 +++++---- src/modules/sdlog2/sdlog2_messages.h | 3 ++- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 2b21bb5a5a..e83fb7dd3a 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -890,13 +890,14 @@ 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.main_state = (unsigned char) buf_status.main_state; - log_msg.body.log_STAT.navigation_state = (unsigned char) buf_status.navigation_state; - log_msg.body.log_STAT.arming_state = (unsigned char) buf_status.arming_state; + log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state; + log_msg.body.log_STAT.navigation_state = (uint8_t) buf_status.navigation_state; + log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state; 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; + log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning; + log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed; LOGBUFFER_WRITE_AND_COUNT(STAT); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index d99892fe2e..4eeb65a87e 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -155,6 +155,7 @@ struct log_STAT_s { float battery_current; float battery_remaining; uint8_t battery_warning; + uint8_t landed; }; /* --- RC - RC INPUT CHANNELS --- */ @@ -263,7 +264,7 @@ static const struct log_format_s log_formats[] = { 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"), - LOG_FORMAT(STAT, "BBBfffB", "MainState,NavState,ArmState,BatV,BatC,BatRem,BatWarn"), + LOG_FORMAT(STAT, "BBBfffBB", "MainState,NavState,ArmState,BatV,BatC,BatRem,BatWarn,Landed"), 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"), From b986607a750db74e05c60d09b048971bb0cfb18f Mon Sep 17 00:00:00 2001 From: tstellanova Date: Wed, 28 Aug 2013 18:32:16 -0700 Subject: [PATCH 515/872] Add defines for RC15 params (16 channels total) minor cleanup of rc sanity check code --- src/modules/sensors/sensor_params.c | 7 +++++++ src/modules/sensors/sensors.cpp | 23 ++++++++++++++--------- 2 files changed, 21 insertions(+), 9 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 16bfc8094f..dd86f3e2e1 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -158,6 +158,13 @@ PARAM_DEFINE_FLOAT(RC14_MAX, 2000); PARAM_DEFINE_FLOAT(RC14_REV, 1.0f); PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC15_MIN, 1000); +PARAM_DEFINE_FLOAT(RC15_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC15_MAX, 2000); +PARAM_DEFINE_FLOAT(RC15_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC15_DZ, 0.0f); + + 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 */ diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 88a6bebb81..e98c4d5489 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -660,7 +660,9 @@ int Sensors::parameters_update() { bool rc_valid = true; - + float tmpScaleFactor = 0.0f; + float tmpRevFactor = 0.0f; + /* rc values */ for (unsigned int i = 0; i < RC_CHANNELS_MAX; i++) { @@ -670,18 +672,21 @@ Sensors::parameters_update() param_get(_parameter_handles.rev[i], &(_parameters.rev[i])); param_get(_parameter_handles.dz[i], &(_parameters.dz[i])); - _parameters.scaling_factor[i] = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]); - + tmpScaleFactor = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]); + tmpRevFactor = tmpScaleFactor * _parameters.rev[i]; + /* handle blowup in the scaling factor calculation */ - if (!isfinite(_parameters.scaling_factor[i]) || - _parameters.scaling_factor[i] * _parameters.rev[i] < 0.000001f || - _parameters.scaling_factor[i] * _parameters.rev[i] > 0.2f) { - warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, _parameters.scaling_factor[i], (int)(_parameters.rev[i])); + if (!isfinite(tmpScaleFactor) || + (tmpRevFactor < 0.000001f) || + (tmpRevFactor > 0.2f) ) { + warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, tmpScaleFactor, (int)(_parameters.rev[i])); /* scaling factors do not make sense, lock them down */ - _parameters.scaling_factor[i] = 0; + _parameters.scaling_factor[i] = 0.0f; rc_valid = false; } - + else { + _parameters.scaling_factor[i] = tmpScaleFactor; + } } /* handle wrong values */ From 55cfa5152c5e3ca3148eee776ec4467dd53eeca3 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 29 Aug 2013 16:41:59 +0200 Subject: [PATCH 516/872] Made low/critical warnings consisting --- src/drivers/blinkm/blinkm.cpp | 4 ++-- src/modules/commander/commander.cpp | 16 ++++++++-------- src/modules/uORB/topics/vehicle_status.h | 6 +++--- 3 files changed, 13 insertions(+), 13 deletions(-) diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 490002254d..2361f4dd18 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -541,7 +541,7 @@ BlinkM::led() printf(" cells found:%d\n", num_of_cells); } else { - if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { + if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_LOW) { /* LED Pattern for battery low warning */ led_color_1 = LED_YELLOW; led_color_2 = LED_YELLOW; @@ -553,7 +553,7 @@ BlinkM::led() led_color_8 = LED_YELLOW; led_blink = LED_BLINK; - } else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { + } else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { /* LED Pattern for battery critical alerting */ led_color_1 = LED_RED; led_color_2 = LED_RED; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index a548f943e2..e90300affc 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -890,7 +890,7 @@ int commander_thread_main(int argc, char *argv[]) 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.battery_warning = VEHICLE_BATTERY_WARNING_LOW; status_changed = true; battery_tune_played = false; } @@ -902,7 +902,7 @@ int commander_thread_main(int argc, char *argv[]) if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { critical_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY: CRITICAL BATTERY"); - status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; + status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; battery_tune_played = false; if (armed.armed) { @@ -1160,12 +1160,12 @@ int commander_thread_main(int argc, char *argv[]) if (tune_arm() == OK) arm_tune_played = true; - } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { + } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW) { /* play tune on battery warning */ if (tune_low_bat() == OK) battery_tune_played = true; - } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { + } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { /* play tune on battery critical */ if (tune_critical_bat() == OK) battery_tune_played = true; @@ -1258,10 +1258,10 @@ 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) { + if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) { pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER; - } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { + } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; } else { @@ -1272,10 +1272,10 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang } else if (armed->ready_to_arm) { for (i = 0; i < 3; i++) { - if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { + if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) { pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER; - } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { + } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; } else { diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 43218eac4e..1c184d3a7a 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -153,9 +153,9 @@ 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 /**< alerting of low voltage 2. stage */ + VEHICLE_BATTERY_WARNING_NONE = 0, /**< no battery low voltage warning active */ + VEHICLE_BATTERY_WARNING_LOW, /**< warning of low voltage */ + VEHICLE_BATTERY_WARNING_CRITICAL /**< alerting of critical voltage */ }; /** From 9bcfe49cff7b8fce6dfbeac7f8233e554672ebff Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 29 Aug 2013 16:45:33 +0200 Subject: [PATCH 517/872] Changed RGBLED behaviour, breathe mode when disarmed and ready to arm --- src/drivers/drv_rgbled.h | 2 + src/drivers/rgbled/rgbled.cpp | 46 ++++++++++++-- src/modules/commander/commander.cpp | 95 +++++++++++------------------ 3 files changed, 78 insertions(+), 65 deletions(-) diff --git a/src/drivers/drv_rgbled.h b/src/drivers/drv_rgbled.h index 3c8bdec5d3..07c6186dde 100644 --- a/src/drivers/drv_rgbled.h +++ b/src/drivers/drv_rgbled.h @@ -78,6 +78,7 @@ /** 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 @@ -115,6 +116,7 @@ typedef enum { RGBLED_MODE_BLINK_SLOW, RGBLED_MODE_BLINK_NORMAL, RGBLED_MODE_BLINK_FAST, + RGBLED_MODE_BREATHE, RGBLED_MODE_PATTERN } rgbled_mode_t; diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 05f079ede2..feb8f1c6c9 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.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 @@ -96,6 +96,11 @@ private: rgbled_mode_t _mode; rgbled_pattern_t _pattern; + float _brightness; + uint8_t _r; + uint8_t _g; + uint8_t _b; + bool _should_run; bool _running; int _led_interval; @@ -104,6 +109,7 @@ private: void set_color(rgbled_color_t ledcolor); void set_mode(rgbled_mode_t mode); void set_pattern(rgbled_pattern_t *pattern); + void set_brightness(float brightness); static void led_trampoline(void *arg); void led(); @@ -128,6 +134,10 @@ RGBLED::RGBLED(int bus, int rgbled) : _color(RGBLED_COLOR_OFF), _mode(RGBLED_MODE_OFF), _running(false), + _brightness(1.0f), + _r(0), + _g(0), + _b(0), _led_interval(0), _counter(0) { @@ -191,7 +201,6 @@ int RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg) { int ret = ENOTTY; - switch (cmd) { case RGBLED_SET_RGB: /* set the specified RGB values */ @@ -246,6 +255,15 @@ RGBLED::led() else set_on(false); break; + case RGBLED_MODE_BREATHE: + if (_counter >= 30) + _counter = 0; + if (_counter <= 15) { + set_brightness(((float)_counter)*((float)_counter)/(15.0f*15.0f)); + } else { + set_brightness(((float)(30-_counter))*((float)(30-_counter))/(15.0f*15.0f)); + } + 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) @@ -294,7 +312,7 @@ RGBLED::set_color(rgbled_color_t color) { case RGBLED_COLOR_AMBER: // amber set_rgb(255,20,0); break; - case RGBLED_COLOR_DIM_RED: // red + case RGBLED_COLOR_DIM_RED: // red set_rgb(90,0,0); break; case RGBLED_COLOR_DIM_YELLOW: // yellow @@ -306,7 +324,7 @@ RGBLED::set_color(rgbled_color_t color) { case RGBLED_COLOR_DIM_GREEN: // green set_rgb(0,90,0); break; - case RGBLED_COLOR_DIM_BLUE: // blue + case RGBLED_COLOR_DIM_BLUE: // blue set_rgb(0,0,90); break; case RGBLED_COLOR_DIM_WHITE: // white @@ -347,6 +365,12 @@ RGBLED::set_mode(rgbled_mode_t mode) _should_run = true; _led_interval = 100; break; + case RGBLED_MODE_BREATHE: + _should_run = true; + set_on(true); + _counter = 0; + _led_interval = 1000/15; + break; case RGBLED_MODE_PATTERN: _should_run = true; set_on(true); @@ -377,6 +401,13 @@ RGBLED::set_pattern(rgbled_pattern_t *pattern) set_mode(RGBLED_MODE_PATTERN); } +void +RGBLED::set_brightness(float brightness) { + + _brightness = brightness; + set_rgb(_r, _g, _b); +} + int RGBLED::set(bool on, uint8_t r, uint8_t g, uint8_t b) { @@ -413,7 +444,12 @@ RGBLED::set_on(bool on) 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)}; + /* save the RGB values in case we want to change the brightness later */ + _r = r; + _g = g; + _b = b; + + const uint8_t msg[6] = { SUB_ADDR_PWM0, (uint8_t)((float)b/255.0f*15.0f*_brightness), SUB_ADDR_PWM1, (uint8_t)((float)g/255.0f*15.0f*_brightness), SUB_ADDR_PWM2, (uint8_t)((float)r/255.0f*15.0f*_brightness)}; return transfer(msg, sizeof(msg), nullptr, 0); } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e90300affc..776cd5766d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -689,6 +689,8 @@ int commander_thread_main(int argc, char *argv[]) struct subsystem_info_s info; memset(&info, 0, sizeof(info)); + toggle_status_leds(&status, &armed, true); + /* now initialized */ commander_initialized = true; thread_running = true; @@ -1252,72 +1254,45 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang if (changed) { - int i; - rgbled_pattern_t pattern; - memset(&pattern, 0, sizeof(pattern)); + /* XXX TODO blink fast when armed and serious error occurs */ if (armed->armed) { - /* armed, solid */ - if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) { - pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER; - - } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { - 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.duration[0] = 1000; - + rgbled_set_mode(RGBLED_MODE_ON); } else if (armed->ready_to_arm) { - for (i = 0; i < 3; i++) { - if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) { - pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER; - - } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { - 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 + 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; - } - + rgbled_set_mode(RGBLED_MODE_BREATHE); } 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; - } - - /* not ready to arm, blink at 10Hz */ + rgbled_set_mode(RGBLED_MODE_BLINK_FAST); } - rgbled_set_pattern(&pattern); + + } + + if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) { + switch (status->battery_warning) { + case VEHICLE_BATTERY_WARNING_LOW: + rgbled_set_color(RGBLED_COLOR_YELLOW); + break; + case VEHICLE_BATTERY_WARNING_CRITICAL: + rgbled_set_color(RGBLED_COLOR_AMBER); + break; + default: + break; + } + } else { + switch (status->main_state) { + case MAIN_STATE_MANUAL: + rgbled_set_color(RGBLED_COLOR_WHITE); + break; + case MAIN_STATE_SEATBELT: + case MAIN_STATE_EASY: + rgbled_set_color(RGBLED_COLOR_GREEN); + break; + case MAIN_STATE_AUTO: + rgbled_set_color(RGBLED_COLOR_BLUE); + break; + default: + break; + } } /* give system warnings on error LED, XXX maybe add memory usage warning too */ From 5146dd8ff80f6cff127fbdac27f4cb92e5954924 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 30 Aug 2013 10:09:31 +0200 Subject: [PATCH 518/872] position_estimator_inav: critical bug fixed --- .../position_estimator_inav_main.c | 35 +++++++++++-------- 1 file changed, 20 insertions(+), 15 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 4a4572b09c..88057b3237 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -429,26 +429,31 @@ 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 < 4.0f) { + if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout) { /* initialize reference position if needed */ if (!ref_xy_inited) { - if (ref_xy_init_start == 0) { - ref_xy_init_start = t; + /* require EPH < 10m */ + if (gps.eph_m < 10.0f) { + 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; + } 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; + 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); + /* 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); + } + } else { + ref_xy_init_start = 0; } } From 3a00def1899223514ea0f9bd6bd567ac11d02f7e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 30 Aug 2013 10:11:24 +0200 Subject: [PATCH 519/872] commander: switch to AUTO_READY or AUTO_MISSION immediately, don't try to stay on ground --- src/modules/commander/commander.cpp | 84 +++++++++---------- src/modules/commander/commander_params.c | 2 +- .../commander/state_machine_helper.cpp | 23 ++--- 3 files changed, 52 insertions(+), 57 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 776cd5766d..8ddd86d03f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -153,6 +153,7 @@ static uint64_t last_print_mode_reject_time = 0; /* if connected via USB */ static bool on_usb_power = false; +static float takeoff_alt = 5.0f; /* tasks waiting for low prio thread */ typedef enum { @@ -492,9 +493,10 @@ int commander_thread_main(int argc, char *argv[]) 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"); + param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT"); /* welcome user */ - warnx("[commander] starting"); + warnx("starting"); /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -733,8 +735,11 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_component_id, &(status.component_id)); status_changed = true; - /* Re-check RC calibration */ + /* re-check RC calibration */ rc_calibration_ok = (OK == rc_calibration_check()); + + /* navigation parameters */ + param_get(_param_takeoff_alt, &takeoff_alt); } } @@ -1253,7 +1258,6 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang #endif if (changed) { - /* XXX TODO blink fast when armed and serious error occurs */ if (armed->armed) { @@ -1263,8 +1267,6 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang } else { rgbled_set_mode(RGBLED_MODE_BLINK_FAST); } - - } if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) { @@ -1455,54 +1457,52 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c if (status->main_state == MAIN_STATE_AUTO) { if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) { + // TODO AUTO_LAND handling 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) { - res = TRANSITION_NOT_CHANGED; + if (local_pos->z > -takeoff_alt || status->condition_landed) { + return TRANSITION_NOT_CHANGED; } } - - 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 + if (status->navigation_state != NAVIGATION_STATE_AUTO_TAKEOFF && + status->navigation_state != NAVIGATION_STATE_AUTO_LOITER && + status->navigation_state != NAVIGATION_STATE_AUTO_MISSION && + status->navigation_state != NAVIGATION_STATE_AUTO_RTL) { + /* possibly on ground, switch to TAKEOFF if needed */ + if (local_pos->z > -takeoff_alt || status->condition_landed) { + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); + return res; + } + } + /* switch to AUTO mode */ + 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 { - /* 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); - } - } + if (status->mission_switch == MISSION_SWITCH_MISSION) { + /* MISSION */ + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, 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); - } + /* LOITER */ + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_LOITER, control_mode); } } - } + } else { + /* switch to MISSION when no RC control and first time in some AUTO mode */ + 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 { /* disarmed, always switch to AUTO_READY */ res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode); diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 0a1703b2e0..f22dac0c15 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_FAILSAFE_LL, 0); /**< Go into low-level failsafe after 0 ms */ +PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 5.0f); 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 674f3feda3..3cef10185e 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -348,20 +348,15 @@ navigation_state_transition(struct vehicle_status_s *status, navigation_state_t break; case NAVIGATION_STATE_AUTO_TAKEOFF: - - /* only transitions from 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; - 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; - control_mode->flag_control_auto_enabled = true; - } - + 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; + control_mode->flag_control_auto_enabled = true; break; case NAVIGATION_STATE_AUTO_LOITER: From 61936412f3ccf9aff5a12d47b8c5fe34ff1a04fb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 30 Aug 2013 15:40:28 +0200 Subject: [PATCH 520/872] Speeded up IO startup, needs review --- ROMFS/px4fmu_common/init.d/rcS | 6 +++++- src/drivers/px4io/uploader.h | 2 +- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 56b068ccac..982e638fb5 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -110,8 +110,12 @@ then # # Upgrade PX4IO firmware # - if px4io update + if px4io detect then + echo "PX4IO running, not upgrading" + else + echo "Attempting to upgrade PX4IO" + px4io update if [ -d /fs/microsd ] then echo "Flashed PX4IO Firmware OK" > /fs/microsd/px4io.log diff --git a/src/drivers/px4io/uploader.h b/src/drivers/px4io/uploader.h index a4a8a88fe6..135202dd1b 100644 --- a/src/drivers/px4io/uploader.h +++ b/src/drivers/px4io/uploader.h @@ -91,7 +91,7 @@ private: void drain(); int send(uint8_t c); int send(uint8_t *p, unsigned count); - int get_sync(unsigned timeout = 100); + int get_sync(unsigned timeout = 1000); int sync(); int get_info(int param, uint32_t &val); int erase(); From e2b602339adef80af84d6d396adc1962b1f86826 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 30 Aug 2013 17:05:21 +0200 Subject: [PATCH 521/872] Cleanup of detect return --- src/drivers/px4io/px4io.cpp | 20 +++++++++----------- 1 file changed, 9 insertions(+), 11 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 1fa3fbbfbe..026b879497 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1966,18 +1966,16 @@ detect(int argc, char *argv[]) if (g_dev == nullptr) errx(1, "driver alloc failed"); - if (OK != g_dev->detect()) { - delete g_dev; - g_dev = nullptr; + ret = g_dev->detect() + + delete g_dev; + g_dev = nullptr; + + if (ret) + /* nonzero, error */ exit(1); - } - - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - } - - exit(0); + else + exit(0); } void From 2b62497fb5dd83db17b1d2851f686049841faa7e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 30 Aug 2013 17:19:03 +0200 Subject: [PATCH 522/872] Fixed build error --- src/drivers/px4io/px4io.cpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 026b879497..47baa5770a 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -890,7 +890,6 @@ PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len) int PX4IO::set_min_values(const uint16_t *vals, unsigned len) { - uint16_t regs[_max_actuators]; if (len > _max_actuators) /* fail with error */ @@ -903,7 +902,6 @@ PX4IO::set_min_values(const uint16_t *vals, unsigned len) int PX4IO::set_max_values(const uint16_t *vals, unsigned len) { - uint16_t regs[_max_actuators]; if (len > _max_actuators) /* fail with error */ @@ -1370,7 +1368,7 @@ 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 != (int)num_values) { debug("io_reg_set(%u,%u,%u): error %d", page, offset, num_values, ret); return -1; } @@ -1393,7 +1391,7 @@ 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 != num_values) { + if (ret != (int)num_values) { debug("io_reg_get(%u,%u,%u): data error %d", page, offset, num_values, ret); return -1; } @@ -1966,16 +1964,17 @@ detect(int argc, char *argv[]) if (g_dev == nullptr) errx(1, "driver alloc failed"); - ret = g_dev->detect() + int ret = g_dev->detect(); delete g_dev; g_dev = nullptr; - if (ret) + if (ret) { /* nonzero, error */ exit(1); - else + } else { exit(0); + } } void From fff1856377b687bc290f4a828363de0d416b401c Mon Sep 17 00:00:00 2001 From: "Hyon Lim (Retina)" Date: Sat, 31 Aug 2013 03:11:58 +0900 Subject: [PATCH 523/872] There was a bug in position_estimator_mc. Solved --- .../position_estimator_mc/position_estimator_mc_main.c | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/modules/position_estimator_mc/position_estimator_mc_main.c b/src/modules/position_estimator_mc/position_estimator_mc_main.c index 984bd13298..3639618193 100755 --- a/src/modules/position_estimator_mc/position_estimator_mc_main.c +++ b/src/modules/position_estimator_mc/position_estimator_mc_main.c @@ -497,7 +497,12 @@ int position_estimator_mc_thread_main(int argc, char *argv[]) local_pos_est.vz = x_z_aposteriori_k[1]; local_pos_est.timestamp = hrt_absolute_time(); if ((isfinite(x_x_aposteriori_k[0])) && (isfinite(x_x_aposteriori_k[1])) && (isfinite(x_y_aposteriori_k[0])) && (isfinite(x_y_aposteriori_k[1])) && (isfinite(x_z_aposteriori_k[0])) && (isfinite(x_z_aposteriori_k[1]))){ - orb_publish(ORB_ID(vehicle_local_position), local_pos_est_pub, &local_pos_est); + if(local_pos_est_pub > 0) + orb_publish(ORB_ID(vehicle_local_position), local_pos_est_pub, &local_pos_est); + else + local_pos_est_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos_est); + //char buf[0xff]; sprintf(buf,"[pos_est_mc] x:%f, y:%f, z:%f",x_x_aposteriori_k[0],x_y_aposteriori_k[0],x_z_aposteriori_k[0]); + //mavlink_log_info(mavlink_fd, buf); } } } /* end of poll return value check */ From 9f2419698ff4d0703f58e67f29f4981fa7b4f0c5 Mon Sep 17 00:00:00 2001 From: "Hyon Lim (Retina)" Date: Sat, 31 Aug 2013 03:22:19 +0900 Subject: [PATCH 524/872] Topic has been changed for quaternion-based attitude controller. --- src/modules/uORB/topics/vehicle_attitude_setpoint.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h index a7cda34a83..686fd765c5 100644 --- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h +++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h @@ -64,6 +64,12 @@ struct vehicle_attitude_setpoint_s float R_body[9]; /**< Rotation matrix describing the setpoint as rotation from the current body frame */ bool R_valid; /**< Set to true if rotation matrix is valid */ + //! For quaternion-based attitude control + float q_d[4]; /** Desired quaternion for quaternion control */ + bool q_d_valid; /**< Set to true if quaternion vector is valid */ + float q_e[4]; /** Attitude error in quaternion */ + bool q_e_valid; /**< Set to true if quaternion error vector is valid */ + float thrust; /**< Thrust in Newton the power system should generate */ }; From 6a6fe6937146723cb86dc0fc6e46db38b328dacd Mon Sep 17 00:00:00 2001 From: "Hyon Lim (Retina)" Date: Sat, 31 Aug 2013 03:23:03 +0900 Subject: [PATCH 525/872] Bug in so3 estimator related to R matrix --- .../attitude_estimator_so3_comp_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 236052b569..86bda3c75e 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 @@ -730,7 +730,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds // Because proper mount of PX4 will give you a reversed accelerometer readings. NonlinearSO3AHRSupdate(gyro[0],gyro[1],gyro[2],-acc[0],-acc[1],-acc[2],mag[0],mag[1],mag[2],so3_comp_params.Kp,so3_comp_params.Ki, dt); - // Convert q->R. + // Convert q->R, This R converts inertial frame to body frame. Rot_matrix[0] = q0q0 + q1q1 - q2q2 - q3q3;// 11 Rot_matrix[1] = 2.0 * (q1*q2 + q0*q3); // 12 Rot_matrix[2] = 2.0 * (q1*q3 - q0*q2); // 13 @@ -794,7 +794,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds memcpy(&att.rate_offsets, &(gyro_bias), sizeof(att.rate_offsets)); /* copy rotation matrix */ - memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix)); + memcpy(&att.R, Rot_matrix, sizeof(float)*9); att.R_valid = true; if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) { From efca6f2cb1848523969c7f29abba7389e48affbc Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Fri, 30 Aug 2013 20:12:35 -0400 Subject: [PATCH 526/872] Remove unused tones and save about 6K flash space - Comment out unused tones - Create symbolic tone numbers rather than hardcoded --- src/drivers/drv_tone_alarm.h | 20 ++ src/drivers/stm32/tone_alarm/tone_alarm.cpp | 321 +++++++++--------- src/modules/commander/commander_helper.cpp | 16 +- .../preflight_check/preflight_check.c | 6 +- src/systemcmds/tests/test_hrt.c | 4 +- 5 files changed, 194 insertions(+), 173 deletions(-) diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h index 0c6afc64c4..4a399076d4 100644 --- a/src/drivers/drv_tone_alarm.h +++ b/src/drivers/drv_tone_alarm.h @@ -125,4 +125,24 @@ enum tone_pitch { TONE_NOTE_MAX }; +enum { + TONE_STOP_TUNE = 0, + TONE_STARTUP_TUNE, + TONE_ERROR_TUNE, + TONE_NOTIFY_POSITIVE_TUNE, + TONE_NOTIFY_NEUTRAL_TUNE, + TONE_NOTIFY_NEGATIVE_TUNE, + TONE_CHARGE_TUNE, + /* Do not include these unused tunes + TONE_DIXIE_TUNE, + TONE_CUCURACHA_TUNE, + TONE_YANKEE_TUNE, + TONE_DAISY_TUNE, + TONE_WILLIAM_TELL_TUNE, */ + TONE_ARMING_WARNING_TUNE, + TONE_BATTERY_WARNING_SLOW_TUNE, + TONE_BATTERY_WARNING_FAST_TUNE, + TONE_NUMBER_OF_TUNES +}; + #endif /* DRV_TONE_ALARM_H_ */ diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index ad21f7143f..a726247d55 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -233,8 +233,7 @@ public: private: static const unsigned _tune_max = 1024; // be reasonable about user tunes - static const char * const _default_tunes[]; - static const unsigned _default_ntunes; + static const char * _default_tunes[TONE_NUMBER_OF_TUNES - 1]; static const uint8_t _note_tab[]; unsigned _default_tune_number; // number of currently playing default tune (0 for none) @@ -305,161 +304,7 @@ private: }; // predefined tune array -const char * const ToneAlarm::_default_tunes[] = { - "MFT240L8 O4aO5dc O4aO5dc O4aO5dc L16dcdcdcdc", // startup tune - "MBT200a8a8a8PaaaP", // ERROR tone - "MFT200e8a8a", // NotifyPositive tone - "MFT200e8e", // NotifyNeutral tone - "MFT200e8c8e8c8e8c8", // NotifyNegative tone - "MFT90O3C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C4", // charge! - "MFT60O3C32O2A32F16F16F32G32A32A+32O3C16C16C16O2A16", // dixie - "MFT90O2C16C16C16F8.A8C16C16C16F8.A4P16P8", // cucuracha - "MNT150L8O2GGABGBADGGABL4GL8F+", // yankee - "MFT200O3C4.O2A4.G4.F4.D8E8F8D4F8C2.O2G4.O3C4.O2A4.F4.D8E8F8G4A8G2P8", // daisy - "T200O2B4P8B16B16B4P8B16B16B8G+8E8G+8B8G+8B8O3E8" // william tell - "O2B8G+8E8G+8B8G+8B8O3E8O2B4P8B16B16B4P8B16" - "O2B16B4P8B16B16B4P8B16B16B8B16B16B8B8B8B16" - "O2B16B8B8B8B16B16B8B8B8B16B16B8B8B2B2B8P8" - "P4P4P8O1B16B16B8B16B16B8B16B16O2E8F+8G+8" - "O1B16B16B8B16B16O2E8G+16G+16F+8D+8O1B8B16" - "O1B16B8B16B16B8B16B16O2E8F+8G+8E16G+16B4" - "O2B16A16G+16F+16E8G+8E8O3B16B16B8B16B16B8" - "O3B16B16O4E8F+8G+8O3B16B16B8B16B16O4E8G+16" - "O4G+16F+8D+8O3B8B16B16B8B16B16B8B16B16O4E8" - "O4F+8G+8E16G+16B4B16A16G+16F+16E8G+8E8O3G+16" - "O3G+16G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8" - "O4C+8O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8" - "O3G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8" - "O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8" - "O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8" - "O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16" - "O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8O2B16" - "O2B16B8F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8" - "O2E4G+8F+8F+8F+8O3F+16F+16F+8F+16F+16F+8" - "O3G+8A8F+4A8G+8E4G+8F+8O2B16B16B8O1B16B16" - "O1B8B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16" - "O1B16O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16" - "O1B8B16B16O2E8F+8G+8E16G+16B4B16A16G+16F+16" - "O2E8G+8E8O3B16B16B8B16B16B8B16B16O4E8F+8" - "O4G+8O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8" - "O3B16B16B8B16B16B8B16B16O4E8F+8G+8E16G+16" - "O4B4B16A16G+16F+16E8G+8E8O3E64F64G64A64B64" - "O4C64D64E8E16E16E8E8G+4.F+8E8D+8E8C+8O3B16" - "O4C+16O3B16O4C+16O3B16O4C+16D+16E16O3A16" - "O3B16A16B16A16B16O4C+16D+16O3G+16A16G+16" - "O3A16G+16A16B16O4C+16O3F+16G+16F+16G+16F+16" - "O3G+16F+16G+16F+16G+16F+16D+16O2B16O3B16" - "O4C+16D+16E8D+8E8C+8O3B16O4C+16O3B16O4C+16" - "O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16" - "O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16" - "O3G+16F+16G+16F+16A16F+16E16E8P8C+4C+16O2C16" - "O3C+16O2C16O3D+16C+16O2B16A16A16G+16E16C+16" - "O2C+16C+16C+16C+16E16D+16O1C16G+16G+16G+16" - "O1G+16G+16G+16O2C+16E16G+16O3C+16C+16C+16" - "O3C+16C+16O2C16O3C+16O2C16O3D+16C+16O2B16" - "O2A16A16G+16E16C+16C+16C+16C+16C+16E16D+16" - "O1C16G+16G+16G+16G+16G+16G+16O2C+16E16G+16" - "O3C+16E16D+16C+16D+16O2C16G+16G+16G+16O3G+16" - "O3E16C+16D+16O2C16G+16G+16G+16O3G+16E16C+16" - "O3D+16O2B16G+16G+16A+16G16D+16D+16G+16G16" - "O2G+16G16G+16A16G+16F+16E16O1B16A+16B16O2E16" - "O1B16O2F+16O1B16O2G+16E16D+16E16G+16E16A16" - "O2F+16B16O3G+16F+16E16D+16F+16E16C+16O2B16" - "O3C+16O2B16O3C+16D+16E16F+16G+16O2A16B16" - "O2A16B16O3C+16D+16E16F+16O2G+16A16G+16A16" - "O2C16O3C+16D+16E16O2F+16G+16F+16G+16F+16" - "O2G+16F+16G+16F+16G+16F+16D+16O1B16C16O2C+16" - "O2D+16E16O1B16A+16B16O2E16O1B16O2F+16O1B16" - "O2G+16E16D+16E16G+16E16A16F+16B16O3G+16F+16" - "O3E16D+16F+16E16C+16O2B16O3C+16O2B16O3C+16" - "O3D+16E16F+16G+16O2A16B16A16B16O3C+16D+16" - "O3E16F+16O2G+16A16G+16A16B16O3C+16D+16E16" - "O2F+16O3C+16O2C16O3C+16D+16C+16O2A16F+16" - "O2E16O3E16F+16G+16A16B16O4C+16D+16E8E16E16" - "O4E8E8G+4.F8E8D+8E8C+8O3B16O4C+16O3B16O4C+16" - "O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16" - "O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16" - "O3G+16F+16G+16F+16G+16F+16G+16F+16G+16F+16" - "O3D+16O2B16O3B16O4C+16D+16E8E16E16E8E8G+4." - "O4F+8E8D+8E8C+8O3B16O4C+16O3B16O4C+16O3B16" - "O4C+16D+16E16O3A16B16A16B16A16B16O4C+16D+16" - "O3G+16A16G+16A16G+16A16B16O4C+16O3F+16G+16" - "O3F+16G+16F+16A16G+16F+16E8O2B8O3E8G+16G+16" - "O3G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8" - "O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8G+16" - "O3G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8" - "O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8" - "O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8" - "O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16" - "O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8A+8" - "O3B8O2F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8" - "O2E4G+8F+8B8O1B8O2F+16F+16F+8F+16F+16F+8" - "O2G+8A8F+4A8G+8E4G+8F+8B16B16B8O1B16B16B8" - "O1B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16B16" - "O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16B8B16" - "O1B16O2E8F+8G+8E16G+16B4B16A16G+16F+16E8" - "O1B8O2E8O3B16B16B8B16B16B8B16B16O4E8F+8G+8" - "O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8B16" - "O3B16B8B16B16B8B16B16O4E8F+8G+8O3E16G+16" - "O3B4B16A16G+16F+16E16F+16G+16A16G+16A16B16" - "O4C+16O3B16O4C+16D+16E16D+16E16F+16G+16A16" - "O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16" - "O3B16O4A16O3B16O4A16O3B16O4A16O3B16E16F+16" - "O3G+16A16G+16A16B16O4C+16O3B16O4C+16D+16" - "O4E16D+16E16F+16G+16A16O3B16O4A16O3B16O4A16" - "O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16" - "O3B16O4A16O3B16P16G+16O4G+16O3G+16P16D+16" - "O4D+16O3D+16P16E16O4E16O3E16P16A16O4A16O3A16" - "P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16" - "P16O3E16O4E16O3E16P16A16O4A16O3A16O4G16O3G16" - "O4G16O3G16O4G16O3G16O4G16O3G16O4G8E8C8E8" - "O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+16" - "O3G+16O4G+8E8O3B8O4E8G+16O3G+16O4G+16O3G+16" - "O4G+16O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16" - "O3A+16O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16" - "O4A+8G8E8G8B8P16A+16P16A16P16G+16P16F+16" - "P16O4E16P16D+16P16C+16P16O3B16P16A+16P16" - "O3A16P16G+16P16F+16P16E16P16D+16P16F+16E16" - "O3F+16G+16A16G+16A16B16O4C+16O3B16O4C+16" - "O4D+16E16D+16E16F+16G+16A16O3B16O4A16O3B16" - "O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16" - "O4A16O3B16O4A16O3B16E16F+16G+16A16G+16A16" - "O3B16O4C+16O3B16O4C+16D+16E16D+16E16F+16" - "O4G+16A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16" - "O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16" - "P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16" - "P16O3E16O4E16O3E16P16A16O4A16O3A16P16G+16" - "O4G+16O3G+16P16D+16O4D+16O3D+16P16E16O4E16" - "O3E16P16A16O4A16O3A16O4G16O3G16O4G16O3G16" - "O4G16O3G16O4G16O3G16O4G8E8C8E8G+16O3G+16" - "O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+8" - "O4E8O3B8O4E8G+16O3G+16O4G+16O3G+16O4G+16" - "O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16O3A+16" - "O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16O4A+8" - "O4G8E8G8B8P16A+16P16A16P16G+16P16F+16P16" - "O4E16P16D+16P16C+16P16O3B16P16A+16P16A16" - "P16O3G+16P16F+16P16E16P16D+16P16F16E16D+16" - "O3E16D+16E8B16B16B8B16B16B8B16B16O4E8F+8" - "O4G+8O3B16B16B8B16B16B8B16B16O4G+8A8B8P8" - "O4E8F+8G+8P8O3G+8A8B8P8P2O2B16C16O3C+16D16" - "O3D+16E16F16F+16G16G+16A16A+16B16C16O4C+16" - "O4D+16E16D+16F+16D+16E16D+16F+16D+16E16D+16" - "O4F+16D+16E16D+16F+16D+16E16D+16F+16D+16" - "O4E16D+16F+16D+16E16D+16F+16D+16E16D+16F+16" - "O4D+16E8E16O3E16O4E16O3E16O4E16O3E16O4E8" - "O3B16O2B16O3B16O2B16O3B16O2B16O3B8G+16O2G+16" - "O3G+16O2G+16O3G+16O2G+16O3G8E16O2E16O3E16" - "O2E16O3E16O2E16O3E8E16E16E8E8E8O2B16B16B8" - "O2B8B8G+16G+16G+8G+8G+8E16E16E8E8E8O1B8O2E8" - "O1B8O2G+8E8B8G+8O3E8O2B8O3E8O2B8O3G+8E8B8" - "O3G+8O4E4P8E16E16E8E8E8E8E4P8E16E4P8O2E16" - "O2E2P64", - "MNT75L1O2G", //arming warning - "MBNT100a8", //battery warning slow - "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]); +const char * ToneAlarm::_default_tunes[TONE_NUMBER_OF_TUNES - 1]; // semitone offsets from C for the characters 'A'-'G' const uint8_t ToneAlarm::_note_tab[] = {9, 11, 0, 2, 4, 5, 7}; @@ -479,6 +324,162 @@ ToneAlarm::ToneAlarm() : { // enable debug() calls //_debug_enabled = true; + _default_tunes[TONE_STARTUP_TUNE - 1] = "MFT240L8 O4aO5dc O4aO5dc O4aO5dc L16dcdcdcdc"; // startup tune + _default_tunes[TONE_ERROR_TUNE - 1] = "MBT200a8a8a8PaaaP"; // ERROR tone + _default_tunes[TONE_NOTIFY_POSITIVE_TUNE - 1] = "MFT200e8a8a"; // NotifyPositive tone + _default_tunes[TONE_NOTIFY_NEUTRAL_TUNE - 1] = "MFT200e8e"; // NotifyNeutral tone + _default_tunes[TONE_NOTIFY_NEGATIVE_TUNE - 1] = "MFT200e8c8e8c8e8c8"; // NotifyNegative tone + _default_tunes[TONE_CHARGE_TUNE - 1] = + "MFT90O3C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C4"; // charge! +#if 0 // don't include unused tunes... but keep them for nostalgic reason + _default_tunes[TONE_DIXIE_TUNE - 1] = "MFT60O3C32O2A32F16F16F32G32A32A+32O3C16C16C16O2A16"; // dixie + _default_tunes[TONE_CUCURACHA_TUNE - 1] = "MFT90O2C16C16C16F8.A8C16C16C16F8.A4P16P8"; // cucuracha + _default_tunes[TONE_YANKEE_TUNE - 1] = "MNT150L8O2GGABGBADGGABL4GL8F+"; // yankee + _default_tunes[TONE_DAISY_TUNE - 1] = + "MFT200O3C4.O2A4.G4.F4.D8E8F8D4F8C2.O2G4.O3C4.O2A4.F4.D8E8F8G4A8G2P8"; // daisy + _default_tunes[TONE_WILLIAM_TELL_TUNE - 1] = + "T200O2B4P8B16B16B4P8B16B16B8G+8E8G+8B8G+8B8O3E8" // william tell + "O2B8G+8E8G+8B8G+8B8O3E8O2B4P8B16B16B4P8B16" + "O2B16B4P8B16B16B4P8B16B16B8B16B16B8B8B8B16" + "O2B16B8B8B8B16B16B8B8B8B16B16B8B8B2B2B8P8" + "P4P4P8O1B16B16B8B16B16B8B16B16O2E8F+8G+8" + "O1B16B16B8B16B16O2E8G+16G+16F+8D+8O1B8B16" + "O1B16B8B16B16B8B16B16O2E8F+8G+8E16G+16B4" + "O2B16A16G+16F+16E8G+8E8O3B16B16B8B16B16B8" + "O3B16B16O4E8F+8G+8O3B16B16B8B16B16O4E8G+16" + "O4G+16F+8D+8O3B8B16B16B8B16B16B8B16B16O4E8" + "O4F+8G+8E16G+16B4B16A16G+16F+16E8G+8E8O3G+16" + "O3G+16G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8" + "O4C+8O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8" + "O3G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8" + "O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8" + "O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8" + "O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16" + "O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8O2B16" + "O2B16B8F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8" + "O2E4G+8F+8F+8F+8O3F+16F+16F+8F+16F+16F+8" + "O3G+8A8F+4A8G+8E4G+8F+8O2B16B16B8O1B16B16" + "O1B8B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16" + "O1B16O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16" + "O1B8B16B16O2E8F+8G+8E16G+16B4B16A16G+16F+16" + "O2E8G+8E8O3B16B16B8B16B16B8B16B16O4E8F+8" + "O4G+8O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8" + "O3B16B16B8B16B16B8B16B16O4E8F+8G+8E16G+16" + "O4B4B16A16G+16F+16E8G+8E8O3E64F64G64A64B64" + "O4C64D64E8E16E16E8E8G+4.F+8E8D+8E8C+8O3B16" + "O4C+16O3B16O4C+16O3B16O4C+16D+16E16O3A16" + "O3B16A16B16A16B16O4C+16D+16O3G+16A16G+16" + "O3A16G+16A16B16O4C+16O3F+16G+16F+16G+16F+16" + "O3G+16F+16G+16F+16G+16F+16D+16O2B16O3B16" + "O4C+16D+16E8D+8E8C+8O3B16O4C+16O3B16O4C+16" + "O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16" + "O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16" + "O3G+16F+16G+16F+16A16F+16E16E8P8C+4C+16O2C16" + "O3C+16O2C16O3D+16C+16O2B16A16A16G+16E16C+16" + "O2C+16C+16C+16C+16E16D+16O1C16G+16G+16G+16" + "O1G+16G+16G+16O2C+16E16G+16O3C+16C+16C+16" + "O3C+16C+16O2C16O3C+16O2C16O3D+16C+16O2B16" + "O2A16A16G+16E16C+16C+16C+16C+16C+16E16D+16" + "O1C16G+16G+16G+16G+16G+16G+16O2C+16E16G+16" + "O3C+16E16D+16C+16D+16O2C16G+16G+16G+16O3G+16" + "O3E16C+16D+16O2C16G+16G+16G+16O3G+16E16C+16" + "O3D+16O2B16G+16G+16A+16G16D+16D+16G+16G16" + "O2G+16G16G+16A16G+16F+16E16O1B16A+16B16O2E16" + "O1B16O2F+16O1B16O2G+16E16D+16E16G+16E16A16" + "O2F+16B16O3G+16F+16E16D+16F+16E16C+16O2B16" + "O3C+16O2B16O3C+16D+16E16F+16G+16O2A16B16" + "O2A16B16O3C+16D+16E16F+16O2G+16A16G+16A16" + "O2C16O3C+16D+16E16O2F+16G+16F+16G+16F+16" + "O2G+16F+16G+16F+16G+16F+16D+16O1B16C16O2C+16" + "O2D+16E16O1B16A+16B16O2E16O1B16O2F+16O1B16" + "O2G+16E16D+16E16G+16E16A16F+16B16O3G+16F+16" + "O3E16D+16F+16E16C+16O2B16O3C+16O2B16O3C+16" + "O3D+16E16F+16G+16O2A16B16A16B16O3C+16D+16" + "O3E16F+16O2G+16A16G+16A16B16O3C+16D+16E16" + "O2F+16O3C+16O2C16O3C+16D+16C+16O2A16F+16" + "O2E16O3E16F+16G+16A16B16O4C+16D+16E8E16E16" + "O4E8E8G+4.F8E8D+8E8C+8O3B16O4C+16O3B16O4C+16" + "O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16" + "O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16" + "O3G+16F+16G+16F+16G+16F+16G+16F+16G+16F+16" + "O3D+16O2B16O3B16O4C+16D+16E8E16E16E8E8G+4." + "O4F+8E8D+8E8C+8O3B16O4C+16O3B16O4C+16O3B16" + "O4C+16D+16E16O3A16B16A16B16A16B16O4C+16D+16" + "O3G+16A16G+16A16G+16A16B16O4C+16O3F+16G+16" + "O3F+16G+16F+16A16G+16F+16E8O2B8O3E8G+16G+16" + "O3G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8" + "O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8G+16" + "O3G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8" + "O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8" + "O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8" + "O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16" + "O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8A+8" + "O3B8O2F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8" + "O2E4G+8F+8B8O1B8O2F+16F+16F+8F+16F+16F+8" + "O2G+8A8F+4A8G+8E4G+8F+8B16B16B8O1B16B16B8" + "O1B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16B16" + "O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16B8B16" + "O1B16O2E8F+8G+8E16G+16B4B16A16G+16F+16E8" + "O1B8O2E8O3B16B16B8B16B16B8B16B16O4E8F+8G+8" + "O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8B16" + "O3B16B8B16B16B8B16B16O4E8F+8G+8O3E16G+16" + "O3B4B16A16G+16F+16E16F+16G+16A16G+16A16B16" + "O4C+16O3B16O4C+16D+16E16D+16E16F+16G+16A16" + "O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16" + "O3B16O4A16O3B16O4A16O3B16O4A16O3B16E16F+16" + "O3G+16A16G+16A16B16O4C+16O3B16O4C+16D+16" + "O4E16D+16E16F+16G+16A16O3B16O4A16O3B16O4A16" + "O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16" + "O3B16O4A16O3B16P16G+16O4G+16O3G+16P16D+16" + "O4D+16O3D+16P16E16O4E16O3E16P16A16O4A16O3A16" + "P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16" + "P16O3E16O4E16O3E16P16A16O4A16O3A16O4G16O3G16" + "O4G16O3G16O4G16O3G16O4G16O3G16O4G8E8C8E8" + "O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+16" + "O3G+16O4G+8E8O3B8O4E8G+16O3G+16O4G+16O3G+16" + "O4G+16O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16" + "O3A+16O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16" + "O4A+8G8E8G8B8P16A+16P16A16P16G+16P16F+16" + "P16O4E16P16D+16P16C+16P16O3B16P16A+16P16" + "O3A16P16G+16P16F+16P16E16P16D+16P16F+16E16" + "O3F+16G+16A16G+16A16B16O4C+16O3B16O4C+16" + "O4D+16E16D+16E16F+16G+16A16O3B16O4A16O3B16" + "O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16" + "O4A16O3B16O4A16O3B16E16F+16G+16A16G+16A16" + "O3B16O4C+16O3B16O4C+16D+16E16D+16E16F+16" + "O4G+16A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16" + "O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16" + "P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16" + "P16O3E16O4E16O3E16P16A16O4A16O3A16P16G+16" + "O4G+16O3G+16P16D+16O4D+16O3D+16P16E16O4E16" + "O3E16P16A16O4A16O3A16O4G16O3G16O4G16O3G16" + "O4G16O3G16O4G16O3G16O4G8E8C8E8G+16O3G+16" + "O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+8" + "O4E8O3B8O4E8G+16O3G+16O4G+16O3G+16O4G+16" + "O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16O3A+16" + "O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16O4A+8" + "O4G8E8G8B8P16A+16P16A16P16G+16P16F+16P16" + "O4E16P16D+16P16C+16P16O3B16P16A+16P16A16" + "P16O3G+16P16F+16P16E16P16D+16P16F16E16D+16" + "O3E16D+16E8B16B16B8B16B16B8B16B16O4E8F+8" + "O4G+8O3B16B16B8B16B16B8B16B16O4G+8A8B8P8" + "O4E8F+8G+8P8O3G+8A8B8P8P2O2B16C16O3C+16D16" + "O3D+16E16F16F+16G16G+16A16A+16B16C16O4C+16" + "O4D+16E16D+16F+16D+16E16D+16F+16D+16E16D+16" + "O4F+16D+16E16D+16F+16D+16E16D+16F+16D+16" + "O4E16D+16F+16D+16E16D+16F+16D+16E16D+16F+16" + "O4D+16E8E16O3E16O4E16O3E16O4E16O3E16O4E8" + "O3B16O2B16O3B16O2B16O3B16O2B16O3B8G+16O2G+16" + "O3G+16O2G+16O3G+16O2G+16O3G8E16O2E16O3E16" + "O2E16O3E16O2E16O3E8E16E16E8E8E8O2B16B16B8" + "O2B8B8G+16G+16G+8G+8G+8E16E16E8E8E8O1B8O2E8" + "O1B8O2G+8E8B8G+8O3E8O2B8O3E8O2B8O3G+8E8B8" + "O3G+8O4E4P8E16E16E8E8E8E8E4P8E16E4P8O2E16" + "O2E2P64"; +#endif + _default_tunes[TONE_ARMING_WARNING_TUNE - 1] = "MNT75L1O2G"; //arming warning + _default_tunes[TONE_BATTERY_WARNING_SLOW_TUNE - 1] = "MBNT100a8"; //battery warning slow + _default_tunes[TONE_BATTERY_WARNING_FAST_TUNE - 1] = "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8"; //battery warning fast } ToneAlarm::~ToneAlarm() @@ -873,7 +874,7 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg) case TONE_SET_ALARM: debug("TONE_SET_ALARM %u", arg); - if (arg <= _default_ntunes) { + if (arg <= TONE_NUMBER_OF_TUNES) { if (arg == 0) { // stop the tune _tune = nullptr; @@ -1008,10 +1009,10 @@ tone_alarm_main(int argc, char *argv[]) } if ((argc > 1) && !strcmp(argv[1], "start")) - play_tune(1); + play_tune(TONE_STARTUP_TUNE); if ((argc > 1) && !strcmp(argv[1], "stop")) - play_tune(0); + play_tune(TONE_STOP_TUNE); if ((tune = strtol(argv[1], nullptr, 10)) != 0) play_tune(tune); diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 5df5d8d0c3..7feace2b4b 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -99,44 +99,44 @@ void buzzer_deinit() void tune_error() { - ioctl(buzzer, TONE_SET_ALARM, 2); + ioctl(buzzer, TONE_SET_ALARM, TONE_ERROR_TUNE); } void tune_positive() { - ioctl(buzzer, TONE_SET_ALARM, 3); + ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_POSITIVE_TUNE); } void tune_neutral() { - ioctl(buzzer, TONE_SET_ALARM, 4); + ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEUTRAL_TUNE); } void tune_negative() { - ioctl(buzzer, TONE_SET_ALARM, 5); + ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE); } int tune_arm() { - return ioctl(buzzer, TONE_SET_ALARM, 12); + return ioctl(buzzer, TONE_SET_ALARM, TONE_ARMING_WARNING_TUNE); } int tune_low_bat() { - return ioctl(buzzer, TONE_SET_ALARM, 13); + return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_SLOW_TUNE); } int tune_critical_bat() { - return ioctl(buzzer, TONE_SET_ALARM, 14); + return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_FAST_TUNE); } void tune_stop() { - ioctl(buzzer, TONE_SET_ALARM, 0); + ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE); } static int leds; diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index 4c19dcffb6..a323261cc1 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -176,15 +176,15 @@ system_eval: led_toggle(leds, LED_AMBER); if (i % 10 == 0) { - ioctl(buzzer, TONE_SET_ALARM, 4); + ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEUTRAL_TUNE); } else if (i % 5 == 0) { - ioctl(buzzer, TONE_SET_ALARM, 2); + ioctl(buzzer, TONE_SET_ALARM, TONE_ERROR_TUNE); } usleep(100000); } /* stop alarm */ - ioctl(buzzer, TONE_SET_ALARM, 0); + ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE); /* switch on leds */ led_on(leds, LED_BLUE); diff --git a/src/systemcmds/tests/test_hrt.c b/src/systemcmds/tests/test_hrt.c index f6e540401f..ba6b86adba 100644 --- a/src/systemcmds/tests/test_hrt.c +++ b/src/systemcmds/tests/test_hrt.c @@ -137,7 +137,7 @@ int test_tone(int argc, char *argv[]) tone = atoi(argv[1]); if (tone == 0) { - result = ioctl(fd, TONE_SET_ALARM, 0); + result = ioctl(fd, TONE_SET_ALARM, TONE_STOP_TUNE); if (result < 0) { printf("failed clearing alarms\n"); @@ -148,7 +148,7 @@ int test_tone(int argc, char *argv[]) } } else { - result = ioctl(fd, TONE_SET_ALARM, 0); + result = ioctl(fd, TONE_SET_ALARM, TONE_STOP_TUNE); if (result < 0) { printf("failed clearing alarms\n"); From f246b68c7b53c44ae00e0f3fca724d1fbfab8f5d Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Fri, 30 Aug 2013 20:24:14 -0400 Subject: [PATCH 527/872] Fix parameter range check --- src/drivers/stm32/tone_alarm/tone_alarm.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index a726247d55..262fe6e4c2 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -874,7 +874,7 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg) case TONE_SET_ALARM: debug("TONE_SET_ALARM %u", arg); - if (arg <= TONE_NUMBER_OF_TUNES) { + if (arg < TONE_NUMBER_OF_TUNES) { if (arg == 0) { // stop the tune _tune = nullptr; From b80f0d46ae97aaa4038958ff97165e0c154b745c Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Sat, 31 Aug 2013 01:03:32 -0400 Subject: [PATCH 528/872] Allow tone_alarm cmd to use tone names as well as number - remove script dependency on hard coded tone numbers - fix bug restarting repeating tone after stop --- ROMFS/px4fmu_common/init.d/rc.io | 2 +- ROMFS/px4fmu_common/init.d/rcS | 2 +- src/drivers/drv_tone_alarm.h | 2 +- src/drivers/stm32/tone_alarm/tone_alarm.cpp | 86 +++++++++++++-------- 4 files changed, 56 insertions(+), 36 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.io b/ROMFS/px4fmu_common/init.d/rc.io index 85f00e582e..aaf91b3166 100644 --- a/ROMFS/px4fmu_common/init.d/rc.io +++ b/ROMFS/px4fmu_common/init.d/rc.io @@ -19,5 +19,5 @@ then fi else # SOS - tone_alarm 6 + tone_alarm error fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 982e638fb5..b1648daa7d 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -20,7 +20,7 @@ then else echo "[init] no microSD card found" # Play SOS - tone_alarm 2 + tone_alarm error fi # diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h index 4a399076d4..f0b8606200 100644 --- a/src/drivers/drv_tone_alarm.h +++ b/src/drivers/drv_tone_alarm.h @@ -132,8 +132,8 @@ enum { TONE_NOTIFY_POSITIVE_TUNE, TONE_NOTIFY_NEUTRAL_TUNE, TONE_NOTIFY_NEGATIVE_TUNE, - TONE_CHARGE_TUNE, /* Do not include these unused tunes + TONE_CHARGE_TUNE, TONE_DIXIE_TUNE, TONE_CUCURACHA_TUNE, TONE_YANKEE_TUNE, diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index 262fe6e4c2..d181065418 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -230,10 +230,14 @@ public: virtual int ioctl(file *filp, int cmd, unsigned long arg); virtual ssize_t write(file *filp, const char *buffer, size_t len); + inline const char *name(int tune) { + return _tune_names[tune]; + } private: static const unsigned _tune_max = 1024; // be reasonable about user tunes - static const char * _default_tunes[TONE_NUMBER_OF_TUNES - 1]; + const char * _default_tunes[TONE_NUMBER_OF_TUNES]; + const char * _tune_names[TONE_NUMBER_OF_TUNES]; static const uint8_t _note_tab[]; unsigned _default_tune_number; // number of currently playing default tune (0 for none) @@ -303,9 +307,6 @@ private: }; -// predefined tune array -const char * ToneAlarm::_default_tunes[TONE_NUMBER_OF_TUNES - 1]; - // semitone offsets from C for the characters 'A'-'G' const uint8_t ToneAlarm::_note_tab[] = {9, 11, 0, 2, 4, 5, 7}; @@ -324,20 +325,20 @@ ToneAlarm::ToneAlarm() : { // enable debug() calls //_debug_enabled = true; - _default_tunes[TONE_STARTUP_TUNE - 1] = "MFT240L8 O4aO5dc O4aO5dc O4aO5dc L16dcdcdcdc"; // startup tune - _default_tunes[TONE_ERROR_TUNE - 1] = "MBT200a8a8a8PaaaP"; // ERROR tone - _default_tunes[TONE_NOTIFY_POSITIVE_TUNE - 1] = "MFT200e8a8a"; // NotifyPositive tone - _default_tunes[TONE_NOTIFY_NEUTRAL_TUNE - 1] = "MFT200e8e"; // NotifyNeutral tone - _default_tunes[TONE_NOTIFY_NEGATIVE_TUNE - 1] = "MFT200e8c8e8c8e8c8"; // NotifyNegative tone - _default_tunes[TONE_CHARGE_TUNE - 1] = - "MFT90O3C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C4"; // charge! + _default_tunes[TONE_STARTUP_TUNE] = "MFT240L8 O4aO5dc O4aO5dc O4aO5dc L16dcdcdcdc"; // startup tune + _default_tunes[TONE_ERROR_TUNE] = "MBT200a8a8a8PaaaP"; // ERROR tone + _default_tunes[TONE_NOTIFY_POSITIVE_TUNE] = "MFT200e8a8a"; // Notify Positive tone + _default_tunes[TONE_NOTIFY_NEUTRAL_TUNE] = "MFT200e8e"; // Notify Neutral tone + _default_tunes[TONE_NOTIFY_NEGATIVE_TUNE] = "MFT200e8c8e8c8e8c8"; // Notify Negative tone #if 0 // don't include unused tunes... but keep them for nostalgic reason - _default_tunes[TONE_DIXIE_TUNE - 1] = "MFT60O3C32O2A32F16F16F32G32A32A+32O3C16C16C16O2A16"; // dixie - _default_tunes[TONE_CUCURACHA_TUNE - 1] = "MFT90O2C16C16C16F8.A8C16C16C16F8.A4P16P8"; // cucuracha - _default_tunes[TONE_YANKEE_TUNE - 1] = "MNT150L8O2GGABGBADGGABL4GL8F+"; // yankee - _default_tunes[TONE_DAISY_TUNE - 1] = + _default_tunes[TONE_CHARGE_TUNE] = + "MFT90O3C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C4"; // charge! + _default_tunes[TONE_DIXIE_TUNE] = "MFT60O3C32O2A32F16F16F32G32A32A+32O3C16C16C16O2A16"; // dixie + _default_tunes[TONE_CUCURACHA_TUNE] = "MFT90O2C16C16C16F8.A8C16C16C16F8.A4P16P8"; // cucuracha + _default_tunes[TONE_YANKEE_TUNE] = "MNT150L8O2GGABGBADGGABL4GL8F+"; // yankee + _default_tunes[TONE_DAISY_TUNE] = "MFT200O3C4.O2A4.G4.F4.D8E8F8D4F8C2.O2G4.O3C4.O2A4.F4.D8E8F8G4A8G2P8"; // daisy - _default_tunes[TONE_WILLIAM_TELL_TUNE - 1] = + _default_tunes[TONE_WILLIAM_TELL_TUNE] = "T200O2B4P8B16B16B4P8B16B16B8G+8E8G+8B8G+8B8O3E8" // william tell "O2B8G+8E8G+8B8G+8B8O3E8O2B4P8B16B16B4P8B16" "O2B16B4P8B16B16B4P8B16B16B8B16B16B8B8B8B16" @@ -477,9 +478,18 @@ ToneAlarm::ToneAlarm() : "O3G+8O4E4P8E16E16E8E8E8E8E4P8E16E4P8O2E16" "O2E2P64"; #endif - _default_tunes[TONE_ARMING_WARNING_TUNE - 1] = "MNT75L1O2G"; //arming warning - _default_tunes[TONE_BATTERY_WARNING_SLOW_TUNE - 1] = "MBNT100a8"; //battery warning slow - _default_tunes[TONE_BATTERY_WARNING_FAST_TUNE - 1] = "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8"; //battery warning fast + _default_tunes[TONE_ARMING_WARNING_TUNE] = "MNT75L1O2G"; //arming warning + _default_tunes[TONE_BATTERY_WARNING_SLOW_TUNE] = "MBNT100a8"; //battery warning slow + _default_tunes[TONE_BATTERY_WARNING_FAST_TUNE] = "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8"; //battery warning fast + + _tune_names[TONE_STARTUP_TUNE] = "startup"; // startup tune + _tune_names[TONE_ERROR_TUNE] = "error"; // ERROR tone + _tune_names[TONE_NOTIFY_POSITIVE_TUNE] = "positive"; // Notify Positive tone + _tune_names[TONE_NOTIFY_NEUTRAL_TUNE] = "neutral"; // Notify Neutral tone + _tune_names[TONE_NOTIFY_NEGATIVE_TUNE] = "negative"; // Notify Negative tone + _tune_names[TONE_ARMING_WARNING_TUNE] = "arming"; // arming warning + _tune_names[TONE_BATTERY_WARNING_SLOW_TUNE] = "slow_bat"; // battery warning slow + _tune_names[TONE_BATTERY_WARNING_FAST_TUNE] = "fast_bat"; // battery warning fast } ToneAlarm::~ToneAlarm() @@ -875,16 +885,18 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg) debug("TONE_SET_ALARM %u", arg); if (arg < TONE_NUMBER_OF_TUNES) { - if (arg == 0) { + if (arg == TONE_STOP_TUNE) { // stop the tune _tune = nullptr; _next = nullptr; + _repeat = false; + _default_tune_number = 0; } else { /* 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]); + start_tune(_default_tunes[arg]); } } } else { @@ -1008,22 +1020,30 @@ tone_alarm_main(int argc, char *argv[]) } } - if ((argc > 1) && !strcmp(argv[1], "start")) - play_tune(TONE_STARTUP_TUNE); + if (argc > 1) { + if (!strcmp(argv[1], "start")) + play_tune(TONE_STARTUP_TUNE); - if ((argc > 1) && !strcmp(argv[1], "stop")) - play_tune(TONE_STOP_TUNE); + if (!strcmp(argv[1], "stop")) + play_tune(TONE_STOP_TUNE); - if ((tune = strtol(argv[1], nullptr, 10)) != 0) - play_tune(tune); + if ((tune = strtol(argv[1], nullptr, 10)) != 0) + play_tune(tune); - /* if it looks like a PLAY string... */ - if (strlen(argv[1]) > 2) { - const char *str = argv[1]; - if (str[0] == 'M') { - play_string(str); + /* It might be a tune name */ + for (tune = 1; tune < TONE_NUMBER_OF_TUNES; tune++) + if (!strcmp(g_dev->name(tune), argv[1])) + play_tune(tune); + + /* if it looks like a PLAY string... */ + if (strlen(argv[1]) > 2) { + const char *str = argv[1]; + if (str[0] == 'M') { + play_string(str); + } } + } - errx(1, "unrecognised command, try 'start', 'stop' or an alarm number"); + errx(1, "unrecognised command, try 'start', 'stop' or an alarm number or name"); } From 7292e8c7220eb473fef5eb3a95a11fefd1d1e7e8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 31 Aug 2013 11:21:57 +0200 Subject: [PATCH 529/872] Hotfix for mavlink logbuffer, needs another round of validation. --- src/modules/mavlink/mavlink.c | 5 ++++- src/modules/systemlib/mavlink_log.c | 8 ++++++++ 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index d7b0fa9c7e..78e01cefbf 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -516,7 +516,7 @@ void mavlink_update_system(void) int mavlink_thread_main(int argc, char *argv[]) { /* initialize mavlink text message buffering */ - mavlink_logbuffer_init(&lb, 5); + mavlink_logbuffer_init(&lb, 2); int ch; char *device_name = "/dev/ttyS1"; @@ -738,6 +738,9 @@ int mavlink_thread_main(int argc, char *argv[]) /* Reset the UART flags to original state */ tcsetattr(uart, TCSANOW, &uart_config_original); + /* destroy log buffer */ + mavlink_logbuffer_destroy(&lb); + thread_running = false; exit(0); diff --git a/src/modules/systemlib/mavlink_log.c b/src/modules/systemlib/mavlink_log.c index 27608bdbfe..03ca71375f 100644 --- a/src/modules/systemlib/mavlink_log.c +++ b/src/modules/systemlib/mavlink_log.c @@ -54,6 +54,14 @@ __EXPORT void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size) lb->elems = (struct mavlink_logmessage *)calloc(lb->size, sizeof(struct mavlink_logmessage)); } +__EXPORT void mavlink_logbuffer_destroy(struct mavlink_logbuffer *lb) +{ + lb->size = 0; + lb->start = 0; + lb->count = 0; + free(lb->elems); +} + __EXPORT int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb) { return lb->count == (int)lb->size; From acc06e7eb1dafa8b4bc22b34c2b2c567d67e6b37 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 31 Aug 2013 11:22:15 +0200 Subject: [PATCH 530/872] Added logbuffer free function --- src/include/mavlink/mavlink_log.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/include/mavlink/mavlink_log.h b/src/include/mavlink/mavlink_log.h index a28ff3a687..5054937e07 100644 --- a/src/include/mavlink/mavlink_log.h +++ b/src/include/mavlink/mavlink_log.h @@ -107,7 +107,6 @@ struct mavlink_logmessage { struct mavlink_logbuffer { unsigned int start; - // unsigned int end; unsigned int size; int count; struct mavlink_logmessage *elems; @@ -115,6 +114,8 @@ struct mavlink_logbuffer { void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size); +void mavlink_logbuffer_destroy(struct mavlink_logbuffer *lb); + int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb); int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb); From 669a8029212a715118bf380a524fb7ced64ccacc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 31 Aug 2013 11:22:41 +0200 Subject: [PATCH 531/872] Hotfix: Better PX4IO detection feedbak --- src/drivers/px4io/px4io.cpp | 19 ++++++++++++------- 1 file changed, 12 insertions(+), 7 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 47baa5770a..c88abe59a8 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -497,7 +497,13 @@ PX4IO::detect() /* get some parameters */ unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); if (protocol != PX4IO_PROTOCOL_VERSION) { - log("IO not installed"); + if (protocol == _io_reg_get_error) { + log("IO not installed"); + } else { + log("IO version error"); + mavlink_log_emergency(_mavlink_fd, "IO VERSION MISMATCH, PLEASE UPGRADE SOFTWARE!"); + } + return -1; } log("IO found"); @@ -914,7 +920,6 @@ PX4IO::set_max_values(const uint16_t *vals, unsigned len) int PX4IO::set_idle_values(const uint16_t *vals, unsigned len) { - uint16_t regs[_max_actuators]; if (len > _max_actuators) /* fail with error */ @@ -1612,7 +1617,7 @@ 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(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); printf("\nidle values"); for (unsigned i = 0; i < _max_actuators; i++) printf(" %u", io_reg_get(PX4IO_PAGE_IDLE_PWM, i)); @@ -2233,7 +2238,7 @@ px4io_main(int argc, char *argv[]) /* 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++) { + for (unsigned 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) { @@ -2265,7 +2270,7 @@ px4io_main(int argc, char *argv[]) /* 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++) + for (unsigned 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) { @@ -2300,7 +2305,7 @@ px4io_main(int argc, char *argv[]) /* 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++) + for (unsigned 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) { @@ -2335,7 +2340,7 @@ px4io_main(int argc, char *argv[]) /* 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++) + for (unsigned 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) { From 56975e0dd1e68ed34194b86eb5f82c51ae648391 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 31 Aug 2013 11:23:03 +0200 Subject: [PATCH 532/872] Hotfixed position control param update --- .../multirotor_pos_control.c | 61 +++++++++---------- 1 file changed, 30 insertions(+), 31 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 8227f76c5e..a25448af2b 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -252,36 +252,35 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) 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; - while (!thread_should_exit) { - /* check parameters at 1 Hz */ - if (++paramcheck_counter >= 50) { - paramcheck_counter = 0; - bool param_updated; - orb_check(param_sub, ¶m_updated); - if (param_updated) { - parameters_update(¶ms_h, ¶ms); + bool param_updated; + orb_check(param_sub, ¶m_updated); - 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); - /* use integral_limit_out = tilt_max / 2 */ - float i_limit; + if (param_updated) { + /* clear updated flag */ + struct parameter_update_s ps; + orb_copy(ORB_ID(parameter_update), param_sub, &ps); + /* update params */ + parameters_update(¶ms_h, ¶ms); - if (params.xy_vel_i == 0.0) { - i_limit = params.tilt_max / params.xy_vel_i / 2.0; + 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); + /* use integral_limit_out = tilt_max / 2 */ + float i_limit; - } else { - i_limit = 1.0f; // not used really - } + if (params.xy_vel_i == 0.0f) { + i_limit = params.tilt_max / params.xy_vel_i / 2.0f; - pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max); + } else { + i_limit = 1.0f; // not used really } - 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); + 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); } bool updated; @@ -351,7 +350,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) 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); + mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double)-local_pos_sp.z); } /* move altitude setpoint with throttle stick */ @@ -377,7 +376,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) 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); + mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); } /* move position setpoint with roll/pitch stick */ @@ -429,7 +428,7 @@ 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); + mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double)-local_pos_sp.z); } } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) { @@ -444,7 +443,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) 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); + mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home); } if (global_pos_sp_reproject) { @@ -468,7 +467,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) 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); + mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y); } else { if (!local_pos_sp_valid) { @@ -481,7 +480,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) 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); + mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); } } @@ -505,7 +504,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* 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); + mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", (double)-local_pos_sp.z); } reset_sp_z = true; @@ -515,7 +514,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) 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); + mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", (double)-local_pos_sp.z); } } @@ -527,7 +526,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) 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); + mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); } } } @@ -588,7 +587,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } thrust_pid_set_integral(&z_vel_pid, -i); - mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", i); + mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i); } thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]); From 14cfb53534265cae8690734f2b0d98b8bbf34da5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 31 Aug 2013 11:23:24 +0200 Subject: [PATCH 533/872] Hotfix: Correct frame print string --- ROMFS/px4fmu_common/init.d/11_dji_f450 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/11_dji_f450 b/ROMFS/px4fmu_common/init.d/11_dji_f450 index fb141e8a73..1827a2c114 100644 --- a/ROMFS/px4fmu_common/init.d/11_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/11_dji_f450 @@ -1,6 +1,6 @@ #!nsh -echo "[init] PX4FMU v1, v2 with or without IO on DJI F330" +echo "[init] PX4FMU v1, v2 with or without IO on DJI F450" # # Load default params for this platform From fb4ca82b84c5c49a968cdaf7a9fa8098a9ed9e15 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 31 Aug 2013 11:33:57 +0200 Subject: [PATCH 534/872] Hotfix: Cleanup on startup scripts --- ROMFS/px4fmu_common/init.d/rc.sensors | 8 -------- ROMFS/px4fmu_common/init.d/rc.usb | 8 ++++++++ 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 61bb097281..2828a108bf 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -7,14 +7,6 @@ # Start sensor drivers here. # -# -# Check for UORB -# -if uorb start -then - echo "uORB started" -fi - ms5611 start adc start diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index abeed0ca3c..9264985d6d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -8,6 +8,14 @@ echo "Starting MAVLink on this USB console" # Stop tone alarm tone_alarm stop +# +# Check for UORB +# +if uorb start +then + echo "uORB started" +fi + # Tell MAVLink that this link is "fast" if mavlink stop then From 87f7ebdd75ac11139f7b1ae568590ab1eabbd45c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 31 Aug 2013 11:38:29 +0200 Subject: [PATCH 535/872] Hotfix: Rely on gyro calibration --- .../attitude_estimator_ekf_main.cpp | 22 ++++++++++--------- 1 file changed, 12 insertions(+), 10 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 65abcde1e3..a70a14fe4b 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -308,18 +308,20 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); if (!initialized) { + // XXX disabling init for now + initialized = true; - gyro_offsets[0] += raw.gyro_rad_s[0]; - gyro_offsets[1] += raw.gyro_rad_s[1]; - gyro_offsets[2] += raw.gyro_rad_s[2]; - offset_count++; + // gyro_offsets[0] += raw.gyro_rad_s[0]; + // gyro_offsets[1] += raw.gyro_rad_s[1]; + // gyro_offsets[2] += raw.gyro_rad_s[2]; + // offset_count++; - if (hrt_absolute_time() - start_time > 3000000LL) { - initialized = true; - gyro_offsets[0] /= offset_count; - gyro_offsets[1] /= offset_count; - gyro_offsets[2] /= offset_count; - } + // if (hrt_absolute_time() - start_time > 3000000LL) { + // initialized = true; + // gyro_offsets[0] /= offset_count; + // gyro_offsets[1] /= offset_count; + // gyro_offsets[2] /= offset_count; + // } } else { From 3f4315b4767ff221936e135b3252794a952f2b95 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 31 Aug 2013 11:49:36 +0200 Subject: [PATCH 536/872] Hotfix: Increase stack size for low prio commander task --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 8ddd86d03f..45e0307e6a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -594,7 +594,7 @@ int commander_thread_main(int argc, char *argv[]) pthread_attr_t commander_low_prio_attr; pthread_attr_init(&commander_low_prio_attr); - pthread_attr_setstacksize(&commander_low_prio_attr, 2048); + pthread_attr_setstacksize(&commander_low_prio_attr, 2992); struct sched_param param; /* low priority */ From ccc5bef3af2852172b18f33edaffb809a0a1ffcb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 31 Aug 2013 11:56:39 +0200 Subject: [PATCH 537/872] Hotfix for IO-less systems --- ROMFS/px4fmu_common/init.d/rcS | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index b1648daa7d..9bb8e4a494 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -115,14 +115,18 @@ then echo "PX4IO running, not upgrading" else echo "Attempting to upgrade PX4IO" - px4io update - if [ -d /fs/microsd ] + if px4io update then - echo "Flashed PX4IO Firmware OK" > /fs/microsd/px4io.log - fi + if [ -d /fs/microsd ] + then + echo "Flashed PX4IO Firmware OK" > /fs/microsd/px4io.log + fi - # Allow IO to safely kick back to app - usleep 200000 + # Allow IO to safely kick back to app + usleep 200000 + else + echo "No PX4IO to upgrade here" + fi fi # From 54644e4206ffb40764f8f00e5ec41d98f54104a1 Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Sat, 31 Aug 2013 16:06:15 -0400 Subject: [PATCH 538/872] Allow tone_alarm cmd to take filename as parameter - Restore ability to play beloved William Tell Overture... from file --- misc/tones/charge.txt | 1 + misc/tones/cucuracha.txt | 1 + misc/tones/daisy.txt | 1 + misc/tones/dixie.txt | 1 + misc/tones/tell.txt | 1 + misc/tones/yankee.txt | 1 + src/drivers/stm32/tone_alarm/tone_alarm.cpp | 193 ++++---------------- 7 files changed, 40 insertions(+), 159 deletions(-) create mode 100644 misc/tones/charge.txt create mode 100644 misc/tones/cucuracha.txt create mode 100644 misc/tones/daisy.txt create mode 100644 misc/tones/dixie.txt create mode 100644 misc/tones/tell.txt create mode 100644 misc/tones/yankee.txt diff --git a/misc/tones/charge.txt b/misc/tones/charge.txt new file mode 100644 index 0000000000..cf33c58b3e --- /dev/null +++ b/misc/tones/charge.txt @@ -0,0 +1 @@ +MFT90O3C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C4 \ No newline at end of file diff --git a/misc/tones/cucuracha.txt b/misc/tones/cucuracha.txt new file mode 100644 index 0000000000..dce328c03b --- /dev/null +++ b/misc/tones/cucuracha.txt @@ -0,0 +1 @@ +MFT90O2C16C16C16F8.A8C16C16C16F8.A4P16P8 \ No newline at end of file diff --git a/misc/tones/daisy.txt b/misc/tones/daisy.txt new file mode 100644 index 0000000000..e39410621c --- /dev/null +++ b/misc/tones/daisy.txt @@ -0,0 +1 @@ +MFT200O3C4.O2A4.G4.F4.D8E8F8D4F8C2.O2G4.O3C4.O2A4.F4.D8E8F8G4A8G2P8 \ No newline at end of file diff --git a/misc/tones/dixie.txt b/misc/tones/dixie.txt new file mode 100644 index 0000000000..f3ddd19ed3 --- /dev/null +++ b/misc/tones/dixie.txt @@ -0,0 +1 @@ +MFT60O3C32O2A32F16F16F32G32A32A+32O3C16C16C16O2A16 \ No newline at end of file diff --git a/misc/tones/tell.txt b/misc/tones/tell.txt new file mode 100644 index 0000000000..ea77d1a245 --- /dev/null +++ b/misc/tones/tell.txt @@ -0,0 +1 @@ +T200O2B4P8B16B16B4P8B16B16B8G+8E8G+8B8G+8B8O3E8O2B8G+8E8G+8B8G+8B8O3E8O2B4P8B16B16B4P8B16O2B16B4P8B16B16B4P8B16B16B8B16B16B8B8B8B16O2B16B8B8B8B16B16B8B8B8B16B16B8B8B2B2B8P8 P4P4P8O1B16B16B8B16B16B8B16B16O2E8F+8G+8 O1B16B16B8B16B16O2E8G+16G+16F+8D+8O1B8B16 O1B16B8B16B16B8B16B16O2E8F+8G+8E16G+16B4 O2B16A16G+16F+16E8G+8E8O3B16B16B8B16B16B8 O3B16B16O4E8F+8G+8O3B16B16B8B16B16O4E8G+16 O4G+16F+8D+8O3B8B16B16B8B16B16B8B16B16O4E8 O4F+8G+8E16G+16B4B16A16G+16F+16E8G+8E8O3G+16 O3G+16G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8 O4C+8O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8 O3G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8 O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8 O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8 O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16 O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8O2B16 O2B16B8F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8 O2E4G+8F+8F+8F+8O3F+16F+16F+8F+16F+16F+8 O3G+8A8F+4A8G+8E4G+8F+8O2B16B16B8O1B16B16 O1B8B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16 O1B16O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16 O1B8B16B16O2E8F+8G+8E16G+16B4B16A16G+16F+16 O2E8G+8E8O3B16B16B8B16B16B8B16B16O4E8F+8 O4G+8O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8 O3B16B16B8B16B16B8B16B16O4E8F+8G+8E16G+16 O4B4B16A16G+16F+16E8G+8E8O3E64F64G64A64B64 O4C64D64E8E16E16E8E8G+4.F+8E8D+8E8C+8O3B16 O4C+16O3B16O4C+16O3B16O4C+16D+16E16O3A16 O3B16A16B16A16B16O4C+16D+16O3G+16A16G+16 O3A16G+16A16B16O4C+16O3F+16G+16F+16G+16F+16 O3G+16F+16G+16F+16G+16F+16D+16O2B16O3B16 O4C+16D+16E8D+8E8C+8O3B16O4C+16O3B16O4C+16 O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16 O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16 O3G+16F+16G+16F+16A16F+16E16E8P8C+4C+16O2C16 O3C+16O2C16O3D+16C+16O2B16A16A16G+16E16C+16 O2C+16C+16C+16C+16E16D+16O1C16G+16G+16G+16 O1G+16G+16G+16O2C+16E16G+16O3C+16C+16C+16 O3C+16C+16O2C16O3C+16O2C16O3D+16C+16O2B16 O2A16A16G+16E16C+16C+16C+16C+16C+16E16D+16 O1C16G+16G+16G+16G+16G+16G+16O2C+16E16G+16 O3C+16E16D+16C+16D+16O2C16G+16G+16G+16O3G+16 O3E16C+16D+16O2C16G+16G+16G+16O3G+16E16C+16 O3D+16O2B16G+16G+16A+16G16D+16D+16G+16G16 O2G+16G16G+16A16G+16F+16E16O1B16A+16B16O2E16 O1B16O2F+16O1B16O2G+16E16D+16E16G+16E16A16 O2F+16B16O3G+16F+16E16D+16F+16E16C+16O2B16 O3C+16O2B16O3C+16D+16E16F+16G+16O2A16B16 O2A16B16O3C+16D+16E16F+16O2G+16A16G+16A16 O2C16O3C+16D+16E16O2F+16G+16F+16G+16F+16 O2G+16F+16G+16F+16G+16F+16D+16O1B16C16O2C+16 O2D+16E16O1B16A+16B16O2E16O1B16O2F+16O1B16 O2G+16E16D+16E16G+16E16A16F+16B16O3G+16F+16 O3E16D+16F+16E16C+16O2B16O3C+16O2B16O3C+16 O3D+16E16F+16G+16O2A16B16A16B16O3C+16D+16 O3E16F+16O2G+16A16G+16A16B16O3C+16D+16E16 O2F+16O3C+16O2C16O3C+16D+16C+16O2A16F+16 O2E16O3E16F+16G+16A16B16O4C+16D+16E8E16E16 O4E8E8G+4.F8E8D+8E8C+8O3B16O4C+16O3B16O4C+16 O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16 O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16 O3G+16F+16G+16F+16G+16F+16G+16F+16G+16F+16 O3D+16O2B16O3B16O4C+16D+16E8E16E16E8E8G+4." O4F+8E8D+8E8C+8O3B16O4C+16O3B16O4C+16O3B16 O4C+16D+16E16O3A16B16A16B16A16B16O4C+16D+16 O3G+16A16G+16A16G+16A16B16O4C+16O3F+16G+16 O3F+16G+16F+16A16G+16F+16E8O2B8O3E8G+16G+16 O3G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8 O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8G+16 O3G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8 O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8 O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8 O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16 O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8A+8 O3B8O2F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8 O2E4G+8F+8B8O1B8O2F+16F+16F+8F+16F+16F+8 O2G+8A8F+4A8G+8E4G+8F+8B16B16B8O1B16B16B8 O1B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16B16 O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16B8B16 O1B16O2E8F+8G+8E16G+16B4B16A16G+16F+16E8 O1B8O2E8O3B16B16B8B16B16B8B16B16O4E8F+8G+8 O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8B16 O3B16B8B16B16B8B16B16O4E8F+8G+8O3E16G+16 O3B4B16A16G+16F+16E16F+16G+16A16G+16A16B16 O4C+16O3B16O4C+16D+16E16D+16E16F+16G+16A16 O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16 O3B16O4A16O3B16O4A16O3B16O4A16O3B16E16F+16 O3G+16A16G+16A16B16O4C+16O3B16O4C+16D+16 O4E16D+16E16F+16G+16A16O3B16O4A16O3B16O4A16 O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16 O3B16O4A16O3B16P16G+16O4G+16O3G+16P16D+16 O4D+16O3D+16P16E16O4E16O3E16P16A16O4A16O3A16 P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16 P16O3E16O4E16O3E16P16A16O4A16O3A16O4G16O3G16 O4G16O3G16O4G16O3G16O4G16O3G16O4G8E8C8E8 O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+16 O3G+16O4G+8E8O3B8O4E8G+16O3G+16O4G+16O3G+16 O4G+16O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16 O3A+16O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16 O4A+8G8E8G8B8P16A+16P16A16P16G+16P16F+16 P16O4E16P16D+16P16C+16P16O3B16P16A+16P16 O3A16P16G+16P16F+16P16E16P16D+16P16F+16E16 O3F+16G+16A16G+16A16B16O4C+16O3B16O4C+16 O4D+16E16D+16E16F+16G+16A16O3B16O4A16O3B16 O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16 O4A16O3B16O4A16O3B16E16F+16G+16A16G+16A16 O3B16O4C+16O3B16O4C+16D+16E16D+16E16F+16 O4G+16A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16 O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16 P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16 P16O3E16O4E16O3E16P16A16O4A16O3A16P16G+16 O4G+16O3G+16P16D+16O4D+16O3D+16P16E16O4E16 O3E16P16A16O4A16O3A16O4G16O3G16O4G16O3G16 O4G16O3G16O4G16O3G16O4G8E8C8E8G+16O3G+16 O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+8 O4E8O3B8O4E8G+16O3G+16O4G+16O3G+16O4G+16 O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16O3A+16 O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16O4A+8 O4G8E8G8B8P16A+16P16A16P16G+16P16F+16P16 O4E16P16D+16P16C+16P16O3B16P16A+16P16A16 P16O3G+16P16F+16P16E16P16D+16P16F16E16D+16 O3E16D+16E8B16B16B8B16B16B8B16B16O4E8F+8 O4G+8O3B16B16B8B16B16B8B16B16O4G+8A8B8P8 O4E8F+8G+8P8O3G+8A8B8P8P2O2B16C16O3C+16D16 O3D+16E16F16F+16G16G+16A16A+16B16C16O4C+16 O4D+16E16D+16F+16D+16E16D+16F+16D+16E16D+16 O4F+16D+16E16D+16F+16D+16E16D+16F+16D+16 O4E16D+16F+16D+16E16D+16F+16D+16E16D+16F+16 O4D+16E8E16O3E16O4E16O3E16O4E16O3E16O4E8 O3B16O2B16O3B16O2B16O3B16O2B16O3B8G+16O2G+16 O3G+16O2G+16O3G+16O2G+16O3G8E16O2E16O3E16 O2E16O3E16O2E16O3E8E16E16E8E8E8O2B16B16B8 O2B8B8G+16G+16G+8G+8G+8E16E16E8E8E8O1B8O2E8 O1B8O2G+8E8B8G+8O3E8O2B8O3E8O2B8O3G+8E8B8 O3G+8O4E4P8E16E16E8E8E8E8E4P8E16E4P8O2E16 O2E2P64 \ No newline at end of file diff --git a/misc/tones/yankee.txt b/misc/tones/yankee.txt new file mode 100644 index 0000000000..3677b39c17 --- /dev/null +++ b/misc/tones/yankee.txt @@ -0,0 +1 @@ +MNT150L8O2GGABGBADGGABL4GL8F+ \ No newline at end of file diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index d181065418..a582ece174 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -235,7 +235,7 @@ public: } private: - static const unsigned _tune_max = 1024; // be reasonable about user tunes + static const unsigned _tune_max = 1024 * 8; // be reasonable about user tunes const char * _default_tunes[TONE_NUMBER_OF_TUNES]; const char * _tune_names[TONE_NUMBER_OF_TUNES]; static const uint8_t _note_tab[]; @@ -330,154 +330,6 @@ ToneAlarm::ToneAlarm() : _default_tunes[TONE_NOTIFY_POSITIVE_TUNE] = "MFT200e8a8a"; // Notify Positive tone _default_tunes[TONE_NOTIFY_NEUTRAL_TUNE] = "MFT200e8e"; // Notify Neutral tone _default_tunes[TONE_NOTIFY_NEGATIVE_TUNE] = "MFT200e8c8e8c8e8c8"; // Notify Negative tone -#if 0 // don't include unused tunes... but keep them for nostalgic reason - _default_tunes[TONE_CHARGE_TUNE] = - "MFT90O3C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C4"; // charge! - _default_tunes[TONE_DIXIE_TUNE] = "MFT60O3C32O2A32F16F16F32G32A32A+32O3C16C16C16O2A16"; // dixie - _default_tunes[TONE_CUCURACHA_TUNE] = "MFT90O2C16C16C16F8.A8C16C16C16F8.A4P16P8"; // cucuracha - _default_tunes[TONE_YANKEE_TUNE] = "MNT150L8O2GGABGBADGGABL4GL8F+"; // yankee - _default_tunes[TONE_DAISY_TUNE] = - "MFT200O3C4.O2A4.G4.F4.D8E8F8D4F8C2.O2G4.O3C4.O2A4.F4.D8E8F8G4A8G2P8"; // daisy - _default_tunes[TONE_WILLIAM_TELL_TUNE] = - "T200O2B4P8B16B16B4P8B16B16B8G+8E8G+8B8G+8B8O3E8" // william tell - "O2B8G+8E8G+8B8G+8B8O3E8O2B4P8B16B16B4P8B16" - "O2B16B4P8B16B16B4P8B16B16B8B16B16B8B8B8B16" - "O2B16B8B8B8B16B16B8B8B8B16B16B8B8B2B2B8P8" - "P4P4P8O1B16B16B8B16B16B8B16B16O2E8F+8G+8" - "O1B16B16B8B16B16O2E8G+16G+16F+8D+8O1B8B16" - "O1B16B8B16B16B8B16B16O2E8F+8G+8E16G+16B4" - "O2B16A16G+16F+16E8G+8E8O3B16B16B8B16B16B8" - "O3B16B16O4E8F+8G+8O3B16B16B8B16B16O4E8G+16" - "O4G+16F+8D+8O3B8B16B16B8B16B16B8B16B16O4E8" - "O4F+8G+8E16G+16B4B16A16G+16F+16E8G+8E8O3G+16" - "O3G+16G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8" - "O4C+8O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8" - "O3G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8" - "O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8" - "O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8" - "O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16" - "O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8O2B16" - "O2B16B8F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8" - "O2E4G+8F+8F+8F+8O3F+16F+16F+8F+16F+16F+8" - "O3G+8A8F+4A8G+8E4G+8F+8O2B16B16B8O1B16B16" - "O1B8B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16" - "O1B16O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16" - "O1B8B16B16O2E8F+8G+8E16G+16B4B16A16G+16F+16" - "O2E8G+8E8O3B16B16B8B16B16B8B16B16O4E8F+8" - "O4G+8O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8" - "O3B16B16B8B16B16B8B16B16O4E8F+8G+8E16G+16" - "O4B4B16A16G+16F+16E8G+8E8O3E64F64G64A64B64" - "O4C64D64E8E16E16E8E8G+4.F+8E8D+8E8C+8O3B16" - "O4C+16O3B16O4C+16O3B16O4C+16D+16E16O3A16" - "O3B16A16B16A16B16O4C+16D+16O3G+16A16G+16" - "O3A16G+16A16B16O4C+16O3F+16G+16F+16G+16F+16" - "O3G+16F+16G+16F+16G+16F+16D+16O2B16O3B16" - "O4C+16D+16E8D+8E8C+8O3B16O4C+16O3B16O4C+16" - "O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16" - "O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16" - "O3G+16F+16G+16F+16A16F+16E16E8P8C+4C+16O2C16" - "O3C+16O2C16O3D+16C+16O2B16A16A16G+16E16C+16" - "O2C+16C+16C+16C+16E16D+16O1C16G+16G+16G+16" - "O1G+16G+16G+16O2C+16E16G+16O3C+16C+16C+16" - "O3C+16C+16O2C16O3C+16O2C16O3D+16C+16O2B16" - "O2A16A16G+16E16C+16C+16C+16C+16C+16E16D+16" - "O1C16G+16G+16G+16G+16G+16G+16O2C+16E16G+16" - "O3C+16E16D+16C+16D+16O2C16G+16G+16G+16O3G+16" - "O3E16C+16D+16O2C16G+16G+16G+16O3G+16E16C+16" - "O3D+16O2B16G+16G+16A+16G16D+16D+16G+16G16" - "O2G+16G16G+16A16G+16F+16E16O1B16A+16B16O2E16" - "O1B16O2F+16O1B16O2G+16E16D+16E16G+16E16A16" - "O2F+16B16O3G+16F+16E16D+16F+16E16C+16O2B16" - "O3C+16O2B16O3C+16D+16E16F+16G+16O2A16B16" - "O2A16B16O3C+16D+16E16F+16O2G+16A16G+16A16" - "O2C16O3C+16D+16E16O2F+16G+16F+16G+16F+16" - "O2G+16F+16G+16F+16G+16F+16D+16O1B16C16O2C+16" - "O2D+16E16O1B16A+16B16O2E16O1B16O2F+16O1B16" - "O2G+16E16D+16E16G+16E16A16F+16B16O3G+16F+16" - "O3E16D+16F+16E16C+16O2B16O3C+16O2B16O3C+16" - "O3D+16E16F+16G+16O2A16B16A16B16O3C+16D+16" - "O3E16F+16O2G+16A16G+16A16B16O3C+16D+16E16" - "O2F+16O3C+16O2C16O3C+16D+16C+16O2A16F+16" - "O2E16O3E16F+16G+16A16B16O4C+16D+16E8E16E16" - "O4E8E8G+4.F8E8D+8E8C+8O3B16O4C+16O3B16O4C+16" - "O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16" - "O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16" - "O3G+16F+16G+16F+16G+16F+16G+16F+16G+16F+16" - "O3D+16O2B16O3B16O4C+16D+16E8E16E16E8E8G+4." - "O4F+8E8D+8E8C+8O3B16O4C+16O3B16O4C+16O3B16" - "O4C+16D+16E16O3A16B16A16B16A16B16O4C+16D+16" - "O3G+16A16G+16A16G+16A16B16O4C+16O3F+16G+16" - "O3F+16G+16F+16A16G+16F+16E8O2B8O3E8G+16G+16" - "O3G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8" - "O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8G+16" - "O3G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8" - "O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8" - "O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8" - "O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16" - "O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8A+8" - "O3B8O2F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8" - "O2E4G+8F+8B8O1B8O2F+16F+16F+8F+16F+16F+8" - "O2G+8A8F+4A8G+8E4G+8F+8B16B16B8O1B16B16B8" - "O1B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16B16" - "O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16B8B16" - "O1B16O2E8F+8G+8E16G+16B4B16A16G+16F+16E8" - "O1B8O2E8O3B16B16B8B16B16B8B16B16O4E8F+8G+8" - "O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8B16" - "O3B16B8B16B16B8B16B16O4E8F+8G+8O3E16G+16" - "O3B4B16A16G+16F+16E16F+16G+16A16G+16A16B16" - "O4C+16O3B16O4C+16D+16E16D+16E16F+16G+16A16" - "O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16" - "O3B16O4A16O3B16O4A16O3B16O4A16O3B16E16F+16" - "O3G+16A16G+16A16B16O4C+16O3B16O4C+16D+16" - "O4E16D+16E16F+16G+16A16O3B16O4A16O3B16O4A16" - "O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16" - "O3B16O4A16O3B16P16G+16O4G+16O3G+16P16D+16" - "O4D+16O3D+16P16E16O4E16O3E16P16A16O4A16O3A16" - "P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16" - "P16O3E16O4E16O3E16P16A16O4A16O3A16O4G16O3G16" - "O4G16O3G16O4G16O3G16O4G16O3G16O4G8E8C8E8" - "O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+16" - "O3G+16O4G+8E8O3B8O4E8G+16O3G+16O4G+16O3G+16" - "O4G+16O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16" - "O3A+16O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16" - "O4A+8G8E8G8B8P16A+16P16A16P16G+16P16F+16" - "P16O4E16P16D+16P16C+16P16O3B16P16A+16P16" - "O3A16P16G+16P16F+16P16E16P16D+16P16F+16E16" - "O3F+16G+16A16G+16A16B16O4C+16O3B16O4C+16" - "O4D+16E16D+16E16F+16G+16A16O3B16O4A16O3B16" - "O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16" - "O4A16O3B16O4A16O3B16E16F+16G+16A16G+16A16" - "O3B16O4C+16O3B16O4C+16D+16E16D+16E16F+16" - "O4G+16A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16" - "O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16" - "P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16" - "P16O3E16O4E16O3E16P16A16O4A16O3A16P16G+16" - "O4G+16O3G+16P16D+16O4D+16O3D+16P16E16O4E16" - "O3E16P16A16O4A16O3A16O4G16O3G16O4G16O3G16" - "O4G16O3G16O4G16O3G16O4G8E8C8E8G+16O3G+16" - "O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+8" - "O4E8O3B8O4E8G+16O3G+16O4G+16O3G+16O4G+16" - "O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16O3A+16" - "O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16O4A+8" - "O4G8E8G8B8P16A+16P16A16P16G+16P16F+16P16" - "O4E16P16D+16P16C+16P16O3B16P16A+16P16A16" - "P16O3G+16P16F+16P16E16P16D+16P16F16E16D+16" - "O3E16D+16E8B16B16B8B16B16B8B16B16O4E8F+8" - "O4G+8O3B16B16B8B16B16B8B16B16O4G+8A8B8P8" - "O4E8F+8G+8P8O3G+8A8B8P8P2O2B16C16O3C+16D16" - "O3D+16E16F16F+16G16G+16A16A+16B16C16O4C+16" - "O4D+16E16D+16F+16D+16E16D+16F+16D+16E16D+16" - "O4F+16D+16E16D+16F+16D+16E16D+16F+16D+16" - "O4E16D+16F+16D+16E16D+16F+16D+16E16D+16F+16" - "O4D+16E8E16O3E16O4E16O3E16O4E16O3E16O4E8" - "O3B16O2B16O3B16O2B16O3B16O2B16O3B8G+16O2G+16" - "O3G+16O2G+16O3G+16O2G+16O3G8E16O2E16O3E16" - "O2E16O3E16O2E16O3E8E16E16E8E8E8O2B16B16B8" - "O2B8B8G+16G+16G+8G+8G+8E16E16E8E8E8O1B8O2E8" - "O1B8O2G+8E8B8G+8O3E8O2B8O3E8O2B8O3G+8E8B8" - "O3G+8O4E4P8E16E16E8E8E8E8E4P8E16E4P8O2E16" - "O2E2P64"; -#endif _default_tunes[TONE_ARMING_WARNING_TUNE] = "MNT75L1O2G"; //arming warning _default_tunes[TONE_BATTERY_WARNING_SLOW_TUNE] = "MBNT100a8"; //battery warning slow _default_tunes[TONE_BATTERY_WARNING_FAST_TUNE] = "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8"; //battery warning fast @@ -983,7 +835,7 @@ play_tune(unsigned tune) } int -play_string(const char *str) +play_string(const char *str, bool free_buffer) { int fd, ret; @@ -995,6 +847,9 @@ play_string(const char *str) ret = write(fd, str, strlen(str) + 1); close(fd); + if (free_buffer) + free((void *)str); + if (ret < 0) err(1, "play tune"); exit(0); @@ -1021,29 +876,49 @@ tone_alarm_main(int argc, char *argv[]) } if (argc > 1) { - if (!strcmp(argv[1], "start")) + const char *argv1 = argv[1]; + + if (!strcmp(argv1, "start")) play_tune(TONE_STARTUP_TUNE); - if (!strcmp(argv[1], "stop")) + if (!strcmp(argv1, "stop")) play_tune(TONE_STOP_TUNE); - if ((tune = strtol(argv[1], nullptr, 10)) != 0) + if ((tune = strtol(argv1, nullptr, 10)) != 0) play_tune(tune); /* It might be a tune name */ for (tune = 1; tune < TONE_NUMBER_OF_TUNES; tune++) - if (!strcmp(g_dev->name(tune), argv[1])) + if (!strcmp(g_dev->name(tune), argv1)) play_tune(tune); + /* If it is a file name then load and play it as a string */ + if (*argv1 == '/') { + FILE *fd = fopen(argv1, "r"); + int sz; + char *buffer; + if (fd == nullptr) + errx(1, "couldn't open '%s'", argv1); + fseek(fd, 0, SEEK_END); + sz = ftell(fd); + fseek(fd, 0, SEEK_SET); + buffer = (char *)malloc(sz + 1); + if (buffer == nullptr) + errx(1, "not enough memory memory"); + fread(buffer, sz, 1, fd); + /* terminate the string */ + buffer[sz] = 0; + play_string(buffer, true); + } + /* if it looks like a PLAY string... */ - if (strlen(argv[1]) > 2) { - const char *str = argv[1]; - if (str[0] == 'M') { - play_string(str); + if (strlen(argv1) > 2) { + if (*argv1 == 'M') { + play_string(argv1, false); } } } - errx(1, "unrecognised command, try 'start', 'stop' or an alarm number or name"); + errx(1, "unrecognized command, try 'start', 'stop', an alarm number or name, or a file name starting with a '/'"); } From a1c4c0fd787401f8ed332fa974a88a74ae079383 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Sep 2013 00:09:59 +0200 Subject: [PATCH 539/872] Comments on scaling factors --- src/modules/sensors/sensor_params.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index bd431c9ebd..6c8e514b67 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -159,6 +159,8 @@ 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 */ /* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */ +/* PX4IOAR: 0.00838095238 */ +/* FMU standalone: 1/(10 / (47+10)) * (3.3 / 4095) = 0.00459340659 */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f)); PARAM_DEFINE_INT32(RC_MAP_ROLL, 1); From c3408332fd0e6d77661a26fade8662a8b9be9970 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Sep 2013 10:29:12 +0200 Subject: [PATCH 540/872] Added test to test unlink() --- src/systemcmds/tests/tests_file.c | 30 ++++++++++++++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/src/systemcmds/tests/tests_file.c b/src/systemcmds/tests/tests_file.c index 6f75b9812e..47f480758c 100644 --- a/src/systemcmds/tests/tests_file.c +++ b/src/systemcmds/tests/tests_file.c @@ -76,9 +76,39 @@ test_file(int argc, char *argv[]) close(fd); + unlink("/fs/microsd/testfile"); + warnx("512KiB in %llu microseconds", end - start); perf_print_counter(wperf); perf_free(wperf); + warnx("running unlink test"); + + /* ensure that common commands do not run against file count limits */ + for (unsigned i = 0; i < 64; i++) { + + warnx("unlink iteration #%u", i); + + int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); + if (fd < 0) + errx(1, "failed opening test file before unlink()"); + int ret = write(fd, buf, sizeof(buf)); + if (ret < 0) + errx(1, "failed writing test file before unlink()"); + close(fd); + + ret = unlink("/fs/microsd/testfile"); + if (ret != OK) + errx(1, "failed unlinking test file"); + + fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); + if (fd < 0) + errx(1, "failed opening test file after unlink()"); + ret = write(fd, buf, sizeof(buf)); + if (ret < 0) + errx(1, "failed writing test file after unlink()"); + close(fd); + } + return 0; } From f6bf1c7bf2d83bfc70fb8c4b9f589b25bcccc5e4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Sep 2013 10:29:30 +0200 Subject: [PATCH 541/872] Closing files that should be closed --- src/modules/commander/commander.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 45e0307e6a..8e8226f091 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -319,6 +319,8 @@ void print_status() break; } + close(state_sub); + warnx("arming: %s", armed_str); } @@ -1764,5 +1766,7 @@ void *commander_low_prio_loop(void *arg) } + close(cmd_sub); + return 0; } From 9eff3170a3aa63d17fbaefd39619866fb745b237 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Sep 2013 10:30:04 +0200 Subject: [PATCH 542/872] More verbosity on RC check --- src/modules/systemlib/rc_check.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c index 9d47d8000d..f3a2aee9eb 100644 --- a/src/modules/systemlib/rc_check.c +++ b/src/modules/systemlib/rc_check.c @@ -112,13 +112,13 @@ int rc_calibration_check(void) { } if (param_trim < param_min) { count++; - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN", i+1); + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN (%d/%d)", i+1, (int)param_trim, (int)param_min); /* 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); + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM > MAX (%d/%d)", i+1, (int)param_trim, (int)param_max); /* give system time to flush error message in case there are more */ usleep(100000); } From 2d83c6f825db02f04624467f3d0b492ee371c72a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Sep 2013 12:47:10 +0200 Subject: [PATCH 543/872] Closing all opened file descriptors, fixed param save issue, tests clean --- .../commander/accelerometer_calibration.cpp | 4 +- .../commander/airspeed_calibration.cpp | 9 ++-- src/modules/commander/gyro_calibration.cpp | 20 +++++---- src/modules/commander/mag_calibration.cpp | 44 +++++-------------- src/modules/commander/rc_calibration.cpp | 3 ++ src/modules/systemlib/param/param.c | 11 ++++- src/modules/systemlib/rc_check.c | 7 ++- .../preflight_check/preflight_check.c | 7 +++ 8 files changed, 57 insertions(+), 48 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index ed6707f045..cfa7d9e8a1 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -241,8 +241,10 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float break; int orient = detect_orientation(mavlink_fd, sensor_combined_sub); - if (orient < 0) + if (orient < 0) { + close(sensor_combined_sub); return ERROR; + } if (data_collected[orient]) { mavlink_log_info(mavlink_fd, "%s done, please rotate to a different axis", orientation_strs[orient]); diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index e414e5f707..248eb4a66e 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -85,6 +85,7 @@ int do_airspeed_calibration(int mavlink_fd) } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ mavlink_log_info(mavlink_fd, "airspeed calibration aborted"); + close(diff_pres_sub); return ERROR; } } @@ -95,6 +96,7 @@ int 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!"); + close(diff_pres_sub); return ERROR; } @@ -104,18 +106,17 @@ int 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"); + close(diff_pres_sub); return ERROR; } - //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"); - + close(diff_pres_sub); return OK; } else { mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); + close(diff_pres_sub); return ERROR; } } diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 33566d4e53..82a0349c9e 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -60,12 +60,12 @@ int do_gyro_calibration(int mavlink_fd) { mavlink_log_info(mavlink_fd, "Gyro calibration starting, do not move unit."); - const int calibration_count = 5000; + const unsigned calibration_count = 5000; int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); struct sensor_combined_s raw; - int calibration_counter = 0; + unsigned calibration_counter = 0; float gyro_offset[3] = {0.0f, 0.0f, 0.0f}; /* set offsets to zero */ @@ -101,6 +101,8 @@ int do_gyro_calibration(int mavlink_fd) gyro_offset[1] += raw.gyro_rad_s[1]; gyro_offset[2] += raw.gyro_rad_s[2]; calibration_counter++; + if (calibration_counter % (calibration_count / 20) == 0) + mavlink_log_info(mavlink_fd, "gyro cal progress <%u> percent", (calibration_counter * 100) / calibration_count); } else { poll_errcount++; @@ -108,6 +110,7 @@ int do_gyro_calibration(int mavlink_fd) if (poll_errcount > 1000) { mavlink_log_info(mavlink_fd, "ERROR: Failed reading gyro sensor"); + close(sub_sensor_combined); return ERROR; } } @@ -147,24 +150,23 @@ int 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"); + close(sub_sensor_combined); return ERROR; } - mavlink_log_info(mavlink_fd, "gyro calibration done"); - tune_neutral(); /* third beep by cal end routine */ } else { mavlink_log_info(mavlink_fd, "offset cal FAILED (NaN)"); + close(sub_sensor_combined); return ERROR; } /*** --- SCALING --- ***/ - 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."); + mavlink_log_info(mavlink_fd, "offset done. Rotate for scale 30x or wait 5s 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; @@ -187,8 +189,8 @@ int 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)) { - mavlink_log_info(mavlink_fd, "gyro scale calibration skipped"); - mavlink_log_info(mavlink_fd, "gyro calibration done"); + mavlink_log_info(mavlink_fd, "scale skipped, gyro calibration done"); + close(sub_sensor_combined); return OK; } @@ -281,9 +283,11 @@ int do_gyro_calibration(int mavlink_fd) mavlink_log_info(mavlink_fd, "gyro calibration done"); /* third beep by cal end routine */ + close(sub_sensor_combined); return OK; } else { mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)"); + close(sub_sensor_combined); return ERROR; } } diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index e386160271..b0d4266be5 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -142,7 +142,6 @@ int do_mag_calibration(int mavlink_fd) 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(); axis_deadline += calibration_interval / 3; @@ -152,14 +151,6 @@ int do_mag_calibration(int mavlink_fd) 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 > 0) { @@ -169,30 +160,10 @@ int do_mag_calibration(int mavlink_fd) 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++; + if (calibration_counter % (calibration_maxcount / 20) == 0) + mavlink_log_info(mavlink_fd, "mag cal progress <%u> percent", 20 + (calibration_counter * 50) / calibration_maxcount); + } else { poll_errcount++; @@ -200,6 +171,10 @@ int do_mag_calibration(int mavlink_fd) if (poll_errcount > 1000) { mavlink_log_info(mavlink_fd, "ERROR: Failed reading mag sensor"); + close(sub_mag); + free(x); + free(y); + free(z); return ERROR; } @@ -211,7 +186,9 @@ int do_mag_calibration(int mavlink_fd) float sphere_z; float sphere_radius; + mavlink_log_info(mavlink_fd, "mag cal progress <70> percent"); sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); + mavlink_log_info(mavlink_fd, "mag cal progress <80> percent"); free(x); free(y); @@ -269,6 +246,7 @@ int 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"); + close(sub_mag); return ERROR; } @@ -288,11 +266,13 @@ int do_mag_calibration(int mavlink_fd) mavlink_log_info(mavlink_fd, "magnetometer calibration completed"); mavlink_log_info(mavlink_fd, "mag cal progress <100> percent"); + close(sub_mag); return OK; /* third beep by cal end routine */ } else { mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)"); + close(sub_mag); return ERROR; } } diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp index fe87a3323b..9fc1d64700 100644 --- a/src/modules/commander/rc_calibration.cpp +++ b/src/modules/commander/rc_calibration.cpp @@ -40,6 +40,7 @@ #include "commander_helper.h" #include +#include #include #include #include @@ -80,9 +81,11 @@ int do_rc_calibration(int mavlink_fd) if (save_ret != 0) { mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed"); + close(sub_man); return ERROR; } mavlink_log_info(mavlink_fd, "trim calibration done"); + close(sub_man); return OK; } diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 69a9bdf9b5..24b52d1a92 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -505,8 +505,15 @@ param_get_default_file(void) int param_save_default(void) { + int result; + /* delete the file in case it exists */ - unlink(param_get_default_file()); + struct stat buffer; + if (stat(param_get_default_file(), &buffer) == 0) { + result = unlink(param_get_default_file()); + if (result != OK) + warnx("unlinking file %s failed.", param_get_default_file()); + } /* create the file */ int fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL); @@ -516,7 +523,7 @@ param_save_default(void) return -1; } - int result = param_export(fd, false); + result = param_export(fd, false); close(fd); if (result != 0) { diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c index f3a2aee9eb..60d6473b8c 100644 --- a/src/modules/systemlib/rc_check.c +++ b/src/modules/systemlib/rc_check.c @@ -66,7 +66,7 @@ int rc_calibration_check(void) { // count++; // } - + int channel_fail_count = 0; for (int i = 0; i < RC_CHANNELS_MAX; i++) { /* should the channel be enabled? */ @@ -142,7 +142,12 @@ int rc_calibration_check(void) { /* sanity checks pass, enable channel */ if (count) { mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); + warnx(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); usleep(100000); } + + channel_fail_count += count; } + + return channel_fail_count; } diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index a323261cc1..f5bd01ce8c 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -142,6 +142,10 @@ int preflight_check_main(int argc, char *argv[]) bool rc_ok = (OK == rc_calibration_check()); + /* warn */ + if (!rc_ok) + warnx("rc calibration test failed"); + /* require RC ok to keep system_ok */ system_ok &= rc_ok; @@ -156,6 +160,9 @@ system_eval: } else { fflush(stdout); + warnx("PREFLIGHT CHECK ERROR! TRIGGERING ALARM"); + fflush(stderr); + int buzzer = open("/dev/tone_alarm", O_WRONLY); int leds = open(LED_DEVICE_PATH, 0); From f0a750714ae07976b295c0a857d85cbd57a09a72 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 1 Sep 2013 14:12:48 +0200 Subject: [PATCH 544/872] pid.c: fixed bad merge of seatbelt_multirotor, should fix EASY/AUTO modes --- src/modules/systemlib/pid/pid.c | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) 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 ff23feb24e2b8a9ad0bfc8fb1556939df5512310 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Sep 2013 15:32:40 +0200 Subject: [PATCH 545/872] Hotfix: Chasing log buffer issue --- src/modules/mavlink/mavlink.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 78e01cefbf..a8ca19d7a4 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -516,7 +516,7 @@ void mavlink_update_system(void) int mavlink_thread_main(int argc, char *argv[]) { /* initialize mavlink text message buffering */ - mavlink_logbuffer_init(&lb, 2); + mavlink_logbuffer_init(&lb, 10); int ch; char *device_name = "/dev/ttyS1"; From e553087d10e7187c12e5e0f28937585a732a46da Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 1 Sep 2013 20:17:24 +0200 Subject: [PATCH 546/872] pid.limit != 0 fix --- src/modules/systemlib/pid/pid.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 562f3ac04b..739503ed41 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -51,6 +51,8 @@ #include "pid.h" #include +#define SIGMA 0.000001f + __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, uint8_t mode, float dt_min) { @@ -168,7 +170,7 @@ __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 ((pid->limit != 0.0f && (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit)) || + if ((pid->limit > SIGMA && (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,7 +188,7 @@ __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 (pid->limit != 0.0f) { + if (pid->limit > SIGMA) { if (output > pid->limit) { output = pid->limit; From fddaa49ed8fadb5f2574225f121d6d8aa8654406 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sun, 1 Sep 2013 22:44:58 +0200 Subject: [PATCH 547/872] Make sure the unit tests aren't included as part of the standard firmware --- makefiles/config_px4fmu-v1_default.mk | 4 ++-- makefiles/config_px4fmu-v2_default.mk | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index fff43c6b47..3e94c35344 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -94,8 +94,8 @@ MODULES += modules/sdlog2 # # Unit tests # -MODULES += modules/unit_test -MODULES += modules/commander/commander_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 ab0da35596..bd73bde5ea 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -88,8 +88,8 @@ MODULES += modules/sdlog2 # # Unit tests # -MODULES += modules/unit_test -MODULES += modules/commander/commander_tests +#MODULES += modules/unit_test +#MODULES += modules/commander/commander_tests # # Library modules From 589ae937ee7afe5903f98f4eea25a26a79aca8f1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 2 Sep 2013 17:55:27 +0200 Subject: [PATCH 548/872] Allow mixer upload when PWM is on --- src/modules/px4iofirmware/mixer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index deed258364..0edd91b243 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -358,8 +358,8 @@ static unsigned mixer_text_length = 0; void mixer_handle_text(const void *buffer, size_t length) { - /* do not allow a mixer change while outputs armed */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { + /* do not allow a mixer change while safety off */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) { return; } From 027d45acbfa06dc155f6a18d0288fb8230d05c9f Mon Sep 17 00:00:00 2001 From: tstellanova Date: Mon, 2 Sep 2013 09:13:36 -0700 Subject: [PATCH 549/872] respect NAV_TAKEOFF_ALT instead of using hardcoded default takeoff value --- src/modules/multirotor_pos_control/multirotor_pos_control.c | 5 +++-- .../multirotor_pos_control/multirotor_pos_control_params.c | 2 ++ .../multirotor_pos_control/multirotor_pos_control_params.h | 2 ++ 3 files changed, 7 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 a25448af2b..c194f627d0 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -227,7 +227,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) hrt_abstime t_prev = 0; 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; @@ -244,6 +244,7 @@ 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, 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.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); @@ -423,7 +424,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) 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.z = -params.takeoff_alt; local_pos_sp.yaw = att.yaw; local_pos_sp_valid = true; att_sp.yaw_body = att.yaw; 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 9c1ef2edb3..4d65118511 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -59,6 +59,7 @@ PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 0.5f); int parameters_init(struct multirotor_position_control_param_handles *h) { + h->takeoff_alt = param_find("NAV_TAKEOFF_ALT"); h->thr_min = param_find("MPC_THR_MIN"); h->thr_max = param_find("MPC_THR_MAX"); h->z_p = param_find("MPC_Z_P"); @@ -84,6 +85,7 @@ int parameters_init(struct multirotor_position_control_param_handles *h) int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p) { + param_get(h->takeoff_alt,&(p->takeoff_alt)); param_get(h->thr_min, &(p->thr_min)); param_get(h->thr_max, &(p->thr_max)); param_get(h->z_p, &(p->z_p)); 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 3ec85a3641..79bc9b72bf 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h @@ -41,6 +41,7 @@ #include struct multirotor_position_control_params { + float takeoff_alt; float thr_min; float thr_max; float z_p; @@ -63,6 +64,7 @@ struct multirotor_position_control_params { }; struct multirotor_position_control_param_handles { + param_t takeoff_alt; param_t thr_min; param_t thr_max; param_t z_p; From 193a52d8132ce03aa0de9547f77ca8aeb67220f1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 2 Sep 2013 23:13:01 +0200 Subject: [PATCH 550/872] multirotor_pos_control: added takeoff gap (hardcoded 3m), fixed code style --- .../multirotor_pos_control/multirotor_pos_control.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index c194f627d0..5260a0963f 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -244,7 +244,7 @@ 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, 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.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); @@ -351,7 +351,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) 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", (double)-local_pos_sp.z); + mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - local_pos_sp.z); } /* move altitude setpoint with throttle stick */ @@ -424,12 +424,12 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (reset_auto_pos) { local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; - local_pos_sp.z = -params.takeoff_alt; + local_pos_sp.z = - params.takeoff_alt - 3.0f; 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", (double)local_pos_sp.x, (double)local_pos_sp.y, (double)-local_pos_sp.z); + mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double) - local_pos_sp.z); } } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) { @@ -505,7 +505,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* 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", (double)-local_pos_sp.z); + mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", (double) - local_pos_sp.z); } reset_sp_z = true; @@ -515,7 +515,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) 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", (double)-local_pos_sp.z); + mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", (double) - local_pos_sp.z); } } From 40e18948727b41843fc0766e520f92b8cc841296 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 2 Sep 2013 23:30:32 +0200 Subject: [PATCH 551/872] Added parameter NAV_TAKEOFF_GAP --- src/modules/commander/commander_params.c | 1 + src/modules/multirotor_pos_control/multirotor_pos_control.c | 2 +- .../multirotor_pos_control/multirotor_pos_control_params.c | 4 +++- .../multirotor_pos_control/multirotor_pos_control_params.h | 6 ++++-- 4 files changed, 9 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index f22dac0c15..40d0386d5e 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -46,6 +46,7 @@ #include PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 5.0f); +PARAM_DEFINE_FLOAT(NAV_TAKEOFF_GAP, 3.0f); 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/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 5260a0963f..3365230724 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -424,7 +424,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (reset_auto_pos) { local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; - local_pos_sp.z = - params.takeoff_alt - 3.0f; + local_pos_sp.z = - params.takeoff_alt - params.takeoff_gap; local_pos_sp.yaw = att.yaw; local_pos_sp_valid = true; att_sp.yaw_body = att.yaw; 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 4d65118511..bf9578ea3d 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -60,6 +60,7 @@ PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 0.5f); int parameters_init(struct multirotor_position_control_param_handles *h) { h->takeoff_alt = param_find("NAV_TAKEOFF_ALT"); + h->takeoff_gap = param_find("NAV_TAKEOFF_GAP"); h->thr_min = param_find("MPC_THR_MIN"); h->thr_max = param_find("MPC_THR_MAX"); h->z_p = param_find("MPC_Z_P"); @@ -85,7 +86,8 @@ int parameters_init(struct multirotor_position_control_param_handles *h) int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p) { - param_get(h->takeoff_alt,&(p->takeoff_alt)); + param_get(h->takeoff_alt, &(p->takeoff_alt)); + param_get(h->takeoff_gap, &(p->takeoff_gap)); param_get(h->thr_min, &(p->thr_min)); param_get(h->thr_max, &(p->thr_max)); param_get(h->z_p, &(p->z_p)); 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 79bc9b72bf..fc658dadb8 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h @@ -41,7 +41,8 @@ #include struct multirotor_position_control_params { - float takeoff_alt; + float takeoff_alt; + float takeoff_gap; float thr_min; float thr_max; float z_p; @@ -64,7 +65,8 @@ struct multirotor_position_control_params { }; struct multirotor_position_control_param_handles { - param_t takeoff_alt; + param_t takeoff_alt; + param_t takeoff_gap; param_t thr_min; param_t thr_max; param_t z_p; From 29c06e39431eb6e5112a2cefa9969d59fa2ff875 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Sep 2013 07:36:43 +0200 Subject: [PATCH 552/872] Set sane defaults for F330 --- ROMFS/px4fmu_common/init.d/10_dji_f330 | 40 ++++++++++++++++++-------- 1 file changed, 28 insertions(+), 12 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10_dji_f330 b/ROMFS/px4fmu_common/init.d/10_dji_f330 index fb141e8a73..743dce9ef6 100644 --- a/ROMFS/px4fmu_common/init.d/10_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/10_dji_f330 @@ -10,18 +10,34 @@ then # Set all params here, then disable autoconfig param set SYS_AUTOCONFIG 0 - param set MC_ATTRATE_D 0.004 - param set MC_ATTRATE_I 0.0 - 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 7.0 - param set MC_YAWPOS_D 0.0 - 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 set MC_ATTRATE_D 0.004 + param set MC_ATTRATE_I 0.0 + 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 7.0 + param set MC_YAWPOS_D 0.0 + 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 set NAV_TAKEOFF_ALT 1.0 + param set MPC_TILT_MAX 0.5 + param set MPC_THR_MAX 0.7 + param set MPC_THR_MIN 0.3 + param set MPC_XY_D 0 + param set MPC_XY_P 0.5 + param set MPC_XY_VEL_D 0 + param set MPC_XY_VEL_I 0 + param set MPC_XY_VEL_MAX 2 + param set MPC_XY_VEL_P 0.2 + param set MPC_Z_D 0 + param set MPC_Z_P 1 + param set MPC_Z_VEL_D 0 + param set MPC_Z_VEL_I 0.10 + param set MPC_Z_VEL_MAX 2 + param set MPC_Z_VEL_P 0.20 param save fi From 3c0c11aec3c73d579659d81f9b68efbf7425846d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Sep 2013 07:49:57 +0200 Subject: [PATCH 553/872] v4 compatibility --- Tools/px_uploader.py | 2 +- src/drivers/px4io/uploader.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index d2ebf16987..cffc316790 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -158,7 +158,7 @@ class uploader(object): INFO_BL_REV = chr(1) # bootloader protocol revision BL_REV_MIN = 2 # minimum supported bootloader protocol - BL_REV_MAX = 3 # maximum supported bootloader protocol + BL_REV_MAX = 4 # 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 diff --git a/src/drivers/px4io/uploader.h b/src/drivers/px4io/uploader.h index 135202dd1b..22387a3e23 100644 --- a/src/drivers/px4io/uploader.h +++ b/src/drivers/px4io/uploader.h @@ -69,7 +69,7 @@ private: PROTO_REBOOT = 0x30, INFO_BL_REV = 1, /**< bootloader protocol revision */ - BL_REV = 3, /**< supported bootloader protocol */ + BL_REV = 4, /**< supported bootloader protocol */ INFO_BOARD_ID = 2, /**< board type */ INFO_BOARD_REV = 3, /**< board revision */ INFO_FLASH_SIZE = 4, /**< max firmware size in bytes */ From 791e22f44245676a743af5d73dd159f92fb1c159 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Sep 2013 07:50:27 +0200 Subject: [PATCH 554/872] MAVLink workaround --- src/modules/mavlink/mavlink.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index a8ca19d7a4..052ce7948c 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -516,6 +516,7 @@ void mavlink_update_system(void) int mavlink_thread_main(int argc, char *argv[]) { /* initialize mavlink text message buffering */ + usleep(1000); mavlink_logbuffer_init(&lb, 10); int ch; From 2457013bbba3e15e3fbfcc45f07428f006d56dcd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Sep 2013 08:17:22 +0200 Subject: [PATCH 555/872] Hotfix for USB: Starting MAVLink only on USB if connected. Needs rewrite of MAVLink and delay / retries for correct approach --- ROMFS/px4fmu_common/init.d/rcS | 8 ++++---- src/modules/mavlink/mavlink.c | 1 - 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 9bb8e4a494..8c79a035a0 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -63,9 +63,8 @@ then if sercon then echo "USB connected" - else - # second attempt - sercon & + sleep 3 + mavlink start -d /dev/ttyACM0 -b 230400 fi # @@ -105,7 +104,7 @@ then fi # Try to get an USB console - nshterm /dev/ttyACM0 & + #nshterm /dev/ttyACM0 & # # Upgrade PX4IO firmware @@ -219,5 +218,6 @@ then gps start fi + # End of autostart fi diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 052ce7948c..a8ca19d7a4 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -516,7 +516,6 @@ void mavlink_update_system(void) int mavlink_thread_main(int argc, char *argv[]) { /* initialize mavlink text message buffering */ - usleep(1000); mavlink_logbuffer_init(&lb, 10); int ch; From 811e1151fa05520f6f937518f3c7d5f6305b9fd9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Sep 2013 16:41:29 +0200 Subject: [PATCH 556/872] Fix UART buf sizes so that MAVLink transfers are not corrupted for all serial ports intended for MAVLink --- nuttx-configs/px4fmu-v1/nsh/defconfig | 4 ++-- nuttx-configs/px4fmu-v2/nsh/defconfig | 8 ++++---- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index 68ab4da32d..5be528f8c9 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -509,8 +509,8 @@ CONFIG_USART2_OFLOWCONTROL=y # # UART5 Configuration # -CONFIG_UART5_RXBUFSIZE=32 -CONFIG_UART5_TXBUFSIZE=32 +CONFIG_UART5_RXBUFSIZE=512 +CONFIG_UART5_TXBUFSIZE=512 CONFIG_UART5_BAUD=57600 CONFIG_UART5_BITS=8 CONFIG_UART5_PARITY=0 diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index e507c89baf..0615950a2d 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -608,8 +608,8 @@ CONFIG_USART6_2STOP=0 # # UART7 Configuration # -CONFIG_UART7_RXBUFSIZE=256 -CONFIG_UART7_TXBUFSIZE=256 +CONFIG_UART7_RXBUFSIZE=512 +CONFIG_UART7_TXBUFSIZE=512 CONFIG_UART7_BAUD=57600 CONFIG_UART7_BITS=8 CONFIG_UART7_PARITY=0 @@ -620,8 +620,8 @@ CONFIG_UART7_2STOP=0 # # UART8 Configuration # -CONFIG_UART8_RXBUFSIZE=256 -CONFIG_UART8_TXBUFSIZE=256 +CONFIG_UART8_RXBUFSIZE=512 +CONFIG_UART8_TXBUFSIZE=512 CONFIG_UART8_BAUD=57600 CONFIG_UART8_BITS=8 CONFIG_UART8_PARITY=0 From e301bb4d9425bbe460384afcc029dac6fa63f3e1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 3 Sep 2013 17:07:41 +0200 Subject: [PATCH 557/872] Reset baudrate after px4io update --- src/drivers/px4io/px4io_uploader.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index 7db28ecad0..fe8561a0b8 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -110,6 +110,10 @@ PX4IO_Uploader::upload(const char *filenames[]) return -errno; } + /* save initial uart configuration to reset after the update */ + struct termios t_original; + tcgetattr(_io_fd, &t_original); + /* adjust line speed to match bootloader */ struct termios t; tcgetattr(_io_fd, &t); @@ -122,6 +126,7 @@ PX4IO_Uploader::upload(const char *filenames[]) if (ret != OK) { /* this is immediately fatal */ log("bootloader not responding"); + tcsetattr(_io_fd, TCSANOW, &t_original); close(_io_fd); _io_fd = -1; return -EIO; @@ -130,6 +135,7 @@ PX4IO_Uploader::upload(const char *filenames[]) struct stat st; if (stat(filename, &st) != 0) { log("Failed to stat %s - %d\n", filename, (int)errno); + tcsetattr(_io_fd, TCSANOW, &t_original); close(_io_fd); _io_fd = -1; return -errno; @@ -137,6 +143,7 @@ PX4IO_Uploader::upload(const char *filenames[]) fw_size = st.st_size; if (_fw_fd == -1) { + tcsetattr(_io_fd, TCSANOW, &t_original); close(_io_fd); _io_fd = -1; return -ENOENT; @@ -151,6 +158,7 @@ PX4IO_Uploader::upload(const char *filenames[]) if (ret != OK) { /* this is immediately fatal */ log("bootloader not responding"); + tcsetattr(_io_fd, TCSANOW, &t_original); close(_io_fd); _io_fd = -1; return -EIO; @@ -164,6 +172,7 @@ PX4IO_Uploader::upload(const char *filenames[]) log("found bootloader revision: %d", bl_rev); } else { log("found unsupported bootloader revision %d, exiting", bl_rev); + tcsetattr(_io_fd, TCSANOW, &t_original); close(_io_fd); _io_fd = -1; return OK; @@ -199,6 +208,9 @@ PX4IO_Uploader::upload(const char *filenames[]) if (ret != OK) { log("reboot failed"); + tcsetattr(_io_fd, TCSANOW, &t_original); + close(_io_fd); + _io_fd = -1; return ret; } @@ -208,6 +220,9 @@ PX4IO_Uploader::upload(const char *filenames[]) break; } + /* reset uart to previous/default baudrate */ + tcsetattr(_io_fd, TCSANOW, &t_original); + close(_fw_fd); close(_io_fd); _io_fd = -1; From 175020cb5b9b5bb4162a765f8d924830105fad58 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Sep 2013 22:45:33 +0200 Subject: [PATCH 558/872] Hotfix: Identified mavlink issue source, workaround, working on a fix --- ROMFS/px4fmu_common/init.d/rcS | 4 +--- src/modules/mavlink/mavlink.c | 2 +- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 8c79a035a0..32fb67a454 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -63,8 +63,6 @@ then if sercon then echo "USB connected" - sleep 3 - mavlink start -d /dev/ttyACM0 -b 230400 fi # @@ -104,7 +102,7 @@ then fi # Try to get an USB console - #nshterm /dev/ttyACM0 & + nshterm /dev/ttyACM0 & # # Upgrade PX4IO firmware diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index a8ca19d7a4..5eb7cba9b2 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -739,7 +739,7 @@ int mavlink_thread_main(int argc, char *argv[]) tcsetattr(uart, TCSANOW, &uart_config_original); /* destroy log buffer */ - mavlink_logbuffer_destroy(&lb); + //mavlink_logbuffer_destroy(&lb); thread_running = false; From 5030a27f48b4ff4a5bb65e118ae85bada09dc85a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 3 Sep 2013 23:16:32 +0200 Subject: [PATCH 559/872] Tabs replaced by spaces in autostart scripts --- ROMFS/px4fmu_common/init.d/10_dji_f330 | 56 ++++++++++----------- ROMFS/px4fmu_common/init.d/11_dji_f450 | 24 ++++----- ROMFS/px4fmu_common/init.d/15_tbs_discovery | 24 ++++----- ROMFS/px4fmu_common/init.d/16_3dr_iris | 24 ++++----- ROMFS/px4fmu_common/init.d/666_fmu_q_x550 | 30 +++++------ 5 files changed, 79 insertions(+), 79 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10_dji_f330 b/ROMFS/px4fmu_common/init.d/10_dji_f330 index 743dce9ef6..709e170830 100644 --- a/ROMFS/px4fmu_common/init.d/10_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/10_dji_f330 @@ -10,34 +10,34 @@ then # Set all params here, then disable autoconfig param set SYS_AUTOCONFIG 0 - param set MC_ATTRATE_D 0.004 - param set MC_ATTRATE_I 0.0 - 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 7.0 - param set MC_YAWPOS_D 0.0 - 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 set NAV_TAKEOFF_ALT 1.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.7 - param set MPC_THR_MIN 0.3 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 2 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.10 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 + param set MC_ATTRATE_D 0.004 + param set MC_ATTRATE_I 0.0 + 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 7.0 + param set MC_YAWPOS_D 0.0 + 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 set NAV_TAKEOFF_ALT 3.0 + param set MPC_TILT_MAX 0.5 + param set MPC_THR_MAX 0.7 + param set MPC_THR_MIN 0.3 + param set MPC_XY_D 0 + param set MPC_XY_P 0.5 + param set MPC_XY_VEL_D 0 + param set MPC_XY_VEL_I 0 + param set MPC_XY_VEL_MAX 2 + param set MPC_XY_VEL_P 0.2 + param set MPC_Z_D 0 + param set MPC_Z_P 1 + param set MPC_Z_VEL_D 0 + param set MPC_Z_VEL_I 0.10 + param set MPC_Z_VEL_MAX 2 + param set MPC_Z_VEL_P 0.20 param save fi diff --git a/ROMFS/px4fmu_common/init.d/11_dji_f450 b/ROMFS/px4fmu_common/init.d/11_dji_f450 index 1827a2c114..73af1465c6 100644 --- a/ROMFS/px4fmu_common/init.d/11_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/11_dji_f450 @@ -10,18 +10,18 @@ then # Set all params here, then disable autoconfig param set SYS_AUTOCONFIG 0 - param set MC_ATTRATE_D 0.004 - param set MC_ATTRATE_I 0.0 - 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 7.0 - param set MC_YAWPOS_D 0.0 - 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 set MC_ATTRATE_D 0.004 + param set MC_ATTRATE_I 0.0 + 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 7.0 + param set MC_YAWPOS_D 0.0 + 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 fi diff --git a/ROMFS/px4fmu_common/init.d/15_tbs_discovery b/ROMFS/px4fmu_common/init.d/15_tbs_discovery index 1fcde3e27f..e4b9aee345 100644 --- a/ROMFS/px4fmu_common/init.d/15_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/15_tbs_discovery @@ -10,18 +10,18 @@ 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_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 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_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 fi diff --git a/ROMFS/px4fmu_common/init.d/16_3dr_iris b/ROMFS/px4fmu_common/init.d/16_3dr_iris index fa2cb1feb0..3fbc848990 100644 --- a/ROMFS/px4fmu_common/init.d/16_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/16_3dr_iris @@ -10,18 +10,18 @@ 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_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 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_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 fi diff --git a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 index 938663f0c2..ae41f2a014 100644 --- a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 +++ b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 @@ -8,21 +8,21 @@ echo "[init] 666_fmu_q_x550: PX4FMU Quad X550 with PWM outputs" 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 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 From 0c7995fd03bff63e765862910eaafff1ffb3197c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 4 Sep 2013 08:12:51 +0200 Subject: [PATCH 560/872] Avoid unneccesary cast --- src/modules/systemlib/mavlink_log.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/systemlib/mavlink_log.c b/src/modules/systemlib/mavlink_log.c index 03ca71375f..f321f9ceba 100644 --- a/src/modules/systemlib/mavlink_log.c +++ b/src/modules/systemlib/mavlink_log.c @@ -51,7 +51,7 @@ __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)); + lb->elems = calloc(lb->size, sizeof(struct mavlink_logmessage)); } __EXPORT void mavlink_logbuffer_destroy(struct mavlink_logbuffer *lb) From 59373399ff8fe08dfd0fb1cee59ed8c73ec75810 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 4 Sep 2013 13:30:00 +0200 Subject: [PATCH 561/872] Uploader works now with Python 2.7 and 3.3 --- Tools/px_uploader.py | 71 +++++++++++++++++++++++++++----------------- 1 file changed, 44 insertions(+), 27 deletions(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index 8a7e5e2501..c2ba6d0342 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -137,31 +137,31 @@ class uploader(object): '''Uploads a firmware file to the PX FMU bootloader''' # protocol bytes - INSYNC = chr(0x12) - EOC = chr(0x20) + INSYNC = b'\x12' + EOC = b'\x20' # reply bytes - OK = chr(0x10) - FAILED = chr(0x11) - INVALID = chr(0x13) # rev3+ + OK = b'\x10' + FAILED = b'\x11' + INVALID = b'\x13' # 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) + NOP = b'\x00' # guaranteed to be discarded by the bootloader + GET_SYNC = b'\x21' + GET_DEVICE = b'\x22' + CHIP_ERASE = b'\x23' + CHIP_VERIFY = b'\x24' # rev2 only + PROG_MULTI = b'\x27' + READ_MULTI = b'\x28' # rev2 only + GET_CRC = b'\x29' # rev3+ + REBOOT = b'\x30' - INFO_BL_REV = chr(1) # bootloader protocol revision + INFO_BL_REV = b'\x01' # bootloader protocol revision BL_REV_MIN = 2 # minimum supported bootloader protocol BL_REV_MAX = 4 # 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 + INFO_BOARD_ID = b'\x02' # board type + INFO_BOARD_REV = b'\x03' # board revision + INFO_FLASH_SIZE = b'\x04' # 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 @@ -176,7 +176,7 @@ class uploader(object): def __send(self, c): # print("send " + binascii.hexlify(c)) - self.port.write(str(c)) + self.port.write(c) def __recv(self, count=1): c = self.port.read(count) @@ -192,9 +192,9 @@ class uploader(object): def __getSync(self): self.port.flush() - c = self.__recv() - if c is not self.INSYNC: - raise RuntimeError("unexpected 0x%x instead of INSYNC" % ord(c)) + c = bytes(self.__recv()) + if c != self.INSYNC: + raise RuntimeError("unexpected %s instead of INSYNC" % c) c = self.__recv() if c == self.INVALID: raise RuntimeError("bootloader reports INVALID OPERATION") @@ -249,17 +249,29 @@ class uploader(object): # send a PROG_MULTI command to write a collection of bytes def __program_multi(self, data): - self.__send(uploader.PROG_MULTI - + chr(len(data))) + + if runningPython3 == True: + length = len(data).to_bytes(1, byteorder='big') + else: + length = chr(len(data)) + + self.__send(uploader.PROG_MULTI) + self.__send(length) self.__send(data) self.__send(uploader.EOC) self.__getSync() # verify multiple bytes in flash def __verify_multi(self, data): - self.__send(uploader.READ_MULTI - + chr(len(data)) - + uploader.EOC) + + if runningPython3 == True: + length = len(data).to_bytes(1, byteorder='big') + else: + length = chr(len(data)) + + self.__send(uploader.READ_MULTI) + self.__send(length) + self.__send(uploader.EOC) self.port.flush() programmed = self.__recv(len(data)) if programmed != data: @@ -351,6 +363,11 @@ class uploader(object): self.__reboot() self.port.close() +# Detect python version +if sys.version_info[0] < 3: + runningPython3 = False +else: + runningPython3 = True # Parse commandline arguments parser = argparse.ArgumentParser(description="Firmware uploader for the PX autopilot system.") From d30ba44f463045aee47fe9ac06436d28c5b4800b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 4 Sep 2013 14:54:16 +0200 Subject: [PATCH 562/872] The python uploader can now reboot the board --- Tools/px_uploader.py | 24 +++++++++++++++++++++++- 1 file changed, 23 insertions(+), 1 deletion(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index c2ba6d0342..cab3c2fd0e 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -165,6 +165,12 @@ class uploader(object): 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 + + NSH_INIT = bytearray(b'\x0d\x0d\x0d') + NSH_REBOOT_BL = b"reboot -b\n" + NSH_REBOOT = b"reboot\n" + MAVLINK_REBOOT_ID1 = bytearray(b'\xfe\x21\x72\xff\x00\x4c\x00\x00\x80\x3f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x01\x00\x00\x48\xf0') + MAVLINK_REBOOT_ID0 = bytearray(b'\xfe\x21\x45\xff\x00\x4c\x00\x00\x80\x3f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x00\x00\x00\xd7\xac') def __init__(self, portname, baudrate): # open the port, keep the default timeout short so we can poll quickly @@ -362,6 +368,18 @@ class uploader(object): print("done, rebooting.") self.__reboot() self.port.close() + + def send_reboot(self): + # try reboot via NSH first + self.__send(uploader.NSH_INIT) + self.__send(uploader.NSH_REBOOT_BL) + self.__send(uploader.NSH_INIT) + self.__send(uploader.NSH_REBOOT) + # then try MAVLINK command + self.__send(uploader.MAVLINK_REBOOT_ID1) + self.__send(uploader.MAVLINK_REBOOT_ID0) + + # Detect python version if sys.version_info[0] < 3: @@ -414,7 +432,11 @@ while True: 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 + # most probably a timeout talking to the port, no bootloader, try to reboot the board + print("attempting reboot on %s..." % port) + up.send_reboot() + # wait for the reboot, without we might run into Serial I/O Error 5 + time.sleep(1.5) continue try: From 09db74da0a88908aa761940a3de8383c0c64d1e4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 5 Sep 2013 10:27:35 +0200 Subject: [PATCH 563/872] Hotfix: Minor param adjustments based on flight testing --- ROMFS/px4fmu_common/init.d/10_dji_f330 | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10_dji_f330 b/ROMFS/px4fmu_common/init.d/10_dji_f330 index 709e170830..32209d11c9 100644 --- a/ROMFS/px4fmu_common/init.d/10_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/10_dji_f330 @@ -30,12 +30,12 @@ then param set MPC_XY_P 0.5 param set MPC_XY_VEL_D 0 param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 2 + param set MPC_XY_VEL_MAX 3 param set MPC_XY_VEL_P 0.2 param set MPC_Z_D 0 param set MPC_Z_P 1 param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.10 + param set MPC_Z_VEL_I 0.1 param set MPC_Z_VEL_MAX 2 param set MPC_Z_VEL_P 0.20 From 1d7f3d0aa5f886d56bd69018fe7759a7df1ef6ef Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 5 Sep 2013 12:20:03 +0200 Subject: [PATCH 564/872] position_estimator_inav: land detector bug fixed --- .../position_estimator_inav_main.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 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 88057b3237..932f610886 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -53,7 +53,7 @@ #include #include #include -#include +#include #include #include #include @@ -181,7 +181,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) uint32_t baro_counter = 0; /* declare and safely initialize all structs */ - struct actuator_controls_effective_s actuator; + struct actuator_controls_s actuator; memset(&actuator, 0, sizeof(actuator)); struct actuator_armed_s armed; memset(&armed, 0, sizeof(armed)); @@ -200,7 +200,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* subscribe */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); - int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); + int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); 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)); @@ -337,7 +337,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* actuator */ if (fds[1].revents & POLLIN) { - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, actuator_sub, &actuator); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_sub, &actuator); } /* armed */ @@ -546,7 +546,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) 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; + float thrust = armed.armed ? actuator.control[3] : 0.0f; if (landed) { if (alt_disp > land_disp2 && thrust > params.land_thr) { From aa785b0d2b339a8fcc730e11b63264b1ff8d146d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 5 Sep 2013 13:24:21 +0200 Subject: [PATCH 565/872] Hotfix: Better error reporting, fixed sched param setup --- src/modules/commander/commander.cpp | 25 ++++++++++++++++++++---- src/modules/commander/rc_calibration.cpp | 14 +++++++------ src/modules/systemlib/param/param.c | 4 ++-- 3 files changed, 31 insertions(+), 12 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 8e8226f091..333fe30ae2 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -50,6 +50,7 @@ #include #include #include +#include #include #include #include @@ -599,6 +600,7 @@ int commander_thread_main(int argc, char *argv[]) pthread_attr_setstacksize(&commander_low_prio_attr, 2992); struct sched_param param; + (void)pthread_attr_getschedparam(&commander_low_prio_attr, ¶m); /* low priority */ param.sched_priority = SCHED_PRIORITY_DEFAULT - 50; (void)pthread_attr_setschedparam(&commander_low_prio_attr, ¶m); @@ -1655,13 +1657,13 @@ void *commander_low_prio_loop(void *arg) if (((int)(cmd.param1)) == 1) { answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - usleep(1000000); + usleep(100000); /* reboot */ systemreset(false); } else if (((int)(cmd.param1)) == 3) { answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - usleep(1000000); + usleep(100000); /* reboot to bootloader */ systemreset(true); @@ -1704,6 +1706,7 @@ void *commander_low_prio_loop(void *arg) } else if ((int)(cmd.param4) == 1) { /* RC calibration */ answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + calib_ret = do_rc_calibration(mavlink_fd); } else if ((int)(cmd.param5) == 1) { /* accelerometer calibration */ @@ -1729,22 +1732,36 @@ void *commander_low_prio_loop(void *arg) case VEHICLE_CMD_PREFLIGHT_STORAGE: { if (((int)(cmd.param1)) == 0) { - if (0 == param_load_default()) { + int ret = param_load_default(); + if (ret == OK) { 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"); + /* convenience as many parts of NuttX use negative errno */ + if (ret < 0) + ret = -ret; + + if (ret < 1000) + mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret)); answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); } } else if (((int)(cmd.param1)) == 1) { - if (0 == param_save_default()) { + int ret = param_save_default(); + if (ret == OK) { 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"); + /* convenience as many parts of NuttX use negative errno */ + if (ret < 0) + ret = -ret; + + if (ret < 1000) + mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret)); answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); } } diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp index 9fc1d64700..90ede499a1 100644 --- a/src/modules/commander/rc_calibration.cpp +++ b/src/modules/commander/rc_calibration.cpp @@ -57,14 +57,16 @@ int do_rc_calibration(int mavlink_fd) { mavlink_log_info(mavlink_fd, "trim calibration starting"); - /* XXX fix this */ - // if (current_status.rc_signal) { - // 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; + bool changed; + orb_check(sub_man, &changed); + + if (!changed) { + mavlink_log_critical(mavlink_fd, "no manual control, aborting"); + return ERROR; + } + orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp); /* set parameters */ diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 24b52d1a92..c69de52b70 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -520,7 +520,7 @@ param_save_default(void) if (fd < 0) { warn("opening '%s' for writing failed", param_get_default_file()); - return -1; + return fd; } result = param_export(fd, false); @@ -529,7 +529,7 @@ param_save_default(void) if (result != 0) { warn("error exporting parameters to '%s'", param_get_default_file()); unlink(param_get_default_file()); - return -2; + return result; } return 0; From 5dbe53877ab543728d4f2d288716d4989464237a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 5 Sep 2013 13:24:37 +0200 Subject: [PATCH 566/872] Fixed sched param setup in MAVLink app --- src/modules/mavlink/mavlink_receiver.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index af43542da4..4674f7a248 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -747,6 +747,7 @@ receive_start(int uart) fcntl(uart, F_SETFL, flags | O_NONBLOCK); struct sched_param param; + (void)pthread_attr_getschedparam(&receiveloop_attr, ¶m); param.sched_priority = SCHED_PRIORITY_MAX - 40; (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); From e8c309fb1442f281051874ca6d82af9b52605c3b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 5 Sep 2013 15:57:09 +0200 Subject: [PATCH 567/872] Workaround to prevent crash during mag calibration --- src/drivers/hmc5883/hmc5883.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index a5229b2371..3ede90a174 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -962,11 +962,12 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) warnx("sampling 500 samples for scaling offset"); /* set the queue depth to 10 */ - if (OK != ioctl(filp, SENSORIOCSQUEUEDEPTH, 10)) { - warn("failed to set queue depth"); - ret = 1; - goto out; - } + /* don't do this for now, it can lead to a crash in start() respectively work_queue() */ +// if (OK != ioctl(filp, SENSORIOCSQUEUEDEPTH, 10)) { +// warn("failed to set queue depth"); +// ret = 1; +// goto out; +// } /* start the sensor polling at 50 Hz */ if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) { From 373a74adb9abc218ca8084fc3f59c0e50daf9bf4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 5 Sep 2013 17:39:11 +0200 Subject: [PATCH 568/872] Setting correct battery scaling for FMU-only setups --- ROMFS/px4fmu_common/init.d/10_dji_f330 | 1 + ROMFS/px4fmu_common/init.d/11_dji_f450 | 1 + ROMFS/px4fmu_common/init.d/15_tbs_discovery | 1 + ROMFS/px4fmu_common/init.d/16_3dr_iris | 1 + 4 files changed, 4 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/10_dji_f330 b/ROMFS/px4fmu_common/init.d/10_dji_f330 index 32209d11c9..b8fe5fc31c 100644 --- a/ROMFS/px4fmu_common/init.d/10_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/10_dji_f330 @@ -69,6 +69,7 @@ else mavlink start -d /dev/ttyS0 usleep 5000 fmu mode_pwm + param set BAT_V_SCALING 0.004593 set EXIT_ON_END yes fi diff --git a/ROMFS/px4fmu_common/init.d/11_dji_f450 b/ROMFS/px4fmu_common/init.d/11_dji_f450 index 73af1465c6..388f40a477 100644 --- a/ROMFS/px4fmu_common/init.d/11_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/11_dji_f450 @@ -53,6 +53,7 @@ else mavlink start -d /dev/ttyS0 usleep 5000 fmu mode_pwm + param set BAT_V_SCALING 0.004593 set EXIT_ON_END yes fi diff --git a/ROMFS/px4fmu_common/init.d/15_tbs_discovery b/ROMFS/px4fmu_common/init.d/15_tbs_discovery index e4b9aee345..cbfde6d3c0 100644 --- a/ROMFS/px4fmu_common/init.d/15_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/15_tbs_discovery @@ -53,6 +53,7 @@ else mavlink start -d /dev/ttyS0 usleep 5000 fmu mode_pwm + param set BAT_V_SCALING 0.004593 set EXIT_ON_END yes fi diff --git a/ROMFS/px4fmu_common/init.d/16_3dr_iris b/ROMFS/px4fmu_common/init.d/16_3dr_iris index 3fbc848990..b6c5fbdea4 100644 --- a/ROMFS/px4fmu_common/init.d/16_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/16_3dr_iris @@ -53,6 +53,7 @@ else mavlink start -d /dev/ttyS0 usleep 5000 fmu mode_pwm + param set BAT_V_SCALING 0.004593 set EXIT_ON_END yes fi From 3d821b8131f762836051b01bc8b2af145d99ebee Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 5 Sep 2013 22:32:13 +0200 Subject: [PATCH 569/872] Hotfix: Gain optimization --- ROMFS/px4fmu_common/init.d/10_dji_f330 | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10_dji_f330 b/ROMFS/px4fmu_common/init.d/10_dji_f330 index b8fe5fc31c..d21759507d 100644 --- a/ROMFS/px4fmu_common/init.d/10_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/10_dji_f330 @@ -10,12 +10,12 @@ then # Set all params here, then disable autoconfig param set SYS_AUTOCONFIG 0 - param set MC_ATTRATE_D 0.004 + param set MC_ATTRATE_D 0.002 param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_P 0.09 param set MC_ATT_D 0.0 param set MC_ATT_I 0.0 - param set MC_ATT_P 7.0 + param set MC_ATT_P 6.8 param set MC_YAWPOS_D 0.0 param set MC_YAWPOS_I 0.0 param set MC_YAWPOS_P 2.0 From 6104d14968f7093cc3299f1dec25b7b7422fa4bb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 6 Sep 2013 16:51:46 +0200 Subject: [PATCH 570/872] Have systems loiter at the last waypoint --- src/modules/mavlink/missionlib.c | 6 +++--- src/modules/mavlink/waypoints.c | 6 ++++-- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c index be88b87948..3175e64ce2 100644 --- a/src/modules/mavlink/missionlib.c +++ b/src/modules/mavlink/missionlib.c @@ -167,9 +167,9 @@ bool set_special_fields(float param1, float param2, float param3, float param4, sp->loiter_direction = (param3 >= 0) ? 1 : -1; sp->param1 = param1; - sp->param1 = param2; - sp->param1 = param3; - sp->param1 = param4; + sp->param2 = param2; + sp->param3 = param3; + sp->param4 = param4; } /** diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index 16a7c2d355..12d82ccc26 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -417,6 +417,7 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ } if (time_elapsed) { + if (cur_wp->autocontinue) { cur_wp->current = 0; @@ -425,9 +426,10 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ if (wpm->current_active_wp_id == wpm->size - 1 && wpm->size > 1) { /* the last waypoint was reached, if auto continue is - * activated restart the waypoint list from the beginning + * activated keep the system loitering there. */ - wpm->current_active_wp_id = 0; + cur_wp->MAV_CMD_NAV_LOITER_UNLIM; + cur_wp->param3 = 15.0f // XXX magic number 15 m loiter radius } else { if ((uint16_t)(wpm->current_active_wp_id + 1) < wpm->size) From 58adc7e40b886a9d5facd3aae61ec5d91e7f4cbf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 6 Sep 2013 17:07:11 +0200 Subject: [PATCH 571/872] Make bool on FMUv1 and FMUv2 behave the same --- nuttx-configs/px4fmu-v1/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index 5be528f8c9..a1e128a400 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -712,7 +712,7 @@ CONFIG_SCHED_LPWORKSTACKSIZE=2048 # # Basic CXX Support # -# CONFIG_C99_BOOL8 is not set +CONFIG_C99_BOOL8=y CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y # CONFIG_CXX_NEWLONG is not set From 23a355644b69585d11011954f1d4cbc9e04a8f3b Mon Sep 17 00:00:00 2001 From: tstellanova Date: Fri, 6 Sep 2013 10:18:08 -0700 Subject: [PATCH 572/872] grab the git hash and inject it into every log file header --- Makefile | 11 +++++++++++ makefiles/firmware.mk | 8 ++++++++ src/modules/sdlog2/sdlog2_messages.h | 19 +++++++++++++++++++ 3 files changed, 38 insertions(+) diff --git a/Makefile b/Makefile index 83a6f3ce9f..0a85622512 100644 --- a/Makefile +++ b/Makefile @@ -39,6 +39,17 @@ export PX4_BASE := $(realpath $(dir $(lastword $(MAKEFILE_LIST))))/ include $(PX4_BASE)makefiles/setup.mk +# +# Get a version string provided by git +# This assumes that git command is available and that +# the directory holding this file also contains .git directory +# +GIT_DESC := $(shell git log -1 --pretty=format:%H) +ifneq ($(words $(GIT_DESC)),1) + GIT_DESC := "unknown_git_version" +endif +export GIT_DESC + # # Canned firmware configurations that we (know how to) build. # diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index b3e50501c7..cb20d9cd19 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -110,6 +110,8 @@ ifneq ($(words $(PX4_BASE)),1) $(error Cannot build when the PX4_BASE path contains one or more space characters.) endif +$(info % GIT_DESC = $(GIT_DESC)) + # # Set a default target so that included makefiles or errors here don't # cause confusion. @@ -177,6 +179,12 @@ GLOBAL_DEPS += $(MAKEFILE_LIST) # EXTRA_CLEANS = + +# +# Extra defines for compilation +# +export EXTRADEFINES := -DGIT_VERSION=$(GIT_DESC) + # # Append the per-board driver directory to the header search path. # diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 4eeb65a87e..1343bb3d0e 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -250,8 +250,26 @@ struct log_GVSP_s { float vz; }; +/* --- FWRV - FIRMWARE REVISION --- */ +#define LOG_FWRV_MSG 20 +struct log_FWRV_s { + char fw_revision[64]; +}; + #pragma pack(pop) + +/* + GIT_VERSION is defined at build time via a Makefile call to the + git command line. We create a fake log message format for + the firmware revision "FWRV" that is written to every log + header. This makes it easier to determine which version + of the firmware was running when a log was created. + */ +#define FREEZE_STR(s) #s +#define STRINGIFY(s) FREEZE_STR(s) +#define FW_VERSION_STR STRINGIFY(GIT_VERSION) + /* construct list of all message formats */ static const struct log_format_s log_formats[] = { @@ -274,6 +292,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"), LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), + LOG_FORMAT(FWRV,"Z",FW_VERSION_STR), }; static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); From fb9e98fb59ebe9db572dcdba6396a9c78021b857 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 7 Sep 2013 12:17:27 +0200 Subject: [PATCH 573/872] Cleanup of fixedwing startup code --- ROMFS/px4fmu_common/init.d/100_mpx_easystar | 54 +++++++++----- ROMFS/px4fmu_common/init.d/101_hk_bixler | 82 +++++++++++++++++++++ ROMFS/px4fmu_common/init.d/30_io_camflyer | 63 +++++++++------- ROMFS/px4fmu_common/init.d/31_io_phantom | 54 +++++++++----- ROMFS/px4fmu_common/init.d/rcS | 12 +++ 5 files changed, 198 insertions(+), 67 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/101_hk_bixler diff --git a/ROMFS/px4fmu_common/init.d/100_mpx_easystar b/ROMFS/px4fmu_common/init.d/100_mpx_easystar index e1cefdb993..4ed73909e0 100644 --- a/ROMFS/px4fmu_common/init.d/100_mpx_easystar +++ b/ROMFS/px4fmu_common/init.d/100_mpx_easystar @@ -1,6 +1,6 @@ #!nsh -echo "[init] Multiplex Easystar" +echo "[init] PX4FMU v1, v2 with or without IO on EasyStar" # # Load default params for this platform @@ -20,28 +20,31 @@ fi # param set MAV_TYPE 1 -# -# Start MAVLink (depends on orb) -# -mavlink start -d /dev/ttyS1 -b 57600 -usleep 5000 +set EXIT_ON_END no # -# Start and configure PX4IO interface +# Start and configure PX4IO or FMU interface # -sh /etc/init.d/rc.io +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start + usleep 5000 -# -# Set actuator limit to 100 Hz update (50 Hz PWM) -px4io limit 100 + sh /etc/init.d/rc.io + # Limit to 100 Hz updates and (implicit) 50 Hz PWM + px4io limit 100 +else + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + usleep 5000 + fmu mode_pwm + param set BAT_V_SCALING 0.004593 + set EXIT_ON_END yes +fi # -# Start the commander -# -commander start - -# -# Start the sensors +# Start the sensors and test them. # sh /etc/init.d/rc.sensors @@ -49,9 +52,14 @@ sh /etc/init.d/rc.sensors # Start logging (depends on sensors) # sh /etc/init.d/rc.logging - + # -# Start GPS interface +# Start the commander. +# +commander start + +# +# Start GPS interface (depends on orb) # gps start @@ -65,4 +73,10 @@ att_pos_estimator_ekf start # mixer load /dev/pwm_output /etc/mixers/FMU_RET.mix fw_att_control start -fw_pos_control_l1 start +# Not ready yet for prime-time +#fw_pos_control_l1 start + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/101_hk_bixler b/ROMFS/px4fmu_common/init.d/101_hk_bixler new file mode 100644 index 0000000000..b4daa8f5aa --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/101_hk_bixler @@ -0,0 +1,82 @@ +#!nsh + +echo "[init] PX4FMU v1, v2 with or without IO on HK Bixler" + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + # TODO + + param set SYS_AUTOCONFIG 0 + param save +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing +# +param set MAV_TYPE 1 + +set EXIT_ON_END no + +# +# Start and configure PX4IO or FMU interface +# +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start + usleep 5000 + + sh /etc/init.d/rc.io + # Limit to 100 Hz updates and (implicit) 50 Hz PWM + px4io limit 100 +else + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + usleep 5000 + fmu mode_pwm + param set BAT_V_SCALING 0.004593 + set EXIT_ON_END yes +fi + +# +# Start the sensors and test them. +# +sh /etc/init.d/rc.sensors + +# +# Start logging (depends on sensors) +# +sh /etc/init.d/rc.logging + +# +# Start the commander. +# +commander start + +# +# Start GPS interface (depends on orb) +# +gps start + +# +# Start the attitude and position estimator +# +att_pos_estimator_ekf start + +# +# Load mixer and start controllers (depends on px4io) +# +mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix +fw_att_control start +# Not ready yet for prime-time +#fw_pos_control_l1 start + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/30_io_camflyer b/ROMFS/px4fmu_common/init.d/30_io_camflyer index f7e653362f..c4c4ea3da0 100644 --- a/ROMFS/px4fmu_common/init.d/30_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/30_io_camflyer @@ -1,6 +1,6 @@ #!nsh -echo "[init] 30_io_camflyer: PX4FMU+PX4IO on Camflyer" +echo "[init] PX4FMU v1, v2 with or without IO on Camflyer" # # Load default params for this platform @@ -19,34 +19,32 @@ fi # MAV_TYPE 1 = fixed wing # param set MAV_TYPE 1 - -# -# Start MAVLink (depends on orb) -# -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 + +set EXIT_ON_END no # -# Set actuator limit to 100 Hz update (50 Hz PWM) -px4io limit 100 +# Start and configure PX4IO or FMU interface +# +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start + usleep 5000 + + sh /etc/init.d/rc.io + # Limit to 100 Hz updates and (implicit) 50 Hz PWM + px4io limit 100 +else + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + usleep 5000 + fmu mode_pwm + param set BAT_V_SCALING 0.004593 + set EXIT_ON_END yes +fi # -# Start the sensors (depends on orb, px4io) +# Start the sensors and test them. # sh /etc/init.d/rc.sensors @@ -54,7 +52,12 @@ sh /etc/init.d/rc.sensors # Start logging (depends on sensors) # sh /etc/init.d/rc.logging - + +# +# Start the commander. +# +commander start + # # Start GPS interface (depends on orb) # @@ -70,4 +73,10 @@ att_pos_estimator_ekf start # mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix fw_att_control start -fw_pos_control_l1 start +# Not ready yet for prime-time +#fw_pos_control_l1 start + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/31_io_phantom b/ROMFS/px4fmu_common/init.d/31_io_phantom index e1e6099270..8d4158a182 100644 --- a/ROMFS/px4fmu_common/init.d/31_io_phantom +++ b/ROMFS/px4fmu_common/init.d/31_io_phantom @@ -1,6 +1,6 @@ #!nsh -echo "[init] 31_io_phantom: PX4FMU+PX4IO on Phantom" +echo "[init] PX4FMU v1, v2 with or without IO on Phantom FPV" # # Load default params for this platform @@ -20,28 +20,31 @@ fi # param set MAV_TYPE 1 -# -# Start MAVLink (depends on orb) -# -mavlink start -d /dev/ttyS1 -b 57600 -usleep 5000 +set EXIT_ON_END no # -# Start and configure PX4IO interface +# Start and configure PX4IO or FMU interface # -sh /etc/init.d/rc.io +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start + usleep 5000 -# -# Set actuator limit to 100 Hz update (50 Hz PWM) -px4io limit 100 + sh /etc/init.d/rc.io + # Limit to 100 Hz updates and (implicit) 50 Hz PWM + px4io limit 100 +else + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + usleep 5000 + fmu mode_pwm + param set BAT_V_SCALING 0.004593 + set EXIT_ON_END yes +fi # -# Start the commander -# -commander start - -# -# Start the sensors +# Start the sensors and test them. # sh /etc/init.d/rc.sensors @@ -49,9 +52,14 @@ sh /etc/init.d/rc.sensors # Start logging (depends on sensors) # sh /etc/init.d/rc.logging - + # -# Start GPS interface +# Start the commander. +# +commander start + +# +# Start GPS interface (depends on orb) # gps start @@ -65,4 +73,10 @@ att_pos_estimator_ekf start # mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix fw_att_control start -fw_pos_control_l1 start +# Not ready yet for prime-time +#fw_pos_control_l1 start + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 32fb67a454..c2ef375266 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -178,6 +178,18 @@ then sh /etc/init.d/31_io_phantom set MODE custom fi + + if param compare SYS_AUTOSTART 100 + then + sh /etc/init.d/100_mpx_easystar + set MODE custom + fi + + if param compare SYS_AUTOSTART 101 + then + sh /etc/init.d/101_hk_bixler + set MODE custom + fi # Start any custom extensions that might be missing if [ -f /fs/microsd/etc/rc.local ] From b599a32c1695d429849d41b398c1b4a586300ee9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 7 Sep 2013 12:22:24 +0200 Subject: [PATCH 574/872] Removed dysfunctional includes. Need to be re-done properly when finally implementing SITL. No use to leave untested stuff in tree. --- src/lib/mathlib/math/filter/module.mk | 1 - src/lib/mathlib/module.mk | 1 - 2 files changed, 2 deletions(-) diff --git a/src/lib/mathlib/math/filter/module.mk b/src/lib/mathlib/math/filter/module.mk index fe92c8c70f..f5c0647c88 100644 --- a/src/lib/mathlib/math/filter/module.mk +++ b/src/lib/mathlib/math/filter/module.mk @@ -41,4 +41,3 @@ SRCS = LowPassFilter2p.cpp # current makefile name, since app.mk needs it. # APP_MAKEFILE := $(lastword $(MAKEFILE_LIST)) --include $(TOPDIR)/.config diff --git a/src/lib/mathlib/module.mk b/src/lib/mathlib/module.mk index 2146a14131..72bc7db8a1 100644 --- a/src/lib/mathlib/module.mk +++ b/src/lib/mathlib/module.mk @@ -49,7 +49,6 @@ SRCS = math/test/test.cpp \ # current makefile name, since app.mk needs it. # APP_MAKEFILE := $(lastword $(MAKEFILE_LIST)) --include $(TOPDIR)/.config ifeq ($(CONFIG_ARCH_CORTEXM4)$(CONFIG_ARCH_FPU),yy) INCLUDE_DIRS += math/arm From 8398de7515671b52df19a032162ff2064d68772a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 7 Sep 2013 12:53:39 +0200 Subject: [PATCH 575/872] WIP on controllers --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 6 ++++++ src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 2 +- src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 6 ++++++ 3 files changed, 13 insertions(+), 1 deletion(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 0faba287d8..c1c2bc5af1 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -51,6 +51,12 @@ ECL_PitchController::ECL_PitchController() : float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler, bool lock_integrator, float airspeed_min, float airspeed_max, float aspeed) { + /* get the usual dt estimate */ + uint64_t dt_micros = ecl_elapsed_time(&_last_run); + _last_run = ecl_absolute_time(); + + float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000; + return 0.0f; } diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index b28ecdabee..cdb221b264 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -61,7 +61,7 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000; - + float integrator_limit_scaled = 0.0f; return 0.0f; } diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 0d8a0513f0..6dd90f6762 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -54,6 +54,12 @@ ECL_YawController::ECL_YawController() : float ECL_YawController::control(float roll, float yaw_rate, float accel_y, float scaler, bool lock_integrator, float airspeed_min, float airspeed_max, float aspeed) { + /* get the usual dt estimate */ + uint64_t dt_micros = ecl_elapsed_time(&_last_run); + _last_run = ecl_absolute_time(); + + float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000; + return 0.0f; } From 056fe1c0b93094e86fa80de4102b12f2d406de5a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 7 Sep 2013 13:50:38 +0200 Subject: [PATCH 576/872] WIP --- .../ecl/attitude_fw/ecl_pitch_controller.cpp | 63 +++++++++++++++++-- .../ecl/attitude_fw/ecl_pitch_controller.h | 5 +- .../ecl/attitude_fw/ecl_roll_controller.cpp | 55 ++++++++++++++-- src/lib/ecl/attitude_fw/ecl_roll_controller.h | 5 +- 4 files changed, 116 insertions(+), 12 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index c1c2bc5af1..0e1a0d7f62 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -44,20 +44,75 @@ ECL_PitchController::ECL_PitchController() : _last_output(0.0f), _integrator(0.0f), _rate_error(0.0f), - _desired_rate(0.0f) + _desired_rate(0.0f), + _max_deflection_rad(math::radians(45.0f)) { } float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler, - bool lock_integrator, float airspeed_min, float airspeed_max, float aspeed) + bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed) { /* get the usual dt estimate */ uint64_t dt_micros = ecl_elapsed_time(&_last_run); _last_run = ecl_absolute_time(); - float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000; + float dt = dt_micros / 1000000; - return 0.0f; + /* lock integral for long intervals */ + if (dt_micros > 500000) + lock_integrator = true; + + float k_roll_ff = max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); + float k_i_rate = _k_i * _tc; + + /* input conditioning */ + if (!isfinite(airspeed)) { + /* airspeed is NaN, +- INF or not available, pick center of band */ + airspeed = 0.5f * (airspeed_min + airspeed_max); + } else if (airspeed < airspeed_min) { + airspeed = airspeed_min; + } + + /* calculate the offset in the rate resulting from rolling */ + float turn_offset = fabsf((CONSTANTS_ONE_G / airspeed) * + tanf(roll) * sinf(roll)) * _roll; + + float pitch_error = pitch_setpoint - pitch; + /* rate setpoint from current error and time constant */ + _rate_setpoint = pitch_error / _tc; + + /* add turn offset */ + _rate_setpoint += turn_offset; + + _rate_error = _rate_setpoint - pitch_rate; + + float ilimit_scaled = 0.0f; + + if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5 * airspeed_min) { + + float id = _rate_error * k_i_rate * dt * scaler; + + /* + * anti-windup: do not allow integrator to increase into the + * wrong direction if actuator is at limit + */ + if (_last_output < -_max_deflection_rad) { + /* only allow motion to center: increase value */ + id = max(id, 0.0f); + } else if (_last_output > _max_deflection_rad) { + /* only allow motion to center: decrease value */ + id = min(id, 0.0f); + } + + _integrator += id; + } + + /* integrator limit */ + _integrator = constrain(_integrator, -ilimit_scaled, ilimit_scaled); + /* store non-limited output */ + _last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_roll_ff)) * scaler; + + return constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); } void ECL_PitchController::reset_integrator() diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h index 391f90b9df..53134556d7 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h @@ -49,7 +49,7 @@ public: ECL_PitchController(); float control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler = 1.0f, - bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float aspeed = (0.0f / 0.0f)); + bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f)); void reset_integrator(); @@ -100,7 +100,8 @@ private: float _last_output; float _integrator; float _rate_error; - float _desired_rate; + float _rate_setpoint; + float _max_deflection_rad; }; #endif // ECL_PITCH_CONTROLLER_H diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index cdb221b264..1152b2964c 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -46,13 +46,14 @@ ECL_RollController::ECL_RollController() : _last_output(0.0f), _integrator(0.0f), _rate_error(0.0f), - _desired_rate(0.0f) + _rate_setpoint(0.0f), + _max_deflection_rad(math::radians(45.0f)) { } float ECL_RollController::control(float roll_setpoint, float roll, float roll_rate, - float scaler, bool lock_integrator, float airspeed_min, float airspeed_max, float aspeed) + float scaler, bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed) { /* get the usual dt estimate */ uint64_t dt_micros = ecl_elapsed_time(&_last_run); @@ -60,10 +61,56 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000; + float k_ff = max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); + float k_i_rate = _k_i * _tc; - float integrator_limit_scaled = 0.0f; + /* input conditioning */ + if (!isfinite(airspeed)) { + /* airspeed is NaN, +- INF or not available, pick center of band */ + airspeed = 0.5f * (airspeed_min + airspeed_max); + } else if (airspeed < airspeed_min) { + airspeed = airspeed_min; + } - return 0.0f; + float roll_error = roll_setpoint - roll; + _rate_setpoint = roll_error / _tc; + + /* limit the rate */ + if (_max_rate > 0.01f) { + _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; + _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint; + } + + _rate_error = _rate_setpoint - roll_rate; + + + float ilimit_scaled = 0.0f; + + if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5 * airspeed_min) { + + float id = _rate_error * k_i_rate * dt * scaler; + + /* + * anti-windup: do not allow integrator to increase into the + * wrong direction if actuator is at limit + */ + if (_last_output < -_max_deflection_rad) { + /* only allow motion to center: increase value */ + id = max(id, 0.0f); + } else if (_last_output > _max_deflection_rad) { + /* only allow motion to center: decrease value */ + id = min(id, 0.0f); + } + + _integrator += id; + } + + /* integrator limit */ + _integrator = constrain(_integrator, -ilimit_scaled, ilimit_scaled); + /* store non-limited output */ + _last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_roll_ff)) * scaler; + + return constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); } void ECL_RollController::reset_integrator() diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h index bba9ae1634..011820765e 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h @@ -49,7 +49,7 @@ public: ECL_RollController(); float control(float roll_setpoint, float roll, float roll_rate, - float scaler = 1.0f, bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float aspeed = (0.0f / 0.0f)); + float scaler = 1.0f, bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f)); void reset_integrator(); @@ -93,7 +93,8 @@ private: float _last_output; float _integrator; float _rate_error; - float _desired_rate; + float _rate_setpoint; + float _max_deflection_rad; }; #endif // ECL_ROLL_CONTROLLER_H From 9b48fe36229f27242ce8d80d2807f0849b2b55e5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 8 Sep 2013 20:04:39 +0200 Subject: [PATCH 577/872] Compiling attitude control, ready for tests --- .../ecl/attitude_fw/ecl_pitch_controller.cpp | 22 ++++++++++++------- .../ecl/attitude_fw/ecl_pitch_controller.h | 2 +- .../ecl/attitude_fw/ecl_roll_controller.cpp | 19 ++++++++++------ src/lib/ecl/attitude_fw/ecl_roll_controller.h | 2 +- .../ecl/attitude_fw/ecl_yaw_controller.cpp | 5 +++++ 5 files changed, 33 insertions(+), 17 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 0e1a0d7f62..77ec15c53d 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -38,13 +38,19 @@ */ #include "ecl_pitch_controller.h" +#include +#include +#include +#include +#include +#include ECL_PitchController::ECL_PitchController() : _last_run(0), _last_output(0.0f), _integrator(0.0f), _rate_error(0.0f), - _desired_rate(0.0f), + _rate_setpoint(0.0f), _max_deflection_rad(math::radians(45.0f)) { } @@ -62,7 +68,7 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc if (dt_micros > 500000) lock_integrator = true; - float k_roll_ff = max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); + float k_roll_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); float k_i_rate = _k_i * _tc; /* input conditioning */ @@ -75,7 +81,7 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc /* calculate the offset in the rate resulting from rolling */ float turn_offset = fabsf((CONSTANTS_ONE_G / airspeed) * - tanf(roll) * sinf(roll)) * _roll; + tanf(roll) * sinf(roll)) * _roll_ff; float pitch_error = pitch_setpoint - pitch; /* rate setpoint from current error and time constant */ @@ -88,7 +94,7 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc float ilimit_scaled = 0.0f; - if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5 * airspeed_min) { + if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) { float id = _rate_error * k_i_rate * dt * scaler; @@ -98,21 +104,21 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc */ if (_last_output < -_max_deflection_rad) { /* only allow motion to center: increase value */ - id = max(id, 0.0f); + id = math::max(id, 0.0f); } else if (_last_output > _max_deflection_rad) { /* only allow motion to center: decrease value */ - id = min(id, 0.0f); + id = math::min(id, 0.0f); } _integrator += id; } /* integrator limit */ - _integrator = constrain(_integrator, -ilimit_scaled, ilimit_scaled); + _integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled); /* store non-limited output */ _last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_roll_ff)) * scaler; - return constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); + return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); } void ECL_PitchController::reset_integrator() diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h index 53134556d7..02ca62dada 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h @@ -83,7 +83,7 @@ public: } float get_desired_rate() { - return _desired_rate; + return _rate_setpoint; } private: diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 1152b2964c..6de30fc339 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -39,6 +39,11 @@ #include "../ecl.h" #include "ecl_roll_controller.h" +#include +#include +#include +#include +#include ECL_RollController::ECL_RollController() : _last_run(0), @@ -61,7 +66,7 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000; - float k_ff = max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); + float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); float k_i_rate = _k_i * _tc; /* input conditioning */ @@ -86,7 +91,7 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra float ilimit_scaled = 0.0f; - if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5 * airspeed_min) { + if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) { float id = _rate_error * k_i_rate * dt * scaler; @@ -96,21 +101,21 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra */ if (_last_output < -_max_deflection_rad) { /* only allow motion to center: increase value */ - id = max(id, 0.0f); + id = math::max(id, 0.0f); } else if (_last_output > _max_deflection_rad) { /* only allow motion to center: decrease value */ - id = min(id, 0.0f); + id = math::min(id, 0.0f); } _integrator += id; } /* integrator limit */ - _integrator = constrain(_integrator, -ilimit_scaled, ilimit_scaled); + _integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled); /* store non-limited output */ - _last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_roll_ff)) * scaler; + _last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_ff)) * scaler; - return constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); + return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); } void ECL_RollController::reset_integrator() diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h index 011820765e..7fbcdf4b86 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h @@ -79,7 +79,7 @@ public: } float get_desired_rate() { - return _desired_rate; + return _rate_setpoint; } private: diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 6dd90f6762..a0f901e71b 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -38,6 +38,11 @@ */ #include "ecl_yaw_controller.h" +#include +#include +#include +#include +#include ECL_YawController::ECL_YawController() : _last_run(0), From 56a35cc8896b077e70226541a43aa0d449e8d9bb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 8 Sep 2013 20:05:15 +0200 Subject: [PATCH 578/872] Fixed commander start / stop to ensure the state is sane once NSH returns --- src/modules/commander/commander.cpp | 45 ++++++++++++++++++++++------- 1 file changed, 34 insertions(+), 11 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 333fe30ae2..830ee9743b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -144,8 +144,8 @@ static int mavlink_fd; /* flags */ static bool commander_initialized = false; -static bool thread_should_exit = false; /**< daemon exit flag */ -static bool thread_running = false; /**< daemon status flag */ +static volatile bool thread_should_exit = false; /**< daemon exit flag */ +static volatile bool thread_running = false; /**< daemon status flag */ static int daemon_task; /**< Handle of daemon task / thread */ static unsigned int leds_counter; @@ -230,7 +230,7 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (thread_running) { - warnx("commander already running\n"); + warnx("commander already running"); /* this is not an error */ exit(0); } @@ -242,21 +242,38 @@ int commander_main(int argc, char *argv[]) 3000, commander_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); + + while (!thread_running) { + usleep(200); + } + exit(0); } if (!strcmp(argv[1], "stop")) { + + if (!thread_running) + errx(0, "commander already stopped"); + thread_should_exit = true; + + while (thread_running) { + usleep(200000); + warnx("."); + } + + warnx("terminated."); + exit(0); } if (!strcmp(argv[1], "status")) { if (thread_running) { - warnx("\tcommander is running\n"); + warnx("\tcommander is running"); print_status(); } else { - warnx("\tcommander not started\n"); + warnx("\tcommander not started"); } exit(0); @@ -595,16 +612,20 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[cmd] started"); + int ret; + pthread_attr_t commander_low_prio_attr; pthread_attr_init(&commander_low_prio_attr); pthread_attr_setstacksize(&commander_low_prio_attr, 2992); struct sched_param param; (void)pthread_attr_getschedparam(&commander_low_prio_attr, ¶m); + /* 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); + pthread_attr_destroy(&commander_low_prio_attr); /* Start monitoring loop */ unsigned counter = 0; @@ -1200,7 +1221,12 @@ int commander_thread_main(int argc, char *argv[]) } /* wait for threads to complete */ - pthread_join(commander_low_prio_thread, NULL); + ret = pthread_join(commander_low_prio_thread, NULL); + if (ret) { + warn("join failed", ret); + } + + rgbled_set_mode(RGBLED_MODE_OFF); /* close fds */ led_deinit(); @@ -1218,9 +1244,6 @@ int commander_thread_main(int argc, char *argv[]) close(param_changed_sub); close(battery_sub); - warnx("exiting"); - fflush(stdout); - thread_running = false; return 0; @@ -1628,7 +1651,7 @@ void *commander_low_prio_loop(void *arg) while (!thread_should_exit) { /* wait for up to 100ms for data */ - int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000); + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 200); /* timed out - periodic check for _task_should_exit, etc. */ if (pret == 0) @@ -1785,5 +1808,5 @@ void *commander_low_prio_loop(void *arg) close(cmd_sub); - return 0; + return NULL; } From c3bb6960e6f85d07d65fefdfebfdc0650e81aa92 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 8 Sep 2013 20:05:38 +0200 Subject: [PATCH 579/872] Fixed mavlink start / stop to ensure process is in a sane state once NSH return --- src/modules/mavlink/mavlink.c | 16 +++++++++++++--- src/modules/mavlink/mavlink_receiver.cpp | 2 ++ src/modules/mavlink/orb_listener.c | 2 ++ 3 files changed, 17 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 5eb7cba9b2..cbcd4adfb8 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -743,7 +743,7 @@ int mavlink_thread_main(int argc, char *argv[]) thread_running = false; - exit(0); + return 0; } static void @@ -767,7 +767,7 @@ int mavlink_main(int argc, char *argv[]) /* this is not an error */ if (thread_running) - errx(0, "mavlink already running\n"); + errx(0, "mavlink already running"); thread_should_exit = false; mavlink_task = task_spawn_cmd("mavlink", @@ -776,15 +776,25 @@ int mavlink_main(int argc, char *argv[]) 2048, mavlink_thread_main, (const char **)argv); + + while (!thread_running) { + usleep(200); + } + exit(0); } if (!strcmp(argv[1], "stop")) { + + /* this is not an error */ + if (!thread_running) + errx(0, "mavlink already stopped"); + thread_should_exit = true; while (thread_running) { usleep(200000); - printf("."); + warnx("."); } warnx("terminated."); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 4674f7a248..222d1f45f2 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -755,5 +755,7 @@ receive_start(int uart) pthread_t thread; pthread_create(&thread, &receiveloop_attr, receive_thread, &uart); + + pthread_attr_destroy(&receiveloop_attr); return thread; } diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 53d86ec005..d11a67fc6f 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -829,5 +829,7 @@ uorb_receive_start(void) pthread_t thread; pthread_create(&thread, &uorb_attr, uorb_receive_thread, NULL); + + pthread_attr_destroy(&uorb_attr); return thread; } From 2d6dfe2a9e1fc04a9cadc3e4defa3e9cd615db1f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 8 Sep 2013 20:40:26 +0200 Subject: [PATCH 580/872] Allow px4io detect to be run when IO is already running --- src/drivers/px4io/px4io.cpp | 32 +++++++++++++++++--------------- 1 file changed, 17 insertions(+), 15 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index c88abe59a8..78d1d3e63f 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -487,25 +487,27 @@ PX4IO::detect() { int ret; - ASSERT(_task == -1); + if (_task == -1) { - /* do regular cdev init */ - ret = CDev::init(); - if (ret != OK) - return ret; + /* do regular cdev init */ + ret = CDev::init(); + if (ret != OK) + return ret; - /* get some parameters */ - unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); - if (protocol != PX4IO_PROTOCOL_VERSION) { - if (protocol == _io_reg_get_error) { - log("IO not installed"); - } else { - log("IO version error"); - mavlink_log_emergency(_mavlink_fd, "IO VERSION MISMATCH, PLEASE UPGRADE SOFTWARE!"); + /* get some parameters */ + unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); + if (protocol != PX4IO_PROTOCOL_VERSION) { + if (protocol == _io_reg_get_error) { + log("IO not installed"); + } else { + log("IO version error"); + mavlink_log_emergency(_mavlink_fd, "IO VERSION MISMATCH, PLEASE UPGRADE SOFTWARE!"); + } + + return -1; } - - return -1; } + log("IO found"); return 0; From 2e8b675c6c5ddc3fd2db22a8e22a664b50ef18a9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 8 Sep 2013 20:40:55 +0200 Subject: [PATCH 581/872] Fixed (temporarily) HIL by allowing index 1000 to start a matching setup --- ROMFS/px4fmu_common/init.d/rc.hil | 8 ++++---- ROMFS/px4fmu_common/init.d/rc.usb | 8 -------- ROMFS/px4fmu_common/init.d/rcS | 11 ++++++++--- 3 files changed, 12 insertions(+), 15 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.hil b/ROMFS/px4fmu_common/init.d/rc.hil index eccb2b7678..b924d62bc8 100644 --- a/ROMFS/px4fmu_common/init.d/rc.hil +++ b/ROMFS/px4fmu_common/init.d/rc.hil @@ -5,10 +5,9 @@ echo "[HIL] starting.." -uorb start - # Tell MAVLink that this link is "fast" -mavlink start -b 230400 -d /dev/ttyS1 +sleep 2 +mavlink start -b 230400 -d /dev/ttyACM0 # Create a fake HIL /dev/pwm_output interface hil mode_pwm @@ -50,7 +49,8 @@ att_pos_estimator_ekf start # Load mixer and start controllers (depends on px4io) # mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix -fw_pos_control_l1 start +#fw_pos_control_l1 start fw_att_control start echo "[HIL] setup done, running" + diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 9264985d6d..ccf2cd47ea 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -21,7 +21,6 @@ if mavlink stop then echo "stopped other MAVLink instance" fi -sleep 2 mavlink start -b 230400 -d /dev/ttyACM0 # Stop commander @@ -37,13 +36,6 @@ 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/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index c2ef375266..1f466e49f0 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -101,8 +101,14 @@ then fi fi - # Try to get an USB console - nshterm /dev/ttyACM0 & + if param compare SYS_AUTOSTART 1000 + then + sh /etc/init.d/rc.hil + set MODE custom + else + # Try to get an USB console + nshterm /dev/ttyACM0 & + fi # # Upgrade PX4IO firmware @@ -202,7 +208,6 @@ then then # Telemetry port is on both FMU boards ttyS1 mavlink start -b 57600 -d /dev/ttyS1 - usleep 5000 # Start commander commander start From d3ac8c9ff31ac609d555613e552b38782a2b2490 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 8 Sep 2013 21:06:55 +0200 Subject: [PATCH 582/872] Fixed HIL mode switching, HIL works --- src/modules/commander/commander.cpp | 10 ++++++++-- src/modules/commander/state_machine_helper.cpp | 2 ++ 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 830ee9743b..5eeb018fd5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -343,6 +343,9 @@ void print_status() warnx("arming: %s", armed_str); } +static orb_advert_t control_mode_pub; +static orb_advert_t status_pub; + 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 */ @@ -356,6 +359,10 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe uint8_t base_mode = (uint8_t) cmd->param1; uint8_t custom_main_mode = (uint8_t) cmd->param2; + /* set HIL state */ + hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF; + hil_state_transition(new_hil_state, status_pub, status, control_mode_pub, control_mode, mavlink_fd); + // TODO remove debug code //mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode); /* set arming state */ @@ -543,7 +550,6 @@ int commander_thread_main(int argc, char *argv[]) } /* Main state machine */ - 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 @@ -555,7 +561,7 @@ int commander_thread_main(int argc, char *argv[]) /* 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)); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 3cef10185e..7d759b8d25 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -502,6 +502,8 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s current_control_mode->timestamp = hrt_absolute_time(); orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, current_control_mode); + // XXX also set lockdown here + ret = OK; } else { From 11e4fbc3745193a61f6f56619318dc1bc0b60307 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 8 Sep 2013 21:49:59 +0200 Subject: [PATCH 583/872] Added additional vector functions, fixed seatbelt for global estimators --- src/lib/geo/geo.c | 34 +++++++++++++++++++ src/lib/geo/geo.h | 8 ++--- .../commander/state_machine_helper.cpp | 10 +++--- .../uORB/topics/vehicle_global_position.h | 14 ++++---- 4 files changed, 51 insertions(+), 15 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 63792dda52..e862b1dc06 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -210,6 +210,40 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub return theta; } +__EXPORT float get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy) +{ + double lat_now_rad = lat_now * M_DEG_TO_RAD; + double lon_now_rad = lon_now * M_DEG_TO_RAD; + double lat_next_rad = lat_next * M_DEG_TO_RAD; + double lon_next_rad = lon_next * M_DEG_TO_RAD; + + double d_lat = lat_next_rad - lat_now_rad; + double d_lon = lon_next_rad - lon_now_rad; + + /* conscious mix of double and float trig function to maximize speed and efficiency */ + *vy = CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad) + *vx = CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon)); + + return theta; +} + +__EXPORT float get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy) +{ + double lat_now_rad = lat_now * M_DEG_TO_RAD; + double lon_now_rad = lon_now * M_DEG_TO_RAD; + double lat_next_rad = lat_next * M_DEG_TO_RAD; + double lon_next_rad = lon_next * M_DEG_TO_RAD; + + double d_lat = lat_next_rad - lat_now_rad; + double d_lon = lon_next_rad - lon_now_rad; + + /* conscious mix of double and float trig function to maximize speed and efficiency */ + *vy = CONSTANTS_RADIUS_OF_EARTH * d_lon; + *vx = CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad); + + return theta; +} + // Additional functions - @author Doug Weibel __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end) diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index dadec51ec9..0459909e45 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -57,10 +57,6 @@ __BEGIN_DECLS #define CONSTANTS_ABSOLUTE_NULL_CELSIUS -273.15f /* °C */ #define CONSTANTS_RADIUS_OF_EARTH 6371000 /* meters (m) */ -/* compatibility aliases */ -#define RADIUS_OF_EARTH CONSTANTS_RADIUS_OF_EARTH -#define GRAVITY_MSS CONSTANTS_ONE_G - // XXX remove struct crosstrack_error_s { bool past_end; // Flag indicating we are past the end of the line/arc segment @@ -116,6 +112,10 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou */ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); +__EXPORT float get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy); + +__EXPORT float get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy); + __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end); __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center, diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 3cef10185e..316b06127b 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -228,8 +228,9 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m case MAIN_STATE_SEATBELT: - /* need altitude estimate */ - if (current_state->condition_local_altitude_valid) { + /* need at minimum altitude estimate */ + if (current_state->condition_local_altitude_valid || + current_state->condition_global_position_valid) { ret = TRANSITION_CHANGED; } @@ -237,8 +238,9 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m case MAIN_STATE_EASY: - /* need local position estimate */ - if (current_state->condition_local_position_valid) { + /* need at minimum local position estimate */ + if (current_state->condition_local_position_valid || + current_state->condition_global_position_valid) { ret = TRANSITION_CHANGED; } diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index 0fc0ed5c97..143734e372 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 */ - bool valid; /**< true if position satisfies validity criteria of estimator */ + 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 */ int32_t lon; /**< Longitude in 1E7 degrees */ - float alt; /**< Altitude in meters */ + 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 yaw; /**< Compass heading in radians -PI..+PI. */ + 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 yaw; /**< Compass heading in radians -PI..+PI. */ }; /** From 98ac914cb08728a5acf5322d69f176fe2d07d46e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 8 Sep 2013 22:07:33 +0200 Subject: [PATCH 584/872] Add setting queue depth to HMC test --- src/drivers/hmc5883/hmc5883.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 3ede90a174..0de82c3042 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1317,6 +1317,10 @@ test() if (fd < 0) err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH); + /* set the queue depth to 10 */ + if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) + errx(1, "failed to set queue depth"); + /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); @@ -1332,7 +1336,7 @@ test() errx(1, "failed to get if mag is onboard or external"); warnx("device active: %s", ret ? "external" : "onboard"); - /* set the queue depth to 10 */ + /* set the queue depth to 5 */ if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) errx(1, "failed to set queue depth"); From b6a0437c7c474b62400ed5430d4cc1b308eee513 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 8 Sep 2013 22:30:56 +0200 Subject: [PATCH 585/872] Fixed compile error --- src/lib/geo/geo.c | 12 ++++-------- src/lib/geo/geo.h | 4 ++-- 2 files changed, 6 insertions(+), 10 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index e862b1dc06..43105fdba7 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -210,7 +210,7 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub return theta; } -__EXPORT float get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy) +__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy) { double lat_now_rad = lat_now * M_DEG_TO_RAD; double lon_now_rad = lon_now * M_DEG_TO_RAD; @@ -221,13 +221,11 @@ __EXPORT float get_vector_to_next_waypoint(double lat_now, double lon_now, doubl double d_lon = lon_next_rad - lon_now_rad; /* conscious mix of double and float trig function to maximize speed and efficiency */ - *vy = CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad) - *vx = CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon)); - - return theta; + *vy = CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad); + *vx = CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon); } -__EXPORT float get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy) +__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy) { double lat_now_rad = lat_now * M_DEG_TO_RAD; double lon_now_rad = lon_now * M_DEG_TO_RAD; @@ -240,8 +238,6 @@ __EXPORT float get_vector_to_next_waypoint_fast(double lat_now, double lon_now, /* conscious mix of double and float trig function to maximize speed and efficiency */ *vy = CONSTANTS_RADIUS_OF_EARTH * d_lon; *vx = CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad); - - return theta; } // Additional functions - @author Doug Weibel diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 0459909e45..123ff80f16 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -112,9 +112,9 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou */ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); -__EXPORT float get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy); +__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy); -__EXPORT float get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy); +__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy); __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end); From c12955fbc0fca071fde4f64f0c9bf255b0a89420 Mon Sep 17 00:00:00 2001 From: Buzz Date: Tue, 10 Sep 2013 13:20:45 +1000 Subject: [PATCH 586/872] the "rgbled rgb X X X" command was broken, and would set green when you asked for red, and blue when you asked for green, and never set red. - off by 1 error in parameter numbering. --- src/drivers/rgbled/rgbled.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index feb8f1c6c9..ee1d472a2e 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -590,9 +590,9 @@ rgbled_main(int argc, char *argv[]) errx(1, "Usage: rgbled rgb "); } 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); + v.red = strtol(argv[2], NULL, 0); + v.green = strtol(argv[3], NULL, 0); + v.blue = strtol(argv[4], NULL, 0); ret = ioctl(fd, RGBLED_SET_RGB, (unsigned long)&v); close(fd); exit(ret); From 5182860a68fe5733062bd342fbe85310497e20d3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 9 Sep 2013 21:06:08 +0200 Subject: [PATCH 587/872] Added support for inverted flight --- .../ecl/attitude_fw/ecl_pitch_controller.cpp | 22 +++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 77ec15c53d..d0b5fcab77 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -79,9 +79,31 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc airspeed = airspeed_min; } + /* flying inverted (wings upside down) ? */ + bool inverted = false; + + /* roll is used as feedforward term and inverted flight needs to be considered */ + if (fabsf(roll) < math::radians(90.0f)) { + /* not inverted, but numerically still potentially close to infinity */ + roll = math::constrain(roll, math::radians(-80.0f), math::radians(80.0f)); + } else { + /* inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity */ + + /* note: the ranges are extended by 10 deg here to avoid numeric resolution effects */ + if (roll > 0.0f) { + /* right hemisphere */ + roll = math::constrain(roll, math::radians(100.0f), math::radians(180.0f)); + } else { + /* left hemisphere */ + roll = math::constrain(roll, math::radians(-100.0f), math::radians(-180.0f)); + } + } + /* calculate the offset in the rate resulting from rolling */ float turn_offset = fabsf((CONSTANTS_ONE_G / airspeed) * tanf(roll) * sinf(roll)) * _roll_ff; + if (inverted) + turn_offset = -turn_offset; float pitch_error = pitch_setpoint - pitch; /* rate setpoint from current error and time constant */ From 12e4e393bdedc4a8f929bff8bff73b00e35752dd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Sep 2013 11:29:05 +0200 Subject: [PATCH 588/872] Checked in L1 position and TECS altitude control. Not test-flown in HIL or outdoors yet --- .../ecl/attitude_fw/ecl_pitch_controller.cpp | 2 +- .../ecl/attitude_fw/ecl_pitch_controller.h | 2 +- .../ecl/attitude_fw/ecl_roll_controller.cpp | 2 +- src/lib/ecl/attitude_fw/ecl_roll_controller.h | 2 +- .../ecl/attitude_fw/ecl_yaw_controller.cpp | 2 +- src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 2 +- src/lib/ecl/ecl.h | 1 + src/lib/ecl/l1/ecl_l1_pos_control.cpp | 323 +++++ src/lib/ecl/l1/ecl_l1_pos_control.h | 236 ++++ src/lib/ecl/module.mk | 5 +- src/lib/external_lgpl/module.mk | 48 + src/lib/external_lgpl/tecs/TECS.cpp | 534 +++++++++ src/lib/external_lgpl/tecs/TECS.h | 350 ++++++ .../fw_pos_control_l1_main.cpp | 1044 +++++++++++++++++ .../fw_pos_control_l1_params.c | 109 ++ src/modules/fw_pos_control_l1/module.mk | 41 + 16 files changed, 2695 insertions(+), 8 deletions(-) create mode 100644 src/lib/ecl/l1/ecl_l1_pos_control.cpp create mode 100644 src/lib/ecl/l1/ecl_l1_pos_control.h create mode 100644 src/lib/external_lgpl/module.mk create mode 100644 src/lib/external_lgpl/tecs/TECS.cpp create mode 100644 src/lib/external_lgpl/tecs/TECS.h create mode 100644 src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp create mode 100644 src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c create mode 100644 src/modules/fw_pos_control_l1/module.mk diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index d0b5fcab77..44b339ce52 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name APL nor the names of its contributors may be + * 3. Neither the name ECL nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h index 02ca62dada..7fbfd6fbcb 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name APL nor the names of its contributors may be + * 3. Neither the name ECL nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 6de30fc339..f3a8585ae2 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name APL nor the names of its contributors may be + * 3. Neither the name ECL nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h index 7fbcdf4b86..3427b67c20 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name APL nor the names of its contributors may be + * 3. Neither the name ECL nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index a0f901e71b..2b7429996d 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name APL nor the names of its contributors may be + * 3. Neither the name ECL nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index cfaa6c2331..66b2279182 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name APL nor the names of its contributors may be + * 3. Neither the name ECL nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * diff --git a/src/lib/ecl/ecl.h b/src/lib/ecl/ecl.h index 2bd76ce97a..e0f207696a 100644 --- a/src/lib/ecl/ecl.h +++ b/src/lib/ecl/ecl.h @@ -38,6 +38,7 @@ */ #include +#include #define ecl_absolute_time hrt_absolute_time #define ecl_elapsed_time hrt_elapsed_time \ No newline at end of file diff --git a/src/lib/ecl/l1/ecl_l1_pos_control.cpp b/src/lib/ecl/l1/ecl_l1_pos_control.cpp new file mode 100644 index 0000000000..533409217e --- /dev/null +++ b/src/lib/ecl/l1/ecl_l1_pos_control.cpp @@ -0,0 +1,323 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). 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 vector_ABove copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the vector_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 ECL 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 MERCHANTvector_ABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIvector_ABLE 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 LIvector_ABILITY, WHETHER IN CONTRACT, STRICT + * LIvector_ABILITY, 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 ecl_l1_pos_control.h + * Implementation of L1 position control. + * Authors and acknowledgements in header. + * + */ + +#include "ecl_l1_pos_control.h" + +float ECL_L1_Pos_Control::nav_roll() +{ + float ret = atanf(_lateral_accel * 1.0f / CONSTANTS_ONE_G); + ret = math::constrain(ret, (-M_PI_F) / 2.0f, M_PI_F / 2.0f); + return ret; +} + +float ECL_L1_Pos_Control::nav_lateral_acceleration_demand() +{ + return _lateral_accel; +} + +float ECL_L1_Pos_Control::nav_bearing() +{ + return _wrap_pi(_nav_bearing); +} + +float ECL_L1_Pos_Control::bearing_error() +{ + return _bearing_error; +} + +float ECL_L1_Pos_Control::target_bearing() +{ + return _target_bearing; +} + +float ECL_L1_Pos_Control::switch_distance(float wp_radius) +{ + return math::min(wp_radius, _L1_distance); +} + +bool ECL_L1_Pos_Control::reached_loiter_target(void) +{ + return _circle_mode; +} + +float ECL_L1_Pos_Control::crosstrack_error(void) +{ + return _crosstrack_error; +} + +void ECL_L1_Pos_Control::navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position, + const math::Vector2f &ground_speed_vector) +{ + + float eta; + float xtrack_vel; + float ltrack_vel; + + /* get the direction between the last (visited) and next waypoint */ + _target_bearing = get_bearing_to_next_waypoint(vector_curr_position.getX(), vector_curr_position.getY(), vector_B.getX(), vector_B.getY()); + + /* enforce a minimum ground speed of 0.1 m/s to avoid singularities */ + float ground_speed = math::max(ground_speed_vector.length(), 0.1f); + + /* calculate the L1 length required for the desired period */ + _L1_distance = _L1_ratio * ground_speed; + + /* calculate vector from A to B */ + math::Vector2f vector_AB = get_local_planar_vector(vector_A, vector_B); + + /* + * check if waypoints are on top of each other. If yes, + * skip A and directly continue to B + */ + if (vector_AB.length() < 1.0e-6f) { + vector_AB = get_local_planar_vector(vector_curr_position, vector_B); + } + + vector_AB.normalize(); + + /* calculate the vector from waypoint A to the aircraft */ + math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); + + /* calculate crosstrack error (output only) */ + _crosstrack_error = vector_AB % vector_A_to_airplane; + + /* + * If the current position is in a +-135 degree angle behind waypoint A + * and further away from A than the L1 distance, then A becomes the L1 point. + * If the aircraft is already between A and B normal L1 logic is applied. + */ + float distance_A_to_airplane = vector_A_to_airplane.length(); + float alongTrackDist = vector_A_to_airplane * vector_AB; + + if (distance_A_to_airplane > _L1_distance && alongTrackDist / math::max(distance_A_to_airplane , 1.0f) < -0.7071f) { + + /* calculate eta to fly to waypoint A */ + + /* unit vector from waypoint A to current position */ + math::Vector2f vector_A_to_airplane_unit = vector_A_to_airplane.normalized(); + /* velocity across / orthogonal to line */ + xtrack_vel = ground_speed_vector % (-vector_A_to_airplane_unit); + /* velocity along line */ + ltrack_vel = ground_speed_vector * (-vector_A_to_airplane_unit); + eta = atan2f(xtrack_vel, ltrack_vel); + /* bearing from current position to L1 point */ + _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX()); + + } else { + + /* calculate eta to fly along the line between A and B */ + + /* velocity across / orthogonal to line */ + xtrack_vel = ground_speed_vector % vector_AB; + /* velocity along line */ + ltrack_vel = ground_speed_vector * vector_AB; + /* calculate eta2 (angle of velocity vector relative to line) */ + float eta2 = atan2f(xtrack_vel, ltrack_vel); + /* calculate eta1 (angle to L1 point) */ + float xtrackErr = vector_A_to_airplane % vector_AB; + float sine_eta1 = xtrackErr / math::max(_L1_distance , 0.1f); + /* limit output to 45 degrees */ + sine_eta1 = math::constrain(sine_eta1, -M_PI_F / 4.0f, +M_PI_F / 4.0f); + float eta1 = asinf(sine_eta1); + eta = eta1 + eta2; + /* bearing from current position to L1 point */ + _nav_bearing = atan2f(vector_AB.getY(), vector_AB.getX()) + eta1; + } + + /* limit angle to +-90 degrees */ + eta = math::constrain(eta, (-M_PI_F) / 2.0f, +M_PI_F / 2.0f); + _lateral_accel = _K_L1 * ground_speed * ground_speed / _L1_distance * sinf(eta); + + /* flying to waypoints, not circling them */ + _circle_mode = false; + + /* the bearing angle, in NED frame */ + _bearing_error = eta; +} + +void ECL_L1_Pos_Control::navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction, + const math::Vector2f &ground_speed_vector) +{ + + /* calculate the gains for the PD loop (circle tracking) */ + float omega = (2.0f * M_PI_F / _L1_period); + float K_crosstrack = omega * omega; + float K_velocity = 2.0f * _L1_damping * omega; + + /* update bearing to next waypoint */ + _target_bearing = get_bearing_to_next_waypoint(vector_curr_position.getX(), vector_curr_position.getY(), vector_A.getX(), vector_A.getY()); + + /* ground speed, enforce minimum of 0.1 m/s to avoid singularities */ + float ground_speed = math::max(ground_speed_vector.length() , 0.1f); + + /* calculate the L1 length required for the desired period */ + _L1_distance = _L1_ratio * ground_speed; + + /* calculate the vector from waypoint A to current position */ + math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); + + /* store the normalized vector from waypoint A to current position */ + math::Vector2f vector_A_to_airplane_unit = (vector_A_to_airplane).normalized(); + + /* calculate eta angle towards the loiter center */ + + /* velocity across / orthogonal to line from waypoint to current position */ + float xtrack_vel_center = vector_A_to_airplane_unit % ground_speed_vector; + /* velocity along line from waypoint to current position */ + float ltrack_vel_center = - (ground_speed_vector * vector_A_to_airplane_unit); + float eta = atan2f(xtrack_vel_center, ltrack_vel_center); + /* limit eta to 90 degrees */ + eta = math::constrain(eta, -M_PI_F / 2.0f, +M_PI_F / 2.0f); + + /* calculate the lateral acceleration to capture the center point */ + float lateral_accel_sp_center = _K_L1 * ground_speed * ground_speed / _L1_distance * sinf(eta); + + /* for PD control: Calculate radial position and velocity errors */ + + /* radial velocity error */ + float xtrack_vel_circle = -ltrack_vel_center; + /* radial distance from the loiter circle (not center) */ + float xtrack_err_circle = vector_A_to_airplane.length() - radius; + + /* cross track error for feedback */ + _crosstrack_error = xtrack_err_circle; + + /* calculate PD update to circle waypoint */ + float lateral_accel_sp_circle_pd = (xtrack_err_circle * K_crosstrack + xtrack_vel_circle * K_velocity); + + /* calculate velocity on circle / along tangent */ + float tangent_vel = xtrack_vel_center * loiter_direction; + + /* prevent PD output from turning the wrong way */ + if (tangent_vel < 0.0f) { + lateral_accel_sp_circle_pd = math::max(lateral_accel_sp_circle_pd , 0.0f); + } + + /* calculate centripetal acceleration setpoint */ + float lateral_accel_sp_circle_centripetal = tangent_vel * tangent_vel / math::max((0.5f * radius) , (radius + xtrack_err_circle)); + + /* add PD control on circle and centripetal acceleration for total circle command */ + float lateral_accel_sp_circle = loiter_direction * (lateral_accel_sp_circle_pd + lateral_accel_sp_circle_centripetal); + + /* + * Switch between circle (loiter) and capture (towards waypoint center) mode when + * the commands switch over. Only fly towards waypoint if outside the circle. + */ + + // XXX check switch over + if ((lateral_accel_sp_center < lateral_accel_sp_circle && loiter_direction > 0 && xtrack_err_circle > 0.0f) | + (lateral_accel_sp_center > lateral_accel_sp_circle && loiter_direction < 0 && xtrack_err_circle > 0.0f)) { + _lateral_accel = lateral_accel_sp_center; + _circle_mode = false; + /* angle between requested and current velocity vector */ + _bearing_error = eta; + /* bearing from current position to L1 point */ + _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX()); + + } else { + _lateral_accel = lateral_accel_sp_circle; + _circle_mode = true; + _bearing_error = 0.0f; + /* bearing from current position to L1 point */ + _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX()); + } +} + + +void ECL_L1_Pos_Control::navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed_vector) +{ + + float eta; + + /* + * As the commanded heading is the only reference + * (and no crosstrack correction occurs), + * target and navigation bearing become the same + */ + _target_bearing = _nav_bearing = _wrap_pi(navigation_heading); + eta = _target_bearing - _wrap_pi(current_heading); + eta = _wrap_pi(eta); + + /* consequently the bearing error is exactly eta: */ + _bearing_error = eta; + + /* ground speed is the length of the ground speed vector */ + float ground_speed = ground_speed_vector.length(); + + /* adjust L1 distance to keep constant frequency */ + _L1_distance = ground_speed / _heading_omega; + float omega_vel = ground_speed * _heading_omega; + + /* not circling a waypoint */ + _circle_mode = false; + + /* navigating heading means by definition no crosstrack error */ + _crosstrack_error = 0; + + /* limit eta to 90 degrees */ + eta = math::constrain(eta, (-M_PI_F) / 2.0f, +M_PI_F / 2.0f); + _lateral_accel = 2.0f * sinf(eta) * omega_vel; +} + +void ECL_L1_Pos_Control::navigate_level_flight(float current_heading) +{ + /* reset all heading / error measures resulting in zero roll */ + _target_bearing = current_heading; + _nav_bearing = current_heading; + _bearing_error = 0; + _crosstrack_error = 0; + _lateral_accel = 0; + + /* not circling a waypoint when flying level */ + _circle_mode = false; + +} + + +math::Vector2f ECL_L1_Pos_Control::get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const +{ + math::Vector2f out; + + out.setX(math::radians((target.getX() - origin.getX()))); + out.setY(math::radians((target.getY() - origin.getY())*cosf(math::radians(origin.getX())))); + + return out * static_cast(CONSTANTS_RADIUS_OF_EARTH); +} + diff --git a/src/lib/ecl/l1/ecl_l1_pos_control.h b/src/lib/ecl/l1/ecl_l1_pos_control.h new file mode 100644 index 0000000000..661651f086 --- /dev/null +++ b/src/lib/ecl/l1/ecl_l1_pos_control.h @@ -0,0 +1,236 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_l1_pos_control.h + * Implementation of L1 position control. + * + * @author Lorenz Meier + * + * Acknowledgements and References: + * + * Original publication: + * S. Park, J. Deyst, and J. P. How, "A New Nonlinear Guidance Logic for Trajectory Tracking," + * Proceedings of the AIAA Guidance, Navigation and Control + * Conference, Aug 2004. AIAA-2004-4900. + * + * Original navigation logic modified by Paul Riseborough 2013: + * - Explicit control over frequency and damping + * - Explicit control over track capture angle + * - Ability to use loiter radius smaller than L1 length + * - Modified to use PD control for circle tracking to enable loiter radius less than L1 length + * - Modified to enable period and damping of guidance loop to be set explicitly + * - Modified to provide explicit control over capture angle + * + */ + +#ifndef ECL_L1_POS_CONTROL_H +#define ECL_L1_POS_CONTROL_H + +#include +#include +#include + +/** + * L1 Nonlinear Guidance Logic + */ +class __EXPORT ECL_L1_Pos_Control +{ +public: + ECL_L1_Pos_Control() { + _L1_period = 25; + _L1_damping = 0.75f; + } + + /** + * The current target bearing + * + * @return bearing angle (-pi..pi, in NED frame) + */ + float nav_bearing(); + + /** + * Get lateral acceleration demand. + * + * @return Lateral acceleration in m/s^2 + */ + float nav_lateral_acceleration_demand(); + + // return the heading error angle +ve to left of track + + + /** + * Heading error. + * + * The heading error is either compared to the current track + * or to the tangent of the current loiter radius. + */ + float bearing_error(); + + + /** + * Bearing from aircraft to current target. + * + * @return bearing angle (-pi..pi, in NED frame) + */ + float target_bearing(); + + + /** + * Get roll angle setpoint for fixed wing. + * + * @return Roll angle (in NED frame) + */ + float nav_roll(); + + + /** + * Get the current crosstrack error. + * + * @return Crosstrack error in meters. + */ + float crosstrack_error(); + + + /** + * Returns true if the loiter waypoint has been reached + */ + bool reached_loiter_target(); + + + /** + * Get the switch distance + * + * This is the distance at which the system will + * switch to the next waypoint. This depends on the + * period and damping + * + * @param waypoint_switch_radius The switching radius the waypoint has set. + */ + float switch_distance(float waypoint_switch_radius); + + + /** + * Navigate between two waypoints + * + * Calling this function with two waypoints results in the + * control outputs to fly to the line segment defined by + * the points and once captured following the line segment. + * + * @return sets _lateral_accel setpoint + */ + void navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position, + const math::Vector2f &ground_speed); + + + /** + * Navigate on an orbit around a loiter waypoint. + * + * @return sets _lateral_accel setpoint + */ + void navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction, + const math::Vector2f &ground_speed_vector); + + + /** + * Navigate on a fixed bearing. + * + * This only holds a certain direction and does not perform cross + * track correction. + * + * @return sets _lateral_accel setpoint + */ + void navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed); + + + /** + * Keep the wings level + */ + void navigate_level_flight(float current_heading); + + + /** + * Set the L1 period. + */ + void set_l1_period(float period) { + _L1_period = period; + _L1_ratio = 1.0f / M_PI_F * _L1_damping * _L1_period; + /* calculate normalized frequency for heading tracking */ + _heading_omega = sqrtf(2.0f) * M_PI_F / _L1_period; + } + + + /** + * Set the L1 damping factor. + * + * The original publication recommends a default of sqrt(2) / 2 = 0.707 + */ + void set_l1_damping(float damping) { + _L1_damping = damping; + _L1_ratio = 1.0f / M_PI_F * _L1_damping * _L1_period; + _K_L1 = 4.0f * _L1_damping * _L1_damping; + } + +private: + + float _lateral_accel; ///< Lateral acceleration setpoint in m/s^2 + float _L1_distance; ///< L1 lead distance, defined by period and damping + bool _circle_mode; ///< flag for loiter mode + float _nav_bearing; ///< bearing to L1 reference point + float _bearing_error; ///< bearing error + float _crosstrack_error; ///< crosstrack error in meters + float _target_bearing; ///< the heading setpoint + + float _L1_period; ///< L1 tracking period in seconds + float _L1_damping; ///< L1 damping ratio + float _L1_ratio; ///< L1 ratio for navigation + float _K_L1; ///< L1 control gain for _L1_damping + float _heading_omega; ///< Normalized frequency + + /** + * Convert a 2D vector from WGS84 to planar coordinates. + * + * This converts from latitude and longitude to planar + * coordinates with (0,0) being at the position of ref and + * returns a vector in meters towards wp. + * + * @param ref The reference position in WGS84 coordinates + * @param wp The point to convert to into the local coordinates, in WGS84 coordinates + * @return The vector in meters pointing from the reference position to the coordinates + */ + math::Vector2f get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const; + +}; + + +#endif /* ECL_L1_POS_CONTROL_H */ diff --git a/src/lib/ecl/module.mk b/src/lib/ecl/module.mk index 19610872e5..e8bca3c76a 100644 --- a/src/lib/ecl/module.mk +++ b/src/lib/ecl/module.mk @@ -36,5 +36,6 @@ # SRCS = attitude_fw/ecl_pitch_controller.cpp \ - attitude_fw/ecl_roll_controller.cpp \ - attitude_fw/ecl_yaw_controller.cpp + attitude_fw/ecl_roll_controller.cpp \ + attitude_fw/ecl_yaw_controller.cpp \ + l1/ecl_l1_pos_control.cpp diff --git a/src/lib/external_lgpl/module.mk b/src/lib/external_lgpl/module.mk new file mode 100644 index 0000000000..619a1a5df3 --- /dev/null +++ b/src/lib/external_lgpl/module.mk @@ -0,0 +1,48 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# W A R N I N G: The contents of this directory are license-incompatible +# with the rest of the codebase. Do NOT copy any parts of it +# into other folders. +# +# Acknowledgements: +# +# The algorithms in this folder have been developed by Paul Riseborough +# with support from Andrew Tridgell. +# Originally licensed as LGPL for APM. As this is built as library and +# linked, use of this code does not change the usability of PX4 in general +# or any of the license implications. +# + +SRCS = tecs/TECS.cpp diff --git a/src/lib/external_lgpl/tecs/TECS.cpp b/src/lib/external_lgpl/tecs/TECS.cpp new file mode 100644 index 0000000000..3f53552437 --- /dev/null +++ b/src/lib/external_lgpl/tecs/TECS.cpp @@ -0,0 +1,534 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +#include "TECS.h" +#include + +using namespace math; + +#ifndef CONSTANTS_ONE_G +#define CONSTANTS_ONE_G GRAVITY +#endif + +/** + * @file TECS.cpp + * + * @author Paul Riseborough + * + * Written by Paul Riseborough 2013 to provide: + * - Combined control of speed and height using throttle to control + * total energy and pitch angle to control exchange of energy between + * potential and kinetic. + * Selectable speed or height priority modes when calculating pitch angle + * - Fallback mode when no airspeed measurement is available that + * sets throttle based on height rate demand and switches pitch angle control to + * height priority + * - Underspeed protection that demands maximum throttle and switches pitch angle control + * to speed priority mode + * - Relative ease of tuning through use of intuitive time constant, integrator and damping gains and the use + * of easy to measure aircraft performance data + * + */ + +void TECS::update_50hz(float baro_altitude, float airspeed, const math::Dcm &rotMat, const math::Vector3 &accel_body, const math::Vector3 &accel_earth) +{ + // Implement third order complementary filter for height and height rate + // estimted height rate = _integ2_state + // estimated height = _integ3_state + // Reference Paper : + // Optimising the Gains of the Baro-Inertial Vertical Channel + // Widnall W.S, Sinha P.K, + // AIAA Journal of Guidance and Control, 78-1307R + + // Calculate time in seconds since last update + uint64_t now = ecl_absolute_time(); + float DT = max((now - _update_50hz_last_usec), 0ULL) * 1.0e-6f; + + // printf("dt: %10.6f baro alt: %6.2f eas: %6.2f R(0,0): %6.2f, R(1,1): %6.2f\naccel body: %6.2f %6.2f %6.2f\naccel earth: %6.2f %6.2f %6.2f\n", + // DT, baro_altitude, airspeed, rotMat(0, 0), rotMat(1, 1), accel_body(0), accel_body(1), accel_body(2), + // accel_earth(0), accel_earth(1), accel_earth(2)); + + if (DT > 1.0f) { + _integ3_state = baro_altitude; + _integ2_state = 0.0f; + _integ1_state = 0.0f; + DT = 0.02f; // when first starting TECS, use a + // small time constant + } + + _update_50hz_last_usec = now; + _EAS = airspeed; + + // Get height acceleration + float hgt_ddot_mea = -(accel_earth(2) + CONSTANTS_ONE_G); + // Perform filter calculation using backwards Euler integration + // Coefficients selected to place all three filter poles at omega + float omega2 = _hgtCompFiltOmega * _hgtCompFiltOmega; + float hgt_err = baro_altitude - _integ3_state; + float integ1_input = hgt_err * omega2 * _hgtCompFiltOmega; + _integ1_state = _integ1_state + integ1_input * DT; + float integ2_input = _integ1_state + hgt_ddot_mea + hgt_err * omega2 * 3.0f; + _integ2_state = _integ2_state + integ2_input * DT; + float integ3_input = _integ2_state + hgt_err * _hgtCompFiltOmega * 3.0f; + + // If more than 1 second has elapsed since last update then reset the integrator state + // to the measured height + if (DT > 1.0f) { + _integ3_state = baro_altitude; + + } else { + _integ3_state = _integ3_state + integ3_input * DT; + } + + // Update and average speed rate of change + // Only required if airspeed is being measured and controlled + float temp = 0; + + if (isfinite(airspeed) && airspeed_sensor_enabled()) { + // Get DCM + // Calculate speed rate of change + // XXX check + temp = rotMat(2, 0) * CONSTANTS_ONE_G + accel_body(0); + // take 5 point moving average + //_vel_dot = _vdot_filter.apply(temp); + // XXX resolve this properly + _vel_dot = 0.9f * _vel_dot + 0.1f * temp; + + } else { + _vel_dot = 0.0f; + } + +} + +void TECS::_update_speed(float airspeed_demand, float indicated_airspeed, + float indicated_airspeed_min, float indicated_airspeed_max, float EAS2TAS) +{ + // Calculate time in seconds since last update + uint64_t now = ecl_absolute_time(); + float DT = max((now - _update_speed_last_usec), 0ULL) * 1.0e-6f; + _update_speed_last_usec = now; + + // Convert equivalent airspeeds to true airspeeds + + _EAS_dem = airspeed_demand; + _TAS_dem = _EAS_dem * EAS2TAS; + _TASmax = indicated_airspeed_max * EAS2TAS; + _TASmin = indicated_airspeed_min * EAS2TAS; + + // Reset states of time since last update is too large + if (DT > 1.0f) { + _integ5_state = (_EAS * EAS2TAS); + _integ4_state = 0.0f; + DT = 0.1f; // when first starting TECS, use a + // small time constant + } + + // Get airspeed or default to halfway between min and max if + // airspeed is not being used and set speed rate to zero + if (!isfinite(indicated_airspeed) || !airspeed_sensor_enabled()) { + // If no airspeed available use average of min and max + _EAS = 0.5f * (indicated_airspeed_min + indicated_airspeed_max); + + } else { + _EAS = indicated_airspeed; + } + + // Implement a second order complementary filter to obtain a + // smoothed airspeed estimate + // airspeed estimate is held in _integ5_state + float aspdErr = (_EAS * EAS2TAS) - _integ5_state; + float integ4_input = aspdErr * _spdCompFiltOmega * _spdCompFiltOmega; + + // Prevent state from winding up + if (_integ5_state < 3.1f) { + integ4_input = max(integ4_input , 0.0f); + } + + _integ4_state = _integ4_state + integ4_input * DT; + float integ5_input = _integ4_state + _vel_dot + aspdErr * _spdCompFiltOmega * 1.4142f; + _integ5_state = _integ5_state + integ5_input * DT; + // limit the airspeed to a minimum of 3 m/s + _integ5_state = max(_integ5_state, 3.0f); + +} + +void TECS::_update_speed_demand(void) +{ + // Set the airspeed demand to the minimum value if an underspeed condition exists + // or a bad descent condition exists + // This will minimise the rate of descent resulting from an engine failure, + // enable the maximum climb rate to be achieved and prevent continued full power descent + // into the ground due to an unachievable airspeed value + if ((_badDescent) || (_underspeed)) { + _TAS_dem = _TASmin; + } + + // Constrain speed demand + _TAS_dem = constrain(_TAS_dem, _TASmin, _TASmax); + + // calculate velocity rate limits based on physical performance limits + // provision to use a different rate limit if bad descent or underspeed condition exists + // Use 50% of maximum energy rate to allow margin for total energy contgroller + float velRateMax; + float velRateMin; + + if ((_badDescent) || (_underspeed)) { + velRateMax = 0.5f * _STEdot_max / _integ5_state; + velRateMin = 0.5f * _STEdot_min / _integ5_state; + + } else { + velRateMax = 0.5f * _STEdot_max / _integ5_state; + velRateMin = 0.5f * _STEdot_min / _integ5_state; + } + + // Apply rate limit + if ((_TAS_dem - _TAS_dem_adj) > (velRateMax * 0.1f)) { + _TAS_dem_adj = _TAS_dem_adj + velRateMax * 0.1f; + _TAS_rate_dem = velRateMax; + + } else if ((_TAS_dem - _TAS_dem_adj) < (velRateMin * 0.1f)) { + _TAS_dem_adj = _TAS_dem_adj + velRateMin * 0.1f; + _TAS_rate_dem = velRateMin; + + } else { + _TAS_dem_adj = _TAS_dem; + _TAS_rate_dem = (_TAS_dem - _TAS_dem_last) / 0.1f; + } + + // Constrain speed demand again to protect against bad values on initialisation. + _TAS_dem_adj = constrain(_TAS_dem_adj, _TASmin, _TASmax); + _TAS_dem_last = _TAS_dem; +} + +void TECS::_update_height_demand(float demand) +{ + // Apply 2 point moving average to demanded height + // This is required because height demand is only updated at 5Hz + _hgt_dem = 0.5f * (demand + _hgt_dem_in_old); + _hgt_dem_in_old = _hgt_dem; + + // printf("hgt_dem: %6.2f hgt_dem_last: %6.2f max_climb_rate: %6.2f\n", _hgt_dem, _hgt_dem_prev, + // _maxClimbRate); + + // Limit height rate of change + if ((_hgt_dem - _hgt_dem_prev) > (_maxClimbRate * 0.1f)) { + _hgt_dem = _hgt_dem_prev + _maxClimbRate * 0.1f; + + } else if ((_hgt_dem - _hgt_dem_prev) < (-_maxSinkRate * 0.1f)) { + _hgt_dem = _hgt_dem_prev - _maxSinkRate * 0.1f; + } + + _hgt_dem_prev = _hgt_dem; + + // Apply first order lag to height demand + _hgt_dem_adj = 0.05f * _hgt_dem + 0.95f * _hgt_dem_adj_last; + _hgt_rate_dem = (_hgt_dem_adj - _hgt_dem_adj_last) / 0.1f; + _hgt_dem_adj_last = _hgt_dem_adj; + + // printf("hgt_dem: %6.2f hgt_dem_adj: %6.2f hgt_dem_last: %6.2f hgt_rate_dem: %6.2f\n", _hgt_dem, _hgt_dem_adj, _hgt_dem_adj_last, + // _hgt_rate_dem); +} + +void TECS::_detect_underspeed(void) +{ + if (((_integ5_state < _TASmin * 0.9f) && (_throttle_dem >= _THRmaxf * 0.95f)) || ((_integ3_state < _hgt_dem_adj) && _underspeed)) { + _underspeed = true; + + } else { + _underspeed = false; + } +} + +void TECS::_update_energies(void) +{ + // Calculate specific energy demands + _SPE_dem = _hgt_dem_adj * CONSTANTS_ONE_G; + _SKE_dem = 0.5f * _TAS_dem_adj * _TAS_dem_adj; + + // Calculate specific energy rate demands + _SPEdot_dem = _hgt_rate_dem * CONSTANTS_ONE_G; + _SKEdot_dem = _integ5_state * _TAS_rate_dem; + + // Calculate specific energy + _SPE_est = _integ3_state * CONSTANTS_ONE_G; + _SKE_est = 0.5f * _integ5_state * _integ5_state; + + // Calculate specific energy rate + _SPEdot = _integ2_state * CONSTANTS_ONE_G; + _SKEdot = _integ5_state * _vel_dot; +} + +void TECS::_update_throttle(float throttle_cruise, const math::Dcm &rotMat) +{ + // Calculate total energy values + _STE_error = _SPE_dem - _SPE_est + _SKE_dem - _SKE_est; + float STEdot_dem = constrain((_SPEdot_dem + _SKEdot_dem), _STEdot_min, _STEdot_max); + float STEdot_error = STEdot_dem - _SPEdot - _SKEdot; + + // Apply 0.5 second first order filter to STEdot_error + // This is required to remove accelerometer noise from the measurement + STEdot_error = 0.2f * STEdot_error + 0.8f * _STEdotErrLast; + _STEdotErrLast = STEdot_error; + + // Calculate throttle demand + // If underspeed condition is set, then demand full throttle + if (_underspeed) { + _throttle_dem_unc = 1.0f; + + } else { + // Calculate gain scaler from specific energy error to throttle + float K_STE2Thr = 1 / (_timeConst * (_STEdot_max - _STEdot_min)); + + // Calculate feed-forward throttle + float ff_throttle = 0; + float nomThr = throttle_cruise; + // Use the demanded rate of change of total energy as the feed-forward demand, but add + // additional component which scales with (1/cos(bank angle) - 1) to compensate for induced + // drag increase during turns. + float cosPhi = sqrtf((rotMat(0, 1) * rotMat(0, 1)) + (rotMat(1, 1) * rotMat(1, 1))); + STEdot_dem = STEdot_dem + _rollComp * (1.0f / constrain(cosPhi * cosPhi , 0.1f, 1.0f) - 1.0f); + + if (STEdot_dem >= 0) { + ff_throttle = nomThr + STEdot_dem / _STEdot_max * (1.0f - nomThr); + + } else { + ff_throttle = nomThr - STEdot_dem / _STEdot_min * nomThr; + } + + // Calculate PD + FF throttle + _throttle_dem = (_STE_error + STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle; + + // Rate limit PD + FF throttle + // Calculate the throttle increment from the specified slew time + if (fabsf(_throttle_slewrate) < 0.01f) { + float thrRateIncr = _DT * (_THRmaxf - _THRminf) * _throttle_slewrate; + + _throttle_dem = constrain(_throttle_dem, + _last_throttle_dem - thrRateIncr, + _last_throttle_dem + thrRateIncr); + _last_throttle_dem = _throttle_dem; + } + + + // Calculate integrator state upper and lower limits + // Set to a value thqat will allow 0.1 (10%) throttle saturation to allow for noise on the demand + float integ_max = (_THRmaxf - _throttle_dem + 0.1f); + float integ_min = (_THRminf - _throttle_dem - 0.1f); + + // Calculate integrator state, constraining state + // Set integrator to a max throttle value dduring climbout + _integ6_state = _integ6_state + (_STE_error * _integGain) * _DT * K_STE2Thr; + + if (_climbOutDem) { + _integ6_state = integ_max; + + } else { + _integ6_state = constrain(_integ6_state, integ_min, integ_max); + } + + // Sum the components. + // Only use feed-forward component if airspeed is not being used + if (airspeed_sensor_enabled()) { + _throttle_dem = _throttle_dem + _integ6_state; + + } else { + _throttle_dem = ff_throttle; + } + } + + // Constrain throttle demand + _throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf); +} + +void TECS::_detect_bad_descent(void) +{ + // Detect a demanded airspeed too high for the aircraft to achieve. This will be + // evident by the the following conditions: + // 1) Underspeed protection not active + // 2) Specific total energy error > 200 (greater than ~20m height error) + // 3) Specific total energy reducing + // 4) throttle demand > 90% + // If these four conditions exist simultaneously, then the protection + // mode will be activated. + // Once active, the following condition are required to stay in the mode + // 1) Underspeed protection not active + // 2) Specific total energy error > 0 + // This mode will produce an undulating speed and height response as it cuts in and out but will prevent the aircraft from descending into the ground if an unachievable speed demand is set + float STEdot = _SPEdot + _SKEdot; + + if ((!_underspeed && (_STE_error > 200.0f) && (STEdot < 0.0f) && (_throttle_dem >= _THRmaxf * 0.9f)) || (_badDescent && !_underspeed && (_STE_error > 0.0f))) { + _badDescent = true; + + } else { + _badDescent = false; + } +} + +void TECS::_update_pitch(void) +{ + // Calculate Speed/Height Control Weighting + // This is used to determine how the pitch control prioritises speed and height control + // A weighting of 1 provides equal priority (this is the normal mode of operation) + // A SKE_weighting of 0 provides 100% priority to height control. This is used when no airspeed measurement is available + // A SKE_weighting of 2 provides 100% priority to speed control. This is used when an underspeed condition is detected + // or during takeoff/climbout where a minimum pitch angle is set to ensure height is gained. In this instance, if airspeed + // rises above the demanded value, the pitch angle will be increased by the TECS controller. + float SKE_weighting = constrain(_spdWeight, 0.0f, 2.0f); + + if ((_underspeed || _climbOutDem) && airspeed_sensor_enabled()) { + SKE_weighting = 2.0f; + + } else if (!airspeed_sensor_enabled()) { + SKE_weighting = 0.0f; + } + + float SPE_weighting = 2.0f - SKE_weighting; + + // Calculate Specific Energy Balance demand, and error + float SEB_dem = _SPE_dem * SPE_weighting - _SKE_dem * SKE_weighting; + float SEBdot_dem = _SPEdot_dem * SPE_weighting - _SKEdot_dem * SKE_weighting; + float SEB_error = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting); + float SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * SKE_weighting); + + // Calculate integrator state, constraining input if pitch limits are exceeded + float integ7_input = SEB_error * _integGain; + + if (_pitch_dem_unc > _PITCHmaxf) { + integ7_input = min(integ7_input, _PITCHmaxf - _pitch_dem_unc); + + } else if (_pitch_dem_unc < _PITCHminf) { + integ7_input = max(integ7_input, _PITCHminf - _pitch_dem_unc); + } + + _integ7_state = _integ7_state + integ7_input * _DT; + + // Apply max and min values for integrator state that will allow for no more than + // 5deg of saturation. This allows for some pitch variation due to gusts before the + // integrator is clipped. Otherwise the effectiveness of the integrator will be reduced in turbulence + float gainInv = (_integ5_state * _timeConst * CONSTANTS_ONE_G); + float temp = SEB_error + SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst; + _integ7_state = constrain(_integ7_state, (gainInv * (_PITCHminf - 0.0783f)) - temp, (gainInv * (_PITCHmaxf + 0.0783f)) - temp); + + // Calculate pitch demand from specific energy balance signals + _pitch_dem_unc = (temp + _integ7_state) / gainInv; + + // Constrain pitch demand + _pitch_dem = constrain(_pitch_dem_unc, _PITCHminf, _PITCHmaxf); + + // Rate limit the pitch demand to comply with specified vertical + // acceleration limit + float ptchRateIncr = _DT * _vertAccLim / _integ5_state; + + if ((_pitch_dem - _last_pitch_dem) > ptchRateIncr) { + _pitch_dem = _last_pitch_dem + ptchRateIncr; + + } else if ((_pitch_dem - _last_pitch_dem) < -ptchRateIncr) { + _pitch_dem = _last_pitch_dem - ptchRateIncr; + } + + _last_pitch_dem = _pitch_dem; +} + +void TECS::_initialise_states(float pitch, float throttle_cruise, float baro_altitude, float ptchMinCO_rad) +{ + // Initialise states and variables if DT > 1 second or in climbout + if (_DT > 1.0f) { + _integ6_state = 0.0f; + _integ7_state = 0.0f; + _last_throttle_dem = throttle_cruise; + _last_pitch_dem = pitch; + _hgt_dem_adj_last = baro_altitude; + _hgt_dem_adj = _hgt_dem_adj_last; + _hgt_dem_prev = _hgt_dem_adj_last; + _hgt_dem_in_old = _hgt_dem_adj_last; + _TAS_dem_last = _TAS_dem; + _TAS_dem_adj = _TAS_dem; + _underspeed = false; + _badDescent = false; + _DT = 0.1f; // when first starting TECS, use a + // small time constant + + } else if (_climbOutDem) { + _PITCHminf = ptchMinCO_rad; + _THRminf = _THRmaxf - 0.01f; + _hgt_dem_adj_last = baro_altitude; + _hgt_dem_adj = _hgt_dem_adj_last; + _hgt_dem_prev = _hgt_dem_adj_last; + _TAS_dem_last = _TAS_dem; + _TAS_dem_adj = _TAS_dem; + _underspeed = false; + _badDescent = false; + } +} + +void TECS::_update_STE_rate_lim(void) +{ + // Calculate Specific Total Energy Rate Limits + // This is a tivial calculation at the moment but will get bigger once we start adding altitude effects + _STEdot_max = _maxClimbRate * CONSTANTS_ONE_G; + _STEdot_min = - _minSinkRate * CONSTANTS_ONE_G; +} + +void TECS::update_pitch_throttle(const math::Dcm &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO, + float throttle_min, float throttle_max, float throttle_cruise, + float pitch_limit_min, float pitch_limit_max) +{ + // Calculate time in seconds since last update + uint64_t now = ecl_absolute_time(); + _DT = max((now - _update_pitch_throttle_last_usec), 0ULL) * 1.0e-6f; + _update_pitch_throttle_last_usec = now; + + // printf("tecs in: dt:%10.6f pitch: %6.2f baro_alt: %6.2f alt sp: %6.2f\neas sp: %6.2f eas: %6.2f, eas2tas: %6.2f\n %s pitch min C0: %6.2f thr min: %6.2f, thr max: %6.2f thr cruis: %6.2f pt min: %6.2f, pt max: %6.2f\n", + // _DT, pitch, baro_altitude, hgt_dem, EAS_dem, indicated_airspeed, EAS2TAS, (climbOutDem) ? "climb" : "level", ptchMinCO, throttle_min, throttle_max, throttle_cruise, pitch_limit_min, pitch_limit_max); + + // Update the speed estimate using a 2nd order complementary filter + _update_speed(EAS_dem, indicated_airspeed, _indicated_airspeed_min, _indicated_airspeed_max, EAS2TAS); + + // Convert inputs + _THRmaxf = throttle_max; + _THRminf = throttle_min; + _PITCHmaxf = pitch_limit_max; + _PITCHminf = pitch_limit_min; + _climbOutDem = climbOutDem; + + // initialise selected states and variables if DT > 1 second or in climbout + _initialise_states(pitch, throttle_cruise, baro_altitude, ptchMinCO); + + // Calculate Specific Total Energy Rate Limits + _update_STE_rate_lim(); + + // Calculate the speed demand + _update_speed_demand(); + + // Calculate the height demand + _update_height_demand(hgt_dem); + + // Detect underspeed condition + _detect_underspeed(); + + // Calculate specific energy quantitiues + _update_energies(); + + // Calculate throttle demand + _update_throttle(throttle_cruise, rotMat); + + // Detect bad descent due to demanded airspeed being too high + _detect_bad_descent(); + + // Calculate pitch demand + _update_pitch(); + +// // Write internal variables to the log_tuning structure. This +// // structure will be logged in dataflash at 10Hz + // log_tuning.hgt_dem = _hgt_dem_adj; + // log_tuning.hgt = _integ3_state; + // log_tuning.dhgt_dem = _hgt_rate_dem; + // log_tuning.dhgt = _integ2_state; + // log_tuning.spd_dem = _TAS_dem_adj; + // log_tuning.spd = _integ5_state; + // log_tuning.dspd = _vel_dot; + // log_tuning.ithr = _integ6_state; + // log_tuning.iptch = _integ7_state; + // log_tuning.thr = _throttle_dem; + // log_tuning.ptch = _pitch_dem; + // log_tuning.dspd_dem = _TAS_rate_dem; +} diff --git a/src/lib/external_lgpl/tecs/TECS.h b/src/lib/external_lgpl/tecs/TECS.h new file mode 100644 index 0000000000..3732156986 --- /dev/null +++ b/src/lib/external_lgpl/tecs/TECS.h @@ -0,0 +1,350 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +/// @file TECS.h +/// @brief Combined Total Energy Speed & Height Control. + +/* + * Written by Paul Riseborough 2013 to provide: + * - Combined control of speed and height using throttle to control + * total energy and pitch angle to control exchange of energy between + * potential and kinetic. + * Selectable speed or height priority modes when calculating pitch angle + * - Fallback mode when no airspeed measurement is available that + * sets throttle based on height rate demand and switches pitch angle control to + * height priority + * - Underspeed protection that demands maximum throttle switches pitch angle control + * to speed priority mode + * - Relative ease of tuning through use of intuitive time constant, trim rate and damping parameters and the use + * of easy to measure aircraft performance data + */ + +#ifndef TECS_H +#define TECS_H + +#include +#include + +class __EXPORT TECS +{ +public: + TECS() : + + _airspeed_enabled(false), + _throttle_slewrate(0.0f), + _climbOutDem(false), + _hgt_dem_prev(0.0f), + _hgt_dem_adj_last(0.0f), + _hgt_dem_in_old(0.0f), + _TAS_dem_last(0.0f), + _TAS_dem_adj(0.0f), + _TAS_dem(0.0f), + _integ1_state(0.0f), + _integ2_state(0.0f), + _integ3_state(0.0f), + _integ4_state(0.0f), + _integ5_state(0.0f), + _integ6_state(0.0f), + _integ7_state(0.0f), + _pitch_dem(0.0f), + _last_pitch_dem(0.0f), + _SPE_dem(0.0f), + _SKE_dem(0.0f), + _SPEdot_dem(0.0f), + _SKEdot_dem(0.0f), + _SPE_est(0.0f), + _SKE_est(0.0f), + _SPEdot(0.0f), + _SKEdot(0.0f) { + + } + + bool airspeed_sensor_enabled() { + return _airspeed_enabled; + } + + void enable_airspeed(bool enabled) { + _airspeed_enabled = enabled; + } + + // Update of the estimated height and height rate internal state + // Update of the inertial speed rate internal state + // Should be called at 50Hz or greater + void update_50hz(float baro_altitude, float airspeed, const math::Dcm &rotMat, const math::Vector3 &accel_body, const math::Vector3 &accel_earth); + + // Update the control loop calculations + void update_pitch_throttle(const math::Dcm &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO, + float throttle_min, float throttle_max, float throttle_cruise, + float pitch_limit_min, float pitch_limit_max); + // demanded throttle in percentage + // should return 0 to 100 + float get_throttle_demand(void) { + return _throttle_dem; + } + int32_t get_throttle_demand_percent(void) { + return get_throttle_demand(); + } + + + float get_pitch_demand() { return _pitch_dem; } + + // demanded pitch angle in centi-degrees + // should return between -9000 to +9000 + int32_t get_pitch_demand_cd() { return int32_t(get_pitch_demand() * 5729.5781f);} + + // Rate of change of velocity along X body axis in m/s^2 + float get_VXdot(void) { return _vel_dot; } + + // log data on internal state of the controller. Called at 10Hz + // void log_data(DataFlash_Class &dataflash, uint8_t msgid); + + // struct PACKED log_TECS_Tuning { + // LOG_PACKET_HEADER; + // float hgt; + // float dhgt; + // float hgt_dem; + // float dhgt_dem; + // float spd_dem; + // float spd; + // float dspd; + // float ithr; + // float iptch; + // float thr; + // float ptch; + // float dspd_dem; + // } log_tuning; + + void set_time_const(float time_const) { + _timeConst = time_const; + } + + void set_min_sink_rate(float rate) { + _minSinkRate = rate; + } + + void set_max_sink_rate(float sink_rate) { + _maxSinkRate = sink_rate; + } + + void set_max_climb_rate(float climb_rate) { + _maxClimbRate = climb_rate; + } + + void set_throttle_damp(float throttle_damp) { + _thrDamp = throttle_damp; + } + + void set_integrator_gain(float gain) { + _integGain = gain; + } + + void set_vertical_accel_limit(float limit) { + _vertAccLim = limit; + } + + void set_height_comp_filter_omega(float omega) { + _hgtCompFiltOmega = omega; + } + + void set_speed_comp_filter_omega(float omega) { + _spdCompFiltOmega = omega; + } + + void set_roll_throttle_compensation(float compensation) { + _rollComp = compensation; + } + + void set_speed_weight(float weight) { + _spdWeight = weight; + } + + void set_pitch_damping(float damping) { + _ptchDamp = damping; + } + + void set_throttle_slewrate(float slewrate) { + _throttle_slewrate = slewrate; + } + + void set_indicated_airspeed_min(float airspeed) { + _indicated_airspeed_min = airspeed; + } + + void set_indicated_airspeed_max(float airspeed) { + _indicated_airspeed_max = airspeed; + } + +private: + // Last time update_50Hz was called + uint64_t _update_50hz_last_usec; + + // Last time update_speed was called + uint64_t _update_speed_last_usec; + + // Last time update_pitch_throttle was called + uint64_t _update_pitch_throttle_last_usec; + + // TECS tuning parameters + float _hgtCompFiltOmega; + float _spdCompFiltOmega; + float _maxClimbRate; + float _minSinkRate; + float _maxSinkRate; + float _timeConst; + float _ptchDamp; + float _thrDamp; + float _integGain; + float _vertAccLim; + float _rollComp; + float _spdWeight; + + // throttle demand in the range from 0.0 to 1.0 + float _throttle_dem; + + // pitch angle demand in radians + float _pitch_dem; + + // Integrator state 1 - height filter second derivative + float _integ1_state; + + // Integrator state 2 - height rate + float _integ2_state; + + // Integrator state 3 - height + float _integ3_state; + + // Integrator state 4 - airspeed filter first derivative + float _integ4_state; + + // Integrator state 5 - true airspeed + float _integ5_state; + + // Integrator state 6 - throttle integrator + float _integ6_state; + + // Integrator state 6 - pitch integrator + float _integ7_state; + + // throttle demand rate limiter state + float _last_throttle_dem; + + // pitch demand rate limiter state + float _last_pitch_dem; + + // Rate of change of speed along X axis + float _vel_dot; + + // Equivalent airspeed + float _EAS; + + // True airspeed limits + float _TASmax; + float _TASmin; + + // Current and last true airspeed demand + float _TAS_dem; + float _TAS_dem_last; + + // Equivalent airspeed demand + float _EAS_dem; + + // height demands + float _hgt_dem; + float _hgt_dem_in_old; + float _hgt_dem_adj; + float _hgt_dem_adj_last; + float _hgt_rate_dem; + float _hgt_dem_prev; + + // Speed demand after application of rate limiting + // This is the demand tracked by the TECS control loops + float _TAS_dem_adj; + + // Speed rate demand after application of rate limiting + // This is the demand tracked by the TECS control loops + float _TAS_rate_dem; + + // Total energy rate filter state + float _STEdotErrLast; + + // Underspeed condition + bool _underspeed; + + // Bad descent condition caused by unachievable airspeed demand + bool _badDescent; + + // climbout mode + bool _climbOutDem; + + // throttle demand before limiting + float _throttle_dem_unc; + + // pitch demand before limiting + float _pitch_dem_unc; + + // Maximum and minimum specific total energy rate limits + float _STEdot_max; + float _STEdot_min; + + // Maximum and minimum floating point throttle limits + float _THRmaxf; + float _THRminf; + + // Maximum and minimum floating point pitch limits + float _PITCHmaxf; + float _PITCHminf; + + // Specific energy quantities + float _SPE_dem; + float _SKE_dem; + float _SPEdot_dem; + float _SKEdot_dem; + float _SPE_est; + float _SKE_est; + float _SPEdot; + float _SKEdot; + + // Specific energy error quantities + float _STE_error; + + // Time since last update of main TECS loop (seconds) + float _DT; + + bool _airspeed_enabled; + float _throttle_slewrate; + float _indicated_airspeed_min; + float _indicated_airspeed_max; + + // Update the airspeed internal state using a second order complementary filter + void _update_speed(float airspeed_demand, float indicated_airspeed, + float indicated_airspeed_min, float indicated_airspeed_max, float EAS2TAS); + + // Update the demanded airspeed + void _update_speed_demand(void); + + // Update the demanded height + void _update_height_demand(float demand); + + // Detect an underspeed condition + void _detect_underspeed(void); + + // Update Specific Energy Quantities + void _update_energies(void); + + // Update Demanded Throttle + void _update_throttle(float throttle_cruise, const math::Dcm &rotMat); + + // Detect Bad Descent + void _detect_bad_descent(void); + + // Update Demanded Pitch Angle + void _update_pitch(void); + + // Initialise states and variables + void _initialise_states(float pitch, float throttle_cruise, float baro_altitude, float ptchMinCO_rad); + + // Calculate specific total energy rate limits + void _update_STE_rate_lim(void); + +}; + +#endif //TECS_H diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp new file mode 100644 index 0000000000..7671cae722 --- /dev/null +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -0,0 +1,1044 @@ +/**************************************************************************** + * + * 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 fw_pos_control_l1_main.c + * Implementation of a generic position controller based on the L1 norm. Outputs a bank / roll + * angle, equivalent to a lateral motion (for copters and rovers). + * + * Original publication for horizontal control: + * S. Park, J. Deyst, and J. P. How, "A New Nonlinear Guidance Logic for Trajectory Tracking," + * Proceedings of the AIAA Guidance, Navigation and Control + * Conference, Aug 2004. AIAA-2004-4900. + * + * Original implementation for total energy control: + * Paul Riseborough and Andrew Tridgell, 2013 (code in lib/external_lgpl) + * + * @author Lorenz Meier + */ + +#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 +#include +#include +#include +#include + +#include +#include + +/** + * L1 control app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int fw_pos_control_l1_main(int argc, char *argv[]); + +class FixedwingPositionControl +{ +public: + /** + * Constructor + */ + FixedwingPositionControl(); + + /** + * Destructor, also kills the sensors task. + */ + ~FixedwingPositionControl(); + + /** + * Start the sensors task. + * + * @return OK on success. + */ + int start(); + +private: + + bool _task_should_exit; /**< if true, sensor task should exit */ + int _control_task; /**< task handle for sensor task */ + + int _global_pos_sub; + int _global_set_triplet_sub; + int _att_sub; /**< vehicle attitude subscription */ + int _attitude_sub; /**< raw rc channels data subscription */ + int _airspeed_sub; /**< airspeed subscription */ + int _control_mode_sub; /**< vehicle status subscription */ + int _params_sub; /**< notification of parameter updates */ + int _manual_control_sub; /**< notification of manual control updates */ + int _accel_sub; /**< body frame accelerations */ + + orb_advert_t _attitude_sp_pub; /**< attitude setpoint */ + + struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ + struct manual_control_setpoint_s _manual; /**< r/c channel data */ + struct airspeed_s _airspeed; /**< airspeed */ + struct vehicle_control_mode_s _control_mode; /**< vehicle status */ + struct vehicle_global_position_s _global_pos; /**< global vehicle position */ + struct vehicle_global_position_set_triplet_s _global_triplet; /**< triplet of global setpoints */ + struct accel_report _accel; /**< body frame accelerations */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + bool _setpoint_valid; /**< flag if the position control setpoint is valid */ + + /** manual control states */ + float _seatbelt_hold_heading; /**< heading the system should hold in seatbelt mode */ + float _loiter_hold_lat; + float _loiter_hold_lon; + float _loiter_hold_alt; + bool _loiter_hold; + + float _launch_lat; + float _launch_lon; + float _launch_alt; + bool _launch_valid; + + /* throttle and airspeed states */ + float _airspeed_error; ///< airspeed error to setpoint in m/s + bool _airspeed_valid; ///< flag if a valid airspeed estimate exists + uint64_t _airspeed_last_valid; ///< last time airspeed was valid. Used to detect sensor failures + float _groundspeed_undershoot; ///< ground speed error to min. speed in m/s + bool _global_pos_valid; ///< global position is valid + math::Dcm _R_nb; ///< current attitude + + ECL_L1_Pos_Control _l1_control; + TECS _tecs; + + struct { + float l1_period; + float l1_damping; + + float time_const; + float min_sink_rate; + float max_sink_rate; + float max_climb_rate; + float throttle_damp; + float integrator_gain; + float vertical_accel_limit; + float height_comp_filter_omega; + float speed_comp_filter_omega; + float roll_throttle_compensation; + float speed_weight; + float pitch_damping; + + float airspeed_min; + float airspeed_trim; + float airspeed_max; + + float pitch_limit_min; + float pitch_limit_max; + float throttle_min; + float throttle_max; + float throttle_cruise; + + float loiter_hold_radius; + } _parameters; /**< local copies of interesting parameters */ + + struct { + + param_t l1_period; + param_t l1_damping; + + param_t time_const; + param_t min_sink_rate; + param_t max_sink_rate; + param_t max_climb_rate; + param_t throttle_damp; + param_t integrator_gain; + param_t vertical_accel_limit; + param_t height_comp_filter_omega; + param_t speed_comp_filter_omega; + param_t roll_throttle_compensation; + param_t speed_weight; + param_t pitch_damping; + + param_t airspeed_min; + param_t airspeed_trim; + param_t airspeed_max; + + param_t pitch_limit_min; + param_t pitch_limit_max; + param_t throttle_min; + param_t throttle_max; + param_t throttle_cruise; + + param_t loiter_hold_radius; + } _parameter_handles; /**< handles for interesting parameters */ + + + /** + * Update our local parameter cache. + */ + int parameters_update(); + + /** + * Update control outputs + * + */ + void control_update(); + + /** + * Check for changes in vehicle status. + */ + void vehicle_control_mode_poll(); + + /** + * Check for airspeed updates. + */ + bool vehicle_airspeed_poll(); + + /** + * Check for position updates. + */ + void vehicle_attitude_poll(); + + /** + * Check for accel updates. + */ + void vehicle_accel_poll(); + + /** + * Check for set triplet updates. + */ + void vehicle_setpoint_poll(); + + /** + * Control position. + */ + bool control_position(const math::Vector2f &global_pos, const math::Vector2f &ground_speed, + const struct vehicle_global_position_set_triplet_s &global_triplet); + + float calculate_target_airspeed(float airspeed_demand); + void calculate_gndspeed_undershoot(); + + /** + * Shim for calling task_main from task_create. + */ + static void task_main_trampoline(int argc, char *argv[]); + + /** + * Main sensor collection task. + */ + void task_main() __attribute__((noreturn)); +}; + +namespace l1_control +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +FixedwingPositionControl *g_control; +} + +FixedwingPositionControl::FixedwingPositionControl() : + + _task_should_exit(false), + _control_task(-1), + +/* subscriptions */ + _global_pos_sub(-1), + _global_set_triplet_sub(-1), + _att_sub(-1), + _airspeed_sub(-1), + _control_mode_sub(-1), + _params_sub(-1), + _manual_control_sub(-1), + +/* publications */ + _attitude_sp_pub(-1), + +/* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")), +/* states */ + _setpoint_valid(false), + _loiter_hold(false), + _airspeed_error(0.0f), + _airspeed_valid(false), + _groundspeed_undershoot(0.0f), + _global_pos_valid(false) +{ + _parameter_handles.l1_period = param_find("FW_L1_PERIOD"); + _parameter_handles.l1_damping = param_find("FW_L1_DAMPING"); + _parameter_handles.loiter_hold_radius = param_find("FW_LOITER_R"); + + _parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN"); + _parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM"); + _parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX"); + + _parameter_handles.pitch_limit_min = param_find("FW_P_LIM_MIN"); + _parameter_handles.pitch_limit_max = param_find("FW_P_LIM_MAX"); + _parameter_handles.throttle_min = param_find("FW_THR_MIN"); + _parameter_handles.throttle_max = param_find("FW_THR_MAX"); + _parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE"); + + _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); + _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); + _parameter_handles.max_sink_rate = param_find("FW_T_SINK_MAX"); + _parameter_handles.max_climb_rate = param_find("FW_T_CLMB_MAX"); + _parameter_handles.throttle_damp = param_find("FW_T_THR_DAMP"); + _parameter_handles.integrator_gain = param_find("FW_T_INTEG_GAIN"); + _parameter_handles.vertical_accel_limit = param_find("FW_T_VERT_ACC"); + _parameter_handles.height_comp_filter_omega = param_find("FW_T_HGT_OMEGA"); + _parameter_handles.speed_comp_filter_omega = param_find("FW_T_SPD_OMEGA"); + _parameter_handles.roll_throttle_compensation = param_find("FW_T_RLL2THR"); + _parameter_handles.speed_weight = param_find("FW_T_SPDWEIGHT"); + _parameter_handles.pitch_damping = param_find("FW_T_PTCH_DAMP"); + + /* fetch initial parameter values */ + parameters_update(); +} + +FixedwingPositionControl::~FixedwingPositionControl() +{ + if (_control_task != -1) { + + /* task wakes up every 100ms or so at the longest */ + _task_should_exit = true; + + /* wait for a second for the task to quit at our request */ + unsigned i = 0; + + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + task_delete(_control_task); + break; + } + } while (_control_task != -1); + } + + l1_control::g_control = nullptr; +} + +int +FixedwingPositionControl::parameters_update() +{ + + /* L1 control parameters */ + param_get(_parameter_handles.l1_damping, &(_parameters.l1_damping)); + param_get(_parameter_handles.l1_period, &(_parameters.l1_period)); + param_get(_parameter_handles.loiter_hold_radius, &(_parameters.loiter_hold_radius)); + + param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min)); + param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim)); + param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max)); + + param_get(_parameter_handles.pitch_limit_min, &(_parameters.pitch_limit_min)); + param_get(_parameter_handles.pitch_limit_max, &(_parameters.pitch_limit_max)); + param_get(_parameter_handles.throttle_min, &(_parameters.throttle_min)); + param_get(_parameter_handles.throttle_max, &(_parameters.throttle_max)); + param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise)); + + param_get(_parameter_handles.time_const, &(_parameters.time_const)); + param_get(_parameter_handles.min_sink_rate, &(_parameters.min_sink_rate)); + param_get(_parameter_handles.max_sink_rate, &(_parameters.max_sink_rate)); + param_get(_parameter_handles.throttle_damp, &(_parameters.throttle_damp)); + param_get(_parameter_handles.integrator_gain, &(_parameters.integrator_gain)); + param_get(_parameter_handles.vertical_accel_limit, &(_parameters.vertical_accel_limit)); + param_get(_parameter_handles.height_comp_filter_omega, &(_parameters.height_comp_filter_omega)); + param_get(_parameter_handles.speed_comp_filter_omega, &(_parameters.speed_comp_filter_omega)); + param_get(_parameter_handles.roll_throttle_compensation, &(_parameters.roll_throttle_compensation)); + param_get(_parameter_handles.speed_weight, &(_parameters.speed_weight)); + param_get(_parameter_handles.pitch_damping, &(_parameters.pitch_damping)); + param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate)); + + _l1_control.set_l1_damping(_parameters.l1_damping); + _l1_control.set_l1_period(_parameters.l1_period); + + _tecs.set_time_const(_parameters.time_const); + _tecs.set_min_sink_rate(_parameters.min_sink_rate); + _tecs.set_max_sink_rate(_parameters.max_sink_rate); + _tecs.set_throttle_damp(_parameters.throttle_damp); + _tecs.set_integrator_gain(_parameters.integrator_gain); + _tecs.set_vertical_accel_limit(_parameters.vertical_accel_limit); + _tecs.set_height_comp_filter_omega(_parameters.height_comp_filter_omega); + _tecs.set_speed_comp_filter_omega(_parameters.speed_comp_filter_omega); + _tecs.set_roll_throttle_compensation(math::radians(_parameters.roll_throttle_compensation)); + _tecs.set_speed_weight(_parameters.speed_weight); + _tecs.set_pitch_damping(_parameters.pitch_damping); + _tecs.set_indicated_airspeed_min(_parameters.airspeed_min); + _tecs.set_indicated_airspeed_max(_parameters.airspeed_min); + _tecs.set_max_climb_rate(_parameters.max_climb_rate); + + /* sanity check parameters */ + if (_parameters.airspeed_max < _parameters.airspeed_min || + _parameters.airspeed_max < 5.0f || + _parameters.airspeed_min > 100.0f || + _parameters.airspeed_trim < _parameters.airspeed_min || + _parameters.airspeed_trim > _parameters.airspeed_max) { + warnx("error: airspeed parameters invalid"); + return 1; + } + + return OK; +} + +void +FixedwingPositionControl::vehicle_control_mode_poll() +{ + bool vstatus_updated; + + /* Check HIL state if vehicle status has changed */ + orb_check(_control_mode_sub, &vstatus_updated); + + if (vstatus_updated) { + + bool was_armed = _control_mode.flag_armed; + + orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); + + if (!was_armed && _control_mode.flag_armed) { + _launch_lat = _global_pos.lat / 1e7f; + _launch_lon = _global_pos.lon / 1e7f; + _launch_alt = _global_pos.alt; + _launch_valid = true; + } + } +} + +bool +FixedwingPositionControl::vehicle_airspeed_poll() +{ + /* check if there is an airspeed update or if it timed out */ + bool airspeed_updated; + orb_check(_airspeed_sub, &airspeed_updated); + + if (airspeed_updated) { + orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); + _airspeed_valid = true; + _airspeed_last_valid = hrt_absolute_time(); + return true; + + } else { + + /* no airspeed updates for one second */ + if (_airspeed_valid && (hrt_absolute_time() - _airspeed_last_valid) > 1e6) { + _airspeed_valid = false; + } + } + + /* update TECS state */ + _tecs.enable_airspeed(_airspeed_valid); + + return false; +} + +void +FixedwingPositionControl::vehicle_attitude_poll() +{ + /* check if there is a new position */ + bool att_updated; + orb_check(_att_sub, &att_updated); + + if (att_updated) { + orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); + + /* set rotation matrix */ + for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) + _R_nb(i, j) = _att.R[i][j]; + } +} + +void +FixedwingPositionControl::vehicle_accel_poll() +{ + /* check if there is a new position */ + bool accel_updated; + orb_check(_accel_sub, &accel_updated); + + if (accel_updated) { + orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); + } +} + +void +FixedwingPositionControl::vehicle_setpoint_poll() +{ + /* check if there is a new setpoint */ + bool global_sp_updated; + orb_check(_global_set_triplet_sub, &global_sp_updated); + + if (global_sp_updated) { + orb_copy(ORB_ID(vehicle_global_position_set_triplet), _global_set_triplet_sub, &_global_triplet); + _setpoint_valid = true; + } +} + +void +FixedwingPositionControl::task_main_trampoline(int argc, char *argv[]) +{ + l1_control::g_control->task_main(); +} + +float +FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand) +{ + float airspeed; + + if (_airspeed_valid) { + airspeed = _airspeed.true_airspeed_m_s; + + } else { + airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) / 2.0f; + } + + /* cruise airspeed for all modes unless modified below */ + float target_airspeed = airspeed_demand; + + /* add minimum ground speed undershoot (only non-zero in presence of sufficient wind) */ + target_airspeed += _groundspeed_undershoot; + + if (0/* throttle nudging enabled */) { + //target_airspeed += nudge term. + } + + /* sanity check: limit to range */ + target_airspeed = math::constrain(target_airspeed, _parameters.airspeed_min, _parameters.airspeed_max); + + /* plain airspeed error */ + _airspeed_error = target_airspeed - airspeed; + + return target_airspeed; +} + +void +FixedwingPositionControl::calculate_gndspeed_undershoot() +{ + + if (_global_pos_valid) { + /* get ground speed vector */ + math::Vector2f ground_speed_vector(_global_pos.vx, _global_pos.vy); + + /* rotate with current attitude */ + math::Vector2f yaw_vector(_R_nb(0, 0), _R_nb(1, 0)); + yaw_vector.normalize(); + float ground_speed_body = yaw_vector * ground_speed_vector; + + /* + * Ground speed undershoot is the amount of ground velocity not reached + * by the plane. Consequently it is zero if airspeed is >= min ground speed + * and positive if airspeed < min ground speed. + * + * This error value ensures that a plane (as long as its throttle capability is + * not exceeded) travels towards a waypoint (and is not pushed more and more away + * by wind). Not countering this would lead to a fly-away. + */ + _groundspeed_undershoot = math::max(_parameters.airspeed_min - ground_speed_body, 0.0f); + + } else { + _groundspeed_undershoot = 0; + } +} + +bool +FixedwingPositionControl::control_position(const math::Vector2f ¤t_position, const math::Vector2f &ground_speed, + const struct vehicle_global_position_set_triplet_s &global_triplet) +{ + bool setpoint = true; + + calculate_gndspeed_undershoot(); + + float eas2tas = 1.0f; // XXX calculate actual number based on current measurements + + // XXX re-visit + float baro_altitude = _global_pos.alt; + + /* filter speed and altitude for controller */ + math::Vector3 accel_body(_accel.x, _accel.y, _accel.z); + math::Vector3 accel_earth = _R_nb.transpose() * accel_body; + + _tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); + float altitude_error = _global_triplet.current.altitude - _global_pos.alt; + + /* AUTONOMOUS FLIGHT */ + + // XXX this should only execute if auto AND safety off (actuators active), + // else integrators should be constantly reset. + if (_control_mode.flag_control_position_enabled) { + + /* execute navigation once we have a setpoint */ + if (_setpoint_valid) { + + float altitude_error = _global_triplet.current.altitude - _global_pos.alt; + + /* current waypoint (the one currently heading for) */ + math::Vector2f next_wp(global_triplet.current.lat / 1e7f, global_triplet.current.lon / 1e7f); + + /* previous waypoint */ + math::Vector2f prev_wp; + + if (global_triplet.previous_valid) { + prev_wp.setX(global_triplet.previous.lat / 1e7f); + prev_wp.setY(global_triplet.previous.lon / 1e7f); + + } else { + /* + * No valid next waypoint, go for heading hold. + * This is automatically handled by the L1 library. + */ + prev_wp.setX(global_triplet.current.lat / 1e7f); + prev_wp.setY(global_triplet.current.lon / 1e7f); + + } + + // XXX add RTL switch + if (global_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH && _launch_valid) { + + math::Vector2f rtl_pos(_launch_lat, _launch_lon); + + _l1_control.navigate_waypoints(rtl_pos, rtl_pos, current_position, ground_speed); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _launch_alt, calculate_target_airspeed(_parameters.airspeed_trim), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, math::radians(_parameters.pitch_limit_min), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + + // XXX handle case when having arrived at home (loiter) + + } else if (global_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) { + /* waypoint is a plain navigation waypoint */ + _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, math::radians(_parameters.pitch_limit_min), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + + } else if (global_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + global_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + global_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { + + /* waypoint is a loiter waypoint */ + _l1_control.navigate_loiter(next_wp, current_position, global_triplet.current.loiter_radius, + global_triplet.current.loiter_direction, ground_speed); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, math::radians(_parameters.pitch_limit_min), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + + } else if (global_triplet.current.nav_cmd == NAV_CMD_LAND) { + + _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + + /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ + // XXX this could make a great param + if (altitude_error > -20.0f) { + + float flare_angle_rad = math::radians(15.0f);//math::radians(global_triplet.current.param1) + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min), + _airspeed.indicated_airspeed_m_s, eas2tas, + true, flare_angle_rad, + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + + /* limit roll motion to prevent wings from touching the ground first */ + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-20.0f), math::radians(20.0f)); + + } else { + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, math::radians(_parameters.pitch_limit_min), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + } + + } else if (global_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { + + _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + + /* apply minimum pitch and limit roll if target altitude is not within 10 meters */ + if (altitude_error > 10.0f) { + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min), + _airspeed.indicated_airspeed_m_s, eas2tas, + true, math::radians(global_triplet.current.param1), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + + /* limit roll motion to ensure enough lift */ + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f)); + + } else { + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, math::radians(_parameters.pitch_limit_min), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + } + } + + // warnx("nav bearing: %8.4f bearing err: %8.4f target bearing: %8.4f", (double)_l1_control.nav_bearing(), + // (double)_l1_control.bearing_error(), (double)_l1_control.target_bearing()); + // warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp.getX(), (double)prev_wp.getY(), + // (double)next_wp.getX(), (double)next_wp.getY(), (global_triplet.previous_valid) ? "valid" : "invalid"); + + // XXX at this point we always want no loiter hold if a + // mission is active + _loiter_hold = false; + + } else if (_control_mode.flag_armed) { + + /* hold position, but only if armed, climb 20m in case this is engaged on ground level */ + + // XXX rework with smarter state machine + + if (!_loiter_hold) { + _loiter_hold_lat = _global_pos.lat / 1e7f; + _loiter_hold_lon = _global_pos.lon / 1e7f; + _loiter_hold_alt = _global_pos.alt + 25.0f; + _loiter_hold = true; + } + + float altitude_error = _loiter_hold_alt - _global_pos.alt; + + math::Vector2f loiter_hold_pos(_loiter_hold_lat, _loiter_hold_lon); + + /* loiter around current position */ + _l1_control.navigate_loiter(loiter_hold_pos, current_position, _parameters.loiter_hold_radius, + 1, ground_speed); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + + /* climb with full throttle if the altitude error is bigger than 5 meters */ + bool climb_out = (altitude_error > 5); + + float min_pitch; + + if (climb_out) { + min_pitch = math::radians(20.0f); + + } else { + min_pitch = math::radians(_parameters.pitch_limit_min); + } + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _loiter_hold_alt, calculate_target_airspeed(_parameters.airspeed_trim), + _airspeed.indicated_airspeed_m_s, eas2tas, + climb_out, min_pitch, + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + + if (climb_out) { + /* limit roll motion to ensure enough lift */ + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f)); + } + } + + } else if (0/* seatbelt mode enabled */) { + + /** SEATBELT FLIGHT **/ + + if (0/* switched from another mode to seatbelt */) { + _seatbelt_hold_heading = _att.yaw; + } + + if (0/* seatbelt on and manual control yaw non-zero */) { + _seatbelt_hold_heading = _att.yaw + _manual.yaw; + } + + /* if in seatbelt mode, set airspeed based on manual control */ + + // XXX check if ground speed undershoot should be applied here + float seatbelt_airspeed = _parameters.airspeed_min + + (_parameters.airspeed_max - _parameters.airspeed_min) * + _manual.throttle; + + _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, + seatbelt_airspeed, + _airspeed.indicated_airspeed_m_s, eas2tas, + false, _parameters.pitch_limit_min, + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + _parameters.pitch_limit_min, _parameters.pitch_limit_max); + + } else { + + /** MANUAL FLIGHT **/ + + /* no flight mode applies, do not publish an attitude setpoint */ + setpoint = false; + } + + _att_sp.pitch_body = _tecs.get_pitch_demand(); + _att_sp.thrust = _tecs.get_throttle_demand(); + + return setpoint; +} + +void +FixedwingPositionControl::task_main() +{ + + /* inform about start */ + warnx("Initializing.."); + fflush(stdout); + + /* + * do subscriptions + */ + _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + _global_set_triplet_sub = orb_subscribe(ORB_ID(vehicle_global_position_set_triplet)); + _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); + _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + + /* rate limit vehicle status updates to 5Hz */ + orb_set_interval(_control_mode_sub, 200); + /* rate limit position updates to 50 Hz */ + orb_set_interval(_global_pos_sub, 20); + + /* abort on a nonzero return value from the parameter init */ + if (parameters_update()) { + /* parameter setup went wrong, abort */ + warnx("aborting startup due to errors."); + _task_should_exit = true; + } + + /* wakeup source(s) */ + struct pollfd fds[2]; + + /* Setup of loop */ + fds[0].fd = _params_sub; + fds[0].events = POLLIN; + fds[1].fd = _global_pos_sub; + fds[1].events = POLLIN; + + while (!_task_should_exit) { + + /* wait for up to 500ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + + /* timed out - periodic check for _task_should_exit, etc. */ + if (pret == 0) + continue; + + /* 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; + } + + perf_begin(_loop_perf); + + /* check vehicle status for changes to publication state */ + vehicle_control_mode_poll(); + + /* only update parameters if they changed */ + if (fds[0].revents & POLLIN) { + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), _params_sub, &update); + + /* update parameters from storage */ + parameters_update(); + } + + /* only run controller if position changed */ + if (fds[1].revents & POLLIN) { + + + static uint64_t last_run = 0; + float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; + last_run = hrt_absolute_time(); + + /* guard against too large deltaT's */ + if (deltaT > 1.0f) + deltaT = 0.01f; + + /* load local copies */ + orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); + + // XXX add timestamp check + _global_pos_valid = true; + + vehicle_attitude_poll(); + vehicle_setpoint_poll(); + vehicle_accel_poll(); + vehicle_airspeed_poll(); + // vehicle_baro_poll(); + + math::Vector2f ground_speed(_global_pos.vx, _global_pos.vy); + math::Vector2f current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f); + + /* + * Attempt to control position, on success (= sensors present and not in manual mode), + * publish setpoint. + */ + if (control_position(current_position, ground_speed, _global_triplet)) { + _att_sp.timestamp = hrt_absolute_time(); + + /* lazily publish the setpoint only once available */ + if (_attitude_sp_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &_att_sp); + + } else { + /* advertise and publish */ + _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + } + + } + + } + + perf_end(_loop_perf); + } + + warnx("exiting.\n"); + + _control_task = -1; + _exit(0); +} + +int +FixedwingPositionControl::start() +{ + ASSERT(_control_task == -1); + + /* start the task */ + _control_task = task_spawn_cmd("fw_pos_control_l1", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 4048, + (main_t)&FixedwingPositionControl::task_main_trampoline, + nullptr); + + if (_control_task < 0) { + warn("task start failed"); + return -errno; + } + + return OK; +} + +int fw_pos_control_l1_main(int argc, char *argv[]) +{ + if (argc < 1) + errx(1, "usage: fw_pos_control_l1 {start|stop|status}"); + + if (!strcmp(argv[1], "start")) { + + if (l1_control::g_control != nullptr) + errx(1, "already running"); + + l1_control::g_control = new FixedwingPositionControl; + + if (l1_control::g_control == nullptr) + errx(1, "alloc failed"); + + if (OK != l1_control::g_control->start()) { + delete l1_control::g_control; + l1_control::g_control = nullptr; + err(1, "start failed"); + } + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + if (l1_control::g_control == nullptr) + errx(1, "not running"); + + delete l1_control::g_control; + l1_control::g_control = nullptr; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (l1_control::g_control) { + errx(0, "running"); + + } else { + errx(1, "not running"); + } + } + + warnx("unrecognized command"); + return 1; +} diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c new file mode 100644 index 0000000000..d210ec7126 --- /dev/null +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -0,0 +1,109 @@ +/**************************************************************************** + * + * 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 fw_pos_control_l1_params.c + * + * Parameters defined by the L1 position control task + * + * @author Lorenz Meier + */ + +#include + +#include + +/* + * Controller parameters, accessible via MAVLink + * + */ + +PARAM_DEFINE_FLOAT(FW_L1_PERIOD, 25.0f); + + +PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f); + + +PARAM_DEFINE_FLOAT(FW_LOITER_R, 50.0f); + + +PARAM_DEFINE_FLOAT(FW_THR_CRUISE, 0.7f); + + +PARAM_DEFINE_FLOAT(FW_P_LIM_MIN, -45.0f); + + +PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 45.0f); + + +PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); + + +PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f); + + +PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f); + + +PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f); + + +PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f); + + +PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f); + + +PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f); + + +PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f); + + +PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f); + + +PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f); + + +PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f); + + +PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f); + + +PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f); + + +PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); diff --git a/src/modules/fw_pos_control_l1/module.mk b/src/modules/fw_pos_control_l1/module.mk new file mode 100644 index 0000000000..b00b9aa5a5 --- /dev/null +++ b/src/modules/fw_pos_control_l1/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Fixedwing L1 position control application +# + +MODULE_COMMAND = fw_pos_control_l1 + +SRCS = fw_pos_control_l1_main.cpp \ + fw_pos_control_l1_params.c From 778cfaa0b9f5d546ac6c6441b9dfe0cb4cf1d62f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 8 Sep 2013 21:42:00 +1000 Subject: [PATCH 589/872] hmc5883: add perf count, and removed unnecessary checks for -32768 we've already checked that the absolute value is <= 2048 --- src/drivers/hmc5883/hmc5883.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 0de82c3042..378f433cd1 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -842,8 +842,10 @@ HMC5883::collect() */ if ((abs(report.x) > 2048) || (abs(report.y) > 2048) || - (abs(report.z) > 2048)) + (abs(report.z) > 2048)) { + perf_count(_comms_errors); goto out; + } /* * RAW outputs @@ -852,7 +854,7 @@ HMC5883::collect() * and y needs to be negated */ _reports[_next_report].x_raw = report.y; - _reports[_next_report].y_raw = ((report.x == -32768) ? 32767 : -report.x); + _reports[_next_report].y_raw = -report.x; /* z remains z */ _reports[_next_report].z_raw = report.z; @@ -878,14 +880,14 @@ HMC5883::collect() /* to align the sensor axes with the board, x and y need to be flipped */ _reports[_next_report].x = ((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; } else { #endif /* the standard external mag by 3DR has x pointing to the right, y pointing backwards, and z down, * 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; + _reports[_next_report].x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; /* flip axes and negate value for y */ _reports[_next_report].y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; /* z remains z */ From f9f41f577084e615082ce0008532eee9f97bf674 Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 9 Sep 2013 17:36:07 +1000 Subject: [PATCH 590/872] ringbuffer: added force() and use lockless methods this adds force() which can be used for drivers wanting consumers to get the latest data when the buffer overflows --- src/drivers/device/ringbuffer.h | 136 ++++++++++++++++++++++++++++---- 1 file changed, 120 insertions(+), 16 deletions(-) diff --git a/src/drivers/device/ringbuffer.h b/src/drivers/device/ringbuffer.h index dc0c84052b..c859be647a 100644 --- a/src/drivers/device/ringbuffer.h +++ b/src/drivers/device/ringbuffer.h @@ -55,13 +55,29 @@ public: bool put(T &val); /** - * Put an item into the buffer. + * Put an item into the buffer if there is space. * * @param val Item to put * @return true if the item was put, false if the buffer is full */ bool put(const T &val); + /** + * Force an item into the buffer, discarding an older item if there is not space. + * + * @param val Item to put + * @return true if an item was discarded to make space + */ + bool force(T &val); + + /** + * Force an item into the buffer, discarding an older item if there is not space. + * + * @param val Item to put + * @return true if an item was discarded to make space + */ + bool force(const T &val); + /** * Get an item from the buffer. * @@ -73,8 +89,8 @@ public: /** * Get an item from the buffer (scalars only). * - * @return The value that was fetched, or zero if the buffer was - * empty. + * @return The value that was fetched. If the buffer is empty, + * returns zero. */ T get(void); @@ -97,23 +113,23 @@ public: /* * Returns true if the buffer is empty. */ - bool empty() { return _tail == _head; } + bool empty(); /* * Returns true if the buffer is full. */ - bool full() { return _next(_head) == _tail; } + bool full(); /* * Returns the capacity of the buffer, or zero if the buffer could * not be allocated. */ - unsigned size() { return (_buf != nullptr) ? _size : 0; } + unsigned size(); /* * Empties the buffer. */ - void flush() { _head = _tail = _size; } + void flush(); private: T *const _buf; @@ -139,6 +155,38 @@ RingBuffer::~RingBuffer() delete[] _buf; } +template +bool RingBuffer::empty() +{ + return _tail == _head; +} + +template +bool RingBuffer::full() +{ + return _next(_head) == _tail; +} + +template +unsigned RingBuffer::size() +{ + return (_buf != nullptr) ? _size : 0; +} + +template +void RingBuffer::flush() +{ + T junk; + while (!empty()) + get(junk); +} + +template +unsigned RingBuffer::_next(unsigned index) +{ + return (0 == index) ? _size : (index - 1); +} + template bool RingBuffer::put(T &val) { @@ -165,12 +213,55 @@ bool RingBuffer::put(const T &val) } } +template +bool RingBuffer::force(T &val) +{ + bool overwrote = false; + + for (;;) { + if (put(val)) + break; + T junk; + get(junk); + overwrote = true; + } + return overwrote; +} + +template +bool RingBuffer::force(const T &val) +{ + bool overwrote = false; + + for (;;) { + if (put(val)) + break; + T junk; + get(junk); + overwrote = true; + } + return overwrote; +} + template bool RingBuffer::get(T &val) { if (_tail != _head) { - val = _buf[_tail]; - _tail = _next(_tail); + unsigned candidate; + unsigned next; + do { + /* decide which element we think we're going to read */ + candidate = _tail; + + /* and what the corresponding next index will be */ + next = _next(candidate); + + /* go ahead and read from this index */ + val = _buf[candidate]; + + /* if the tail pointer didn't change, we got our item */ + } while (!__sync_bool_compare_and_swap(&_tail, candidate, next)); + return true; } else { return false; @@ -187,17 +278,30 @@ T RingBuffer::get(void) template unsigned RingBuffer::space(void) { - return (_tail >= _head) ? (_size - (_tail - _head)) : (_head - _tail - 1); + unsigned tail, head; + + /* + * Make a copy of the head/tail pointers in a fashion that + * may err on the side of under-estimating the free space + * in the buffer in the case that the buffer is being updated + * asynchronously with our check. + * If the head pointer changes (reducing space) while copying, + * re-try the copy. + */ + do { + head = _head; + tail = _tail; + } while (head != _head); + + return (tail >= head) ? (_size - (tail - head)) : (head - tail - 1); } template unsigned RingBuffer::count(void) { + /* + * Note that due to the conservative nature of space(), this may + * over-estimate the number of items in the buffer. + */ return _size - space(); } - -template -unsigned RingBuffer::_next(unsigned index) -{ - return (0 == index) ? _size : (index - 1); -} From 51bc73fb280b029ccf681bc4a34b7eb3a80f708b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 9 Sep 2013 17:37:29 +1000 Subject: [PATCH 591/872] ringbuffer: added resize() and print_info() methods this simplifies the drivers --- src/drivers/device/ringbuffer.h | 44 +++++++++++++++++++++++++++++++-- 1 file changed, 42 insertions(+), 2 deletions(-) diff --git a/src/drivers/device/ringbuffer.h b/src/drivers/device/ringbuffer.h index c859be647a..e3c5a20bdd 100644 --- a/src/drivers/device/ringbuffer.h +++ b/src/drivers/device/ringbuffer.h @@ -131,9 +131,25 @@ public: */ void flush(); + /* + * resize the buffer. This is unsafe to be called while + * a producer or consuming is running. Caller is responsible + * for any locking needed + * + * @param new_size new size for buffer + * @return true if the resize succeeds, false if + * not (allocation error) + */ + bool resize(unsigned new_size); + + /* + * printf() some info on the buffer + */ + void print_info(const char *name); + private: - T *const _buf; - const unsigned _size; + T *_buf; + unsigned _size; volatile unsigned _head; /**< insertion point */ volatile unsigned _tail; /**< removal point */ @@ -305,3 +321,27 @@ unsigned RingBuffer::count(void) */ return _size - space(); } + +template +bool RingBuffer::resize(unsigned new_size) +{ + T *old_buffer; + T *new_buffer = new T[new_size + 1]; + if (new_buffer == nullptr) { + return false; + } + old_buffer = _buf; + _buf = new_buffer; + _size = new_size; + _head = new_size; + _tail = new_size; + delete[] old_buffer; + return true; +} + +template +void RingBuffer::print_info(const char *name) +{ + printf("%s %u (%u/%u @ %p)\n", + name, _size, _head, _tail, _buf); +} From dcee02148e3bf2ee0b50620ae48c4ac6b4ac9afa Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 8 Sep 2013 21:43:24 +1000 Subject: [PATCH 592/872] hmc5883: use a RingBuffer to hold report queue this simplifies the queue handling, and avoids the need for a start()/stop() on queue resize --- src/drivers/hmc5883/hmc5883.cpp | 107 ++++++++++++-------------------- 1 file changed, 40 insertions(+), 67 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 378f433cd1..b838bf16b8 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -65,6 +65,7 @@ #include #include +#include #include #include @@ -148,10 +149,7 @@ private: work_s _work; unsigned _measure_ticks; - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; - mag_report *_reports; + RingBuffer *_reports; mag_scale _scale; float _range_scale; float _range_ga; @@ -310,9 +308,6 @@ private: }; -/* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) - /* * Driver 'main' command. */ @@ -322,9 +317,6 @@ extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]); HMC5883::HMC5883(int bus) : I2C("HMC5883", MAG_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000), _measure_ticks(0), - _num_reports(0), - _next_report(0), - _oldest_report(0), _reports(nullptr), _range_scale(0), /* default range scale from counts to gauss */ _range_ga(1.3f), @@ -356,9 +348,8 @@ HMC5883::~HMC5883() /* make sure we are truly inactive */ stop(); - /* free any existing reports */ if (_reports != nullptr) - delete[] _reports; + delete _reports; // free perf counters perf_free(_sample_perf); @@ -375,21 +366,18 @@ 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]; - + _reports = new RingBuffer(2); if (_reports == nullptr) goto out; - _oldest_report = _next_report = 0; + /* reset the device configuration */ + reset(); /* get a publish handle on the mag topic */ - memset(&_reports[0], 0, sizeof(_reports[0])); - _mag_topic = orb_advertise(ORB_ID(sensor_mag), &_reports[0]); + struct mag_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report); if (_mag_topic < 0) debug("failed to create sensor_mag object"); @@ -493,6 +481,7 @@ ssize_t HMC5883::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct mag_report); + struct mag_report *mag_buf = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -501,17 +490,15 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen) /* 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 (_reports->get(*mag_buf)) { + ret += sizeof(struct mag_report); + mag_buf++; } } @@ -522,7 +509,7 @@ HMC5883::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 { - _oldest_report = _next_report = 0; + _reports->flush(); /* trigger a measurement */ if (OK != measure()) { @@ -539,10 +526,9 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen) break; } - /* state machine will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); - + if (_reports->get(*mag_buf)) { + ret = sizeof(struct mag_report); + } } while (0); return ret; @@ -615,31 +601,22 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) return 1000000/TICK2USEC(_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)) + if ((arg < 1) || (arg > 100)) return -EINVAL; - /* allocate new buffer */ - struct mag_report *buf = new struct mag_report[arg]; - - if (nullptr == buf) + irqstate_t flags = irqsave(); + if (!_reports->resize(arg)) { + irqrestore(flags); return -ENOMEM; - - /* reset the measurement state machine with the new buffer, free the old */ - stop(); - delete[] _reports; - _num_reports = arg; - _reports = buf; - start(); + } + irqrestore(flags); return OK; } case SENSORIOCGQUEUEDEPTH: - return _num_reports - 1; + return _reports->size(); case SENSORIOCRESET: return reset(); @@ -701,7 +678,7 @@ HMC5883::start() { /* reset the report ring and state machine */ _collect_phase = false; - _oldest_report = _next_report = 0; + _reports->flush(); /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&HMC5883::cycle_trampoline, this, 1); @@ -810,9 +787,10 @@ HMC5883::collect() perf_begin(_sample_perf); + struct mag_report new_report; /* this should be fairly close to the end of the measurement, so the best approximation of the time */ - _reports[_next_report].timestamp = hrt_absolute_time(); + new_report.timestamp = hrt_absolute_time(); /* * @note We could read the status register here, which could tell us that @@ -853,10 +831,10 @@ HMC5883::collect() * to align the sensor axes with the board, x and y need to be flipped * and y needs to be negated */ - _reports[_next_report].x_raw = report.y; - _reports[_next_report].y_raw = -report.x; + new_report.x_raw = report.y; + new_report.y_raw = -report.x; /* z remains z */ - _reports[_next_report].z_raw = report.z; + new_report.z_raw = report.z; /* scale values for output */ @@ -878,34 +856,30 @@ HMC5883::collect() #ifdef PX4_I2C_BUS_ONBOARD if (_bus == PX4_I2C_BUS_ONBOARD) { /* to align the sensor axes with the board, x and y need to be flipped */ - _reports[_next_report].x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; + new_report.x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; /* flip axes and negate value for y */ - _reports[_next_report].y = ((-report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; + new_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; + new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; } else { #endif /* the standard external mag by 3DR has x pointing to the right, y pointing backwards, and z down, * therefore switch x and y and invert y */ - _reports[_next_report].x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; + new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; /* flip axes and negate value for y */ - _reports[_next_report].y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; + new_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; + new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; #ifdef PX4_I2C_BUS_ONBOARD } #endif /* publish it */ - orb_publish(ORB_ID(sensor_mag), _mag_topic, &_reports[_next_report]); + orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_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) { + /* post a report to the ring */ + if (_reports->force(new_report)) { perf_count(_buffer_overflows); - INCREMENT(_oldest_report, _num_reports); } /* notify anyone waiting for data */ @@ -1224,8 +1198,7 @@ HMC5883::print_info() 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); + _reports->print_info("report queue"); } /** From c62710f80b3d39a617666de508f923fcf1adb6d5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 9 Sep 2013 10:55:22 +1000 Subject: [PATCH 593/872] mpu6000: use a wrapper struct to avoid a linker error the linker doesn't cope with us having multiple modules implementing RingBuffer this also switches to use force() instead of put(), so we discard old entries when the buffer overflows --- src/drivers/mpu6000/mpu6000.cpp | 133 +++++++++++++++----------------- 1 file changed, 64 insertions(+), 69 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 14f8f44b82..81612aee79 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -194,7 +194,14 @@ private: struct hrt_call _call; unsigned _call_interval; - typedef RingBuffer AccelReportBuffer; + /* + these wrapper types are needed to avoid a linker error for + RingBuffer instances which appear in two drivers. + */ + struct _accel_report { + struct accel_report r; + }; + typedef RingBuffer<_accel_report> AccelReportBuffer; AccelReportBuffer *_accel_reports; struct accel_scale _accel_scale; @@ -202,7 +209,10 @@ private: float _accel_range_m_s2; orb_advert_t _accel_topic; - typedef RingBuffer GyroReportBuffer; + struct _gyro_report { + struct gyro_report r; + }; + typedef RingBuffer<_gyro_report> GyroReportBuffer; GyroReportBuffer *_gyro_reports; struct gyro_scale _gyro_scale; @@ -465,16 +475,16 @@ MPU6000::init() if (gyro_ret != OK) { _gyro_topic = -1; } else { - gyro_report gr; + _gyro_report gr; _gyro_reports->get(gr); - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr); + _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr.r); } /* advertise accel topic */ - accel_report ar; + _accel_report ar; _accel_reports->get(ar); - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar); + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar.r); out: return ret; @@ -655,7 +665,7 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen) return -EAGAIN; /* copy reports out of our buffer to the caller */ - accel_report *arp = reinterpret_cast(buffer); + _accel_report *arp = reinterpret_cast<_accel_report *>(buffer); int transferred = 0; while (count--) { if (!_accel_reports->get(*arp++)) @@ -748,7 +758,7 @@ MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) return -EAGAIN; /* copy reports out of our buffer to the caller */ - gyro_report *arp = reinterpret_cast(buffer); + _gyro_report *arp = reinterpret_cast<_gyro_report *>(buffer); int transferred = 0; while (count--) { if (!_gyro_reports->get(*arp++)) @@ -837,28 +847,19 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) return 1000000 / _call_interval; case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; - - /* allocate new buffer */ - AccelReportBuffer *buf = new AccelReportBuffer(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 _accel_reports; - _accel_reports = buf; - start(); - - return OK; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) + return -EINVAL; + + irqstate_t flags = irqsave(); + if (!_accel_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; } + irqrestore(flags); + + return OK; + } case SENSORIOCGQUEUEDEPTH: return _accel_reports->size(); @@ -935,21 +936,12 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) if ((arg < 1) || (arg > 100)) return -EINVAL; - /* allocate new buffer */ - GyroReportBuffer *buf = new GyroReportBuffer(arg); - - if (nullptr == buf) - return -ENOMEM; - if (buf->size() == 0) { - delete buf; + irqstate_t flags = irqsave(); + if (!_gyro_reports->resize(arg)) { + irqrestore(flags); return -ENOMEM; } - - /* reset the measurement state machine with the new buffer, free the old */ - stop(); - delete _gyro_reports; - _gyro_reports = buf; - start(); + irqrestore(flags); return OK; } @@ -1197,13 +1189,13 @@ MPU6000::measure() /* * Report buffers. */ - accel_report arb; - gyro_report grb; + _accel_report arb; + _gyro_report grb; /* * Adjust and scale results to m/s^2. */ - grb.timestamp = arb.timestamp = hrt_absolute_time(); + grb.r.timestamp = arb.r.timestamp = hrt_absolute_time(); /* @@ -1224,53 +1216,53 @@ MPU6000::measure() /* NOTE: Axes have been swapped to match the board a few lines above. */ - arb.x_raw = report.accel_x; - arb.y_raw = report.accel_y; - arb.z_raw = report.accel_z; + arb.r.x_raw = report.accel_x; + arb.r.y_raw = report.accel_y; + arb.r.z_raw = report.accel_z; float x_in_new = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; float y_in_new = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; float z_in_new = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; - arb.x = _accel_filter_x.apply(x_in_new); - arb.y = _accel_filter_y.apply(y_in_new); - arb.z = _accel_filter_z.apply(z_in_new); + arb.r.x = _accel_filter_x.apply(x_in_new); + arb.r.y = _accel_filter_y.apply(y_in_new); + arb.r.z = _accel_filter_z.apply(z_in_new); - arb.scaling = _accel_range_scale; - arb.range_m_s2 = _accel_range_m_s2; + arb.r.scaling = _accel_range_scale; + arb.r.range_m_s2 = _accel_range_m_s2; - arb.temperature_raw = report.temp; - arb.temperature = (report.temp) / 361.0f + 35.0f; + arb.r.temperature_raw = report.temp; + arb.r.temperature = (report.temp) / 361.0f + 35.0f; - grb.x_raw = report.gyro_x; - grb.y_raw = report.gyro_y; - grb.z_raw = report.gyro_z; + grb.r.x_raw = report.gyro_x; + grb.r.y_raw = report.gyro_y; + grb.r.z_raw = report.gyro_z; float x_gyro_in_new = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; float y_gyro_in_new = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; float z_gyro_in_new = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; - grb.x = _gyro_filter_x.apply(x_gyro_in_new); - grb.y = _gyro_filter_y.apply(y_gyro_in_new); - grb.z = _gyro_filter_z.apply(z_gyro_in_new); + grb.r.x = _gyro_filter_x.apply(x_gyro_in_new); + grb.r.y = _gyro_filter_y.apply(y_gyro_in_new); + grb.r.z = _gyro_filter_z.apply(z_gyro_in_new); - grb.scaling = _gyro_range_scale; - grb.range_rad_s = _gyro_range_rad_s; + grb.r.scaling = _gyro_range_scale; + grb.r.range_rad_s = _gyro_range_rad_s; - grb.temperature_raw = report.temp; - grb.temperature = (report.temp) / 361.0f + 35.0f; + grb.r.temperature_raw = report.temp; + grb.r.temperature = (report.temp) / 361.0f + 35.0f; - _accel_reports->put(arb); - _gyro_reports->put(grb); + _accel_reports->force(arb); + _gyro_reports->force(grb); /* notify anyone waiting for data */ poll_notify(POLLIN); _gyro->parent_poll_notify(); /* and publish for subscribers */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); + orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb.r); if (_gyro_topic != -1) { - orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb); + orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb.r); } /* stop measuring */ @@ -1280,7 +1272,10 @@ MPU6000::measure() void MPU6000::print_info() { + perf_print_counter(_sample_perf); printf("reads: %u\n", _reads); + _accel_reports->print_info("accel queue"); + _gyro_reports->print_info("gyro queue"); } MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) : From 4650c21141433b5c0f679265314f95ce69d0f2b6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 9 Sep 2013 17:16:12 +1000 Subject: [PATCH 594/872] mpu6000: fixed race condition in buffer increment --- src/drivers/mpu6000/mpu6000.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 81612aee79..66d36826a8 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -668,9 +668,10 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen) _accel_report *arp = reinterpret_cast<_accel_report *>(buffer); int transferred = 0; while (count--) { - if (!_accel_reports->get(*arp++)) + if (!_accel_reports->get(*arp)) break; transferred++; + arp++; } /* return the number of bytes transferred */ @@ -758,12 +759,13 @@ MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) return -EAGAIN; /* copy reports out of our buffer to the caller */ - _gyro_report *arp = reinterpret_cast<_gyro_report *>(buffer); + _gyro_report *grp = reinterpret_cast<_gyro_report *>(buffer); int transferred = 0; while (count--) { - if (!_gyro_reports->get(*arp++)) + if (!_gyro_reports->get(*grp)) break; transferred++; + grp++; } /* return the number of bytes transferred */ From 5ee40da720207acde6976d33e721e6bb109617ee Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 9 Sep 2013 16:35:20 +1000 Subject: [PATCH 595/872] airspeed: convert to using RingBuffer class --- src/drivers/airspeed/airspeed.cpp | 74 +++++++++------------ src/drivers/airspeed/airspeed.h | 19 ++++-- src/drivers/ets_airspeed/ets_airspeed.cpp | 28 ++++---- src/drivers/meas_airspeed/meas_airspeed.cpp | 26 +++----- 4 files changed, 67 insertions(+), 80 deletions(-) diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 1ec61eb60b..2a6b190de3 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -68,6 +68,7 @@ #include #include +#include #include #include @@ -77,10 +78,8 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) : I2C("Airspeed", AIRSPEED_DEVICE_PATH, bus, address, 100000), - _num_reports(0), - _next_report(0), - _oldest_report(0), _reports(nullptr), + _max_differential_pressure_pa(0), _sensor_ok(false), _measure_ticks(0), _collect_phase(false), @@ -105,7 +104,7 @@ Airspeed::~Airspeed() /* free any existing reports */ if (_reports != nullptr) - delete[] _reports; + delete _reports; // free perf counters perf_free(_sample_perf); @@ -123,20 +122,14 @@ Airspeed::init() goto out; /* allocate basic report buffers */ - _num_reports = 2; - _reports = new struct differential_pressure_s[_num_reports]; - - for (unsigned i = 0; i < _num_reports; i++) - _reports[i].max_differential_pressure_pa = 0; - + _reports = new RingBuffer(2); if (_reports == nullptr) goto out; - _oldest_report = _next_report = 0; - /* get a publish handle on the airspeed topic */ - memset(&_reports[0], 0, sizeof(_reports[0])); - _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &_reports[0]); + differential_pressure_s zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &zero_report); if (_airspeed_pub < 0) warnx("failed to create airspeed sensor object. Did you start uOrb?"); @@ -229,31 +222,22 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg) 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)) + if ((arg < 1) || (arg > 100)) return -EINVAL; - /* allocate new buffer */ - struct differential_pressure_s *buf = new struct differential_pressure_s[arg]; - - if (nullptr == buf) + irqstate_t flags = irqsave(); + if (!_reports->resize(arg)) { + irqrestore(flags); return -ENOMEM; - - /* reset the measurement state machine with the new buffer, free the old */ - stop(); - delete[] _reports; - _num_reports = arg; - _reports = buf; - start(); + } + irqrestore(flags); return OK; } case SENSORIOCGQUEUEDEPTH: - return _num_reports - 1; + return _reports->size(); case SENSORIOCRESET: /* XXX implement this */ @@ -281,7 +265,8 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg) ssize_t Airspeed::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(struct differential_pressure_s); + unsigned count = buflen / sizeof(differential_pressure_s); + differential_pressure_s *abuf = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -297,10 +282,9 @@ Airspeed::read(struct file *filp, char *buffer, size_t buflen) * 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 (_reports->get(*abuf)) { + ret += sizeof(*abuf); + abuf++; } } @@ -309,9 +293,8 @@ Airspeed::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 { - _oldest_report = _next_report = 0; + _reports->flush(); /* trigger a measurement */ if (OK != measure()) { @@ -329,8 +312,9 @@ Airspeed::read(struct file *filp, char *buffer, size_t buflen) } /* state machine will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); + if (_reports->get(*abuf)) { + ret = sizeof(*abuf); + } } while (0); @@ -342,7 +326,7 @@ Airspeed::start() { /* reset the report ring and state machine */ _collect_phase = false; - _oldest_report = _next_report = 0; + _reports->flush(); /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&Airspeed::cycle_trampoline, this, 1); @@ -385,6 +369,12 @@ Airspeed::print_info() perf_print_counter(_comms_errors); perf_print_counter(_buffer_overflows); warnx("poll interval: %u ticks", _measure_ticks); - warnx("report queue: %u (%u/%u @ %p)", - _num_reports, _oldest_report, _next_report, _reports); + _reports->print_info("report queue"); +} + +void +Airspeed::new_report(const differential_pressure_s &report) +{ + if (!_reports->force(report)) + perf_count(_buffer_overflows); } diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h index b87494b40f..7850ccc7e9 100644 --- a/src/drivers/airspeed/airspeed.h +++ b/src/drivers/airspeed/airspeed.h @@ -68,6 +68,7 @@ #include #include +#include #include #include @@ -102,6 +103,10 @@ public: */ virtual void print_info(); +private: + RingBuffer *_reports; + perf_counter_t _buffer_overflows; + protected: virtual int probe(); @@ -114,10 +119,7 @@ protected: virtual int collect() = 0; work_s _work; - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; - differential_pressure_s *_reports; + uint16_t _max_differential_pressure_pa; bool _sensor_ok; int _measure_ticks; bool _collect_phase; @@ -129,7 +131,6 @@ protected: perf_counter_t _sample_perf; perf_counter_t _comms_errors; - perf_counter_t _buffer_overflows; /** @@ -162,8 +163,12 @@ protected: */ static void cycle_trampoline(void *arg); + /** + * add a new report to the reports queue + * + * @param report differential_pressure_s report + */ + void new_report(const differential_pressure_s &report); }; -/* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index 257b41935c..dd8436b10e 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -68,6 +68,7 @@ #include #include +#include #include #include @@ -173,27 +174,22 @@ ETSAirspeed::collect() diff_pres_pa -= _diff_pres_offset; } - // XXX we may want to smooth out the readings to remove noise. - - _reports[_next_report].timestamp = hrt_absolute_time(); - _reports[_next_report].differential_pressure_pa = diff_pres_pa; - // Track maximum differential pressure measured (so we can work out top speed). - if (diff_pres_pa > _reports[_next_report].max_differential_pressure_pa) { - _reports[_next_report].max_differential_pressure_pa = diff_pres_pa; + if (diff_pres_pa > _max_differential_pressure_pa) { + _max_differential_pressure_pa = diff_pres_pa; } + // XXX we may want to smooth out the readings to remove noise. + differential_pressure_s report; + report.timestamp = hrt_absolute_time(); + report.differential_pressure_pa = diff_pres_pa; + report.voltage = 0; + report.max_differential_pressure_pa = _max_differential_pressure_pa; + /* announce the airspeed if needed, just publish else */ - orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_reports[_next_report]); + orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &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); - } + new_report(report); /* notify anyone waiting for data */ poll_notify(POLLIN); diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index b1cb2b3d84..03d7bbfb91 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -199,27 +199,23 @@ MEASAirspeed::collect() // Calculate differential pressure. As its centered around 8000 // and can go positive or negative, enforce absolute value uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f)); - - _reports[_next_report].timestamp = hrt_absolute_time(); - _reports[_next_report].temperature = temperature; - _reports[_next_report].differential_pressure_pa = diff_press_pa; + struct differential_pressure_s report; // Track maximum differential pressure measured (so we can work out top speed). - if (diff_press_pa > _reports[_next_report].max_differential_pressure_pa) { - _reports[_next_report].max_differential_pressure_pa = diff_press_pa; + if (diff_press_pa > _max_differential_pressure_pa) { + _max_differential_pressure_pa = diff_press_pa; } + report.timestamp = hrt_absolute_time(); + report.temperature = temperature; + report.differential_pressure_pa = diff_press_pa; + report.voltage = 0; + report.max_differential_pressure_pa = _max_differential_pressure_pa; + /* announce the airspeed if needed, just publish else */ - orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_reports[_next_report]); + orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &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); - } + new_report(report); /* notify anyone waiting for data */ poll_notify(POLLIN); From 96b4729b37dc399de5b8666080e102109d2b158a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 9 Sep 2013 16:36:07 +1000 Subject: [PATCH 596/872] l3gd20: convert to using RingBuffer class --- src/drivers/l3gd20/l3gd20.cpp | 126 ++++++++++++++-------------------- 1 file changed, 51 insertions(+), 75 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index e6d765e13a..7fb1cbbbc8 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -61,6 +61,7 @@ #include #include #include +#include #include #include @@ -183,11 +184,8 @@ private: struct hrt_call _call; unsigned _call_interval; - - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; - struct gyro_report *_reports; + + RingBuffer *_reports; struct gyro_scale _gyro_scale; float _gyro_range_scale; @@ -299,16 +297,9 @@ private: int self_test(); }; -/* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) - - L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : SPI("L3GD20", 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), @@ -340,7 +331,7 @@ L3GD20::~L3GD20() /* free any existing reports */ if (_reports != nullptr) - delete[] _reports; + delete _reports; /* delete the perf counter */ perf_free(_sample_perf); @@ -356,16 +347,15 @@ L3GD20::init() goto out; /* allocate basic report buffers */ - _num_reports = 2; - _oldest_report = _next_report = 0; - _reports = new struct gyro_report[_num_reports]; + _reports = new RingBuffer(2); if (_reports == nullptr) goto out; /* advertise sensor topic */ - memset(&_reports[0], 0, sizeof(_reports[0])); - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_reports[0]); + struct gyro_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &zero_report); reset(); @@ -406,6 +396,7 @@ ssize_t L3GD20::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct gyro_report); + struct gyro_report *gbuf = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -421,10 +412,9 @@ L3GD20::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 (_reports->get(*gbuf)) { + ret += sizeof(*gbuf); + gbuf++; } } @@ -433,12 +423,13 @@ L3GD20::read(struct file *filp, char *buffer, size_t buflen) } /* manual measurement */ - _oldest_report = _next_report = 0; + _reports->flush(); measure(); /* measurement will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); + if (_reports->get(*gbuf)) { + ret = sizeof(*gbuf); + } return ret; } @@ -506,31 +497,22 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) 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 < 1) || (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 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; + irqstate_t flags = irqsave(); + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; } + irqrestore(flags); + + return OK; + } case SENSORIOCGQUEUEDEPTH: - return _num_reports - 1; + return _reports->size(); case SENSORIOCRESET: reset(); @@ -699,7 +681,7 @@ L3GD20::start() stop(); /* reset the report ring */ - _oldest_report = _next_report = 0; + _reports->flush(); /* start polling at the specified rate */ hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&L3GD20::measure_trampoline, this); @@ -759,7 +741,7 @@ L3GD20::measure() } raw_report; #pragma pack(pop) - gyro_report *report = &_reports[_next_report]; + gyro_report report; /* start the performance counter */ perf_begin(_sample_perf); @@ -782,61 +764,56 @@ L3GD20::measure() * the offset is 74 from the origin and subtracting * 74 from all measurements centers them around zero. */ - report->timestamp = hrt_absolute_time(); + report.timestamp = hrt_absolute_time(); switch (_orientation) { case SENSOR_BOARD_ROTATION_000_DEG: /* keep axes in place */ - report->x_raw = raw_report.x; - report->y_raw = raw_report.y; + 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; + 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); + 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); + 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.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) - _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.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; + 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); + _reports->force(report); /* notify anyone waiting for data */ poll_notify(POLLIN); /* publish for subscribers */ if (_gyro_topic > 0) - orb_publish(ORB_ID(sensor_gyro), _gyro_topic, report); + orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report); _read++; @@ -849,8 +826,7 @@ 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); + _reports->print_info("report queue"); } int From 4918148aa38698956b823bd0640eba583c5c14eb Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 9 Sep 2013 16:36:22 +1000 Subject: [PATCH 597/872] mb12xx: convert to using RingBuffer class --- src/drivers/mb12xx/mb12xx.cpp | 102 +++++++++++++--------------------- 1 file changed, 39 insertions(+), 63 deletions(-) diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index f834169935..fabe10b872 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -64,6 +64,7 @@ #include #include +#include #include #include @@ -119,10 +120,7 @@ private: float _min_distance; float _max_distance; work_s _work; - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; - range_finder_report *_reports; + RingBuffer *_reports; bool _sensor_ok; int _measure_ticks; bool _collect_phase; @@ -183,9 +181,6 @@ private: }; -/* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) - /* * Driver 'main' command. */ @@ -195,9 +190,6 @@ MB12XX::MB12XX(int bus, int address) : I2C("MB12xx", RANGE_FINDER_DEVICE_PATH, bus, address, 100000), _min_distance(MB12XX_MIN_DISTANCE), _max_distance(MB12XX_MAX_DISTANCE), - _num_reports(0), - _next_report(0), - _oldest_report(0), _reports(nullptr), _sensor_ok(false), _measure_ticks(0), @@ -221,7 +213,7 @@ MB12XX::~MB12XX() /* free any existing reports */ if (_reports != nullptr) - delete[] _reports; + delete _reports; } int @@ -234,17 +226,15 @@ MB12XX::init() goto out; /* allocate basic report buffers */ - _num_reports = 2; - _reports = new struct range_finder_report[_num_reports]; + _reports = new RingBuffer(2); if (_reports == nullptr) goto out; - _oldest_report = _next_report = 0; - /* get a publish handle on the range finder topic */ - memset(&_reports[0], 0, sizeof(_reports[0])); - _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &_reports[0]); + struct range_finder_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report); if (_range_finder_topic < 0) debug("failed to create sensor_range_finder object. Did you start uOrb?"); @@ -354,31 +344,22 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg) 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 range_finder_report *buf = new struct range_finder_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; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) + return -EINVAL; + + irqstate_t flags = irqsave(); + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; } + irqrestore(flags); + + return OK; + } case SENSORIOCGQUEUEDEPTH: - return _num_reports - 1; + return _reports->size(); case SENSORIOCRESET: /* XXX implement this */ @@ -406,6 +387,7 @@ ssize_t MB12XX::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct range_finder_report); + struct range_finder_report *rbuf = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -421,10 +403,9 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen) * 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 (_reports->get(*rbuf)) { + ret += sizeof(*rbuf); + rbuf++; } } @@ -433,9 +414,8 @@ MB12XX::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 { - _oldest_report = _next_report = 0; + _reports->flush(); /* trigger a measurement */ if (OK != measure()) { @@ -453,8 +433,9 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen) } /* state machine will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); + if (_reports->get(*rbuf)) { + ret = sizeof(*rbuf); + } } while (0); @@ -498,26 +479,25 @@ MB12XX::collect() if (ret < 0) { log("error reading from sensor: %d", ret); + perf_count(_comms_errors); + perf_end(_sample_perf); return ret; } uint16_t distance = val[0] << 8 | val[1]; float si_units = (distance * 1.0f)/ 100.0f; /* cm to m */ + struct range_finder_report report; + /* this should be fairly close to the end of the measurement, so the best approximation of the time */ - _reports[_next_report].timestamp = hrt_absolute_time(); - _reports[_next_report].distance = si_units; - _reports[_next_report].valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; + report.timestamp = hrt_absolute_time(); + report.distance = si_units; + report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; /* publish it */ - orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &_reports[_next_report]); + orb_publish(ORB_ID(sensor_range_finder), _range_finder_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, toss it */ - if (_next_report == _oldest_report) { + if (_reports->force(report)) { perf_count(_buffer_overflows); - INCREMENT(_oldest_report, _num_reports); } /* notify anyone waiting for data */ @@ -525,11 +505,8 @@ MB12XX::collect() ret = OK; -out: perf_end(_sample_perf); return ret; - - return ret; } void @@ -537,7 +514,7 @@ MB12XX::start() { /* reset the report ring and state machine */ _collect_phase = false; - _oldest_report = _next_report = 0; + _reports->flush(); /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 1); @@ -626,8 +603,7 @@ MB12XX::print_info() 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); + _reports->print_info("report queue"); } /** From 654599717834b93ac7832bce73249526e4cc52b9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 9 Sep 2013 17:15:19 +1000 Subject: [PATCH 598/872] bma180: convert to using RingBuffer --- src/drivers/bma180/bma180.cpp | 128 +++++++++++++++------------------- 1 file changed, 55 insertions(+), 73 deletions(-) diff --git a/src/drivers/bma180/bma180.cpp b/src/drivers/bma180/bma180.cpp index 079b5d21cb..f14c008cea 100644 --- a/src/drivers/bma180/bma180.cpp +++ b/src/drivers/bma180/bma180.cpp @@ -63,6 +63,7 @@ #include #include +#include /* oddly, ERROR is not defined for c++ */ @@ -146,10 +147,14 @@ private: struct hrt_call _call; unsigned _call_interval; - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; - struct accel_report *_reports; + /* + this wrapper type is needed to avoid a linker error for + RingBuffer instances which appear in two drivers. + */ + struct _accel_report { + accel_report r; + }; + RingBuffer *_reports; struct accel_scale _accel_scale; float _accel_range_scale; @@ -233,16 +238,9 @@ private: int set_lowpass(unsigned frequency); }; -/* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) - - BMA180::BMA180(int bus, spi_dev_e device) : SPI("BMA180", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, 8000000), _call_interval(0), - _num_reports(0), - _next_report(0), - _oldest_report(0), _reports(nullptr), _accel_range_scale(0.0f), _accel_range_m_s2(0.0f), @@ -270,7 +268,7 @@ BMA180::~BMA180() /* free any existing reports */ if (_reports != nullptr) - delete[] _reports; + delete _reports; /* delete the perf counter */ perf_free(_sample_perf); @@ -286,16 +284,15 @@ BMA180::init() goto out; /* allocate basic report buffers */ - _num_reports = 2; - _oldest_report = _next_report = 0; - _reports = new struct accel_report[_num_reports]; + _reports = new RingBuffer<_accel_report>(2); 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]); + struct accel_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report); /* perform soft reset (p48) */ write_reg(ADDR_RESET, SOFT_RESET); @@ -352,6 +349,7 @@ ssize_t BMA180::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct accel_report); + struct _accel_report *arp = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -367,10 +365,9 @@ BMA180::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 (_reports->get(*arp)) { + ret += sizeof(*arp); + arp++; } } @@ -379,12 +376,12 @@ BMA180::read(struct file *filp, char *buffer, size_t buflen) } /* manual measurement */ - _oldest_report = _next_report = 0; + _reports->flush(); measure(); /* measurement will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); + if (_reports->get(*arp)) + ret = sizeof(*arp); return ret; } @@ -449,31 +446,22 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg) 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; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 2) || (arg > 100)) + return -EINVAL; + + irqstate_t flags = irqsave(); + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; } + irqrestore(flags); + + return OK; + } case SENSORIOCGQUEUEDEPTH: - return _num_reports - 1; + return _reports->size(); case SENSORIOCRESET: /* XXX implement */ @@ -654,7 +642,7 @@ BMA180::start() stop(); /* reset the report ring */ - _oldest_report = _next_report = 0; + _reports->flush(); /* start polling at the specified rate */ hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&BMA180::measure_trampoline, this); @@ -688,7 +676,7 @@ BMA180::measure() // } raw_report; // #pragma pack(pop) - accel_report *report = &_reports[_next_report]; + struct _accel_report report; /* start the performance counter */ perf_begin(_sample_perf); @@ -708,45 +696,40 @@ BMA180::measure() * them before. There is no good way to synchronise with the internal * measurement flow without using the external interrupt. */ - _reports[_next_report].timestamp = hrt_absolute_time(); + report.r.timestamp = hrt_absolute_time(); /* * y of board is x of sensor and x of board is -y of sensor * perform only the axis assignment here. * Two non-value bits are discarded directly */ - report->y_raw = read_reg(ADDR_ACC_X_LSB + 0); - report->y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8; - report->x_raw = read_reg(ADDR_ACC_X_LSB + 2); - report->x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8; - report->z_raw = read_reg(ADDR_ACC_X_LSB + 4); - report->z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8; + report.r.y_raw = read_reg(ADDR_ACC_X_LSB + 0); + report.r.y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8; + report.r.x_raw = read_reg(ADDR_ACC_X_LSB + 2); + report.r.x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8; + report.r.z_raw = read_reg(ADDR_ACC_X_LSB + 4); + report.r.z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8; /* discard two non-value bits in the 16 bit measurement */ - report->x_raw = (report->x_raw / 4); - report->y_raw = (report->y_raw / 4); - report->z_raw = (report->z_raw / 4); + report.r.x_raw = (report.r.x_raw / 4); + report.r.y_raw = (report.r.y_raw / 4); + report.r.z_raw = (report.r.z_raw / 4); /* invert y axis, due to 14 bit data no overflow can occur in the negation */ - report->y_raw = -report->y_raw; + report.r.y_raw = -report.r.y_raw; - report->x = ((report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - report->y = ((report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - report->z = ((report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; - report->scaling = _accel_range_scale; - report->range_m_s2 = _accel_range_m_s2; + report.r.x = ((report.r.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + report.r.y = ((report.r.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + report.r.z = ((report.r.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + report.r.scaling = _accel_range_scale; + report.r.range_m_s2 = _accel_range_m_s2; - /* 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); + _reports->force(report); /* 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, &report.r); /* stop the perf counter */ perf_end(_sample_perf); @@ -756,8 +739,7 @@ void BMA180::print_info() { perf_print_counter(_sample_perf); - printf("report queue: %u (%u/%u @ %p)\n", - _num_reports, _oldest_report, _next_report, _reports); + _reports->print_info("report queue"); } /** From 2e0fc9a288d0c431c26a8ab2ec0976ab4fc70a41 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 9 Sep 2013 17:15:39 +1000 Subject: [PATCH 599/872] lsm303d: convert to using RingBuffer --- src/drivers/lsm303d/lsm303d.cpp | 216 +++++++++++++------------------- 1 file changed, 86 insertions(+), 130 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 05d6f1881d..947e177129 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -62,6 +62,7 @@ #include #include #include +#include #include #include @@ -218,15 +219,19 @@ private: 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; + /* + these wrapper types are needed to avoid a linker error for + RingBuffer instances which appear in two drivers. + */ + struct _accel_report { + struct accel_report r; + }; + RingBuffer *_accel_reports; - unsigned _num_mag_reports; - volatile unsigned _next_mag_report; - volatile unsigned _oldest_mag_report; - struct mag_report *_mag_reports; + struct _mag_report { + struct mag_report r; + }; + RingBuffer *_mag_reports; struct accel_scale _accel_scale; unsigned _accel_range_m_s2; @@ -420,22 +425,12 @@ private: }; -/* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) - - LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 8000000), _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), - _num_mag_reports(0), - _next_mag_report(0), - _oldest_mag_report(0), _mag_reports(nullptr), _accel_range_m_s2(0.0f), _accel_range_scale(0.0f), @@ -480,9 +475,9 @@ LSM303D::~LSM303D() /* free any existing reports */ if (_accel_reports != nullptr) - delete[] _accel_reports; + delete _accel_reports; if (_mag_reports != nullptr) - delete[] _mag_reports; + delete _mag_reports; delete _mag; @@ -502,20 +497,17 @@ LSM303D::init() goto out; /* allocate basic report buffers */ - _num_accel_reports = 2; - _oldest_accel_report = _next_accel_report = 0; - _accel_reports = new struct accel_report[_num_accel_reports]; + _accel_reports = new RingBuffer(2); if (_accel_reports == nullptr) goto out; /* advertise accel topic */ - memset(&_accel_reports[0], 0, sizeof(_accel_reports[0])); - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &_accel_reports[0]); + struct accel_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report); - _num_mag_reports = 2; - _oldest_mag_report = _next_mag_report = 0; - _mag_reports = new struct mag_report[_num_mag_reports]; + _mag_reports = new RingBuffer(2); if (_mag_reports == nullptr) goto out; @@ -523,8 +515,9 @@ LSM303D::init() reset(); /* advertise mag topic */ - memset(&_mag_reports[0], 0, sizeof(_mag_reports[0])); - _mag_topic = orb_advertise(ORB_ID(sensor_mag), &_mag_reports[0]); + struct mag_report zero_mag_report; + memset(&zero_mag_report, 0, sizeof(zero_mag_report)); + _mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_mag_report); /* do CDev init for the mag device node, keep it optional */ mag_ret = _mag->init(); @@ -577,6 +570,7 @@ ssize_t LSM303D::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct accel_report); + struct _accel_report *arb = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -585,17 +579,13 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen) /* if automatic measurement is enabled */ if (_call_accel_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_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); + if (_accel_reports->get(*arb)) { + ret += sizeof(*arb); + arb++; } } @@ -604,12 +594,11 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen) } /* manual measurement */ - _oldest_accel_report = _next_accel_report = 0; measure(); /* measurement will have generated a report, copy it out */ - memcpy(buffer, _accel_reports, sizeof(*_accel_reports)); - ret = sizeof(*_accel_reports); + if (_accel_reports->get(*arb)) + ret = sizeof(*arb); return ret; } @@ -618,6 +607,7 @@ ssize_t LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct mag_report); + struct _mag_report *mrb = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -629,14 +619,11 @@ LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen) /* * 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 (_mag_reports->get(*mrb)) { + ret += sizeof(*mrb); + mrb++; } } @@ -645,12 +632,12 @@ LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen) } /* manual measurement */ - _oldest_mag_report = _next_mag_report = 0; + _mag_reports->flush(); measure(); /* measurement will have generated a report, copy it out */ - memcpy(buffer, _mag_reports, sizeof(*_mag_reports)); - ret = sizeof(*_mag_reports); + if (_mag_reports->get(*mrb)) + ret = sizeof(*mrb); return ret; } @@ -718,31 +705,22 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) return 1000000 / _call_accel_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)) + if ((arg < 1) || (arg > 100)) return -EINVAL; - /* allocate new buffer */ - struct accel_report *buf = new struct accel_report[arg]; - - if (nullptr == buf) + irqstate_t flags = irqsave(); + if (!_accel_reports->resize(arg)) { + irqrestore(flags); 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(); + } + irqrestore(flags); return OK; } case SENSORIOCGQUEUEDEPTH: - return _num_accel_reports - 1; + return _accel_reports->size(); case SENSORIOCRESET: reset(); @@ -854,31 +832,22 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) return 1000000 / _call_mag_interval; case SENSORIOCSQUEUEDEPTH: { - /* account for sentinel in the ring */ - arg++; + /* 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 < 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; + irqstate_t flags = irqsave(); + if (!_mag_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; } + irqrestore(flags); + + return OK; + } case SENSORIOCGQUEUEDEPTH: - return _num_mag_reports - 1; + return _mag_reports->size(); case SENSORIOCRESET: reset(); @@ -1211,8 +1180,8 @@ LSM303D::start() stop(); /* reset the report ring */ - _oldest_accel_report = _next_accel_report = 0; - _oldest_mag_report = _next_mag_report = 0; + _accel_reports->flush(); + _mag_reports->flush(); /* start polling at the specified rate */ hrt_call_every(&_accel_call, 1000, _call_accel_interval, (hrt_callout)&LSM303D::measure_trampoline, this); @@ -1259,7 +1228,7 @@ LSM303D::measure() } raw_accel_report; #pragma pack(pop) - accel_report *accel_report = &_accel_reports[_next_accel_report]; + struct _accel_report accel_report; /* start the performance counter */ perf_begin(_accel_sample_perf); @@ -1284,35 +1253,30 @@ LSM303D::measure() */ - accel_report->timestamp = hrt_absolute_time(); + accel_report.r.timestamp = hrt_absolute_time(); - accel_report->x_raw = raw_accel_report.x; - accel_report->y_raw = raw_accel_report.y; - accel_report->z_raw = raw_accel_report.z; + accel_report.r.x_raw = raw_accel_report.x; + accel_report.r.y_raw = raw_accel_report.y; + accel_report.r.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; + float x_in_new = ((accel_report.r.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + float y_in_new = ((accel_report.r.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + float z_in_new = ((accel_report.r.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.r.x = _accel_filter_x.apply(x_in_new); + accel_report.r.y = _accel_filter_y.apply(y_in_new); + accel_report.r.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.r.scaling = _accel_range_scale; + accel_report.r.range_m_s2 = _accel_range_m_s2; - /* post a report to the ring - note, not locked */ - INCREMENT(_next_accel_report, _num_accel_reports); - - /* if we are running up against the oldest report, fix it */ - if (_next_accel_report == _oldest_accel_report) - INCREMENT(_oldest_accel_report, _num_accel_reports); + _accel_reports->force(accel_report); /* notify anyone waiting for data */ poll_notify(POLLIN); /* publish for subscribers */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, accel_report); + orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report.r); _accel_read++; @@ -1334,7 +1298,7 @@ LSM303D::mag_measure() } raw_mag_report; #pragma pack(pop) - mag_report *mag_report = &_mag_reports[_next_mag_report]; + struct _mag_report mag_report; /* start the performance counter */ perf_begin(_mag_sample_perf); @@ -1359,30 +1323,25 @@ LSM303D::mag_measure() */ - mag_report->timestamp = hrt_absolute_time(); + mag_report.r.timestamp = hrt_absolute_time(); - 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; - mag_report->scaling = _mag_range_scale; - mag_report->range_ga = (float)_mag_range_ga; + mag_report.r.x_raw = raw_mag_report.x; + mag_report.r.y_raw = raw_mag_report.y; + mag_report.r.z_raw = raw_mag_report.z; + mag_report.r.x = ((mag_report.r.x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; + mag_report.r.y = ((mag_report.r.y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; + mag_report.r.z = ((mag_report.r.z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; + mag_report.r.scaling = _mag_range_scale; + mag_report.r.range_ga = (float)_mag_range_ga; - /* 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); + _mag_reports->force(mag_report); /* XXX please check this poll_notify, is it the right one? */ /* notify anyone waiting for data */ poll_notify(POLLIN); /* publish for subscribers */ - orb_publish(ORB_ID(sensor_mag), _mag_topic, mag_report); + orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report.r); _mag_read++; @@ -1396,11 +1355,8 @@ 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); - 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); + _accel_reports->print_info("accel reports"); + _mag_reports->print_info("mag reports"); } LSM303D_mag::LSM303D_mag(LSM303D *parent) : From ba712e1a22b4b47b925471d9350ebfdb4e6b8c69 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 9 Sep 2013 17:16:38 +1000 Subject: [PATCH 600/872] ms5611: converted to using RingBuffer --- src/drivers/ms5611/ms5611.cpp | 91 +++++++++++++---------------------- 1 file changed, 34 insertions(+), 57 deletions(-) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 4e43f19c5d..3f57cd68f3 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -60,6 +60,7 @@ #include #include #include +#include #include #include @@ -114,10 +115,7 @@ protected: struct work_s _work; unsigned _measure_ticks; - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; - struct baro_report *_reports; + RingBuffer *_reports; bool _collect_phase; unsigned _measure_phase; @@ -196,9 +194,6 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) : _interface(interface), _prom(prom_buf.s), _measure_ticks(0), - _num_reports(0), - _next_report(0), - _oldest_report(0), _reports(nullptr), _collect_phase(false), _measure_phase(0), @@ -223,7 +218,7 @@ MS5611::~MS5611() /* free any existing reports */ if (_reports != nullptr) - delete[] _reports; + delete _reports; // free perf counters perf_free(_sample_perf); @@ -246,8 +241,7 @@ MS5611::init() } /* allocate basic report buffers */ - _num_reports = 2; - _reports = new struct baro_report[_num_reports]; + _reports = new RingBuffer(2); if (_reports == nullptr) { debug("can't get memory for reports"); @@ -255,11 +249,10 @@ MS5611::init() 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]); + struct baro_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _baro_topic = orb_advertise(ORB_ID(sensor_baro), &zero_report); if (_baro_topic < 0) { debug("failed to create sensor_baro object"); @@ -276,6 +269,7 @@ ssize_t MS5611::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct baro_report); + struct baro_report *brp = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -291,10 +285,9 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) * 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 (_reports->get(*brp)) { + ret += sizeof(*brp); + brp++; } } @@ -303,10 +296,9 @@ 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; + _reports->flush(); /* do temperature first */ if (OK != measure()) { @@ -335,8 +327,8 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) } /* state machine will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); + if (_reports->get(*brp)) + ret = sizeof(*brp); } while (0); @@ -411,31 +403,21 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) 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 < 1) || (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 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; + irqstate_t flags = irqsave(); + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; } + irqrestore(flags); + return OK; + } case SENSORIOCGQUEUEDEPTH: - return _num_reports - 1; + return _reports->size(); case SENSORIOCRESET: /* XXX implement this */ @@ -469,7 +451,7 @@ MS5611::start_cycle() /* reset the report ring and state machine */ _collect_phase = false; _measure_phase = 0; - _oldest_report = _next_report = 0; + _reports->flush(); /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, 1); @@ -588,8 +570,9 @@ MS5611::collect() perf_begin(_sample_perf); + struct baro_report report; /* 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(); + report.timestamp = hrt_absolute_time(); /* read the most recent measurement - read offset/size are hardcoded in the interface */ ret = _interface->read(0, (void *)&raw, 0); @@ -638,8 +621,8 @@ MS5611::collect() 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 */ + report.temperature = _TEMP / 100.0f; + 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 */ @@ -676,18 +659,13 @@ MS5611::collect() * h = ------------------------------- + h1 * a */ - _reports[_next_report].altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; + report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; /* publish it */ - orb_publish(ORB_ID(sensor_baro), _baro_topic, &_reports[_next_report]); + orb_publish(ORB_ID(sensor_baro), _baro_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, toss it */ - if (_next_report == _oldest_report) { + if (_reports->force(report)) { perf_count(_buffer_overflows); - INCREMENT(_oldest_report, _num_reports); } /* notify anyone waiting for data */ @@ -709,8 +687,7 @@ MS5611::print_info() 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); + _reports->print_info("report queue"); printf("TEMP: %d\n", _TEMP); printf("SENS: %lld\n", _SENS); printf("OFF: %lld\n", _OFF); From 465f161427767da4384bf9291f8b1ad782c04c30 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Sep 2013 12:49:17 +0200 Subject: [PATCH 601/872] Hotfix: remove bogus commit --- src/modules/px4iofirmware/controls.c | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 796c6cd9f5..b17276a319 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -124,8 +124,6 @@ controls_tick() { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; perf_end(c_gather_ppm); - ASSERT(r_raw_rc_count <= PX4IO_CONTROL_CHANNELS); - /* * In some cases we may have received a frame, but input has still * been lost. From c3b6cea77a1fe59e58b0019d1f8e5b95daf55494 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Sep 2013 13:22:56 +0200 Subject: [PATCH 602/872] Hotfix for S.Bus systems with more than 8 channels --- src/modules/px4iofirmware/controls.c | 6 ++++-- src/modules/px4iofirmware/px4io.h | 4 ++-- src/modules/px4iofirmware/sbus.c | 12 ++++++------ 3 files changed, 12 insertions(+), 10 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index b17276a319..617b536f48 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -108,9 +108,11 @@ controls_tick() { perf_end(c_gather_dsm); perf_begin(c_gather_sbus); - bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count); - if (sbus_updated) + bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, PX4IO_CONTROL_CHANNELS /* XXX this should be INPUT channels, once untangled */); + if (sbus_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; + r_raw_rc_count = 8; + } perf_end(c_gather_sbus); /* diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index dea67043e3..11f4d053d5 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -51,7 +51,7 @@ */ #define PX4IO_SERVO_COUNT 8 #define PX4IO_CONTROL_CHANNELS 8 -#define PX4IO_INPUT_CHANNELS 12 +#define PX4IO_INPUT_CHANNELS 8 // XXX this should be 18 channels /* * Debug logging @@ -200,7 +200,7 @@ extern int dsm_init(const char *device); extern bool dsm_input(uint16_t *values, uint16_t *num_values); extern void dsm_bind(uint16_t cmd, int pulses); extern int sbus_init(const char *device); -extern bool sbus_input(uint16_t *values, uint16_t *num_values); +extern bool sbus_input(uint16_t *values, uint16_t *num_values, uint16_t max_channels); /** global debug level for isr_debug() */ extern volatile uint8_t debug_level; diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 073ddeabae..c523df6caf 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -66,7 +66,7 @@ static unsigned partial_frame_count; unsigned sbus_frame_drops; -static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values); +static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t max_channels); int sbus_init(const char *device) @@ -97,7 +97,7 @@ sbus_init(const char *device) } bool -sbus_input(uint16_t *values, uint16_t *num_values) +sbus_input(uint16_t *values, uint16_t *num_values, uint16_t max_channels) { ssize_t ret; hrt_abstime now; @@ -154,7 +154,7 @@ sbus_input(uint16_t *values, uint16_t *num_values) * decode it. */ partial_frame_count = 0; - return sbus_decode(now, values, num_values); + return sbus_decode(now, values, num_values, max_channels); } /* @@ -194,7 +194,7 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { }; static bool -sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) +sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t max_values) { /* check frame boundary markers to avoid out-of-sync cases */ if ((frame[0] != 0x0f) || (frame[24] != 0x00)) { @@ -214,8 +214,8 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) /* we have received something we think is a frame */ last_frame_time = frame_time; - unsigned chancount = (PX4IO_INPUT_CHANNELS > SBUS_INPUT_CHANNELS) ? - SBUS_INPUT_CHANNELS : PX4IO_INPUT_CHANNELS; + unsigned chancount = (max_values > SBUS_INPUT_CHANNELS) ? + SBUS_INPUT_CHANNELS : max_values; /* use the decoder matrix to extract channel data */ for (unsigned channel = 0; channel < chancount; channel++) { From 7d9f49adc0fad3c4a68e70eeecee6a8b83d87ade Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Sep 2013 14:08:07 +0200 Subject: [PATCH 603/872] Added pos control in startup scripts --- ROMFS/px4fmu_common/init.d/30_io_camflyer | 3 +-- ROMFS/px4fmu_common/init.d/31_io_phantom | 3 +-- ROMFS/px4fmu_common/init.d/rc.hil | 2 +- 3 files changed, 3 insertions(+), 5 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/30_io_camflyer b/ROMFS/px4fmu_common/init.d/30_io_camflyer index c4c4ea3da0..ff740b6f23 100644 --- a/ROMFS/px4fmu_common/init.d/30_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/30_io_camflyer @@ -73,8 +73,7 @@ att_pos_estimator_ekf start # mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix fw_att_control start -# Not ready yet for prime-time -#fw_pos_control_l1 start +fw_pos_control_l1 start if [ $EXIT_ON_END == yes ] then diff --git a/ROMFS/px4fmu_common/init.d/31_io_phantom b/ROMFS/px4fmu_common/init.d/31_io_phantom index 8d4158a182..838dcb369c 100644 --- a/ROMFS/px4fmu_common/init.d/31_io_phantom +++ b/ROMFS/px4fmu_common/init.d/31_io_phantom @@ -73,8 +73,7 @@ att_pos_estimator_ekf start # mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix fw_att_control start -# Not ready yet for prime-time -#fw_pos_control_l1 start +fw_pos_control_l1 start if [ $EXIT_ON_END == yes ] then diff --git a/ROMFS/px4fmu_common/init.d/rc.hil b/ROMFS/px4fmu_common/init.d/rc.hil index b924d62bc8..6c9bfe6dc3 100644 --- a/ROMFS/px4fmu_common/init.d/rc.hil +++ b/ROMFS/px4fmu_common/init.d/rc.hil @@ -49,7 +49,7 @@ att_pos_estimator_ekf start # Load mixer and start controllers (depends on px4io) # mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix -#fw_pos_control_l1 start +fw_pos_control_l1 start fw_att_control start echo "[HIL] setup done, running" From 34a8c2de9ccce28523be901bf4476bbe418b74c4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Sep 2013 14:08:35 +0200 Subject: [PATCH 604/872] Added position controller to default set --- makefiles/config_px4fmu-v1_default.mk | 3 ++- makefiles/config_px4fmu-v2_default.mk | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 7bb5dc62ed..bd41f68b65 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -79,7 +79,7 @@ MODULES += examples/flow_position_estimator # Vehicle Control # #MODULES += modules/segway # XXX needs state machine update -#MODULES += modules/fw_pos_control_l1 +MODULES += modules/fw_pos_control_l1 MODULES += modules/fw_att_control MODULES += modules/multirotor_att_control MODULES += modules/multirotor_pos_control @@ -112,6 +112,7 @@ LIBRARIES += lib/mathlib/CMSIS MODULES += lib/mathlib MODULES += lib/mathlib/math/filter MODULES += lib/ecl +MODULES += lib/external_lgpl MODULES += lib/geo # diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index d9dab1a2c8..ba0beec3e1 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -76,7 +76,7 @@ MODULES += examples/flow_position_estimator # Vehicle Control # #MODULES += modules/segway # XXX needs state machine update -#MODULES += modules/fw_pos_control_l1 +MODULES += modules/fw_pos_control_l1 MODULES += modules/fw_att_control MODULES += modules/multirotor_att_control MODULES += modules/multirotor_pos_control @@ -107,6 +107,7 @@ LIBRARIES += lib/mathlib/CMSIS MODULES += lib/mathlib MODULES += lib/mathlib/math/filter MODULES += lib/ecl +MODULES += lib/external_lgpl MODULES += lib/geo # From 57769ec4371df6ceabacf11aa130c4e8f4eb0240 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Sep 2013 14:09:09 +0200 Subject: [PATCH 605/872] Reducing timeout to 0.5 seconds --- Tools/com | Bin 0 -> 13632 bytes Tools/com.c | 191 +++++++++++++++++++++++++++++++++++++++++++ Tools/px_uploader.py | 2 +- 3 files changed, 192 insertions(+), 1 deletion(-) create mode 100755 Tools/com create mode 100644 Tools/com.c diff --git a/Tools/com b/Tools/com new file mode 100755 index 0000000000000000000000000000000000000000..1d80d4aa5be7643c59635245881cd263a756d9b6 GIT binary patch literal 13632 zcmeHOdu&_P89$~8q@iFJ$_5*cyNH(d$t1! zT1E^t%EcI2h#;h)iI*5d6AOq!snn1@mLN0@HkgVAn*e32&AO3I8Dw2X_WPZ4uWP$4 z`)B`5x<@+S`CjKc-}%ntCf~h&^VnZLKSK!50wIP@6GHehSAh; zz5ix^^G)QI7EdX8Qi^t#V*tZw^>1%2RgwL*Ql34$fNuish|_JcVI<7{#Btqnd%p89 z8Hf+FBMRvXg(_GfhKfnUh$s3w;{{f^z1)S$UYD)m&`yUc+um{iXwT?2Ls2sZrE+^s z7b$ytZ3Cnk_d+?rzd$?@>k03)70T^ZU##p++j7#yrNlM_hS3+kvnL!hdcs{1Ntdq2 zG20&3oQ3ok6~i#QjjmWnuc@@k{nIZ|{_U}ikj7p+W#K*<#sV5fV?%30k(#FL$PG$1 zXj!;nQMlZ+>#Vh5ghNJrptmy;GNOstDec+arM+UmxSoqqonja}BSqWp1baSB`RCXp zU2gAO#~50}2z7jsf0}J?*zQBR*dFJLtz9UF5r~B2$2}~M->9bi%i29iJFzMlDMF5) z`z@PqyushN**UW{W$OT%rJjcrg?@P?rod9%D^dl(2bK@5v$v?R9ds2sw66vZn)~&9 z3f8E*XZc#tke>-w6Zt5#&r1I}>=M*dmF$&5Ty_%S+2CoYi%?hf#bc{NJ)Nt91EC;j zman4HkLMTu_Rl|ix$ddqCzek>fA2cqrQmTMqN-ZfD;%0Ul;xN&`(?i*$pYXwb*Pld zGr8cWz&yvt#`rA*@r2pC^4gW~qo)&M6#CBb_8>l0XfL0ZUo#2xDjw8;6}iWfExif0{=Y%ZMyYG{obQB`q*uWihAnnnYI~ga4tPK zH!5;(Bfp_%YTv@Y;(GeL!^ie#o>$@}KN#`KP_Rm`OmFXJwbl zr_n`E*Fpat=v)4yY5&YJ4JsQ^MM3*+rF}<1yH#rEsLk9*srGl3wqDS_OlnV2yZ!{* zE0p$E3fi+sc3J)psLj+wsqG4-eRQGY_Zw3CeQFP#p#2^O*3*?^1?|VA_PYqcAtidM zAWBNnJNmtcYIYp-Ua||ugpl!19vy`vuoITbLT8XOm4Y7c%}PCBnT+=N{QRVUh74