/** \fn void navigation_task( void ) * \brief Compute desired_course */ static void navigation_task( void ) { #if defined FAILSAFE_DELAY_WITHOUT_GPS /** This section is used for the failsafe of GPS */ static uint8_t last_pprz_mode; /** If aircraft is launched and is in autonomus mode, go into PPRZ_MODE_GPS_OUT_OF_ORDER mode (Failsafe) if we lost the GPS */ if (launch) { if (GpsTimeoutError) { if (pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME) { last_pprz_mode = pprz_mode; pprz_mode = PPRZ_MODE_GPS_OUT_OF_ORDER; PERIODIC_SEND_PPRZ_MODE(DefaultChannel); gps_lost = TRUE; } } else /* GPS is ok */ if (gps_lost) { /** If aircraft was in failsafe mode, come back in previous mode */ pprz_mode = last_pprz_mode; gps_lost = FALSE; PERIODIC_SEND_PPRZ_MODE(DefaultChannel); } } #endif /* GPS && FAILSAFE_DELAY_WITHOUT_GPS */ common_nav_periodic_task_4Hz(); if (pprz_mode == PPRZ_MODE_HOME) nav_home(); else if (pprz_mode == PPRZ_MODE_GPS_OUT_OF_ORDER) nav_without_gps(); else nav_periodic_task(); #ifdef TCAS CallTCAS(); #endif #ifndef PERIOD_NAVIGATION_DefaultChannel_0 // If not sent periodically (in default 0 mode) SEND_NAVIGATION(DefaultChannel); #endif SEND_CAM(DefaultChannel); /* The nav task computes only nav_altitude. However, we are interested by desired_altitude (= nav_alt+alt_shift) in any case. So we always run the altitude control loop */ if (v_ctl_mode == V_CTL_MODE_AUTO_ALT) v_ctl_altitude_loop(); if (pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME || pprz_mode == PPRZ_MODE_GPS_OUT_OF_ORDER) { #ifdef H_CTL_RATE_LOOP /* Be sure to be in attitude mode, not roll */ h_ctl_auto1_rate = FALSE; #endif if (lateral_mode >=LATERAL_MODE_COURSE) h_ctl_course_loop(); /* aka compute nav_desired_roll */ if (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB) v_ctl_climb_loop(); if (v_ctl_mode == V_CTL_MODE_AUTO_THROTTLE) v_ctl_throttle_setpoint = nav_throttle_setpoint; #if defined V_CTL_THROTTLE_IDLE Bound(v_ctl_throttle_setpoint, TRIM_PPRZ(V_CTL_THROTTLE_IDLE*MAX_PPRZ), MAX_PPRZ); #endif #ifdef V_CTL_POWER_CTL_BAT_NOMINAL if (vsupply > 0.) { v_ctl_throttle_setpoint *= 10. * V_CTL_POWER_CTL_BAT_NOMINAL / (float)vsupply; v_ctl_throttle_setpoint = TRIM_UPPRZ(v_ctl_throttle_setpoint); } #endif h_ctl_pitch_setpoint = nav_pitch; Bound(h_ctl_pitch_setpoint, H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT); if (kill_throttle || (!estimator_flight_time && !launch)) v_ctl_throttle_setpoint = 0; } energy += ((float)current) / 3600.0f * 0.25f; // mAh = mA * dt (4Hz -> hours) }
static void send_nav(void) { SEND_NAVIGATION(DefaultChannel, DefaultDevice); }
void dl_parse_msg(void) { datalink_time = 0; uint8_t msg_id = IdOfMsg(dl_buffer); #if 0 // not ready yet uint8_t sender_id = SenderIdOfMsg(dl_buffer); /* parse telemetry messages coming from other AC */ if (sender_id != 0) { switch (msg_id) { #ifdef TCAS case DL_TCAS_RA: { if (DL_TCAS_RESOLVE_ac_id(dl_buffer) == AC_ID && SenderIdOfMsg(dl_buffer) != AC_ID) { uint8_t ac_id_conflict = SenderIdOfMsg(dl_buffer); tcas_acs_status[the_acs_id[ac_id_conflict]].resolve = DL_TCAS_RA_resolve(dl_buffer); } } #endif } return; } #endif if (msg_id == DL_PING) { DOWNLINK_SEND_PONG(DefaultChannel); } else #ifdef TRAFFIC_INFO if (msg_id == DL_ACINFO && DL_ACINFO_ac_id(dl_buffer) != AC_ID) { uint8_t id = DL_ACINFO_ac_id(dl_buffer); float ux = MOfCm(DL_ACINFO_utm_east(dl_buffer)); float uy = MOfCm(DL_ACINFO_utm_north(dl_buffer)); float a = MOfCm(DL_ACINFO_alt(dl_buffer)); float c = RadOfDeg(((float)DL_ACINFO_course(dl_buffer))/ 10.); float s = MOfCm(DL_ACINFO_speed(dl_buffer)); float cl = MOfCm(DL_ACINFO_climb(dl_buffer)); uint32_t t = DL_ACINFO_itow(dl_buffer); SetAcInfo(id, ux, uy, c, a, s, cl, t); } else #endif #ifdef NAV if (msg_id == DL_MOVE_WP && DL_MOVE_WP_ac_id(dl_buffer) == AC_ID) { uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer); float a = MOfCm(DL_MOVE_WP_alt(dl_buffer)); /* Computes from (lat, long) in the referenced UTM zone */ float lat = RadOfDeg((float)(DL_MOVE_WP_lat(dl_buffer) / 1e7)); float lon = RadOfDeg((float)(DL_MOVE_WP_lon(dl_buffer) / 1e7)); latlong_utm_of(lat, lon, nav_utm_zone0); nav_move_waypoint(wp_id, latlong_utm_x, latlong_utm_y, a); /* Waypoint range is limited. Computes the UTM pos back from the relative coordinates */ latlong_utm_x = waypoints[wp_id].x + nav_utm_east0; latlong_utm_y = waypoints[wp_id].y + nav_utm_north0; DOWNLINK_SEND_WP_MOVED(DefaultChannel, &wp_id, &latlong_utm_x, &latlong_utm_y, &a, &nav_utm_zone0); } else if (msg_id == DL_BLOCK && DL_BLOCK_ac_id(dl_buffer) == AC_ID) { nav_goto_block(DL_BLOCK_block_id(dl_buffer)); SEND_NAVIGATION(DefaultChannel); } else #endif /** NAV */ #ifdef WIND_INFO if (msg_id == DL_WIND_INFO && DL_WIND_INFO_ac_id(dl_buffer) == AC_ID) { wind_east = DL_WIND_INFO_east(dl_buffer); wind_north = DL_WIND_INFO_north(dl_buffer); #ifndef USE_AIRSPEED estimator_airspeed = DL_WIND_INFO_airspeed(dl_buffer); #endif #ifdef WIND_INFO_RET DOWNLINK_SEND_WIND_INFO_RET(DefaultChannel, &wind_east, &wind_north, &estimator_airspeed); #endif } else #endif /** WIND_INFO */ #ifdef HITL /** Infrared and GPS sensors are replaced by messages on the datalink */ if (msg_id == DL_HITL_INFRARED) { /** This code simulates infrared.c:ir_update() */ ir_roll = DL_HITL_INFRARED_roll(dl_buffer); ir_pitch = DL_HITL_INFRARED_pitch(dl_buffer); ir_top = DL_HITL_INFRARED_top(dl_buffer); } else if (msg_id == DL_HITL_UBX) { /** This code simulates gps_ubx.c:parse_ubx() */ if (gps_msg_received) { gps_nb_ovrn++; } else { ubx_class = DL_HITL_UBX_class(dl_buffer); ubx_id = DL_HITL_UBX_id(dl_buffer); uint8_t l = DL_HITL_UBX_ubx_payload_length(dl_buffer); uint8_t *ubx_payload = DL_HITL_UBX_ubx_payload(dl_buffer); memcpy(ubx_msg_buf, ubx_payload, l); gps_msg_received = TRUE; } } else #endif #ifdef DlSetting if (msg_id == DL_SETTING && DL_SETTING_ac_id(dl_buffer) == AC_ID) { uint8_t i = DL_SETTING_index(dl_buffer); float val = DL_SETTING_value(dl_buffer); DlSetting(i, val); DOWNLINK_SEND_DL_VALUE(DefaultChannel, &i, &val); } else if (msg_id == DL_GET_SETTING && DL_GET_SETTING_ac_id(dl_buffer) == AC_ID) { uint8_t i = DL_GET_SETTING_index(dl_buffer); float val = settings_get_value(i); DOWNLINK_SEND_DL_VALUE(DefaultChannel, &i, &val); } else #endif /** Else there is no dl_settings section in the flight plan */ #ifdef USE_JOYSTICK if (msg_id == DL_JOYSTICK_RAW && DL_JOYSTICK_RAW_ac_id(dl_buffer) == AC_ID) { JoystickHandeDatalink(DL_JOYSTICK_RAW_roll(dl_buffer), DL_JOYSTICK_RAW_pitch(dl_buffer), DL_JOYSTICK_RAW_throttle(dl_buffer)); } else #endif // USE_JOYSTICK #if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_DATALINK if (msg_id == DL_RC_3CH /*&& DL_RC_3CH_ac_id(dl_buffer) == TX_ID*/) { LED_TOGGLE(3); parse_rc_datalink( DL_RC_3CH_throttle_mode(dl_buffer), DL_RC_3CH_roll(dl_buffer), DL_RC_3CH_pitch(dl_buffer)); } else #endif // RC_DATALINK { /* Last else */ /* Parse modules datalink */ modules_parse_datalink(msg_id); } }
static void send_nav(struct transport_tx *trans, struct link_device *dev) { SEND_NAVIGATION(trans, dev); }
/** * Compute desired_course */ void navigation_task( void ) { #if defined FAILSAFE_DELAY_WITHOUT_GPS /** This section is used for the failsafe of GPS */ static uint8_t last_pprz_mode; /** If aircraft is launched and is in autonomus mode, go into PPRZ_MODE_GPS_OUT_OF_ORDER mode (Failsafe) if we lost the GPS */ if (launch) { if (GpsTimeoutError) { if (pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME) { last_pprz_mode = pprz_mode; pprz_mode = PPRZ_MODE_GPS_OUT_OF_ORDER; PERIODIC_SEND_PPRZ_MODE(DefaultChannel, DefaultDevice); gps_lost = TRUE; } } else if (gps_lost) { /* GPS is ok */ /** If aircraft was in failsafe mode, come back in previous mode */ pprz_mode = last_pprz_mode; gps_lost = FALSE; PERIODIC_SEND_PPRZ_MODE(DefaultChannel, DefaultDevice); } } #endif /* GPS && FAILSAFE_DELAY_WITHOUT_GPS */ common_nav_periodic_task_4Hz(); if (pprz_mode == PPRZ_MODE_HOME) nav_home(); else if (pprz_mode == PPRZ_MODE_GPS_OUT_OF_ORDER) nav_without_gps(); else nav_periodic_task(); #ifdef TCAS CallTCAS(); #endif #ifndef PERIOD_NAVIGATION_0 // If not sent periodically (in default 0 mode) SEND_NAVIGATION(DefaultChannel, DefaultDevice); #endif SEND_CAM(DefaultChannel, DefaultDevice); /* The nav task computes only nav_altitude. However, we are interested by desired_altitude (= nav_alt+alt_shift) in any case. So we always run the altitude control loop */ if (v_ctl_mode == V_CTL_MODE_AUTO_ALT) v_ctl_altitude_loop(); if (pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME || pprz_mode == PPRZ_MODE_GPS_OUT_OF_ORDER) { #ifdef H_CTL_RATE_LOOP /* Be sure to be in attitude mode, not roll */ h_ctl_auto1_rate = FALSE; #endif if (lateral_mode >=LATERAL_MODE_COURSE) h_ctl_course_loop(); /* aka compute nav_desired_roll */ // climb_loop(); //4Hz } energy += ((float)current) / 3600.0f * 0.25f; // mAh = mA * dt (4Hz -> hours) }