add pd ctrl and lpf (not sure if lpf is needed)

conf/messages.xml

@@ -1757,6 +1757,9 @@
       <field name="reof" type="float"/>
       <field name="dmag" type="float"/>
       <field name="deof" type="float"/>
+      <field name="dmag_lpf" type="float"/>
+      <field name="deof_lpf" type="float"/>
+      <field name="eof_sum" type="float"/>
     </message>
 
     <message name="IMU_GYRO" id="200">

sw/airborne/modules/computer_vision/cv_maze_runner.c

@@ -31,6 +31,8 @@ struct image_t *video_cb(struct image_t *img, uint8_t camera_id __attribute__((u
 
     image_copy(img, &src[0]);
     opencv_frontend_init(img->h, img->w, 0.4, 1.0, 0.45, DISULTRAFAST);
+    // opencv_frontend_init(img->h, img->w, 0.4, 1.0, 0.45, DISFAST); -> 8-15 FPS
+    // opencv_frontend_init(img->h, img->w, 0.4, 1.0, 0.45, DISULTRAFAST); -> 25 FPS
 
     pthread_mutex_lock(&mutex);
     cb_state = CB_STATE_SECOND_RUN;

sw/airborne/modules/maze_runner/maze_runner.c

@@ -48,7 +48,7 @@ void telem_cb(struct transport_tx *trans, struct link_device *dev);
  */
 static abi_event cv_ev;
 static struct cv_info_t cv_info, cv_info_cpy;
-static struct dbg_msg_t dbg_msg, dbg_msg_cpy;
+static struct dbg_msg_t dbg_msg, dbg_msg_cpy, dbg_msg_ctrl;
 static struct cmd_t cmd;
 static pthread_mutex_t mtx_cv_info;
 static pthread_mutex_t mtx_dbg_msg;
@@ -82,7 +82,9 @@ void telem_cb(struct transport_tx *trans, struct link_device *dev)
             &dbg_msg_cpy.fps,
             &dbg_msg_cpy.lmag, &dbg_msg_cpy.rmag,
             &dbg_msg_cpy.leof, &dbg_msg_cpy.reof,
-            &dbg_msg_cpy.dmag, &dbg_msg_cpy.deof
+            &dbg_msg_cpy.dmag, &dbg_msg_cpy.deof,
+            &dbg_msg_cpy.dmag_lpf, &dbg_msg_cpy.deof_lpf,
+            &dbg_msg_cpy.eof_sum
             );
 }
 
@@ -104,12 +106,7 @@ void maze_runner_loop(void)
 
     // fill dbg_msg
     pthread_mutex_lock(&mtx_dbg_msg);
-    dbg_msg.lmag = cv_info_cpy.lmag;
-    dbg_msg.rmag = cv_info_cpy.rmag;
-    dbg_msg.leof = cv_info_cpy.leof;
-    dbg_msg.reof = cv_info_cpy.reof;
-    dbg_msg.dmag = cv_info_cpy.lmag - cv_info_cpy.rmag;
-    dbg_msg.deof = cv_info_cpy.leof - cv_info_cpy.reof;
+    memcpy(&dbg_msg, &dbg_msg_ctrl, sizeof(dbg_msg_ctrl));
     pthread_mutex_unlock(&mtx_dbg_msg);
 
     // get copy of shared resource
@@ -131,7 +128,7 @@ void maze_runner_loop(void)
     goal_wp.z = waypoint_get_alt(WP_GUIDED_GOAL);
 
     // calculate control output, which will be used asap in the next loop
-    ctrl_backend_run(&cmd, &goal_wp, &mav, &cv_info_cpy);
+    ctrl_backend_run(&cmd, &dbg_msg_ctrl, &goal_wp, &mav, &cv_info_cpy);
 
     return;
 }

sw/airborne/modules/maze_runner/maze_runner.h

@@ -64,6 +64,9 @@ struct dbg_msg_t
     float reof;
     float dmag;
     float deof;
+    float dmag_lpf;
+    float deof_lpf;
+    float eof_sum;
     int16_t fps;
 };
 
@@ -83,6 +86,19 @@ struct mav_state_t
     struct FloatRates ang_vel_body;
 };
 
+struct var_t
+{
+    float x;
+    float dx;
+};
+
+struct nav_weights_t
+{
+    float wp;
+    float mag;
+    float eof;
+};
+
 extern void maze_runner_init(void);
 extern void maze_runner_loop(void);
 
