Merge remote-tracking branch 'upstream/master' into mtecs

This commit is contained in:
Thomas Gubler
2014-04-21 12:10:32 +02:00
16 changed files with 182 additions and 82 deletions
+4 -4
View File
@@ -10,8 +10,8 @@ sh /etc/init.d/rc.fw_defaults
if [ $DO_AUTOCONFIG == yes ]
then
param set FW_AIRSPD_MIN 13
param set FW_AIRSPD_TRIM 18
param set FW_AIRSPD_MAX 40
param set FW_AIRSPD_TRIM 15
param set FW_AIRSPD_MAX 25
param set FW_ATT_TC 0.3
param set FW_L1_DAMPING 0.75
param set FW_L1_PERIOD 15
@@ -23,12 +23,12 @@ then
param set FW_P_LIM_MIN -50
param set FW_P_RMAX_NEG 0
param set FW_P_RMAX_POS 0
param set FW_P_ROLLFF 0
param set FW_P_ROLLFF 1
param set FW_RR_FF 0.5
param set FW_RR_I 0.02
param set FW_RR_IMAX 0.2
param set FW_RR_P 0.08
param set FW_R_LIM 70
param set FW_R_LIM 50
param set FW_R_RMAX 0
param set FW_T_HRATE_P 0.01
param set FW_T_RLL2THR 15
+1 -1
View File
@@ -23,7 +23,7 @@ then
param set FW_P_LIM_MIN -45
param set FW_P_RMAX_NEG 0
param set FW_P_RMAX_POS 0
param set FW_P_ROLLFF 0
param set FW_P_ROLLFF 1
param set FW_RR_FF 0.3
param set FW_RR_I 0
param set FW_RR_IMAX 0.2
+1 -1
View File
@@ -405,7 +405,7 @@ CONFIG_SIG_SIGWORK=4
CONFIG_MAX_TASKS=32
CONFIG_MAX_TASK_ARGS=10
CONFIG_NPTHREAD_KEYS=4
CONFIG_NFILE_DESCRIPTORS=36
CONFIG_NFILE_DESCRIPTORS=38
CONFIG_NFILE_STREAMS=8
CONFIG_NAME_MAX=32
CONFIG_PREALLOC_MQ_MSGS=4
+1 -1
View File
@@ -439,7 +439,7 @@ CONFIG_SIG_SIGWORK=4
CONFIG_MAX_TASKS=32
CONFIG_MAX_TASK_ARGS=10
CONFIG_NPTHREAD_KEYS=4
CONFIG_NFILE_DESCRIPTORS=36
CONFIG_NFILE_DESCRIPTORS=38
CONFIG_NFILE_STREAMS=8
CONFIG_NAME_MAX=32
CONFIG_PREALLOC_MQ_MSGS=4
+95 -46
View File
@@ -48,11 +48,14 @@
* The recognized number off cells, will be blinked 5 times in purple color.
* 2 Cells = 2 blinks
* ...
* 5 Cells = 5 blinks
* 6 Cells = 6 blinks
* Now the Application will show the actual selected Flightmode, GPS-Fix and Battery Warnings and Alerts.
*
* System disarmed:
* The BlinkM should lit solid red.
* System disarmed and safe:
* The BlinkM should light solid cyan.
*
* System safety off but not armed:
* The BlinkM should light flashing orange
*
* System armed:
* One message is made of 4 Blinks and a pause in the same length as the 4 blinks.
@@ -67,10 +70,10 @@
* (X = on, _=off)
*
* The first 3 blinks indicates the status of the GPS-Signal (red):
* 0-4 satellites = X-X-X-X-_-_-_-_-_-_-
* 5 satellites = X-X-_-X-_-_-_-_-_-_-
* 6 satellites = X-_-_-X-_-_-_-_-_-_-
* >=7 satellites = _-_-_-X-_-_-_-_-_-_-
* 0-4 satellites = X-X-X-X-X-_-_-_-_-_-
* 5 satellites = X-X-_-X-X-_-_-_-_-_-
* 6 satellites = X-_-_-X-X-_-_-_-_-_-
* >=7 satellites = _-_-_-X-X-_-_-_-_-_-
* If no GPS is found the first 3 blinks are white
*
* The fourth Blink indicates the Flightmode:
@@ -119,6 +122,7 @@
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/safety.h>
static const float MAX_CELL_VOLTAGE = 4.3f;
static const int LED_ONTIME = 120;
@@ -166,10 +170,12 @@ private:
enum ledColors {
LED_OFF,
LED_RED,
LED_ORANGE,
LED_YELLOW,
LED_PURPLE,
LED_GREEN,
LED_BLUE,
LED_CYAN,
LED_WHITE,
LED_AMBER
};
@@ -380,6 +386,7 @@ BlinkM::led()
static int vehicle_control_mode_sub_fd;
static int vehicle_gps_position_sub_fd;
static int actuator_armed_sub_fd;
static int safety_sub_fd;
static int num_of_cells = 0;
static int detected_cells_runcount = 0;
@@ -402,16 +409,20 @@ BlinkM::led()
if(!topic_initialized) {
vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status));
orb_set_interval(vehicle_status_sub_fd, 1000);
orb_set_interval(vehicle_status_sub_fd, 250);
vehicle_control_mode_sub_fd = orb_subscribe(ORB_ID(vehicle_control_mode));
orb_set_interval(vehicle_control_mode_sub_fd, 1000);
orb_set_interval(vehicle_control_mode_sub_fd, 250);
actuator_armed_sub_fd = orb_subscribe(ORB_ID(actuator_armed));
orb_set_interval(actuator_armed_sub_fd, 1000);
orb_set_interval(actuator_armed_sub_fd, 250);
vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position));
orb_set_interval(vehicle_gps_position_sub_fd, 1000);
orb_set_interval(vehicle_gps_position_sub_fd, 250);
/* Subscribe to safety topic */
safety_sub_fd = orb_subscribe(ORB_ID(safety));
orb_set_interval(safety_sub_fd, 250);
topic_initialized = true;
}
@@ -433,7 +444,9 @@ BlinkM::led()
if(num_of_cells > 4) {
t_led_color[4] = LED_PURPLE;
}
t_led_color[5] = LED_OFF;
if(num_of_cells > 5) {
t_led_color[5] = LED_PURPLE;
}
t_led_color[6] = LED_OFF;
t_led_color[7] = LED_OFF;
t_led_blink = LED_BLINK;
@@ -467,14 +480,17 @@ BlinkM::led()
struct vehicle_control_mode_s vehicle_control_mode;
struct actuator_armed_s actuator_armed;
struct vehicle_gps_position_s vehicle_gps_position_raw;
struct safety_s safety;
memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw));
memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw));
memset(&safety, 0, sizeof(safety));
bool new_data_vehicle_status;
bool new_data_vehicle_control_mode;
bool new_data_actuator_armed;
bool new_data_vehicle_gps_position;
bool new_data_safety;
orb_check(vehicle_status_sub_fd, &new_data_vehicle_status);
@@ -520,7 +536,12 @@ BlinkM::led()
no_data_vehicle_gps_position = 3;
}
/* update safety topic */
orb_check(safety_sub_fd, &new_data_safety);
if (new_data_safety) {
orb_copy(ORB_ID(safety), safety_sub_fd, &safety);
}
/* get number of used satellites in navigation */
num_of_used_sats = 0;
@@ -541,19 +562,7 @@ BlinkM::led()
printf("<blinkm> cells found:%d\n", num_of_cells);
} else {
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;
led_color_3 = LED_YELLOW;
led_color_4 = LED_YELLOW;
led_color_5 = LED_YELLOW;
led_color_6 = LED_YELLOW;
led_color_7 = LED_YELLOW;
led_color_8 = LED_YELLOW;
led_blink = LED_BLINK;
} else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) {
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;
@@ -565,21 +574,56 @@ BlinkM::led()
led_color_8 = LED_RED;
led_blink = LED_BLINK;
} else if(vehicle_status_raw.rc_signal_lost) {
/* LED Pattern for FAILSAFE */
led_color_1 = LED_BLUE;
led_color_2 = LED_BLUE;
led_color_3 = LED_BLUE;
led_color_4 = LED_BLUE;
led_color_5 = LED_BLUE;
led_color_6 = LED_BLUE;
led_color_7 = LED_BLUE;
led_color_8 = LED_BLUE;
led_blink = LED_BLINK;
} else 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;
led_color_3 = LED_YELLOW;
led_color_4 = LED_YELLOW;
led_color_5 = LED_YELLOW;
led_color_6 = LED_YELLOW;
led_color_7 = LED_YELLOW;
led_color_8 = LED_YELLOW;
led_blink = LED_BLINK;
} else {
/* no battery warnings here */
if(actuator_armed.armed == false) {
/* system not armed */
led_color_1 = LED_RED;
led_color_2 = LED_RED;
led_color_3 = LED_RED;
led_color_4 = LED_RED;
led_color_5 = LED_RED;
led_color_6 = LED_RED;
led_color_7 = LED_RED;
led_color_8 = LED_RED;
led_blink = LED_NOBLINK;
if(safety.safety_off){
led_color_1 = LED_ORANGE;
led_color_2 = LED_ORANGE;
led_color_3 = LED_ORANGE;
led_color_4 = LED_ORANGE;
led_color_5 = LED_ORANGE;
led_color_6 = LED_ORANGE;
led_color_7 = LED_ORANGE;
led_color_8 = LED_ORANGE;
led_blink = LED_BLINK;
}else{
led_color_1 = LED_CYAN;
led_color_2 = LED_CYAN;
led_color_3 = LED_CYAN;
led_color_4 = LED_CYAN;
led_color_5 = LED_CYAN;
led_color_6 = LED_CYAN;
led_color_7 = LED_CYAN;
led_color_8 = LED_CYAN;
led_blink = LED_NOBLINK;
}
} else {
/* armed system - initial led pattern */
led_color_1 = LED_RED;
@@ -593,23 +637,22 @@ BlinkM::led()
led_blink = LED_BLINK;
if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) {
//XXX please check
if (vehicle_control_mode.flag_control_position_enabled)
/* indicate main control state */
if (vehicle_status_raw.main_state == MAIN_STATE_EASY)
led_color_4 = LED_GREEN;
else if (vehicle_control_mode.flag_control_velocity_enabled)
else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO)
led_color_4 = LED_BLUE;
else if (vehicle_control_mode.flag_control_attitude_enabled)
else if (vehicle_status_raw.main_state == MAIN_STATE_SEATBELT)
led_color_4 = LED_YELLOW;
else if (vehicle_control_mode.flag_control_manual_enabled)
led_color_4 = LED_AMBER;
else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL)
led_color_4 = LED_WHITE;
else
led_color_4 = LED_OFF;
led_color_5 = led_color_4;
}
if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) {
/* handling used sats */
/* handling used satus */
if(num_of_used_sats >= 7) {
led_color_1 = LED_OFF;
led_color_2 = LED_OFF;
@@ -690,8 +733,11 @@ void BlinkM::setLEDColor(int ledcolor) {
case LED_RED: // red
set_rgb(255,0,0);
break;
case LED_ORANGE: // orange
set_rgb(255,150,0);
break;
case LED_YELLOW: // yellow
set_rgb(255,70,0);
set_rgb(200,200,0);
break;
case LED_PURPLE: // purple
set_rgb(255,0,255);
@@ -702,11 +748,14 @@ void BlinkM::setLEDColor(int ledcolor) {
case LED_BLUE: // blue
set_rgb(0,0,255);
break;
case LED_CYAN: // cyan
set_rgb(0,128,128);
break;
case LED_WHITE: // white
set_rgb(255,255,255);
break;
case LED_AMBER: // amber
set_rgb(255,20,0);
set_rgb(255,65,0);
break;
}
}
+9 -2
View File
@@ -46,6 +46,10 @@
#define RANGE_FINDER_DEVICE_PATH "/dev/range_finder"
enum RANGE_FINDER_TYPE {
RANGE_FINDER_TYPE_LASER = 0,
};
/**
* range finder report structure. Reads from the device must be in multiples of this
* structure.
@@ -53,8 +57,11 @@
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 */
unsigned type; /**< type, following RANGE_FINDER_TYPE enum */
float distance; /**< in meters */
float minimum_distance; /**< minimum distance the sensor can measure */
float maximum_distance; /**< maximum distance the sensor can measure */
uint8_t valid; /**< 1 == within sensor range, 0 = outside sensor range */
};
/*
+7 -2
View File
@@ -201,9 +201,14 @@ PX4IO_Uploader::upload(const char *filenames[])
continue;
}
if (bl_rev <= 2)
if (bl_rev <= 2) {
ret = verify_rev2(fw_size);
else if(bl_rev == 3) {
} else if(bl_rev == 3) {
ret = verify_rev3(fw_size);
} else {
/* verify rev 4 and higher still uses the same approach and
* every version *needs* to be verified.
*/
ret = verify_rev3(fw_size);
}
@@ -959,7 +959,7 @@ FixedwingEstimator::task_main()
}
// Publish results
if (_initialized) {
if (_initialized && (check == OK)) {
+3 -2
View File
@@ -832,10 +832,10 @@ void Mavlink::publish_mission()
{
/* Initialize mission publication if necessary */
if (_mission_pub < 0) {
_mission_pub = orb_advertise(ORB_ID(mission), &mission);
_mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission);
} else {
orb_publish(ORB_ID(mission), _mission_pub, &mission);
orb_publish(ORB_ID(offboard_mission), _mission_pub, &mission);
}
}
@@ -1953,6 +1953,7 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult);
configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f * rate_mult);
configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f * rate_mult);
configure_stream("DISTANCE_SENSOR", 0.5f);
break;
case MAVLINK_MODE_CAMERA:
+47
View File
@@ -72,6 +72,7 @@
#include <uORB/topics/navigation_capabilities.h>
#include <drivers/drv_rc_input.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_range_finder.h>
#include "mavlink_messages.h"
@@ -1312,6 +1313,51 @@ protected:
}
};
class MavlinkStreamDistanceSensor : public MavlinkStream
{
public:
const char *get_name()
{
return "DISTANCE_SENSOR";
}
MavlinkStream *new_instance()
{
return new MavlinkStreamDistanceSensor();
}
private:
MavlinkOrbSubscription *range_sub;
struct range_finder_report *range;
protected:
void subscribe(Mavlink *mavlink)
{
range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder));
range = (struct range_finder_report *)range_sub->get_data();
}
void send(const hrt_abstime t)
{
(void)range_sub->update(t);
uint8_t type;
switch (range->type) {
case RANGE_FINDER_TYPE_LASER:
type = MAV_DISTANCE_SENSOR_LASER;
break;
}
uint8_t id = 0;
uint8_t orientation = 0;
uint8_t covariance = 20;
mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation,
range->minimum_distance*100, range->maximum_distance*100, range->distance*100, covariance);
}
};
MavlinkStream *streams_list[] = {
new MavlinkStreamHeartbeat(),
new MavlinkStreamSysStatus(),
@@ -1338,6 +1384,7 @@ MavlinkStream *streams_list[] = {
new MavlinkStreamAttitudeControls(),
new MavlinkStreamNamedValueFloat(),
new MavlinkStreamCameraCapture(),
new MavlinkStreamDistanceSensor(),
new MavlinkStreamViconPositionEstimate(),
nullptr
};
+1 -1
View File
@@ -47,4 +47,4 @@ SRCS += mavlink_main.cpp \
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
MAXOPTIMIZATION = -Os
MAXOPTIMIZATION = -Os
+3 -3
View File
@@ -510,7 +510,7 @@ Navigator::offboard_mission_update(bool isrotaryWing)
{
struct mission_s offboard_mission;
if (orb_copy(ORB_ID(mission), _offboard_mission_sub, &offboard_mission) == OK) {
if (orb_copy(ORB_ID(offboard_mission), _offboard_mission_sub, &offboard_mission) == OK) {
/* Check mission feasibility, for now do not handle the return value,
* however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
@@ -543,7 +543,7 @@ Navigator::onboard_mission_update()
{
struct mission_s onboard_mission;
if (orb_copy(ORB_ID(mission), _onboard_mission_sub, &onboard_mission) == OK) {
if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &onboard_mission) == OK) {
_mission.set_onboard_mission_count(onboard_mission.count);
_mission.set_current_onboard_mission_index(onboard_mission.current_index);
@@ -611,7 +611,7 @@ Navigator::task_main()
* do subscriptions
*/
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_offboard_mission_sub = orb_subscribe(ORB_ID(mission));
_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
+3 -15
View File
@@ -908,9 +908,6 @@ int sdlog2_thread_main(int argc, char *argv[])
hrt_abstime barometer_timestamp = 0;
hrt_abstime differential_pressure_timestamp = 0;
/* track changes in distance status */
bool dist_bottom_present = false;
/* enable logging on start if needed */
if (log_on_start) {
/* check GPS topic to get GPS time */
@@ -1099,6 +1096,8 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_LPOS.x = buf.local_pos.x;
log_msg.body.log_LPOS.y = buf.local_pos.y;
log_msg.body.log_LPOS.z = buf.local_pos.z;
log_msg.body.log_LPOS.ground_dist = buf.local_pos.dist_bottom;
log_msg.body.log_LPOS.ground_dist_rate = buf.local_pos.dist_bottom_rate;
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;
@@ -1108,19 +1107,8 @@ int sdlog2_thread_main(int argc, char *argv[])
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;
log_msg.body.log_LPOS.ground_dist_flags = (buf.local_pos.dist_bottom_valid ? 1 : 0);
LOGBUFFER_WRITE_AND_COUNT(LPOS);
if (buf.local_pos.dist_bottom_valid) {
dist_bottom_present = true;
}
if (dist_bottom_present) {
log_msg.msg_type = LOG_DIST_MSG;
log_msg.body.log_DIST.bottom = buf.local_pos.dist_bottom;
log_msg.body.log_DIST.bottom_rate = buf.local_pos.dist_bottom_rate;
log_msg.body.log_DIST.flags = (buf.local_pos.dist_bottom_valid ? 1 : 0);
LOGBUFFER_WRITE_AND_COUNT(DIST);
}
}
/* --- LOCAL POSITION SETPOINT --- */
+4 -1
View File
@@ -101,6 +101,8 @@ struct log_LPOS_s {
float x;
float y;
float z;
float ground_dist;
float ground_dist_rate;
float vx;
float vy;
float vz;
@@ -110,6 +112,7 @@ struct log_LPOS_s {
uint8_t xy_flags;
uint8_t z_flags;
uint8_t landed;
uint8_t ground_dist_flags;
};
/* --- LPSP - LOCAL POSITION SETPOINT --- */
@@ -343,7 +346,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, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"),
LOG_FORMAT(LPOS, "ffffffLLfBBB", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt,XYFlags,ZFlags,Landed"),
LOG_FORMAT(LPOS, "ffffffffLLfBBBB", "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"),
+1 -1
View File
@@ -127,7 +127,7 @@ ORB_DEFINE(position_setpoint_triplet, struct position_setpoint_triplet_s);
ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s);
#include "topics/mission.h"
ORB_DEFINE(mission, struct mission_s);
ORB_DEFINE(offboard_mission, struct mission_s);
ORB_DEFINE(onboard_mission, struct mission_s);
#include "topics/mission_result.h"
+1 -1
View File
@@ -105,7 +105,7 @@ struct mission_s
*/
/* register this as object request broker structure */
ORB_DECLARE(mission);
ORB_DECLARE(offboard_mission);
ORB_DECLARE(onboard_mission);
#endif