From 62e07358b471df173e1a17fb8cc31b19ece38c55 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 24 Aug 2012 00:01:23 +0200 Subject: Ported almost everything to new param interface, ready for serious testing --- apps/commander/commander.c | 176 +++++++++++++-------------- apps/fixedwing_control/fixedwing_control.c | 79 ++++++++---- apps/mavlink/mavlink.c | 126 ++++++++----------- apps/px4/attitude_estimator_bm/attitude_bm.c | 4 - apps/systemlib/systemlib.c | 155 ++--------------------- apps/systemlib/systemlib.h | 6 - apps/uORB/objects_common.cpp | 3 + apps/uORB/topics/rc_channels.h | 7 +- apps/uORB/topics/subsystem_info.h | 92 ++++++++++++++ 9 files changed, 296 insertions(+), 352 deletions(-) create mode 100644 apps/uORB/topics/subsystem_info.h (limited to 'apps') diff --git a/apps/commander/commander.c b/apps/commander/commander.c index e89dae983..e2d197648 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -68,6 +68,7 @@ #include #include #include +#include #include #include @@ -108,7 +109,7 @@ static int deamon_task; /**< Handle of deamon task / thread */ /* pthread loops */ static void *command_handling_loop(void *arg); -// static void *subsystem_info_loop(void *arg); +static void *orb_receive_loop(void *arg); __EXPORT int commander_main(int argc, char *argv[]); @@ -137,6 +138,16 @@ int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, u */ static 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(int16_t a[], int n); + static int buzzer_init() { buzzer = open("/dev/tone_alarm", O_WRONLY); @@ -227,7 +238,7 @@ int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, u return 0; } -void cal_bsort(int16_t a[], int n) +static void cal_bsort(int16_t a[], int n) { int i,j,t; for(i=0;ipm.param_values[PARAM_SENSOR_GYRO_XOFFSET] = gyro_offset[0]; - global_data_parameter_storage->pm.param_values[PARAM_SENSOR_GYRO_YOFFSET] = gyro_offset[1]; - global_data_parameter_storage->pm.param_values[PARAM_SENSOR_GYRO_ZOFFSET] = gyro_offset[2]; + if (param_set(param_find("SENSOR_GYRO_XOFF"), &(gyro_offset[0]))) { + mavlink_log_critical(mavlink_fd, "[commander] Setting X gyro offset failed!"); + } + + if (param_set(param_find("SENSOR_GYRO_YOFF"), &(gyro_offset[1]))) { + mavlink_log_critical(mavlink_fd, "[commander] Setting Y gyro offset failed!"); + } + + if (param_set(param_find("SENSOR_GYRO_ZOFF"), &(gyro_offset[2]))) { + mavlink_log_critical(mavlink_fd, "[commander] Setting Z gyro offset failed!"); + } char offset_output[50]; sprintf(offset_output, "[commander] gyro cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]); @@ -625,6 +644,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* send any requested ACKs */ if (cmd->confirmation > 0) { /* send acknowledge command */ + // XXX TODO } } @@ -641,7 +661,7 @@ static void *command_handling_loop(void *arg) int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); struct vehicle_command_s cmd; - while (1) { + while (!thread_should_exit) { struct pollfd fds[1] = { { .fd = cmd_sub, .events = POLLIN } }; if (poll(fds, 1, 5000) == 0) { @@ -655,71 +675,37 @@ static void *command_handling_loop(void *arg) } } + close(cmd_sub); + return NULL; } -// static void *subsystem_info_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 subsys", getpid()); - -// uint8_t current_info_local = SUBSYSTEM_INFO_BUFFER_SIZE; -// uint16_t total_counter = 0; - -// while (1) { - -// if (0 == global_data_wait(&global_data_subsystem_info->access_conf)) { -// // printf("got subsystem_info\n"); - -// while (current_info_local != global_data_subsystem_info->current_info) { -// // printf("current_info_local = %d, current_info = %d \n", current_info_local, global_data_subsystem_info->current_info); - -// current_info_local++; - -// if (current_info_local >= SUBSYSTEM_INFO_BUFFER_SIZE) -// current_info_local = 0; - -// /* Handle the new subsystem info and write updated version of global_data_sys_status */ -// subsystem_info_t *info = &(global_data_subsystem_info->info[current_info_local]); - -// // printf("Commander got subsystem info: %d %d %d\n", info->present, info->enabled, info->health); - - -// if (info->present != 0) { -// update_state_machine_subsystem_present(stat_pub, ¤t_status, &info->subsystem_type); - -// } else { -// update_state_machine_subsystem_notpresent(stat_pub, ¤t_status, &info->subsystem_type); -// } - -// if (info->enabled != 0) { -// update_state_machine_subsystem_enabled(stat_pub, ¤t_status, &info->subsystem_type); - -// } else { -// update_state_machine_subsystem_disabled(stat_pub, ¤t_status, &info->subsystem_type); -// } +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 +{ + /* Set thread name */ + prctl(PR_SET_NAME, "commander orb rcv", getpid()); -// if (info->health != 0) { -// update_state_machine_subsystem_healthy(stat_pub, ¤t_status, &info->subsystem_type); + /* Subscribe to command topic */ + int subsys_sub = orb_subscribe(ORB_ID(subsystem_info)); + struct subsystem_info_s info; -// } else { -// update_state_machine_subsystem_unhealthy(stat_pub, ¤t_status, &info->subsystem_type); -// } + while (!thread_should_exit) { + struct pollfd fds[1] = { { .fd = subsys_sub, .events = POLLIN } }; -// total_counter++; -// } + 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); -// if (global_data_subsystem_info->counter - total_counter > SUBSYSTEM_INFO_BUFFER_SIZE) { -// printf("[commander] Warning: Too many subsystem status updates, subsystem_info buffer full\n"); //TODO: add to error queue -// global_data_subsystem_info->counter = total_counter; //this makes sure we print the warning only once -// } + printf("Subsys changed: %d\n", (int)info.subsystem_type); + } + } -// global_data_unlock(&global_data_subsystem_info->access_conf); -// } -// } + close(subsys_sub); -// return NULL; -// } + return NULL; +} @@ -736,16 +722,31 @@ enum BAT_CHEM { */ float battery_remaining_estimate_voltage(int cells, int chemistry, float voltage); -PARAM_DEFINE_FLOAT(BAT_VOLT_EMPTY, 3.2f); -PARAM_DEFINE_FLOAT(BAT_VOLT_FULL, 4.05f); +PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f); +PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f); float battery_remaining_estimate_voltage(int cells, int chemistry, float voltage) { float ret = 0; - // XXX do this properly - // XXX rebase on parameters - const float chemistry_voltage_empty[] = {3.2f}; - const float chemistry_voltage_full[] = {4.05f}; + static param_t bat_volt_empty; + static param_t bat_volt_full; + static bool initialized = false; + static unsigned int counter = 0; + + if (!initialized) { + bat_volt_empty = param_find("BAT_V_EMPTY"); + bat_volt_full = param_find("BAT_V_FULL"); + initialized = true; + } + + float chemistry_voltage_empty[1]; + float chemistry_voltage_full[1]; + + if (counter % 100 == 0) { + param_get(bat_volt_empty, chemistry_voltage_empty+0); + param_get(bat_volt_full, chemistry_voltage_full+0); + } + counter++; ret = (voltage - cells * chemistry_voltage_empty[chemistry]) / (cells * (chemistry_voltage_full[chemistry] - chemistry_voltage_empty[chemistry])); @@ -817,9 +818,9 @@ int commander_thread_main(int argc, char *argv[]) /* welcome user */ printf("[commander] I am in command now!\n"); - /* Pthreads */ + /* pthreads for command and subsystem info handling */ pthread_t command_handling_thread; - // pthread_t subsystem_info_thread; + pthread_t subsystem_info_thread; /* initialize */ if (led_init() != 0) { @@ -853,31 +854,21 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[commander] system is running"); - /* load EEPROM parameters */ - if (OK == get_params_from_eeprom(global_data_parameter_storage)) { - printf("[commander] Loaded EEPROM params in RAM\n"); - mavlink_log_info(mavlink_fd, "[commander] Loaded EEPROM params in RAM"); - - } else { - fprintf(stderr, "[commander] ERROR loading EEPROM params in RAM\n"); - mavlink_log_critical(mavlink_fd, "[commander] ERROR loading EEPROM params in RAM"); - } - /* create pthreads */ pthread_attr_t command_handling_attr; pthread_attr_init(&command_handling_attr); pthread_attr_setstacksize(&command_handling_attr, 4096); pthread_create(&command_handling_thread, &command_handling_attr, command_handling_loop, NULL); - // 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, subsystem_info_loop, NULL); + 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, NULL); /* Start monitoring loop */ uint16_t counter = 0; uint8_t flight_env; - // uint8_t fix_type; + /* Initialize to 3.0V to make sure the low-pass loads below valid threshold */ float battery_voltage = VOLTAGE_BATTERY_HIGH_VOLTS; bool battery_voltage_valid = true; @@ -888,9 +879,6 @@ int commander_thread_main(int argc, char *argv[]) int16_t mode_switch_rc_value; float bat_remain = 1.0f; -// bool arm_done = false; -// bool disarm_done = false; - uint16_t stick_off_counter = 0; uint16_t stick_on_counter = 0; @@ -919,7 +907,7 @@ int commander_thread_main(int argc, char *argv[]) /* now initialized */ commander_initialized = true; - while (1) { + while (!thread_should_exit) { /* Get current values */ orb_copy(ORB_ID(rc_channels), rc_sub, &rc); @@ -930,8 +918,6 @@ int commander_thread_main(int argc, char *argv[]) battery_voltage_valid = sensors.battery_voltage_valid; bat_remain = battery_remaining_estimate_voltage(3, BAT_CHEM_LITHIUM_POLYMERE, battery_voltage); -// flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]); - /* 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 */ @@ -1186,11 +1172,17 @@ 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(subsystem_info_thread, NULL); /* close fds */ led_deinit(); buzzer_deinit(); + close(rc_sub); + close(gps_sub); + close(sensor_sub); + + printf("[commander] exiting..\n"); + fflush(stdout); thread_running = false; diff --git a/apps/fixedwing_control/fixedwing_control.c b/apps/fixedwing_control/fixedwing_control.c index 42534a0a7..1d13b1c6a 100644 --- a/apps/fixedwing_control/fixedwing_control.c +++ b/apps/fixedwing_control/fixedwing_control.c @@ -248,7 +248,7 @@ static float pid(float error, float error_deriv, uint16_t dt, float scale, float * discrete low pass filter, cuts out the * high frequency noise that can drive the controller crazy */ - float RC = 1.0 / (2.0f * M_PI * fCut); + float RC = 1.0f / (2.0f * M_PI_F * fCut); derivative = lderiv + (delta_time / (RC + delta_time)) * (derivative - lderiv); @@ -288,17 +288,17 @@ static float pid(float error, float error_deriv, uint16_t dt, float scale, float return output; } -PARAM_DEFINE_FLOAT(FW_ATT_ROLL_P, 0.0f); -PARAM_DEFINE_FLOAT(FW_ATT_ROLL_I, 0.0f); -PARAM_DEFINE_FLOAT(FW_ATT_ROLL_D, 0.0f); -PARAM_DEFINE_FLOAT(FW_ATT_ROLL_AWU, 0.0f); -PARAM_DEFINE_FLOAT(FW_ATT_ROLL_LIM, 0.0f); +PARAM_DEFINE_FLOAT(FW_ATT_P, 2.0f); +PARAM_DEFINE_FLOAT(FW_ATT_I, 0.0f); +PARAM_DEFINE_FLOAT(FW_ATT_D, 0.0f); +PARAM_DEFINE_FLOAT(FW_ATT_AWU, 5.0f); +PARAM_DEFINE_FLOAT(FW_ATT_LIM, 10.0f); -PARAM_DEFINE_FLOAT(FW_ATT_PITCH_P, 0.0f); -PARAM_DEFINE_FLOAT(FW_ATT_PITCH_I, 0.0f); -PARAM_DEFINE_FLOAT(FW_ATT_PITCH_D, 0.0f); -PARAM_DEFINE_FLOAT(FW_ATT_PITCH_AWU, 0.0f); -PARAM_DEFINE_FLOAT(FW_ATT_PITCH_LIM, 0.0f); +PARAM_DEFINE_FLOAT(FW_POS_P, 2.0f); +PARAM_DEFINE_FLOAT(FW_POS_I, 0.0f); +PARAM_DEFINE_FLOAT(FW_POS_D, 0.0f); +PARAM_DEFINE_FLOAT(FW_POS_AWU, 5.0f); +PARAM_DEFINE_FLOAT(FW_POS_LIM, 10.0f); /** * Load parameters from global storage. @@ -308,21 +308,50 @@ PARAM_DEFINE_FLOAT(FW_ATT_PITCH_LIM, 0.0f); * Fetches the current parameters from the global parameter storage and writes them * to the plane_data structure */ -static void get_parameters(plane_data_t * plane_data) +static void get_parameters(plane_data_t * pdata) { - plane_data->Kp_att = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P]; - plane_data->Ki_att = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I]; - plane_data->Kd_att = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D]; - plane_data->Kp_pos = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P]; - plane_data->Ki_pos = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I]; - plane_data->Kd_pos = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D]; - plane_data->intmax_att = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU]; - plane_data->intmax_pos = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU]; - plane_data->airspeed = global_data_parameter_storage->pm.param_values[PARAM_AIRSPEED]; - plane_data->wp_x = global_data_parameter_storage->pm.param_values[PARAM_WPLON]; - plane_data->wp_y = global_data_parameter_storage->pm.param_values[PARAM_WPLAT]; - plane_data->wp_z = global_data_parameter_storage->pm.param_values[PARAM_WPALT]; - plane_data->mode = global_data_parameter_storage->pm.param_values[PARAM_FLIGHTMODE]; + static bool initialized = false; + static param_t att_p; + static param_t att_i; + static param_t att_d; + static param_t att_awu; + static param_t att_lim; + + static param_t pos_p; + static param_t pos_i; + static param_t pos_d; + static param_t pos_awu; + static param_t pos_lim; + + if (!initialized) { + att_p = param_find("FW_ATT_P"); + att_i = param_find("FW_ATT_I"); + att_d = param_find("FW_ATT_D"); + att_awu = param_find("FW_ATT_AWU"); + att_lim = param_find("FW_ATT_LIM"); + + pos_p = param_find("FW_POS_P"); + pos_i = param_find("FW_POS_I"); + pos_d = param_find("FW_POS_D"); + pos_awu = param_find("FW_POS_AWU"); + pos_lim = param_find("FW_POS_LIM"); + + initialized = true; + } + + param_get(att_p, &(pdata->Kp_att)); + param_get(att_i, &(pdata->Ki_att)); + param_get(att_d, &(pdata->Kd_att)); + param_get(pos_p, &(pdata->Kp_pos)); + param_get(pos_i, &(pdata->Ki_pos)); + param_get(pos_d, &(pdata->Kd_pos)); + param_get(att_awu, &(pdata->intmax_att)); + param_get(pos_awu, &(pdata->intmax_pos)); + pdata->airspeed = 10; + pdata->wp_x = 48; + pdata->wp_y = 8; + pdata->wp_z = 100; + pdata->mode = 1; } /** diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 1b23f6751..4f3a8295d 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -75,9 +75,15 @@ #include #include #include +#include + #include "waypoints.h" #include "mavlink_log.h" +/* define MAVLink specific parameters */ +PARAM_DEFINE_INT32(MAV_SYS_ID, 1); +PARAM_DEFINE_INT32(MAV_COMP_ID, 50); +PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_GENERIC); __EXPORT int mavlink_main(int argc, char *argv[]); int mavlink_thread_main(int argc, char *argv[]); @@ -90,8 +96,7 @@ static int mavlink_task; static bool mavlink_link_termination_allowed = false; static bool mavlink_exit_requested = false; -static int system_type = MAV_TYPE_FIXED_WING; -mavlink_system_t mavlink_system = {100, 50, 0, 0, 0, 0}; // System ID, 1-255, Component/Subsystem ID, 1-255 +mavlink_system_t mavlink_system = {100, 50, MAV_TYPE_FIXED_WING, 0, 0, 0}; // System ID, 1-255, Component/Subsystem ID, 1-255 static uint8_t chan = MAVLINK_COMM_0; static mavlink_status_t status; @@ -531,7 +536,7 @@ static void *uorb_receiveloop(void *arg) struct mavlink_subscriptions *subs = (struct mavlink_subscriptions *)arg; /* Set thread name */ - prctl(PR_SET_NAME, "mavlink uORB", getpid()); + prctl(PR_SET_NAME, "mavlink orb rcv", getpid()); /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ @@ -787,7 +792,7 @@ static void *uorb_receiveloop(void *arg) get_mavlink_mode_and_state(&v_status, &mavlink_state, &mavlink_mode); /* send heartbeat */ - mavlink_msg_heartbeat_send(chan, system_type, MAV_AUTOPILOT_GENERIC, mavlink_mode, v_status.state_machine, mavlink_state); + mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state); } /* --- RC CHANNELS --- */ @@ -1080,7 +1085,7 @@ void handleMessage(mavlink_message_t *msg) || (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 */ //TODO: check what happens with global_data buffers that are read by the mavlink app + /* This is the link shutdown command, terminate mavlink */ printf("[mavlink] Terminating .. \n"); fflush(stdout); usleep(50000); @@ -1250,63 +1255,6 @@ void handleMessage(mavlink_message_t *msg) hil_attitude.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(vehicle_attitude), pub_hil_attitude, &hil_attitude); } - - // if (msg->msgid == MAVLINK_MSG_ID_ATTITUDE) { - // mavlink_attitude_t att; - // mavlink_msg_attitude_decode(msg, &att); - // float RAD2DEG = 57.3f; - - // // printf("\n\n\n ATTITUDE \n\n\n %i \n", (int)(1000*att.rollspeed)); - - // global_data_lock(&global_data_attitude->access_conf); - // global_data_attitude->roll = RAD2DEG * att.roll; - // global_data_attitude->pitch = RAD2DEG * att.pitch; - // global_data_attitude->yaw = RAD2DEG * att.yaw; - // global_data_attitude->rollspeed = att.rollspeed; - // global_data_attitude->pitchspeed = att.pitchspeed; - // global_data_attitude->yawspeed = att.yawspeed; - - // global_data_attitude->counter++; - // global_data_attitude->timestamp = hrt_absolute_time(); - // global_data_unlock(&global_data_attitude->access_conf); - // global_data_broadcast(&global_data_attitude->access_conf); - // } - - // if (msg->msgid == MAVLINK_MSG_ID_RAW_IMU) { - // mavlink_raw_imu_t imu; - // mavlink_msg_raw_imu_decode(msg, &imu); - - // // printf("\n\n\n RAW_IMU : %i \n %i \n %i \n %i \n %i \n %i \n\n\n", (int)(1000*imu.xgyro), - // // (int)(1000*imu.ygyro), (int)(1000*imu.zgyro)); - - // global_data_lock(&global_data_attitude->access_conf); - // global_data_attitude->rollspeed = 1000 * imu.xgyro; - // global_data_attitude->pitchspeed = 1000 * imu.ygyro; - // global_data_attitude->yawspeed = 1000 * imu.zgyro; - - // global_data_attitude->counter++; - // global_data_attitude->timestamp = hrt_absolute_time(); - // global_data_unlock(&global_data_attitude->access_conf); - // global_data_broadcast(&global_data_attitude->access_conf); - // } - - // if (msg->msgid == MAVLINK_MSG_ID_SCALED_IMU) { - // mavlink_raw_imu_t imu; - // mavlink_msg_raw_imu_decode(msg, &imu); - - // // printf("\n\n\n SCALED_IMU : %i \n %i \n %i \n %i \n %i \n %i \n\n\n", (int)(1000*imu.xgyro), - // // (int)(1000*imu.ygyro), (int)(1000*imu.zgyro)); - - // global_data_lock(&global_data_attitude->access_conf); - // global_data_attitude->rollspeed = 1000 * imu.xgyro; - // global_data_attitude->pitchspeed = 1000 * imu.ygyro; - // global_data_attitude->yawspeed = 1000 * imu.zgyro; - - // global_data_attitude->counter++; - // global_data_attitude->timestamp = hrt_absolute_time(); - // global_data_unlock(&global_data_attitude->access_conf); - // global_data_broadcast(&global_data_attitude->access_conf); - // } } } @@ -1483,6 +1431,11 @@ int mavlink_thread_main(int argc, char *argv[]) /* Flush UART */ fflush(stdout); + /* Initialize system properties */ + param_t param_system_id = param_find("MAV_SYS_ID"); + param_t param_component_id = param_find("MAV_COMP_ID"); + param_t param_system_type = param_find("MAV_TYPE"); + /* topics to subscribe globally */ /* subscribe to ORB for global position */ mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); @@ -1507,7 +1460,8 @@ int mavlink_thread_main(int argc, char *argv[]) mavlink_wpm_init(wpm); uint16_t counter = 0; - int lowspeed_counter = 0; + /* arm counter to go off immediately */ + int lowspeed_counter = 10; /* make sure all threads have registered their subscriptions */ while (!mavlink_subs.initialized) { @@ -1565,21 +1519,34 @@ int mavlink_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_global_position), mavlink_subs.global_pos_sub, &global_pos); orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); - /* check if waypoint has been reached against the last positions */ - mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos); + /* 1 Hz */ + if (lowspeed_counter == 10) { + /* update system and component id */ + int32_t system_id; + param_get(param_system_id, &system_id); + if (system_id > 0 && system_id < 255) { + mavlink_system.sysid = system_id; + } - // sleep - usleep(50000); + int32_t component_id; + param_get(param_component_id, &component_id); + if (component_id > 0 && component_id < 255) { + mavlink_system.compid = component_id; + } + + int32_t system_type; + param_get(param_system_type, &system_type); + if (system_type >= 0 && system_type < MAV_AUTOPILOT_ENUM_END) { + mavlink_system.type = system_type; + } - // 1 Hz - if (lowspeed_counter == 10) { /* 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(&v_status, &mavlink_state, &mavlink_mode); /* send heartbeat */ - mavlink_msg_heartbeat_send(chan, system_type, MAV_AUTOPILOT_GENERIC, mavlink_mode, v_status.state_machine, mavlink_state); + mavlink_msg_heartbeat_send(chan, system_type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state); /* send status (values already copied in the section above) */ mavlink_msg_sys_status_send(chan, v_status.onboard_control_sensors_present, v_status.onboard_control_sensors_enabled, @@ -1595,18 +1562,33 @@ int mavlink_thread_main(int argc, char *argv[]) lowspeed_counter = 0; } - lowspeed_counter++; + /* 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); + + /* sleep quarter the time */ + usleep(25000); + /* send parameters at 20 Hz (if queued for sending) */ mavlink_pm_queued_send(); - usleep(50000); + /* sleep quarter the time */ + usleep(25000); mavlink_pm_queued_send(); + /* sleep 10 ms */ + usleep(10000); + /* send one string at 10 Hz */ mavlink_missionlib_send_gcs_string(mavlink_message_string); mavlink_message_string[0] = '\0'; counter++; + + /* sleep 15 ms */ + usleep(15000); } /* wait for threads to complete */ diff --git a/apps/px4/attitude_estimator_bm/attitude_bm.c b/apps/px4/attitude_estimator_bm/attitude_bm.c index 41ef47918..1ffe9f7bd 100644 --- a/apps/px4/attitude_estimator_bm/attitude_bm.c +++ b/apps/px4/attitude_estimator_bm/attitude_bm.c @@ -207,10 +207,6 @@ void attitude_blackmagic_init(void) void attitude_blackmagic(const float_vect3 *accel, const float_vect3 *mag, const float_vect3 *gyro) { - //Transform accelerometer used in all directions - // float_vect3 acc_nav; - //body2navi(&global_data.accel_si, &global_data.attitude, &acc_nav); - // Kalman Filter //Calculate new linearized A matrix diff --git a/apps/systemlib/systemlib.c b/apps/systemlib/systemlib.c index b1a8de7bd..1433d1113 100644 --- a/apps/systemlib/systemlib.c +++ b/apps/systemlib/systemlib.c @@ -123,142 +123,6 @@ void kill_task(FAR _TCB *tcb, FAR void *arg) kill(tcb->pid, SIGUSR1); } -union param_union { - float f; - char c[4]; -}; - -int store_params_in_eeprom(struct global_data_parameter_storage_t *params) -{ - int ret = ERROR; - int fd = open("/dev/eeprom", O_RDWR | O_NONBLOCK); - int lseek_res = lseek(fd, EEPROM_OFFSET, SEEK_SET); //don't touch first 64 bytes - int write_res; - - if (fd < 0) { - fprintf(stderr, "onboard eeprom: open fail\n"); - ret = ERROR; - - } else if (lseek_res < 0) { - fprintf(stderr, "onboard eeprom: set offet fail\n"); - ret = ERROR; - - } else { - /*Write start magic byte */ - uint8_t mb = EEPROM_PARAM_MAGIC_BYTE; - write_res = write(fd, &mb, sizeof(mb)); - - if (write_res != sizeof(mb)) { - ret = ERROR; - - } else { - for (int i = 0; i < PARAM_MAX_COUNT; i++) { - - union param_union p; - p.f = params->pm.param_values[i]; - write_res = write(fd, p.c, sizeof(p.f)); - - if (write_res != sizeof(p.f)) return ERROR; - } - - /*Write end magic byte */ - write_res = write(fd, &mb, sizeof(mb)); - - if (write_res != sizeof(mb)) { - ret = ERROR; - - } else { - ret = OK; - } - } - - } - - close(fd); - - return ret; -} - -int get_params_from_eeprom(struct global_data_parameter_storage_t *params) -{ - int ret = ERROR; - uint8_t magic_byte = 0; - int fd = open("/dev/eeprom", O_RDWR | O_NONBLOCK); - int lseek_res = lseek(fd, EEPROM_OFFSET, SEEK_SET); //don't touch first 64 bytes - - if (fd < 0) { - fprintf(stderr, "onboard eeprom: open fail\n"); - ret = ERROR; - - } else if (lseek_res < 0) { - fprintf(stderr, "onboard eeprom: set offet fail\n"); - ret = ERROR; - - } else { - /*Get start magic byte */ - magic_byte = 0; - int read_res = read(fd, &magic_byte, sizeof(uint8_t)); - - if (read_res != sizeof(uint8_t)) { - ret = ERROR; - - } else { - if (magic_byte != EEPROM_PARAM_MAGIC_BYTE) { - ret = ERROR; - fprintf(stderr, "onboard eeprom: parameters: start magic byte wrong\n"); - - } else { - /*get end magic byte */ - lseek_res = lseek(fd, EEPROM_OFFSET + 1 + params->pm.size * sizeof(float), SEEK_SET); // jump to 2nd magic byte - - if (lseek_res == OK) { - /*Get end magic */ - read_res = read(fd, &magic_byte, sizeof(uint8_t)); - - if (read_res != sizeof(uint8_t)) { - ret = ERROR; - - } else { - if (magic_byte != EEPROM_PARAM_MAGIC_BYTE) { - ret = ERROR; - printf("onboard eeprom: parameters: end magic byte wrong\n"); - - } else { - lseek_res = lseek(fd, EEPROM_OFFSET + 1, SEEK_SET); - - /* read data */ - if (lseek_res == OK) { - - for (int i = 0; i < PARAM_MAX_COUNT; i++) { - union param_union p; - read_res = read(fd, p.c, sizeof(p.f)); - params->pm.param_values[i] = p.f; - if (read_res != sizeof(p.f)) return ERROR; - } - - ret = OK; - - } else { - /* lseek #2 failed */ - ret = ERROR; - } - } - } - - } else { - /* lseek #1 failed */ - ret = ERROR; - } - } - } - - } - - close(fd); - - return ret; -} - #define PX4_BOARD_ID_FMU (5) int fmu_get_board_info(struct fmu_board_info_s *info) @@ -268,23 +132,20 @@ int fmu_get_board_info(struct fmu_board_info_s *info) int statres; /* Copy version-specific fields */ - statres = stat("/dev/bma280", &sb); + statres = stat("/dev/bma180", &sb); if (statres == OK) { - /* BMA280 indicates a v1.7+ board */ - strcpy(info->board_name, "FMU v1.7"); - info->board_version = 17; - - } else { - statres = stat("/dev/bma180", &sb); - - if (statres == OK) { /* BMA180 indicates a v1.5-v1.6 board */ strcpy(info->board_name, "FMU v1.6"); info->board_version = 16; - + } else { + statres = stat("/dev/accel", &sb); + if (statres == OK) { + /* MPU-6000 indicates a v1.7+ board */ + strcpy(info->board_name, "FMU v1.7"); + info->board_version = 17; } else { - /* If no BMA pressure sensor is present, it is a v1.3 board */ + /* If no BMA and no MPU is present, it is a v1.3 board */ strcpy(info->board_name, "FMU v1.3"); info->board_version = 13; } diff --git a/apps/systemlib/systemlib.h b/apps/systemlib/systemlib.h index eec43a5b4..2824cbec9 100644 --- a/apps/systemlib/systemlib.h +++ b/apps/systemlib/systemlib.h @@ -50,12 +50,6 @@ __EXPORT int reboot(void); /** Sends SIGUSR1 to all processes */ __EXPORT void killall(void); -struct global_data_parameter_storage_t; - -__EXPORT int store_params_in_eeprom(struct global_data_parameter_storage_t *params); - -__EXPORT int get_params_from_eeprom(struct global_data_parameter_storage_t *params); - enum MULT_PORTS { MULT_0_US2_RXTX = 0, MULT_1_US2_FLOW, diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp index d6c00f996..c60a93fcb 100644 --- a/apps/uORB/objects_common.cpp +++ b/apps/uORB/objects_common.cpp @@ -108,6 +108,9 @@ ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s); #include "topics/optical_flow.h" ORB_DEFINE(optical_flow, struct optical_flow_s); +#include "topics/subsystem_info.h" +ORB_DEFINE(subsystem_info, struct subsystem_info_s); + #include "topics/actuator_controls.h" ORB_DEFINE(actuator_controls_0, struct actuator_controls_s); ORB_DEFINE(actuator_controls_1, struct actuator_controls_s); diff --git a/apps/uORB/topics/rc_channels.h b/apps/uORB/topics/rc_channels.h index 5bd2adeec..28cc5d7c4 100644 --- a/apps/uORB/topics/rc_channels.h +++ b/apps/uORB/topics/rc_channels.h @@ -62,11 +62,6 @@ enum RC_CHANNELS_STATUS * This defines the mapping of the RC functions. * The value assigned to the specific function corresponds to the entry of * the channel array chan[]. - * Ex. To read out the scaled Pitch value: - * pitch = global_data_rc_channels->chan[PITCH].scale; - * The override is on channel 8, since we don't usually have a 12 channel RC - * and channel 5/6 (GRAUPNER/FUTABA) are mapped to the second ROLL servo, which - * can only be disabled on more advanced RC sets. */ enum RC_CHANNELS_FUNCTION { @@ -98,7 +93,7 @@ struct rc_channels_s { uint16_t override; enum RC_CHANNELS_STATUS status; /**< status of the channel */ } chan[RC_CHANNELS_FUNCTION_MAX]; - uint8_t chan_count; /**< maximum number of valid channels */ + uint8_t chan_count; /**< maximum number of valid channels */ /*String array to store the names of the functions*/ const char function_name[RC_CHANNELS_FUNCTION_MAX][20]; diff --git a/apps/uORB/topics/subsystem_info.h b/apps/uORB/topics/subsystem_info.h new file mode 100644 index 000000000..89b397478 --- /dev/null +++ b/apps/uORB/topics/subsystem_info.h @@ -0,0 +1,92 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier + * @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 subsystem_info.h + * Definition of the subsystem info topic. + */ + +#ifndef TOPIC_SUBSYSTEM_INFO_H_ +#define TOPIC_SUBSYSTEM_INFO_H_ + +#include +#include +#include "../uORB.h" + +/** + * @addtogroup topics + */ + +enum SUBSYSTEM_TYPE +{ + SUBSYSTEM_TYPE_GYRO = 1, + SUBSYSTEM_TYPE_ACC = 2, + SUBSYSTEM_TYPE_MAG = 4, + SUBSYSTEM_TYPE_ABSPRESSURE = 8, + SUBSYSTEM_TYPE_DIFFPRESSURE = 16, + SUBSYSTEM_TYPE_GPS = 32, + SUBSYSTEM_TYPE_OPTICALFLOW = 64, + SUBSYSTEM_TYPE_CVPOSITION = 128, + SUBSYSTEM_TYPE_LASERPOSITION = 256, + SUBSYSTEM_TYPE_EXTERNALGROUNDTRUTH = 512, + SUBSYSTEM_TYPE_ANGULARRATECONTROL = 1024, + SUBSYSTEM_TYPE_ATTITUDESTABILIZATION = 2048, + SUBSYSTEM_TYPE_YAWPOSITION = 4096, + SUBSYSTEM_TYPE_ALTITUDECONTROL = 16384, + SUBSYSTEM_TYPE_POSITIONCONTROL = 32768, + SUBSYSTEM_TYPE_MOTORCONTROL = 65536 +}; + +/** + * State of individual sub systems + */ +struct subsystem_info_s { + bool present; + bool enabled; + bool ok; + + enum SUBSYSTEM_TYPE subsystem_type; +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(subsystem_info); + +#endif /* TOPIC_SUBSYSTEM_INFO_H_ */ + -- cgit v1.2.3