mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-30 19:47:50 +08:00
*** empty log message ***
This commit is contained in:
Pascal Brisset
@@ -106,6 +106,7 @@
|
||||
<define name="ALT_KALMAN_ENABLED" value="TRUE"/>
|
||||
|
||||
<define name="TRIGGER_DELAY" value="1."/>
|
||||
<define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
|
||||
</section>
|
||||
|
||||
<section name="VERTICAL CONTROL" prefix="V_CTL_">
|
||||
|
||||
@@ -54,7 +54,7 @@
|
||||
|
||||
<section name="INFRARED" prefix="IR_">
|
||||
<define name="ROLL_NEUTRAL_DEFAULT" value="0" unit="deg"/>
|
||||
<define name="PITCH_NEUTRAL_DEFAULT" value="7" unit="deg"/>
|
||||
<define name="PITCH_NEUTRAL_DEFAULT" value="6" unit="deg"/>
|
||||
<define name="DEFAULT_CONTRAST" value="200"/>
|
||||
<define name="RAD_OF_IR_CONTRAST" value="0.75"/>
|
||||
<linear name="RollOfIrs" arity="2" coeff1=".5" coeff2="0.5"/>
|
||||
@@ -64,14 +64,14 @@
|
||||
<define name="RAD_OF_IR_MIN_VALUE" value="0.00075"/>
|
||||
<define name="ADC_ROLL_NEUTRAL" value="507"/>
|
||||
<define name="ADC_PITCH_NEUTRAL" value="-16"/>
|
||||
<define name="ADC_TOP_NEUTRAL" value="488"/>
|
||||
<define name="ADC_TOP_NEUTRAL" value="533"/>
|
||||
|
||||
<define name="CORRECTION_UP" value="1.6"/>
|
||||
<define name="CORRECTION_DOWN" value="1.4"/>
|
||||
<define name="CORRECTION_LEFT" value="1."/>
|
||||
<define name="CORRECTION_RIGHT" value="1."/>
|
||||
|
||||
<define name="ESTIMATED_PHI_PI_4" value="0.44"/>
|
||||
<define name="ESTIMATED_PHI_PI_4" value="0.55"/>
|
||||
|
||||
<define name="360_LATERAL_CORRECTION" value="1.25"/>
|
||||
<define name="360_LONGITUDINAL_CORRECTION" value="1.25"/>
|
||||
@@ -79,7 +79,7 @@
|
||||
</section>
|
||||
|
||||
<section name="GYRO" prefix="GYRO_">
|
||||
<define name="ADC_ROLL_NEUTRAL" value="470"/>
|
||||
<define name="ADC_ROLL_NEUTRAL" value="467"/>
|
||||
<define name="ADC_PITCH_NEUTRAL" value="471"/>
|
||||
<define name="ROLL_SCALE" value="1.3846"/>
|
||||
<define name="ROLL_DIRECTION" value="1."/>
|
||||
@@ -101,6 +101,9 @@
|
||||
<define name="ALT_KALMAN_ENABLED" value="TRUE"/>
|
||||
|
||||
<define name="TRIGGER_DELAY" value="1.1"/>
|
||||
<define name="DEFAULT_CIRCLE_RADIUS" value="100."/>
|
||||
|
||||
<define name="UNLOCKED_HOME_MODE" value="TRUE"/>
|
||||
</section>
|
||||
|
||||
<section name="VERTICAL CONTROL" prefix="V_CTL_">
|
||||
|
||||
@@ -51,7 +51,7 @@
|
||||
|
||||
<section name="INFRARED" prefix="IR_">
|
||||
<define name="ROLL_NEUTRAL_DEFAULT" value="0" unit="deg"/>
|
||||
<define name="PITCH_NEUTRAL_DEFAULT" value="-4" unit="deg"/>
|
||||
<define name="PITCH_NEUTRAL_DEFAULT" value="0" unit="deg"/>
|
||||
<define name="DEFAULT_CONTRAST" value="200"/>
|
||||
<define name="RAD_OF_IR_CONTRAST" value="0.75"/>
|
||||
<linear name="RollOfIrs" arity="2" coeff1=".5" coeff2="0.5"/>
|
||||
@@ -59,7 +59,7 @@
|
||||
<linear name="TopOfIr" arity="1" coeff1="-1"/>
|
||||
<define name="RAD_OF_IR_MAX_VALUE" value="0.0045"/>
|
||||
<define name="RAD_OF_IR_MIN_VALUE" value="0.00075"/>
|
||||
<define name="ADC_ROLL_NEUTRAL" value="512"/>
|
||||
<define name="ADC_ROLL_NEUTRAL" value="495"/>
|
||||
<define name="ADC_PITCH_NEUTRAL" value="-7"/>
|
||||
<define name="ADC_TOP_NEUTRAL" value="530"/>
|
||||
|
||||
@@ -68,7 +68,7 @@
|
||||
<define name="CORRECTION_LEFT" value="1."/>
|
||||
<define name="CORRECTION_RIGHT" value="1."/>
|
||||
|
||||
<define name="ESTIMATED_PHI_PI_4" value="0.8"/>
|
||||
<define name="ESTIMATED_PHI_PI_4" value="0.7"/>
|
||||
|
||||
<define name="360_LATERAL_CORRECTION" value="1."/>
|
||||
<define name="360_LONGITUDINAL_CORRECTION" value="1."/>
|
||||
@@ -76,7 +76,7 @@
|
||||
</section>
|
||||
|
||||
<section name="GYRO" prefix="GYRO_">
|
||||
<define name="ADC_ROLL_NEUTRAL" value="449"/>
|
||||
<define name="ADC_ROLL_NEUTRAL" value="453"/>
|
||||
<define name="ADC_PITCH_NEUTRAL" value="471"/>
|
||||
<define name="ROLL_SCALE" value="1.3846"/>
|
||||
<define name="ROLL_DIRECTION" value="1."/>
|
||||
@@ -96,6 +96,9 @@
|
||||
<define name="CONTROL_RATE" value="60" unit="Hz"/>
|
||||
<define name="XBEE_INIT" value="\"ATPL2\rATRN1\rATTT80\r\""/>
|
||||
<define name="ALT_KALMAN_ENABLED" value="TRUE"/>
|
||||
<define name="DEFAULT_CIRCLE_RADIUS" value="100"/>
|
||||
|
||||
<define name="UNLOCKED_HOME_MODE" value="TRUE"/>
|
||||
</section>
|
||||
|
||||
<section name="VERTICAL CONTROL" prefix="V_CTL_">
|
||||
@@ -126,7 +129,7 @@
|
||||
</section>
|
||||
|
||||
<section name="HORIZONTAL CONTROL" prefix="H_CTL_">
|
||||
<define name="COURSE_PGAIN" value="-0.7"/>
|
||||
<define name="COURSE_PGAIN" value="-0.85"/>
|
||||
<define name="COURSE_PRE_BANK_CORRECTION" value="1."/>
|
||||
<define name="ROLL_MAX_SETPOINT" value="0.73" unit="radians"/>
|
||||
<define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
|
||||
|
||||
@@ -99,6 +99,9 @@
|
||||
<define name="CONTROL_RATE" value="60" unit="Hz"/>
|
||||
<define name="XBEE_INIT" value="\"ATPL2\rATRN1\rATTT80\r\""/>
|
||||
<define name="ALT_KALMAN_ENABLED" value="TRUE"/>
|
||||
<define name="DEFAULT_CIRCLE_RADIUS" value="100"/>
|
||||
|
||||
<define name="UNLOCKED_HOME_MODE" value="TRUE"/>
|
||||
</section>
|
||||
|
||||
<section name="VERTICAL CONTROL" prefix="V_CTL_">
|
||||
|
||||
@@ -12,14 +12,14 @@
|
||||
<!ELEMENT exceptions (exception*)>
|
||||
|
||||
<!ELEMENT blocks (block+)>
|
||||
<!ELEMENT block (exception|while|heading|attitude|go|xyz|set|circle|deroute|stay|follow|survey_rectangle|for|return)*>
|
||||
<!ELEMENT block (exception|while|heading|attitude|go|xyz|set|circle|deroute|stay|follow|survey_rectangle|for|return|eight)*>
|
||||
|
||||
<!ELEMENT include (arg|with)*>
|
||||
<!ELEMENT arg EMPTY>
|
||||
<!ELEMENT with EMPTY>
|
||||
|
||||
<!ELEMENT while (exception|while|heading|attitude|go|xyz|set|circle|deroute|stay|follow|survey_rectangle|for|return)*>
|
||||
<!ELEMENT for (exception|while|heading|attitude|go|xyz|set|circle|deroute|stay|follow|survey_rectangle|for|return)*>
|
||||
<!ELEMENT while (exception|while|heading|attitude|go|xyz|set|circle|deroute|stay|follow|survey_rectangle|for|return|eight)*>
|
||||
<!ELEMENT for (exception|while|heading|attitude|go|xyz|set|circle|deroute|stay|follow|survey_rectangle|for|return|eight)*>
|
||||
<!ELEMENT exception EMPTY>
|
||||
<!ELEMENT heading EMPTY>
|
||||
<!ELEMENT attitude EMPTY>
|
||||
@@ -27,6 +27,7 @@
|
||||
<!ELEMENT xyz EMPTY>
|
||||
<!ELEMENT set EMPTY>
|
||||
<!ELEMENT circle EMPTY>
|
||||
<!ELEMENT eight EMPTY>
|
||||
<!ELEMENT survey_rectangle EMPTY>
|
||||
<!ELEMENT deroute EMPTY>
|
||||
<!ELEMENT stay EMPTY>
|
||||
@@ -145,6 +146,11 @@ pitch CDATA #IMPLIED
|
||||
throttle CDATA #IMPLIED
|
||||
until CDATA #IMPLIED>
|
||||
|
||||
<!ATTLIST eight
|
||||
center CDATA #REQUIRED
|
||||
turn_around CDATA #REQUIRED
|
||||
radius CDATA #REQUIRED>
|
||||
|
||||
<!ATTLIST survey_rectangle
|
||||
grid CDATA #REQUIRED
|
||||
wp1 CDATA #REQUIRED
|
||||
|
||||
@@ -24,11 +24,18 @@
|
||||
<dl_settings NAME="ir">
|
||||
<dl_setting MAX="0.3" MIN="-0.3" STEP="0.01" VAR="ir_roll_neutral" shortname="roll_neutral"/>
|
||||
<dl_setting MAX="0.5" MIN="-0.3" STEP="0.01" VAR="ir_pitch_neutral" shortname="pitch_neutral"/>
|
||||
|
||||
<!--
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="ir_correction_left" shortname="corr_left"/>
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="ir_correction_right" shortname="corr_right"/>
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="ir_correction_up" shortname="corr_up"/>
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="ir_correction_down" shortname="corr_down"/>
|
||||
<dl_setting MAX="1." MIN="0.1" STEP="0.05" VAR="ir_estimated_phi_pi_4" shortname="est. phi pi/4"/>
|
||||
-->
|
||||
|
||||
<dl_setting MAX="1.5" MIN="0.4" STEP="0.05" VAR="ir_estimated_phi_pi_4" shortname="est. right"/>
|
||||
<dl_setting MAX="1.5" MIN="0.4" STEP="0.05" VAR="ir_estimated_phi_minus_pi_4" shortname="est. left"/>
|
||||
<dl_setting MAX="1.5" MIN="0.4" STEP="0.05" VAR="ir_estimated_theta_pi_4" shortname="est. up"/>
|
||||
<dl_setting MAX="1.5" MIN="0.4" STEP="0.05" VAR="ir_estimated_phi_minus_pi_4" shortname="est. down"/>
|
||||
<dl_setting MAX="1" MIN="0" STEP="1" VAR="ir_360" shortname="ir_360"/>
|
||||
</dl_settings>
|
||||
|
||||
|
||||
+38
-8
@@ -46,10 +46,27 @@ float ir_roll_neutral;
|
||||
float ir_pitch_neutral;
|
||||
|
||||
bool_t ir_360;
|
||||
float ir_estimated_phi_pi_4;
|
||||
float ir_estimated_phi_pi_4, ir_estimated_phi_minus_pi_4;
|
||||
float ir_estimated_theta_pi_4, ir_estimated_theta_minus_pi_4;
|
||||
|
||||
#ifndef IR_360
|
||||
|
||||
#define IR_360 FALSE
|
||||
|
||||
#else
|
||||
|
||||
#ifndef IR_ESTIMATED_PHI_MINUS_PI_4
|
||||
#define IR_ESTIMATED_PHI_MINUS_PI_4 IR_ESTIMATED_PHI_PI_4
|
||||
#endif
|
||||
|
||||
#ifndef IR_ESTIMATED_THETA_PI_4
|
||||
#define IR_ESTIMATED_THETA_PI_4 IR_ESTIMATED_PHI_PI_4
|
||||
#endif
|
||||
|
||||
#ifndef IR_ESTIMATED_THETA_MINUS_PI_4
|
||||
#define IR_ESTIMATED_THETA_MINUS_PI_4 IR_ESTIMATED_THETA_PI_4
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
@@ -133,6 +150,9 @@ void ir_init(void) {
|
||||
|
||||
ir_360 = IR_360;
|
||||
ir_estimated_phi_pi_4 = IR_ESTIMATED_PHI_PI_4;
|
||||
ir_estimated_phi_minus_pi_4 = IR_ESTIMATED_PHI_MINUS_PI_4;
|
||||
ir_estimated_theta_pi_4 = IR_ESTIMATED_THETA_PI_4;
|
||||
ir_estimated_theta_minus_pi_4 = IR_ESTIMATED_THETA_MINUS_PI_4;
|
||||
|
||||
ir_contrast = IR_DEFAULT_CONTRAST;
|
||||
ir_rad_of_ir = IR_RAD_OF_IR_CONTRAST / IR_DEFAULT_CONTRAST;
|
||||
@@ -249,17 +269,27 @@ void estimator_update_ir_estim( void ) {
|
||||
}
|
||||
|
||||
|
||||
static inline float correct_angle(float m_angle, float est_pi_4) {
|
||||
if (fabs(m_angle) < est_pi_4)
|
||||
/* Correction of the infrared estimation roll angle: returns pi/4 for
|
||||
est_pi_4 */
|
||||
static inline float correct_angle(float m_angle, float est_pi_4, float est_minus_pi_4) {
|
||||
if (m_angle >= 0 && m_angle < est_pi_4)
|
||||
/* 0 .. est_pi_4 */
|
||||
return (m_angle * M_PI_4 / est_pi_4);
|
||||
else if (m_angle > M_PI - est_pi_4)
|
||||
/* pi-est_pi_4 .. pi */
|
||||
return (m_angle - (M_PI - est_pi_4))* M_PI_4 / est_pi_4 + 3*M_PI_4;
|
||||
else if (m_angle >= est_pi_4)
|
||||
/* est_pi_4 .. pi-est_pi_4 */
|
||||
return (m_angle - est_pi_4) * M_PI_4 / (M_PI_2 - est_pi_4) + M_PI_4;
|
||||
else if (m_angle < - (M_PI - est_pi_4))
|
||||
return (m_angle + M_PI)* M_PI_4 / est_pi_4 + -M_PI;
|
||||
else if (m_angle <= 0 && m_angle > -est_minus_pi_4)
|
||||
/* -est_pi_4 .. 0 */
|
||||
return (m_angle * M_PI_4 / est_minus_pi_4);
|
||||
else if (m_angle < - (M_PI - est_minus_pi_4))
|
||||
/* -pi .. -(pi-est_pi_4) */
|
||||
return (m_angle + M_PI)* M_PI_4 / est_minus_pi_4 + -M_PI;
|
||||
else
|
||||
return (m_angle - (-M_PI+est_pi_4)) * M_PI_4 / (M_PI_2 - est_pi_4) - 3 * M_PI_4;
|
||||
/* -(pi-est_pi_4) .. -est_pi_4 */
|
||||
return (m_angle - (-M_PI+est_minus_pi_4)) * M_PI_4 / (M_PI_2 - est_minus_pi_4) - 3 * M_PI_4;
|
||||
}
|
||||
|
||||
|
||||
@@ -306,10 +336,10 @@ void estimator_update_state_infrared( void ) {
|
||||
float tmp_ir_top = ir_top * IR_360_VERTICAL_CORRECTION;
|
||||
|
||||
estimator_phi = atan2(tmp_ir_roll, tmp_ir_top) - ir_roll_neutral;
|
||||
estimator_phi = correct_angle(estimator_phi, ir_estimated_phi_pi_4);
|
||||
estimator_phi = correct_angle(estimator_phi, ir_estimated_phi_pi_4, ir_estimated_phi_minus_pi_4);
|
||||
|
||||
estimator_theta = atan2(tmp_ir_pitch, tmp_ir_top) - ir_pitch_neutral;
|
||||
estimator_theta = correct_angle(estimator_theta, ir_estimated_phi_pi_4);
|
||||
estimator_theta = correct_angle(estimator_theta, ir_estimated_theta_pi_4, ir_estimated_theta_minus_pi_4);
|
||||
if (estimator_theta < -M_PI_2)
|
||||
estimator_theta += M_PI;
|
||||
else if (estimator_theta > M_PI_2)
|
||||
|
||||
@@ -49,7 +49,9 @@ extern float ir_rad_of_ir;
|
||||
extern int16_t ir_contrast;
|
||||
extern float z_contrast_mode;
|
||||
extern bool_t ir_360;
|
||||
extern float ir_estimated_phi_pi_4;
|
||||
extern float ir_estimated_phi_pi_4, ir_estimated_phi_minus_pi_4;
|
||||
extern float ir_estimated_theta_pi_4, ir_estimated_theta_minus_pi_4;
|
||||
|
||||
|
||||
#if defined IR_CORRECTION_LEFT && defined IR_CORRECTION_RIGHT
|
||||
extern float ir_correction_left;
|
||||
|
||||
@@ -116,10 +116,16 @@ uint8_t light_mode = 0;
|
||||
/** \brief Update paparazzi mode
|
||||
*/
|
||||
static inline uint8_t pprz_mode_update( void ) {
|
||||
/** We remain in home mode until explicit reset from the RC */
|
||||
if ((pprz_mode != PPRZ_MODE_HOME &&
|
||||
pprz_mode != PPRZ_MODE_GPS_OUT_OF_ORDER)
|
||||
|| CheckEvent(rc_event_1)) {
|
||||
||
|
||||
#ifdef UNLOCKED_HOME_MODE
|
||||
TRUE
|
||||
#else
|
||||
/** We remain in home mode until explicit reset from the RC */
|
||||
CheckEvent(rc_event_1)
|
||||
#endif
|
||||
) {
|
||||
return ModeUpdate(pprz_mode, PPRZ_MODE_OF_PULSE(fbw_state->channels[RADIO_MODE], fbw_state->status));
|
||||
} else
|
||||
return FALSE;
|
||||
@@ -224,7 +230,7 @@ inline void telecommand_task( void ) {
|
||||
copy_from_to_fbw();
|
||||
|
||||
uint8_t really_lost = bit_is_set(fbw_state->status, RADIO_REALLY_LOST) && (pprz_mode == PPRZ_MODE_AUTO1 || pprz_mode == PPRZ_MODE_MANUAL);
|
||||
if (pprz_mode != PPRZ_MODE_HOME && pprz_mode != PPRZ_MODE_GPS_OUT_OF_ORDER && launch && (/* really_lost || */ too_far_from_home)) {
|
||||
if (pprz_mode != PPRZ_MODE_HOME && pprz_mode != PPRZ_MODE_GPS_OUT_OF_ORDER && launch && (really_lost || too_far_from_home)) {
|
||||
pprz_mode = PPRZ_MODE_HOME;
|
||||
mode_changed = TRUE;
|
||||
}
|
||||
@@ -473,11 +479,6 @@ void periodic_task_ap( void ) {
|
||||
ap_state->commands[COMMAND_ROLL] = h_ctl_aileron_setpoint;
|
||||
ap_state->commands[COMMAND_PITCH] = h_ctl_elevator_setpoint;
|
||||
|
||||
#ifdef COMMAND_HATCH_CMD
|
||||
extern pprz_t hatch_cmd;
|
||||
ap_state->commands[COMMAND_HATCH_CMD] = hatch_cmd;
|
||||
#endif
|
||||
|
||||
#if defined MCU_SPI_LINK
|
||||
link_mcu_send();
|
||||
#elif defined INTER_MCU && defined SINGLE_MCU
|
||||
|
||||
+82
-5
@@ -371,22 +371,28 @@ bool_t moved_waypoints[NB_WAYPOINT+1];
|
||||
* uav has not gone past waypoint.
|
||||
* Return true if it is the case.
|
||||
*/
|
||||
static bool_t approaching(uint8_t wp, float approaching_time) {
|
||||
static bool_t approaching_xy(float x, float y, float approaching_time) {
|
||||
/** distance to waypoint in x */
|
||||
float pw_x = waypoints[wp].x - estimator_x;
|
||||
float pw_x = x - estimator_x;
|
||||
/** distance to waypoint in y */
|
||||
float pw_y = waypoints[wp].y - estimator_y;
|
||||
float pw_y = y - estimator_y;
|
||||
|
||||
dist2_to_wp = pw_x*pw_x + pw_y *pw_y;
|
||||
float min_dist = approaching_time * estimator_hspeed_mod;
|
||||
if (dist2_to_wp < min_dist*min_dist)
|
||||
return TRUE;
|
||||
|
||||
float scal_prod = (waypoints[wp].x - last_x) * pw_x + (waypoints[wp].y - last_y) * pw_y;
|
||||
float scal_prod = (x - last_x) * pw_x + (y - last_y) * pw_y;
|
||||
|
||||
return (scal_prod < 0.);
|
||||
}
|
||||
|
||||
static bool_t approaching(uint8_t wp, float approaching_time) {
|
||||
return approaching_xy(waypoints[wp].x, waypoints[wp].y, approaching_time);
|
||||
}
|
||||
|
||||
|
||||
|
||||
/** static inline void fly_to_xy(float x, float y)
|
||||
* \brief Computes \a desired_x, \a desired_y and \a desired_course.
|
||||
*/
|
||||
@@ -422,7 +428,6 @@ static void route_to_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y
|
||||
float carrot = CARROT * estimator_hspeed_mod;
|
||||
|
||||
alpha += Max(carrot / leg, 0.);
|
||||
alpha = Min(1., alpha);
|
||||
in_segment = TRUE;
|
||||
segment_x_1 = last_wp_x;
|
||||
segment_y_1 = last_wp_y;
|
||||
@@ -531,3 +536,75 @@ void nav_without_gps(void) {
|
||||
nav_desired_gaz = TRIM_UPPRZ((CRUISE_THROTTLE)*MAX_PPRZ);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
enum eight_status { EW, CW, WE, CE };
|
||||
|
||||
static enum eight_status eight_status;
|
||||
void nav_eight_init( void ) {
|
||||
eight_status = CE;
|
||||
}
|
||||
|
||||
void nav_eight(uint8_t center, uint8_t TA1, float radius) {
|
||||
float alt = waypoints[center].a;
|
||||
waypoints[TA1].a = alt;
|
||||
|
||||
float center_TA1_x = waypoints[TA1].x - waypoints[center].x;
|
||||
float center_TA1_y = waypoints[TA1].y - waypoints[center].y;
|
||||
|
||||
struct point TA2 = { waypoints[TA1].x - 2*center_TA1_x,
|
||||
waypoints[TA1].y - 2*center_TA1_y,
|
||||
alt };
|
||||
|
||||
float d = sqrt(center_TA1_x*center_TA1_x+center_TA1_y*center_TA1_y);
|
||||
float u_x = center_TA1_x / d;
|
||||
float u_y = center_TA1_y / d;
|
||||
|
||||
struct point TA1N = { waypoints[TA1].x + radius * -u_y,
|
||||
waypoints[TA1].y + radius * u_x,
|
||||
alt };
|
||||
struct point TA1S = { waypoints[TA1].x - radius * -u_y,
|
||||
waypoints[TA1].y - radius * u_x,
|
||||
alt };
|
||||
|
||||
struct point TA2N = { TA2.x + radius * -u_y,
|
||||
TA2.y + radius * u_x,
|
||||
alt };
|
||||
struct point TA2S = { TA2.x - radius * -u_y,
|
||||
TA2.y - radius * u_x,
|
||||
alt };
|
||||
|
||||
float qdr_out = M_PI - atan2(u_y, u_x);
|
||||
|
||||
switch (eight_status) {
|
||||
case CE :
|
||||
Circle(TA1, radius);
|
||||
if (Qdr(DegOfRad(qdr_out)-10)) {
|
||||
eight_status = EW;
|
||||
InitStage();
|
||||
}
|
||||
return;
|
||||
case EW:
|
||||
route_to_xy(TA1S.x, TA1S.y, TA2N.x, TA2N.y);
|
||||
if (approaching_xy(TA2N.x, TA2N.y,CARROT)) {
|
||||
eight_status = CW;
|
||||
InitStage();
|
||||
}
|
||||
return;
|
||||
case CW :
|
||||
CircleXY(TA2.x, TA2.y, -radius);
|
||||
if (Qdr(DegOfRad(qdr_out)+10)) {
|
||||
eight_status = WE;
|
||||
InitStage();
|
||||
}
|
||||
return;
|
||||
case WE:
|
||||
route_to_xy(TA2S.x, TA2S.y, TA1N.x, TA1N.y);
|
||||
if (approaching_xy(TA1N.x, TA1N.y,CARROT)) {
|
||||
eight_status = CE;
|
||||
InitStage();
|
||||
}
|
||||
return;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@@ -94,6 +94,10 @@ extern uint8_t horizontal_mode;
|
||||
} \
|
||||
}
|
||||
|
||||
extern void nav_eight_init( void );
|
||||
extern void nav_eight(uint8_t, uint8_t, float);
|
||||
#define Eight(a, b, c) nav_eight((a), (b), (c))
|
||||
|
||||
extern float ground_alt;
|
||||
|
||||
extern float survey_west, survey_east, survey_north, survey_south;
|
||||
|
||||
@@ -75,6 +75,7 @@ type aircraft = {
|
||||
mutable in_kill_mode : bool;
|
||||
mutable speed : float;
|
||||
mutable alt : float;
|
||||
mutable target_alt : float;
|
||||
mutable flight_time : int;
|
||||
mutable wind_speed : float;
|
||||
mutable wind_dir : float (* Rad *)
|
||||
@@ -91,7 +92,7 @@ let log =
|
||||
let last = ref M.empty in
|
||||
fun ?(say = false) (a:Pages.alert) ac_id s ->
|
||||
if not (M.mem ac_id !last) || M.find ac_id !last <> s then begin
|
||||
last := M.add ac_id s !last;
|
||||
last := M.add ac_id s (M.remove ac_id !last);
|
||||
if say then Speech.say s;
|
||||
a#add s
|
||||
end
|
||||
@@ -307,7 +308,7 @@ let create_ac = fun alert (geomap:G.widget) (acs_notebook:GPack.notebook) (ac_id
|
||||
let select_this_tab =
|
||||
let n = acs_notebook#page_num ac_frame#coerce in
|
||||
fun () -> acs_notebook#goto_page n in
|
||||
let strip = Strip.add config color select_this_tab center_ac commit_moves (mark geomap ac_id track !Plugin.frame) in
|
||||
let strip = Strip.add config color select_this_tab center_ac (mark geomap ac_id track !Plugin.frame) in
|
||||
|
||||
|
||||
(** Build the XML flight plan, connect then "jump_to_block" *)
|
||||
@@ -340,7 +341,7 @@ let create_ac = fun alert (geomap:G.widget) (acs_notebook:GPack.notebook) (ac_id
|
||||
let label = ExtXml.attrib b "strip_button"
|
||||
and id = ExtXml.int_attrib b "no" in
|
||||
let b = GButton.button ~label () in
|
||||
Strip.add_widget strip b#coerce;
|
||||
strip#add_widget b#coerce;
|
||||
ignore (b#connect#clicked (fun _ -> jump_to_block ac_id id))
|
||||
with
|
||||
_ -> ())
|
||||
@@ -380,18 +381,19 @@ let create_ac = fun alert (geomap:G.widget) (acs_notebook:GPack.notebook) (ac_id
|
||||
prerr_endline (Printexc.to_string exc);
|
||||
Xml.Element("empty", [], [])
|
||||
in
|
||||
let dl_setting_callback = fun idx value ->
|
||||
let vs = ["ac_id", Pprz.String ac_id; "index", Pprz.Int idx;"value", Pprz.Float value] in
|
||||
Ground_Pprz.message_send "dl" "DL_SETTING" vs in
|
||||
let dl_settings_page =
|
||||
try
|
||||
let xml_settings = Xml.children (ExtXml.child settings_xml "dl_settings") in
|
||||
let callback = fun idx value ->
|
||||
let vs = ["ac_id", Pprz.String ac_id; "index", Pprz.Int idx;"value", Pprz.Float value] in
|
||||
Ground_Pprz.message_send "dl" "DL_SETTING" vs in
|
||||
let settings_tab = new Pages.settings ~visible xml_settings callback strip in
|
||||
let settings_tab = new Pages.settings ~visible xml_settings dl_setting_callback strip in
|
||||
|
||||
let tab_label = (GMisc.label ~text:"Settings" ())#coerce in
|
||||
ac_notebook#append_page ~tab_label settings_tab#widget;
|
||||
Some settings_tab
|
||||
with exc ->
|
||||
prerr_endline (Printexc.to_string exc);
|
||||
log alert ac_id (Printexc.to_string exc);
|
||||
None in
|
||||
|
||||
let rc_settings_page =
|
||||
@@ -415,7 +417,7 @@ let create_ac = fun alert (geomap:G.widget) (acs_notebook:GPack.notebook) (ac_id
|
||||
dl_settings_page = dl_settings_page;
|
||||
rc_settings_page = rc_settings_page;
|
||||
strip = strip; first_pos = true;
|
||||
last_block_name = ""; alt = 0.;
|
||||
last_block_name = ""; alt = 0.; target_alt = 0.;
|
||||
in_kill_mode = false; speed = 0.;
|
||||
wind_dir = 42.;
|
||||
wind_speed = 0.; } in
|
||||
@@ -443,7 +445,24 @@ let create_ac = fun alert (geomap:G.widget) (acs_notebook:GPack.notebook) (ac_id
|
||||
true
|
||||
in
|
||||
|
||||
ignore (Glib.Timeout.add 10000 send_wind)
|
||||
ignore (Glib.Timeout.add 10000 send_wind);
|
||||
|
||||
(** Connect the shift altitude buttons *)
|
||||
begin
|
||||
match dl_settings_page with
|
||||
Some settings_tab ->
|
||||
let flight_altitude_id, _flight_altitude_label =
|
||||
try
|
||||
settings_tab#assoc "flight_altitude"
|
||||
with Not_found ->
|
||||
failwith "flight_altitude not setable" in
|
||||
strip#connect_shift_alt
|
||||
(fun x ->
|
||||
dl_setting_callback flight_altitude_id (ac.target_alt +. x));
|
||||
| None -> ()
|
||||
end
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -488,8 +507,8 @@ let get_wind_msg = fun (geomap:G.widget) _sender vs ->
|
||||
let get_fbw_msg = fun _sender vs ->
|
||||
let ac = get_ac vs in
|
||||
let status = Pprz.string_assoc "rc_status" vs in
|
||||
Strip.set_label ac.strip "RC" status;
|
||||
Strip.set_color ac.strip "RC"
|
||||
ac.strip#set_label "RC" status;
|
||||
ac.strip#set_color "RC"
|
||||
(match status with
|
||||
"LOST" | "REALLY_LOST" -> warning_color
|
||||
| _ -> ok_color)
|
||||
@@ -498,9 +517,9 @@ let get_fbw_msg = fun _sender vs ->
|
||||
|
||||
let get_engine_status_msg = fun _sender vs ->
|
||||
let ac = get_ac vs in
|
||||
Strip.set_label ac.strip "throttle"
|
||||
ac.strip#set_label "throttle"
|
||||
(string_of_float (Pprz.float_assoc "throttle" vs));
|
||||
Strip.set_bat ac.strip (Pprz.float_assoc "bat" vs)
|
||||
ac.strip#set_bat (Pprz.float_assoc "bat" vs)
|
||||
|
||||
let get_if_calib_msg = fun _sender vs ->
|
||||
let ac = get_ac vs in
|
||||
@@ -589,14 +608,14 @@ let listen_flight_params = fun geomap auto_center_new_ac alert ->
|
||||
then sprintf "- %.1f" (abs_float value)
|
||||
else sprintf "%.1f" value
|
||||
in
|
||||
Strip.set_label ac.strip lbl_name s
|
||||
ac.strip#set_label lbl_name s
|
||||
in
|
||||
set_label "alt" alt;
|
||||
set_label "speed" speed;
|
||||
set_label "climb" climb;
|
||||
let agl = (a "agl") in
|
||||
ac.alt <- alt;
|
||||
Strip.set_agl ac.strip agl;
|
||||
ac.strip#set_agl agl;
|
||||
if (ac.flight_time > 10 && agl < 20.) then
|
||||
log_and_say alert ac.ac_name (sprintf "%s, %s" ac.ac_name "Ground Proximity Warning")
|
||||
|
||||
@@ -612,25 +631,25 @@ let listen_flight_params = fun geomap auto_center_new_ac alert ->
|
||||
and cur_stage = Pprz.int_assoc "cur_stage" vs in
|
||||
highlight_fp ac cur_block cur_stage;
|
||||
let set_label = fun l f ->
|
||||
Strip.set_label ac.strip l (sprintf "%.1f" (Pprz.float_assoc f vs)) in
|
||||
ac.strip#set_label l (sprintf "%.1f" (Pprz.float_assoc f vs)) in
|
||||
set_label "->" "target_alt";
|
||||
set_label "/" "target_climb";
|
||||
let target_alt = Pprz.float_assoc "target_alt" vs in
|
||||
Strip.set_label ac.strip "diff_target_alt" (sprintf "%+.0f" (ac.alt -. target_alt));
|
||||
|
||||
ac.strip#set_label "diff_target_alt" (sprintf "%+.0f" (ac.alt -. target_alt));
|
||||
ac.target_alt <- target_alt;
|
||||
let b = List.assoc cur_block ac.blocks in
|
||||
if b <> ac.last_block_name then begin
|
||||
log_and_say alert ac.ac_name (sprintf "%s, %s" ac.ac_name b);
|
||||
ac.last_block_name <- b;
|
||||
let b = String.sub b 0 (min 10 (String.length b)) in
|
||||
Strip.set_label ac.strip "block_name" b
|
||||
ac.strip#set_label "block_name" b
|
||||
end;
|
||||
let block_time = Int32.to_int (Pprz.int32_assoc "block_time" vs)
|
||||
and stage_time = Int32.to_int (Pprz.int32_assoc "stage_time" vs) in
|
||||
let bt = sprintf "%02d:%02d" (block_time / 60) (block_time mod 60) in
|
||||
Strip.set_label ac.strip "block_time" bt;
|
||||
ac.strip#set_label "block_time" bt;
|
||||
let st = sprintf "%02d:%02d" (stage_time / 60) (block_time mod 60) in
|
||||
Strip.set_label ac.strip "stage_time" st
|
||||
ac.strip#set_label "stage_time" st
|
||||
in
|
||||
safe_bind "NAV_STATUS" get_ns;
|
||||
|
||||
@@ -686,15 +705,15 @@ let listen_flight_params = fun geomap auto_center_new_ac alert ->
|
||||
if ap_mode <> ac.last_ap_mode then begin
|
||||
log_and_say alert ac.ac_name (sprintf "%s, %s" ac.ac_name ap_mode);
|
||||
ac.last_ap_mode <- ap_mode;
|
||||
Strip.set_label ac.strip "AP" (Pprz.string_assoc "ap_mode" vs);
|
||||
Strip.set_color ac.strip "AP" (if ap_mode="HOME" then alert_color else ok_color);
|
||||
ac.strip#set_label "AP" (Pprz.string_assoc "ap_mode" vs);
|
||||
ac.strip#set_color "AP" (if ap_mode="HOME" then alert_color else ok_color);
|
||||
end;
|
||||
let gps_mode = Pprz.string_assoc "gps_mode" vs in
|
||||
Strip.set_label ac.strip "GPS" gps_mode;
|
||||
Strip.set_color ac.strip "GPS" (if gps_mode<>"3D" then alert_color else ok_color);
|
||||
ac.strip#set_label "GPS" gps_mode;
|
||||
ac.strip#set_color "GPS" (if gps_mode<>"3D" then alert_color else ok_color);
|
||||
let ft =
|
||||
sprintf "%02d:%02d:%02d" (flight_time / 3600) ((flight_time / 60) mod 60) (flight_time mod 60) in
|
||||
Strip.set_label ac.strip "flight_time" ft;
|
||||
ac.strip#set_label "flight_time" ft;
|
||||
let kill_mode = Pprz.string_assoc "kill_mode" vs in
|
||||
if not ac.in_kill_mode then
|
||||
if kill_mode <> "OFF" then begin
|
||||
@@ -775,8 +794,8 @@ let message_request = Ground_Pprz.message_req
|
||||
let get_ts = fun _sender vs ->
|
||||
let ac = get_ac vs in
|
||||
let t = Pprz.float_assoc "time_since_last_bat_msg" vs in
|
||||
Strip.set_label ac.strip "telemetry_status" (if t > 2. then sprintf "%.1f" t else " ");
|
||||
Strip.set_color ac.strip "telemetry_status" (if t > 5. then alert_color else ok_color)
|
||||
ac.strip#set_label "telemetry_status" (if t > 2. then sprintf "%.1f" t else " ");
|
||||
ac.strip#set_color "telemetry_status" (if t > 5. then alert_color else ok_color)
|
||||
|
||||
|
||||
let listen_telemetry_status = fun () ->
|
||||
|
||||
@@ -202,14 +202,15 @@ class misc ~packing (widget: GBin.frame) =
|
||||
(*****************************************************************************)
|
||||
(* Dataling settings paged *)
|
||||
(*****************************************************************************)
|
||||
let one_setting = fun i do_change packing s (tooltips:GData.tooltips) strip ->
|
||||
let one_setting = fun i do_change packing s (tooltips:GData.tooltips) (strip:Strip.t) ->
|
||||
let f = fun a -> float_of_string (ExtXml.attrib s a) in
|
||||
let lower = f "min"
|
||||
and upper = f "max"
|
||||
and step_incr = f "step" in
|
||||
|
||||
let hbox = GPack.hbox ~packing () in
|
||||
let text = try ExtXml.attrib s "shortname" with _ -> ExtXml.attrib s "var" in
|
||||
let varname = ExtXml.attrib s "var" in
|
||||
let text = try ExtXml.attrib s "shortname" with _ -> varname in
|
||||
let _l = GMisc.label ~width:100 ~text ~packing:hbox#pack () in
|
||||
let _v = GMisc.label ~width:50 ~text:"N/A" ~packing:hbox#pack () in
|
||||
|
||||
@@ -261,10 +262,10 @@ let one_setting = fun i do_change packing s (tooltips:GData.tooltips) strip ->
|
||||
let label = ExtXml.attrib x "name"
|
||||
and sp_value = ExtXml.float_attrib x "value" in
|
||||
let b = GButton.button ~label () in
|
||||
Strip.add_widget strip b#coerce;
|
||||
strip#add_widget b#coerce;
|
||||
ignore (b#connect#clicked (fun _ -> do_change i sp_value)))
|
||||
(Xml.children s);
|
||||
_v
|
||||
(i, varname, _v)
|
||||
|
||||
|
||||
let rec build_settings = fun do_change i flat_list xml_settings packing tooltips strip ->
|
||||
@@ -303,17 +304,21 @@ class settings = fun ?(visible = fun _ -> true) xml_settings do_change strip ->
|
||||
let vbox = GPack.vbox ~packing:sw#add_with_viewport () in
|
||||
let tooltips = GData.tooltips () in
|
||||
let i = ref 0 and l = ref [] in
|
||||
let current_values =
|
||||
let ordered_list =
|
||||
build_settings do_change i l xml_settings vbox#add tooltips strip;
|
||||
Array.of_list (List.rev !l) in
|
||||
List.rev !l in
|
||||
let variables = Array.of_list ordered_list in
|
||||
let assocs = List.map (fun (i,var,_v) -> (var, (i, _v))) ordered_list in
|
||||
object (self)
|
||||
method widget = sw#coerce
|
||||
method length = !i
|
||||
method set = fun i v ->
|
||||
if visible self#widget then
|
||||
let s = string_of_float v in
|
||||
if current_values.(i)#text <> s then
|
||||
current_values.(i)#set_text s
|
||||
let (_, _, current_value) = variables.(i) in
|
||||
if current_value#text <> s then
|
||||
current_value#set_text s
|
||||
method assoc var = List.assoc var assocs
|
||||
end
|
||||
|
||||
|
||||
|
||||
@@ -28,6 +28,7 @@ class settings : ?visible:(GObj.widget -> bool) -> Xml.xml list -> (int -> float
|
||||
object
|
||||
method length : int
|
||||
method set : int -> float -> unit
|
||||
method assoc : string -> int * GMisc.label
|
||||
method widget : GObj.widget
|
||||
end
|
||||
|
||||
|
||||
@@ -24,6 +24,14 @@
|
||||
*
|
||||
*)
|
||||
|
||||
type t =
|
||||
< add_widget : GObj.widget -> unit;
|
||||
connect_shift_alt : (float -> unit) -> unit;
|
||||
set_agl : ?color:string -> float -> unit;
|
||||
set_bat : ?color:string -> float -> unit;
|
||||
set_color : string -> string -> unit;
|
||||
set_label : string -> string -> unit >
|
||||
|
||||
let bat_max = 12.5
|
||||
let bat_min = 9.
|
||||
let agl_max = 150.
|
||||
@@ -33,12 +41,58 @@ let scrolled = GBin.scrolled_window ~hpolicy: `AUTOMATIC ~vpolicy: `AUTOMATIC ()
|
||||
let table = GPack.table ~rows: 1 ~columns: 1 ~row_spacings: 5 ~packing: (scrolled#add_with_viewport) ()
|
||||
|
||||
|
||||
type t = {
|
||||
agl: GMisc.drawing_area;
|
||||
gauge: GMisc.drawing_area;
|
||||
labels: (string * (GBin.event_box * GMisc.label)) list;
|
||||
buttons_box : GPack.box
|
||||
}
|
||||
|
||||
|
||||
(** set a label *)
|
||||
let set_label labels name value =
|
||||
try
|
||||
let _eb, l = List.assoc (name^"_value") labels in
|
||||
if l#text <> value then
|
||||
l#set_label value
|
||||
with
|
||||
Not_found ->
|
||||
Printf.fprintf stderr "Strip.set_label: '%s' unknown\n%!" name
|
||||
|
||||
(** set a color *)
|
||||
let set_color labels name color =
|
||||
let eb, _l = List.assoc (name^"_value") labels in
|
||||
eb#coerce#misc#modify_bg [`NORMAL, `NAME color]
|
||||
|
||||
|
||||
let set_gauge = fun ?(color="green") gauge v_min v_max value string ->
|
||||
let {Gtk.width=width; height=height} = gauge#misc#allocation in
|
||||
let dr = GDraw.pixmap ~width ~height ~window:gauge () in
|
||||
dr#set_foreground (`NAME "orange");
|
||||
dr#rectangle ~x:0 ~y:0 ~width ~height ~filled:true ();
|
||||
|
||||
let f = (value -. v_min) /. (v_max -. v_min) in
|
||||
let f = max 0. (min 1. f) in
|
||||
let h = truncate (float height *. f) in
|
||||
dr#set_foreground (`NAME color);
|
||||
dr#rectangle ~x:0 ~y:(height-h) ~width ~height:h ~filled:true ();
|
||||
|
||||
let context = gauge#misc#create_pango_context in
|
||||
let layout = context#create_layout in
|
||||
Pango.Layout.set_text layout string;
|
||||
let (w,h) = Pango.Layout.get_pixel_size layout in
|
||||
dr#put_layout ~x:((width-w)/2) ~y:((height-h)/2) ~fore:`BLACK layout;
|
||||
|
||||
(new GDraw.drawable gauge#misc#window)#put_pixmap ~x:0 ~y:0 dr#pixmap
|
||||
|
||||
|
||||
|
||||
(** set the battery *)
|
||||
let set_bat ?color gauge value =
|
||||
set_gauge ?color gauge bat_min bat_max value (string_of_float value)
|
||||
|
||||
|
||||
(** set the AGL *)
|
||||
let set_agl ?color agl value =
|
||||
set_gauge ?color agl 0. agl_max value (Printf.sprintf "%3.0f" value)
|
||||
|
||||
|
||||
let add_widget = fun buttons_box widget ->
|
||||
buttons_box#add widget
|
||||
|
||||
let labels_name = [|
|
||||
[| "AP" ; "alt" ; "->" |]; [| "RC"; "climb"; "/" |]; [| "GPS"; "speed"; "throttle" |]
|
||||
@@ -53,8 +107,12 @@ let rows = 1 + Array.length labels_name + 1
|
||||
let columns = 1 + 2 * Array.length labels_name.(0) + 1
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
(** add a strip to the panel *)
|
||||
let add config color select center_ac commit_moves mark =
|
||||
let add config color select center_ac mark =
|
||||
let widget = table in
|
||||
(* number of the strip *)
|
||||
let strip_number = gen_int () in
|
||||
@@ -107,13 +165,13 @@ let add config color select center_ac commit_moves mark =
|
||||
gauge#misc#realize ();
|
||||
|
||||
(* AGL gauge *)
|
||||
let agl_box = GBin.event_box ~packing:(strip#attach ~top:1 ~bottom:(rows-1) ~left:(columns-1)) () in
|
||||
let agl_box = GBin.event_box ~packing:(strip#attach ~top:1 ~bottom:3 ~left:(columns-1)) () in
|
||||
let agl = GMisc.drawing_area ~width:30 ~height:60 ~show:true ~packing:agl_box#add () in
|
||||
agl#misc#realize ();
|
||||
tooltips#set_tip agl_box#coerce ~text:"AGL (m)";
|
||||
|
||||
(* Diff to target altitude *)
|
||||
let dta_box = GBin.event_box ~packing:(strip#attach ~top:(rows-1) ~left:(columns-1)) () in
|
||||
let dta_box = GBin.event_box ~packing:(strip#attach ~top:3 ~left:(columns-1)) () in
|
||||
let diff_target_alt = GMisc.label ~text: "+0" ~packing:dta_box#add () in
|
||||
add_label "diff_target_alt_value" (plane_color, diff_target_alt);
|
||||
tooltips#set_tip dta_box#coerce ~text:"Height to target (m)";
|
||||
@@ -141,63 +199,26 @@ let add config color select center_ac commit_moves mark =
|
||||
let top = rows - 1 in
|
||||
let b = GButton.button ~label:"Center A/C" ~packing:(strip#attach ~top ~left:1 ~right:3) () in
|
||||
ignore(b#connect#clicked ~callback:center_ac);
|
||||
let b = GButton.button ~label:"Mark" ~packing:(strip#attach ~top ~left:5 ~right:7) () in
|
||||
let b = GButton.button ~label:"Mark" ~packing:(strip#attach ~top ~left:3 ~right:5) () in
|
||||
ignore (b#connect#clicked ~callback:mark);
|
||||
|
||||
let minus5 = GButton.button ~label:"-5m" ~packing:(strip#attach ~top ~left:5) ()
|
||||
and plus5 = GButton.button ~label:"+5m" ~packing:(strip#attach ~top ~left:6) ()
|
||||
and plus30 = GButton.button ~label:"+30m" ~packing:(strip#attach ~top ~left:7) () in
|
||||
ignore (b#connect#clicked ~callback:mark);
|
||||
|
||||
(* User buttons *)
|
||||
let hbox = GPack.hbox ~packing:framevb#add () in
|
||||
|
||||
{ agl=agl; gauge=gauge ; labels= !strip_labels; buttons_box = hbox}
|
||||
|
||||
|
||||
|
||||
(** set a label *)
|
||||
let set_label strip name value =
|
||||
try
|
||||
let _eb, l = List.assoc (name^"_value") strip.labels in
|
||||
if l#text <> value then
|
||||
l#set_label value
|
||||
with
|
||||
Not_found ->
|
||||
Printf.fprintf stderr "Strip.set_label: '%s' unknown\n%!" name
|
||||
|
||||
(** set a color *)
|
||||
let set_color strip name color =
|
||||
let eb, _l = List.assoc (name^"_value") strip.labels in
|
||||
eb#coerce#misc#modify_bg [`NORMAL, `NAME color]
|
||||
|
||||
|
||||
let set_gauge = fun ?(color="green") gauge v_min v_max value string ->
|
||||
let {Gtk.width=width; height=height} = gauge#misc#allocation in
|
||||
let dr = GDraw.pixmap ~width ~height ~window:gauge () in
|
||||
dr#set_foreground (`NAME "orange");
|
||||
dr#rectangle ~x:0 ~y:0 ~width ~height ~filled:true ();
|
||||
|
||||
let f = (value -. v_min) /. (v_max -. v_min) in
|
||||
let f = max 0. (min 1. f) in
|
||||
let h = truncate (float height *. f) in
|
||||
dr#set_foreground (`NAME color);
|
||||
dr#rectangle ~x:0 ~y:(height-h) ~width ~height:h ~filled:true ();
|
||||
|
||||
let context = gauge#misc#create_pango_context in
|
||||
let layout = context#create_layout in
|
||||
Pango.Layout.set_text layout string;
|
||||
let (w,h) = Pango.Layout.get_pixel_size layout in
|
||||
dr#put_layout ~x:((width-w)/2) ~y:((height-h)/2) ~fore:`BLACK layout;
|
||||
|
||||
(new GDraw.drawable gauge#misc#window)#put_pixmap ~x:0 ~y:0 dr#pixmap
|
||||
|
||||
|
||||
|
||||
(** set the battery *)
|
||||
let set_bat ?color strip value =
|
||||
set_gauge ?color strip.gauge bat_min bat_max value (string_of_float value)
|
||||
|
||||
|
||||
(** set the AGL *)
|
||||
let set_agl ?color strip value =
|
||||
set_gauge ?color strip.agl 0. agl_max value (Printf.sprintf "%3.0f" value)
|
||||
|
||||
|
||||
let add_widget = fun strip widget ->
|
||||
strip.buttons_box#add widget
|
||||
object
|
||||
method set_agl ?color value = set_agl ?color agl value
|
||||
method set_bat ?color value = set_bat ?color gauge value
|
||||
method set_label name value = set_label !strip_labels name value
|
||||
method set_color name value = set_color !strip_labels name value
|
||||
method add_widget w = add_widget hbox w
|
||||
method connect_shift_alt callback =
|
||||
ignore (plus5#connect#clicked (fun () -> callback 5.));
|
||||
ignore (plus30#connect#clicked (fun () -> callback 30.));
|
||||
ignore (minus5#connect#clicked (fun () -> callback (-5.)))
|
||||
end
|
||||
|
||||
@@ -1,11 +1,19 @@
|
||||
type t
|
||||
type t =
|
||||
< add_widget : GObj.widget -> unit;
|
||||
connect_shift_alt : (float -> unit) -> unit;
|
||||
set_agl : ?color:string -> float -> unit;
|
||||
set_bat : ?color:string -> float -> unit;
|
||||
set_color : string -> string -> unit;
|
||||
set_label : string -> string -> unit >
|
||||
|
||||
|
||||
val scrolled : GBin.scrolled_window
|
||||
|
||||
val add :
|
||||
Pprz.values ->
|
||||
string ->
|
||||
(unit -> 'a) -> (unit -> unit) -> (unit -> unit) -> (unit -> unit) -> t
|
||||
val set_label : t -> string -> string -> unit
|
||||
val set_color : t -> string -> string -> unit
|
||||
val set_bat : ?color:string -> t -> float -> unit
|
||||
val set_agl : ?color:string -> t -> float -> unit
|
||||
val scrolled : GBin.scrolled_window
|
||||
val add_widget : t -> GObj.widget -> unit
|
||||
(unit -> 'a) -> (unit -> unit) -> (unit -> unit) ->
|
||||
t
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -105,6 +105,7 @@ type aircraft = {
|
||||
mutable gaz_mode : int;
|
||||
mutable lateral_mode : int;
|
||||
mutable horizontal_mode : int;
|
||||
mutable periodic_callbacks : Glib.Timeout.id list;
|
||||
cam : ac_cam;
|
||||
mutable gps_mode : int;
|
||||
inflight_calib : inflight_calib;
|
||||
|
||||
@@ -92,6 +92,7 @@ type aircraft = {
|
||||
mutable gaz_mode : int;
|
||||
mutable lateral_mode : int;
|
||||
mutable horizontal_mode : int;
|
||||
mutable periodic_callbacks : Glib.Timeout.id list;
|
||||
cam : ac_cam;
|
||||
mutable gps_mode : int;
|
||||
inflight_calib : inflight_calib;
|
||||
|
||||
@@ -575,7 +575,7 @@ let new_aircraft = fun id ->
|
||||
gspeed=0.; course = 0.; alt=0.; climb=0.; cur_block=0; cur_stage=0;
|
||||
throttle = 0.; throttle_accu = 0.; rpm = 0.; temp = 0.; bat = 42.; amp = 0.; energy = 0; ap_mode= -1; agl = 0.;
|
||||
gaz_mode= -1; lateral_mode= -1;
|
||||
gps_mode =0;
|
||||
gps_mode =0; periodic_callbacks = [];
|
||||
desired_altitude = 0.;
|
||||
desired_climb = 0.;
|
||||
pos = { utm_x = 0.; utm_y = 0.; utm_zone = 0 };
|
||||
@@ -607,7 +607,11 @@ let wind_clear = fun _sender vs ->
|
||||
Wind.clear (Pprz.string_assoc "ac_id" vs)
|
||||
|
||||
let periodic = fun period cb ->
|
||||
ignore (Glib.Timeout.add period (fun () -> cb (); true))
|
||||
Glib.Timeout.add period (fun () -> cb (); true)
|
||||
|
||||
|
||||
let register_periodic = fun ac x ->
|
||||
ac.periodic_callbacks <- x :: ac.periodic_callbacks
|
||||
|
||||
(** add the periodic airprox check for the aircraft (name) on all aircraft *)
|
||||
(** already known *)
|
||||
@@ -635,15 +639,15 @@ let periodic_airprox_check = fun name ->
|
||||
|
||||
List.iter
|
||||
(fun ac ->
|
||||
periodic aircraft_alerts_period (fun () -> check_airprox ac)
|
||||
register_periodic thisac (periodic aircraft_alerts_period (fun () -> check_airprox ac))
|
||||
) list_ac
|
||||
|
||||
let register_aircraft = fun name a ->
|
||||
Hashtbl.add aircrafts name a;
|
||||
periodic aircraft_msg_period (fun () -> send_aircraft_msg name);
|
||||
periodic aircraft_alerts_period (fun () -> check_alerts a);
|
||||
register_periodic a (periodic aircraft_msg_period (fun () -> send_aircraft_msg name));
|
||||
register_periodic a (periodic aircraft_alerts_period (fun () -> check_alerts a));
|
||||
periodic_airprox_check name;
|
||||
periodic wind_msg_period (fun () -> send_wind a);
|
||||
register_periodic a (periodic wind_msg_period (fun () -> send_wind a));
|
||||
Wind.new_ac name 1000;
|
||||
ignore(Ground_Pprz.message_bind "WIND_CLEAR" wind_clear)
|
||||
|
||||
|
||||
@@ -185,7 +185,7 @@ class waypoint = fun (wpts_group:group) (name :string) ?(alt=0.) wgs84 ->
|
||||
(* Update AGL on pos or alt change *)
|
||||
let callback = fun _ ->
|
||||
try
|
||||
let wgs84 = LL.of_string (sprintf "WGS84 %s" e_pos#text) in
|
||||
let wgs84 = wgs84_of_string !selected_georef e_pos#text in
|
||||
let agl = float_of_string ea#text -. float (try Srtm.of_wgs84 wgs84 with _ -> 0) in
|
||||
agl_lab#set_text (sprintf " AGL: %4.0fm" agl)
|
||||
with _ -> ()
|
||||
|
||||
@@ -244,7 +244,7 @@ let rec index_stage = fun x ->
|
||||
| "heading" | "attitude" | "go" | "stay" | "xyz" | "set" | "circle" ->
|
||||
incr stage;
|
||||
Xml.Element (Xml.tag x, Xml.attribs x@["no", soi !stage], Xml.children x)
|
||||
| "survey" | "survey_rectangle" ->
|
||||
| "survey" | "survey_rectangle" | "eight" ->
|
||||
incr stage; incr stage;
|
||||
Xml.Element (Xml.tag x, Xml.attribs x@["no", soi !stage], Xml.children x)
|
||||
| "exception" ->
|
||||
@@ -398,6 +398,19 @@ let rec print_stage = fun index_of_waypoints sectors x ->
|
||||
end;
|
||||
output_cam_mode x index_of_waypoints;
|
||||
lprintf "return;\n"
|
||||
| "eight" ->
|
||||
stage ();
|
||||
lprintf "nav_eight_init();\n";
|
||||
lprintf "NextStage();\n";
|
||||
left ();
|
||||
stage ();
|
||||
let center = get_index_waypoint (ExtXml.attrib x "center") index_of_waypoints
|
||||
and turn_about = get_index_waypoint (ExtXml.attrib x "turn_around") index_of_waypoints in
|
||||
let r = parsed_attrib x "radius" in
|
||||
let _vmode = output_vmode x center "" in
|
||||
lprintf "Eight(%s, %s, %s);\n" center turn_about r;
|
||||
output_cam_mode x index_of_waypoints;
|
||||
lprintf "return;\n"
|
||||
| "set" ->
|
||||
stage ();
|
||||
let var = ExtXml.attrib x "var"
|
||||
|
||||
Reference in New Issue
Block a user