fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-07 03:11:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Ported almost everything to new param interface, ready for serious testing

Browse Source
This commit is contained in:
Lorenz Meier
2012-08-24 00:01:23 +02:00
parent b07de1379d
commit 62e07358b4
9 changed files with 297 additions and 353 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/commander/commander.c
+84 -92
View File
@@ -68,6 +68,7 @@
#include <uORB/topics/rc_channels.h> #include <uORB/topics/rc_channels.h>
#include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_command.h> #include <uORB/topics/vehicle_command.h>
#include <uORB/topics/subsystem_info.h>
#include <mavlink/mavlink_log.h> #include <mavlink/mavlink_log.h>
#include <systemlib/param/param.h> #include <systemlib/param/param.h>
@@ -108,7 +109,7 @@ static int deamon_task; /**< Handle of deamon task / thread */
/* pthread loops */ /* pthread loops */
static void *command_handling_loop(void *arg); 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[]); __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); 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() static int buzzer_init()
{ {
buzzer = open("/dev/tone_alarm", O_WRONLY); 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; return 0;
} }
void cal_bsort(int16_t a[], int n) static void cal_bsort(int16_t a[], int n)
{ {
int i,j,t; int i,j,t;
for(i=0;i<n-1;i++) for(i=0;i<n-1;i++)
@@ -465,9 +476,17 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *current_status
gyro_offset[1] = gyro_offset[1] / calibration_count; gyro_offset[1] = gyro_offset[1] / calibration_count;
gyro_offset[2] = gyro_offset[2] / calibration_count; gyro_offset[2] = gyro_offset[2] / calibration_count;
global_data_parameter_storage->pm.param_values[PARAM_SENSOR_GYRO_XOFFSET] = gyro_offset[0]; if (param_set(param_find("SENSOR_GYRO_XOFF"), &(gyro_offset[0]))) {
global_data_parameter_storage->pm.param_values[PARAM_SENSOR_GYRO_YOFFSET] = gyro_offset[1]; mavlink_log_critical(mavlink_fd, "[commander] Setting X gyro offset failed!");
global_data_parameter_storage->pm.param_values[PARAM_SENSOR_GYRO_ZOFFSET] = gyro_offset[2]; }
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]; 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]); 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 */ /* send any requested ACKs */
if (cmd->confirmation > 0) { if (cmd->confirmation > 0) {
/* send acknowledge command */ /* 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)); int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
struct vehicle_command_s cmd; struct vehicle_command_s cmd;
while (1) { while (!thread_should_exit) {
struct pollfd fds[1] = { { .fd = cmd_sub, .events = POLLIN } }; struct pollfd fds[1] = { { .fd = cmd_sub, .events = POLLIN } };
if (poll(fds, 1, 5000) == 0) { if (poll(fds, 1, 5000) == 0) {
@@ -655,71 +675,37 @@ static void *command_handling_loop(void *arg)
} }
} }
close(cmd_sub);
return NULL; 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 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 */ /* Set thread name */
// prctl(PR_SET_NAME, "commander subsys", getpid()); prctl(PR_SET_NAME, "commander orb rcv", getpid());
// uint8_t current_info_local = SUBSYSTEM_INFO_BUFFER_SIZE; /* Subscribe to command topic */
// uint16_t total_counter = 0; int subsys_sub = orb_subscribe(ORB_ID(subsystem_info));
struct subsystem_info_s info;
// while (1) { while (!thread_should_exit) {
struct pollfd fds[1] = { { .fd = subsys_sub, .events = POLLIN } };
// if (0 == global_data_wait(&global_data_subsystem_info->access_conf)) { if (poll(fds, 1, 5000) == 0) {
// // printf("got subsystem_info\n"); /* timeout, but this is no problem, silently ignore */
} else {
/* got command */
orb_copy(ORB_ID(subsystem_info), subsys_sub, &info);
// while (current_info_local != global_data_subsystem_info->current_info) { printf("Subsys changed: %d\n", (int)info.subsystem_type);
// // printf("current_info_local = %d, current_info = %d \n", current_info_local, global_data_subsystem_info->current_info); }
}
// current_info_local++; close(subsys_sub);
// if (current_info_local >= SUBSYSTEM_INFO_BUFFER_SIZE) return NULL;
// 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, &current_status, &info->subsystem_type);
// } else {
// update_state_machine_subsystem_notpresent(stat_pub, &current_status, &info->subsystem_type);
// }
// if (info->enabled != 0) {
// update_state_machine_subsystem_enabled(stat_pub, &current_status, &info->subsystem_type);
// } else {
// update_state_machine_subsystem_disabled(stat_pub, &current_status, &info->subsystem_type);
// }
// if (info->health != 0) {
// update_state_machine_subsystem_healthy(stat_pub, &current_status, &info->subsystem_type);
// } else {
// update_state_machine_subsystem_unhealthy(stat_pub, &current_status, &info->subsystem_type);
// }
// total_counter++;
// }
// 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
// }
// global_data_unlock(&global_data_subsystem_info->access_conf);
// }
// }
// return NULL;
// }
@@ -736,16 +722,31 @@ enum BAT_CHEM {
*/ */
float battery_remaining_estimate_voltage(int cells, int chemistry, float voltage); float battery_remaining_estimate_voltage(int cells, int chemistry, float voltage);
PARAM_DEFINE_FLOAT(BAT_VOLT_EMPTY, 3.2f); PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f);
PARAM_DEFINE_FLOAT(BAT_VOLT_FULL, 4.05f); PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f);
float battery_remaining_estimate_voltage(int cells, int chemistry, float voltage) float battery_remaining_estimate_voltage(int cells, int chemistry, float voltage)
{ {
float ret = 0; float ret = 0;
// XXX do this properly static param_t bat_volt_empty;
// XXX rebase on parameters static param_t bat_volt_full;
const float chemistry_voltage_empty[] = {3.2f}; static bool initialized = false;
const float chemistry_voltage_full[] = {4.05f}; 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])); 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 */ /* welcome user */
printf("[commander] I am in command now!\n"); printf("[commander] I am in command now!\n");
/* Pthreads */ /* pthreads for command and subsystem info handling */
pthread_t command_handling_thread; pthread_t command_handling_thread;
// pthread_t subsystem_info_thread; pthread_t subsystem_info_thread;
/* initialize */ /* initialize */
if (led_init() != 0) { 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"); 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 */ /* create pthreads */
pthread_attr_t command_handling_attr; pthread_attr_t command_handling_attr;
pthread_attr_init(&command_handling_attr); pthread_attr_init(&command_handling_attr);
pthread_attr_setstacksize(&command_handling_attr, 4096); pthread_attr_setstacksize(&command_handling_attr, 4096);
pthread_create(&command_handling_thread, &command_handling_attr, command_handling_loop, NULL); pthread_create(&command_handling_thread, &command_handling_attr, command_handling_loop, NULL);
// pthread_attr_t subsystem_info_attr; pthread_attr_t subsystem_info_attr;
// pthread_attr_init(&subsystem_info_attr); pthread_attr_init(&subsystem_info_attr);
// pthread_attr_setstacksize(&subsystem_info_attr, 2048); pthread_attr_setstacksize(&subsystem_info_attr, 2048);
// pthread_create(&subsystem_info_thread, &subsystem_info_attr, subsystem_info_loop, NULL); pthread_create(&subsystem_info_thread, &subsystem_info_attr, orb_receive_loop, NULL);
/* Start monitoring loop */ /* Start monitoring loop */
uint16_t counter = 0; uint16_t counter = 0;
uint8_t flight_env; uint8_t flight_env;
// uint8_t fix_type;
/* Initialize to 3.0V to make sure the low-pass loads below valid threshold */ /* Initialize to 3.0V to make sure the low-pass loads below valid threshold */
float battery_voltage = VOLTAGE_BATTERY_HIGH_VOLTS; float battery_voltage = VOLTAGE_BATTERY_HIGH_VOLTS;
bool battery_voltage_valid = true; bool battery_voltage_valid = true;
@@ -888,9 +879,6 @@ int commander_thread_main(int argc, char *argv[])
int16_t mode_switch_rc_value; int16_t mode_switch_rc_value;
float bat_remain = 1.0f; float bat_remain = 1.0f;
// bool arm_done = false;
// bool disarm_done = false;
uint16_t stick_off_counter = 0; uint16_t stick_off_counter = 0;
uint16_t stick_on_counter = 0; uint16_t stick_on_counter = 0;
@@ -919,7 +907,7 @@ int commander_thread_main(int argc, char *argv[])
/* now initialized */ /* now initialized */
commander_initialized = true; commander_initialized = true;
while (1) { while (!thread_should_exit) {
/* Get current values */ /* Get current values */
orb_copy(ORB_ID(rc_channels), rc_sub, &rc); 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; battery_voltage_valid = sensors.battery_voltage_valid;
bat_remain = battery_remaining_estimate_voltage(3, BAT_CHEM_LITHIUM_POLYMERE, battery_voltage); 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 */ /* Slow but important 8 Hz checks */
if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) { if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) {
/* toggle activity (blue) led at 1 Hz in standby, 10 Hz in armed mode */ /* 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 */ /* wait for threads to complete */
pthread_join(command_handling_thread, NULL); pthread_join(command_handling_thread, NULL);
// pthread_join(subsystem_info_thread, NULL); pthread_join(subsystem_info_thread, NULL);
/* close fds */ /* close fds */
led_deinit(); led_deinit();
buzzer_deinit(); buzzer_deinit();
close(rc_sub);
close(gps_sub);
close(sensor_sub);
printf("[commander] exiting..\n");
fflush(stdout);
thread_running = false; thread_running = false;
apps/fixedwing_control/fixedwing_control.c
+54 -25
View File
@@ -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 * discrete low pass filter, cuts out the
* high frequency noise that can drive the controller crazy * 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 + derivative = lderiv +
(delta_time / (RC + delta_time)) * (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; return output;
} }
PARAM_DEFINE_FLOAT(FW_ATT_ROLL_P, 0.0f); PARAM_DEFINE_FLOAT(FW_ATT_P, 2.0f);
PARAM_DEFINE_FLOAT(FW_ATT_ROLL_I, 0.0f); PARAM_DEFINE_FLOAT(FW_ATT_I, 0.0f);
PARAM_DEFINE_FLOAT(FW_ATT_ROLL_D, 0.0f); PARAM_DEFINE_FLOAT(FW_ATT_D, 0.0f);
PARAM_DEFINE_FLOAT(FW_ATT_ROLL_AWU, 0.0f); PARAM_DEFINE_FLOAT(FW_ATT_AWU, 5.0f);
PARAM_DEFINE_FLOAT(FW_ATT_ROLL_LIM, 0.0f); PARAM_DEFINE_FLOAT(FW_ATT_LIM, 10.0f);
PARAM_DEFINE_FLOAT(FW_ATT_PITCH_P, 0.0f); PARAM_DEFINE_FLOAT(FW_POS_P, 2.0f);
PARAM_DEFINE_FLOAT(FW_ATT_PITCH_I, 0.0f); PARAM_DEFINE_FLOAT(FW_POS_I, 0.0f);
PARAM_DEFINE_FLOAT(FW_ATT_PITCH_D, 0.0f); PARAM_DEFINE_FLOAT(FW_POS_D, 0.0f);
PARAM_DEFINE_FLOAT(FW_ATT_PITCH_AWU, 0.0f); PARAM_DEFINE_FLOAT(FW_POS_AWU, 5.0f);
PARAM_DEFINE_FLOAT(FW_ATT_PITCH_LIM, 0.0f); PARAM_DEFINE_FLOAT(FW_POS_LIM, 10.0f);
/** /**
* Load parameters from global storage. * 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 * Fetches the current parameters from the global parameter storage and writes them
* to the plane_data structure * 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]; static bool initialized = false;
plane_data->Ki_att = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I]; static param_t att_p;
plane_data->Kd_att = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D]; static param_t att_i;
plane_data->Kp_pos = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P]; static param_t att_d;
plane_data->Ki_pos = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I]; static param_t att_awu;
plane_data->Kd_pos = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D]; static param_t att_lim;
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]; static param_t pos_p;
plane_data->airspeed = global_data_parameter_storage->pm.param_values[PARAM_AIRSPEED]; static param_t pos_i;
plane_data->wp_x = global_data_parameter_storage->pm.param_values[PARAM_WPLON]; static param_t pos_d;
plane_data->wp_y = global_data_parameter_storage->pm.param_values[PARAM_WPLAT]; static param_t pos_awu;
plane_data->wp_z = global_data_parameter_storage->pm.param_values[PARAM_WPALT]; static param_t pos_lim;
plane_data->mode = global_data_parameter_storage->pm.param_values[PARAM_FLIGHTMODE];
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;
} }
/** /**
apps/mavlink/mavlink.c
+55 -73
View File
@@ -75,9 +75,15 @@
#include <uORB/topics/optical_flow.h> #include <uORB/topics/optical_flow.h>
#include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/manual_control_setpoint.h>
#include <systemlib/param/param.h>
#include "waypoints.h" #include "waypoints.h"
#include "mavlink_log.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[]); __EXPORT int mavlink_main(int argc, char *argv[]);
int mavlink_thread_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_link_termination_allowed = false;
static bool mavlink_exit_requested = false; static bool mavlink_exit_requested = false;
static int system_type = MAV_TYPE_FIXED_WING; mavlink_system_t mavlink_system = {100, 50, MAV_TYPE_FIXED_WING, 0, 0, 0}; // System ID, 1-255, Component/Subsystem ID, 1-255
mavlink_system_t mavlink_system = {100, 50, 0, 0, 0, 0}; // System ID, 1-255, Component/Subsystem ID, 1-255
static uint8_t chan = MAVLINK_COMM_0; static uint8_t chan = MAVLINK_COMM_0;
static mavlink_status_t status; static mavlink_status_t status;
@@ -531,7 +536,7 @@ static void *uorb_receiveloop(void *arg)
struct mavlink_subscriptions *subs = (struct mavlink_subscriptions *)arg; struct mavlink_subscriptions *subs = (struct mavlink_subscriptions *)arg;
/* Set thread name */ /* 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 --- */ /* --- 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); get_mavlink_mode_and_state(&v_status, &mavlink_state, &mavlink_mode);
/* send heartbeat */ /* 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 --- */ /* --- RC CHANNELS --- */
@@ -1080,7 +1085,7 @@ void handleMessage(mavlink_message_t *msg)
|| (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
//check for MAVLINK terminate command //check for MAVLINK terminate command
if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) { 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"); printf("[mavlink] Terminating .. \n");
fflush(stdout); fflush(stdout);
usleep(50000); usleep(50000);
@@ -1250,63 +1255,6 @@ void handleMessage(mavlink_message_t *msg)
hil_attitude.timestamp = hrt_absolute_time(); hil_attitude.timestamp = hrt_absolute_time();
orb_publish(ORB_ID(vehicle_attitude), pub_hil_attitude, &hil_attitude); 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 */ /* Flush UART */
fflush(stdout); 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 */ /* topics to subscribe globally */
/* subscribe to ORB for global position */ /* subscribe to ORB for global position */
mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_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); mavlink_wpm_init(wpm);
uint16_t counter = 0; 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 */ /* make sure all threads have registered their subscriptions */
while (!mavlink_subs.initialized) { 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_global_position), mavlink_subs.global_pos_sub, &global_pos);
orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos);
/* check if waypoint has been reached against the last positions */ /* 1 Hz */
mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos);
// sleep
usleep(50000);
// 1 Hz
if (lowspeed_counter == 10) { 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;
}
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;
}
/* translate the current syste state to mavlink state and mode */ /* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state = 0; uint8_t mavlink_state = 0;
uint8_t mavlink_mode = 0; uint8_t mavlink_mode = 0;
get_mavlink_mode_and_state(&v_status, &mavlink_state, &mavlink_mode); get_mavlink_mode_and_state(&v_status, &mavlink_state, &mavlink_mode);
/* send heartbeat */ /* 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) */ /* 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, 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 = 0;
} }
lowspeed_counter++; 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) */ /* send parameters at 20 Hz (if queued for sending) */
mavlink_pm_queued_send(); mavlink_pm_queued_send();
usleep(50000); /* sleep quarter the time */
usleep(25000);
mavlink_pm_queued_send(); mavlink_pm_queued_send();
/* sleep 10 ms */
usleep(10000);
/* send one string at 10 Hz */ /* send one string at 10 Hz */
mavlink_missionlib_send_gcs_string(mavlink_message_string); mavlink_missionlib_send_gcs_string(mavlink_message_string);
mavlink_message_string[0] = '\0'; mavlink_message_string[0] = '\0';
counter++; counter++;
/* sleep 15 ms */
usleep(15000);
} }
/* wait for threads to complete */ /* wait for threads to complete */
apps/px4/attitude_estimator_bm/attitude_bm.c
-4
View File
@@ -207,10 +207,6 @@ void attitude_blackmagic_init(void)
void attitude_blackmagic(const float_vect3 *accel, const float_vect3 *mag, const float_vect3 *gyro) 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 // Kalman Filter
//Calculate new linearized A matrix //Calculate new linearized A matrix
apps/systemlib/systemlib.c
+8 -147
View File
@@ -123,142 +123,6 @@ void kill_task(FAR _TCB *tcb, FAR void *arg)
kill(tcb->pid, SIGUSR1); 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) #define PX4_BOARD_ID_FMU (5)
int fmu_get_board_info(struct fmu_board_info_s *info) 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; int statres;
/* Copy version-specific fields */ /* Copy version-specific fields */
statres = stat("/dev/bma280", &sb); statres = stat("/dev/bma180", &sb);
if (statres == OK) { 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 */ /* BMA180 indicates a v1.5-v1.6 board */
strcpy(info->board_name, "FMU v1.6"); strcpy(info->board_name, "FMU v1.6");
info->board_version = 16; 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 { } 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"); strcpy(info->board_name, "FMU v1.3");
info->board_version = 13; info->board_version = 13;
} }
apps/systemlib/systemlib.h
-6
View File
@@ -50,12 +50,6 @@ __EXPORT int reboot(void);
/** Sends SIGUSR1 to all processes */ /** Sends SIGUSR1 to all processes */
__EXPORT void killall(void); __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 { enum MULT_PORTS {
MULT_0_US2_RXTX = 0, MULT_0_US2_RXTX = 0,
MULT_1_US2_FLOW, MULT_1_US2_FLOW,
apps/uORB/objects_common.cpp
+3
View File
@@ -108,6 +108,9 @@ ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s);
#include "topics/optical_flow.h" #include "topics/optical_flow.h"
ORB_DEFINE(optical_flow, struct optical_flow_s); 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" #include "topics/actuator_controls.h"
ORB_DEFINE(actuator_controls_0, struct actuator_controls_s); ORB_DEFINE(actuator_controls_0, struct actuator_controls_s);
ORB_DEFINE(actuator_controls_1, struct actuator_controls_s); ORB_DEFINE(actuator_controls_1, struct actuator_controls_s);
apps/uORB/topics/rc_channels.h
+1 -6
View File
@@ -62,11 +62,6 @@ enum RC_CHANNELS_STATUS
* This defines the mapping of the RC functions. * This defines the mapping of the RC functions.
* The value assigned to the specific function corresponds to the entry of * The value assigned to the specific function corresponds to the entry of
* the channel array chan[]. * 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 enum RC_CHANNELS_FUNCTION
{ {
@@ -98,7 +93,7 @@ struct rc_channels_s {
uint16_t override; uint16_t override;
enum RC_CHANNELS_STATUS status; /**< status of the channel */ enum RC_CHANNELS_STATUS status; /**< status of the channel */
} chan[RC_CHANNELS_FUNCTION_MAX]; } 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*/ /*String array to store the names of the functions*/
const char function_name[RC_CHANNELS_FUNCTION_MAX][20]; const char function_name[RC_CHANNELS_FUNCTION_MAX][20];
apps/uORB/topics/subsystem_info.h
+92
View File
@@ -0,0 +1,92 @@
/****************************************************************************
*
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (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 <stdint.h>
#include <stdbool.h>
#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_ */