@@ -94,6 +110,12 @@ void cv_cb(uint8_t __attribute__((unused)) sender_id,
            int fps);
 
 void ctrl_backend_init(void);
-void ctrl_backend_run(struct cmd_t *cmd, struct EnuCoor_f *goal, struct mav_state_t *mav, struct cv_info_t *cv);
+void ctrl_backend_run(struct cmd_t *cmd, struct dbg_msg_t *dbg, struct EnuCoor_f *goal, struct mav_state_t *mav, struct cv_info_t *cv);
+
+float get_wp_err(float err_x, float err_y, struct mav_state_t *mav);
+void low_pass_filter(struct var_t *var, float input, float alpha);
+void sigmoid_nav_weight(struct nav_weights_t *weights, struct var_t *eof_sum, float eof_sum_thresh_0, float eof_sum_thresh_1, float switch_rate);
+void constrain(float *x, float min, float max);
+float pd_ctrl(struct var_t *var, float p, float d);
 
 #endif

sw/airborne/modules/maze_runner/maze_runner_helper.c

@@ -39,64 +39,170 @@
  * global variables, only shared within this file
  */
 static int stage = 0;
+static struct var_t err_mag, err_eof, err_wp, err_dst, sum_eof;
+static float p_wp, d_wp, p_mag, d_mag, p_dst, d_dst, p_eof, d_eof;
+static float alpha;
+static struct nav_weights_t nav_weights;
+static float eof_thresh_0 = 150;
+static float eof_thresh_1 = 400;
+static struct EnuCoor_f last_goal;
 
 /**
  * implement functions
  */
-void ctrl_backend_init(void){}
-void ctrl_backend_run(struct cmd_t *cmd, struct EnuCoor_f *goal, struct mav_state_t *mav, struct cv_info_t *cv)
+void ctrl_backend_init(void)
 {
-    float err_mag = cv->lmag - cv->rmag;
-    float err_eof = cv->leof - cv->reof;
+    err_mag.x = err_mag.dx = 0;
+    err_eof.x = err_eof.dx = 0;
+    err_wp.x = err_wp.dx= 0;
+    err_dst.x = err_dst.dx = 0;
+    sum_eof.x = sum_eof.dx = 0;
+    nav_weights.wp = 1;
+    nav_weights.mag = 0;
+    nav_weights.eof = 0;
+
+    last_goal.x = last_goal.y = 0;
+
+    p_wp = 1;
+    d_wp = 0.05;
+    alpha = 0.05 / (0 + 0.05);
+    p_mag = 0.0001;
+    d_mag = 0.00;
+    p_dst = 1;
+    d_dst = 0.1;
+    p_eof = 0.1;
+    d_eof = 0.01;
+}
+
+void ctrl_backend_run(struct cmd_t *cmd, struct dbg_msg_t *dbg, struct EnuCoor_f *goal, struct mav_state_t *mav, struct cv_info_t *cv)
+{
+    // xy err
     float err_x = goal->x - mav->pos_enu.x;
     float err_y = goal->y - mav->pos_enu.y;
-    float mav_heading = mav->rpy_ned.psi;
-    float wp_heading = atan2(err_x, err_y);
-    if(mav_heading < 0)
-    {
-        mav_heading += 2 * M_PI;
-    }
-    if(wp_heading < 0)
-    {
-        wp_heading += 2 * M_PI;
-    }
-    float err_wp = wp_heading - mav_heading;
-    if(err_wp > M_PI)
-    {
-        err_wp -= 2 * M_PI;
-    }
-    else if(err_wp < -M_PI)
-    {
-        err_wp += 2 * M_PI;
-    }
+    float err_dst_new = sqrt(pow(err_x, 2) + pow(err_y, 2));
+    err_dst.dx = err_dst_new - err_dst.x;
+    err_dst.x = err_dst_new;
+
+    // wp heading err
+    float err_wp_new = get_wp_err(err_x, err_y, mav);
+    err_wp.dx = err_wp_new - err_wp.x;
+    err_wp.x = err_wp_new;
+
+    // left right optflow mag err
+    low_pass_filter(&err_mag, cv->lmag - cv->rmag, alpha);
+
+    // left right optflow eof err
+    low_pass_filter(&err_eof, cv->leof - cv->reof, alpha);
+
+    // sum optflow eof
+    low_pass_filter(&sum_eof, cv->leof + cv->reof, alpha);
+
+    // calculate weights for different nav modes
+    sigmoid_nav_weight(&nav_weights, &sum_eof, eof_thresh_0, eof_thresh_1, 20);
+
+
+    // state machine
     switch (stage) {
         case 0:
             cmd->body_vel_x = 0;
             cmd->body_vel_y = 0;
-            cmd->yaw_rate = 0.5 * err_wp;
-            if(fabs(err_wp) < 0.2)
+            cmd->yaw_rate = pd_ctrl(&err_wp, p_wp, d_wp);
+
+            if(fabs(err_wp.x) < 0.1)
             {
                 stage = 1;
             }
             break;
         case 1:
-            cmd->body_vel_x = 0.3;
+            cmd->body_vel_x = 0.4;
             cmd->body_vel_y = 0;
-            cmd->yaw_rate = 0.5 * err_wp;
-            if(fabs(err_x) < 0.5 && fabs(err_y) < 0.5)
+            cmd->yaw_rate = pd_ctrl(&err_wp, p_wp, d_wp);// + pd_ctrl(&err_mag, p_mag, d_mag);
+            constrain(&cmd->yaw_rate, -1, 1);
+
+            if(fabs(err_dst.x) < 0.5)
             {
                 stage = 2;
             }
+
             break;
         case 2:
             cmd->body_vel_x = 0;
             cmd->body_vel_y = 0;
             cmd->yaw_rate = 0;
-            if(fabs(err_wp) > 0.2 || fabs(err_x) > 0.5 || fabs(err_y) > 0.5)
+
+            if(fabs(err_dst.x) > 1 && (last_goal.x != goal->x || last_goal.y != goal->y))
             {
                 stage = 0;
             }
             break;
     }
+    last_goal.x = goal->x;
+    last_goal.y = goal->y;
 
-}
+    // dbg msg
+    dbg->fps = cv->fps;
+    dbg->lmag = cv->lmag;
+    dbg->rmag = cv->rmag;
+    dbg->leof = cv->leof;
+    dbg->reof = cv->reof;
+    dbg->dmag = cv->lmag - cv->rmag;
+    dbg->deof = cv->leof - cv->reof;
+    dbg->dmag_lpf = err_mag.x;
+    dbg->deof_lpf = err_eof.x;
+    dbg->eof_sum = cv->leof + cv->reof;
+}
+
+float get_wp_err(float err_x, float err_y, struct mav_state_t *mav)
+{
+    float mav_heading = mav->rpy_ned.psi;
+    float wp_heading = atan2(err_x, err_y);
+    if(mav_heading < 0)
+    {
+        mav_heading += 2 * M_PI;
+    }
+    if(wp_heading < 0)
+    {
+        wp_heading += 2 * M_PI;
+    }
+    float err_wp = wp_heading - mav_heading;
+    if(err_wp > M_PI)
+    {
+        err_wp -= 2 * M_PI;
+    }
+    else if(err_wp < -M_PI)
+    {
+        err_wp += 2 * M_PI;
+    }
+    return err_wp;
+}
+
+void low_pass_filter(struct var_t *var, float input, float alpha)
+{
+    float new_x = alpha * input + (1 - alpha) * var->x;
+    var->dx = new_x - var->x;
+    var->x = new_x;
+}
+
+void sigmoid_nav_weight(struct nav_weights_t *weights, struct var_t *eof_sum, float eof_sum_thresh_0, float eof_sum_thresh_1, float switch_rate)
+{
+    weights->wp = 1.0 / (1 + exp(switch_rate * (eof_sum->x - eof_sum_thresh_0)));
+    weights->mag = 1.0 / (1 + exp(switch_rate * (eof_sum->x - eof_sum_thresh_1)));
+    weights->eof = 1.0 / (1 + exp(-switch_rate * (eof_sum->x - eof_sum_thresh_1)));
+}
+
+void constrain(float *x, float min, float max)
+{
+    if(*x > max)
+    {
+        *x = max;
+    }
+    if(*x < min)
+    {
+        *x = min;
+    }
+}
+
+float pd_ctrl(struct var_t *var, float p, float d)
+{
+    return var->x * p + var->dx * d;
+}