diff --git a/conf/airframes/tudelft/robird.xml b/conf/airframes/tudelft/robird.xml index aafc0f23cf..558d3e607a 100644 --- a/conf/airframes/tudelft/robird.xml +++ b/conf/airframes/tudelft/robird.xml @@ -90,7 +90,7 @@ Flapping wing frame equiped with - +
diff --git a/conf/gcs/OPENUAS/openuas_bottom_settings.xml b/conf/gcs/OPENUAS/openuas_bottom_settings.xml index 9f6b15b26b..28fa8421e8 100644 --- a/conf/gcs/OPENUAS/openuas_bottom_settings.xml +++ b/conf/gcs/OPENUAS/openuas_bottom_settings.xml @@ -10,15 +10,15 @@ - + - - + + diff --git a/conf/gcs/OPENUAS/openuas_papgets.xml b/conf/gcs/OPENUAS/openuas_papgets.xml index 19101ef7de..800f5cc2f0 100644 --- a/conf/gcs/OPENUAS/openuas_papgets.xml +++ b/conf/gcs/OPENUAS/openuas_papgets.xml @@ -20,8 +20,8 @@ - - + + @@ -53,8 +53,8 @@ - - + + diff --git a/conf/gcs/OPENUAS/openuas_papgets_small.xml b/conf/gcs/OPENUAS/openuas_papgets_small.xml index 1fef213c06..9459b36cdf 100644 --- a/conf/gcs/OPENUAS/openuas_papgets_small.xml +++ b/conf/gcs/OPENUAS/openuas_papgets_small.xml @@ -20,8 +20,8 @@ - - + + @@ -37,8 +37,8 @@ - - + + diff --git a/conf/gcs/bottom_settings.xml b/conf/gcs/bottom_settings.xml index 9d3f3c94ff..b3258875bb 100644 --- a/conf/gcs/bottom_settings.xml +++ b/conf/gcs/bottom_settings.xml @@ -31,9 +31,9 @@ - - - + + + @@ -53,9 +53,9 @@ - + - + @@ -97,8 +97,8 @@ - - + + diff --git a/conf/gcs/papgets.xml b/conf/gcs/papgets.xml index ac17f22b6e..ea525c3bb7 100644 --- a/conf/gcs/papgets.xml +++ b/conf/gcs/papgets.xml @@ -14,25 +14,26 @@ - - + + - - - + + + - - + + + - + @@ -48,9 +49,9 @@ - + - + diff --git a/conf/gcs/solar.xml b/conf/gcs/solar.xml index 17de1ebbf7..305c7cc50e 100644 --- a/conf/gcs/solar.xml +++ b/conf/gcs/solar.xml @@ -2,8 +2,8 @@ - - + + diff --git a/conf/telemetry/AGGIEAIR/aggieair_fixedwing.xml b/conf/telemetry/AGGIEAIR/aggieair_fixedwing.xml index 7b1d63bd2b..eda0b94c4b 100644 --- a/conf/telemetry/AGGIEAIR/aggieair_fixedwing.xml +++ b/conf/telemetry/AGGIEAIR/aggieair_fixedwing.xml @@ -14,7 +14,7 @@ - + @@ -37,7 +37,7 @@ - + @@ -54,7 +54,7 @@ - + @@ -81,7 +81,7 @@ - + @@ -122,10 +122,10 @@ - + - + diff --git a/conf/telemetry/DB/db_quadshot.xml b/conf/telemetry/DB/db_quadshot.xml index 4e2ed00c32..c7fed57569 100644 --- a/conf/telemetry/DB/db_quadshot.xml +++ b/conf/telemetry/DB/db_quadshot.xml @@ -22,7 +22,7 @@ - + diff --git a/conf/telemetry/OPENUAS/openuas_ardrone2_extended.xml b/conf/telemetry/OPENUAS/openuas_ardrone2_extended.xml index 6e6e8768d5..659f380c69 100644 --- a/conf/telemetry/OPENUAS/openuas_ardrone2_extended.xml +++ b/conf/telemetry/OPENUAS/openuas_ardrone2_extended.xml @@ -15,7 +15,7 @@ - + @@ -27,22 +27,22 @@ - + - - - + + + - + diff --git a/conf/telemetry/OPENUAS/openuas_ardrone_extended.xml b/conf/telemetry/OPENUAS/openuas_ardrone_extended.xml index 6e6e8768d5..1a6b46a2ca 100644 --- a/conf/telemetry/OPENUAS/openuas_ardrone_extended.xml +++ b/conf/telemetry/OPENUAS/openuas_ardrone_extended.xml @@ -15,7 +15,7 @@ - + @@ -27,22 +27,22 @@ - + - - - + + + - + diff --git a/conf/telemetry/OPENUAS/openuas_fixedwing_imu.xml b/conf/telemetry/OPENUAS/openuas_fixedwing_imu.xml index 6d9228ae7d..a76812c25b 100644 --- a/conf/telemetry/OPENUAS/openuas_fixedwing_imu.xml +++ b/conf/telemetry/OPENUAS/openuas_fixedwing_imu.xml @@ -11,11 +11,10 @@ - + - @@ -45,7 +44,7 @@ - + @@ -63,7 +62,7 @@ - + @@ -91,7 +90,7 @@ - + diff --git a/conf/telemetry/OPENUAS/openuas_fixedwing_imu_rc.xml b/conf/telemetry/OPENUAS/openuas_fixedwing_imu_rc.xml index 80f027ddf4..028bb3aa8a 100644 --- a/conf/telemetry/OPENUAS/openuas_fixedwing_imu_rc.xml +++ b/conf/telemetry/OPENUAS/openuas_fixedwing_imu_rc.xml @@ -10,11 +10,10 @@ - + - @@ -42,7 +41,7 @@ - + @@ -59,7 +58,7 @@ - + @@ -86,7 +85,7 @@ - + diff --git a/conf/telemetry/OPENUAS/openuas_iridium.xml b/conf/telemetry/OPENUAS/openuas_iridium.xml index 5bd30117b8..3208770b7d 100644 --- a/conf/telemetry/OPENUAS/openuas_iridium.xml +++ b/conf/telemetry/OPENUAS/openuas_iridium.xml @@ -3,34 +3,34 @@ - - - + + + - - - + + + - + - - - + + + - - - - - - - + + + + + + + - - - + + + diff --git a/conf/telemetry/OPENUAS/openuas_xbee868le.xml b/conf/telemetry/OPENUAS/openuas_xbee868le.xml index b7cdd56cc8..acfa0c1803 100644 --- a/conf/telemetry/OPENUAS/openuas_xbee868le.xml +++ b/conf/telemetry/OPENUAS/openuas_xbee868le.xml @@ -3,32 +3,32 @@ - - - + + + - - - + + + - - - - + + + + - - + + - - - - - + + + + + - + diff --git a/conf/telemetry/default_fixedwing.xml b/conf/telemetry/default_fixedwing.xml index 4c8b2801c6..f84d338664 100644 --- a/conf/telemetry/default_fixedwing.xml +++ b/conf/telemetry/default_fixedwing.xml @@ -10,11 +10,10 @@ - + - @@ -42,7 +41,7 @@ - + @@ -60,7 +59,7 @@ - + diff --git a/conf/telemetry/default_fixedwing_gvf.xml b/conf/telemetry/default_fixedwing_gvf.xml index 7c3b2b9629..e08666d525 100644 --- a/conf/telemetry/default_fixedwing_gvf.xml +++ b/conf/telemetry/default_fixedwing_gvf.xml @@ -10,18 +10,17 @@ - + - - + @@ -43,14 +42,14 @@ - + - + @@ -61,12 +60,12 @@ - + - + diff --git a/conf/telemetry/default_fixedwing_imu.xml b/conf/telemetry/default_fixedwing_imu.xml index 9a7a425f5d..7fdcbd2633 100644 --- a/conf/telemetry/default_fixedwing_imu.xml +++ b/conf/telemetry/default_fixedwing_imu.xml @@ -10,11 +10,10 @@ - + - @@ -43,7 +42,7 @@ - + @@ -60,7 +59,7 @@ - + @@ -87,7 +86,7 @@ - + diff --git a/conf/telemetry/default_fixedwing_imu_9k6.xml b/conf/telemetry/default_fixedwing_imu_9k6.xml index f33037d312..97a92b4249 100644 --- a/conf/telemetry/default_fixedwing_imu_9k6.xml +++ b/conf/telemetry/default_fixedwing_imu_9k6.xml @@ -10,18 +10,17 @@ - + - - + @@ -42,14 +41,14 @@ - + - + @@ -59,12 +58,12 @@ - + - + @@ -75,8 +74,8 @@ - - + + @@ -86,12 +85,12 @@ - + - + diff --git a/conf/telemetry/default_rotorcraft_slow.xml b/conf/telemetry/default_rotorcraft_slow.xml index a2ab06a026..609be2fdac 100644 --- a/conf/telemetry/default_rotorcraft_slow.xml +++ b/conf/telemetry/default_rotorcraft_slow.xml @@ -124,7 +124,7 @@ - + diff --git a/conf/telemetry/fixedwing_control_new.xml b/conf/telemetry/fixedwing_control_new.xml index 9b9bb0727c..7b2a8b9520 100644 --- a/conf/telemetry/fixedwing_control_new.xml +++ b/conf/telemetry/fixedwing_control_new.xml @@ -11,7 +11,7 @@ - + diff --git a/conf/telemetry/fixedwing_flight_recorder.xml b/conf/telemetry/fixedwing_flight_recorder.xml index 692cee3fec..0566ad1434 100644 --- a/conf/telemetry/fixedwing_flight_recorder.xml +++ b/conf/telemetry/fixedwing_flight_recorder.xml @@ -10,11 +10,10 @@ - + - @@ -46,7 +45,7 @@ - + @@ -63,7 +62,7 @@ - + @@ -90,7 +89,7 @@ - + @@ -120,7 +119,6 @@ - diff --git a/conf/telemetry/iridium.xml b/conf/telemetry/iridium.xml index 5bd30117b8..2bcad7afb4 100644 --- a/conf/telemetry/iridium.xml +++ b/conf/telemetry/iridium.xml @@ -3,34 +3,34 @@ - - - + + + - - - + + + - + - - - + + + - - - - - - - + + + + + + + - - - + + + diff --git a/conf/telemetry/tudelft/xbee868.xml b/conf/telemetry/tudelft/xbee868.xml index b7cdd56cc8..acfa0c1803 100644 --- a/conf/telemetry/tudelft/xbee868.xml +++ b/conf/telemetry/tudelft/xbee868.xml @@ -3,32 +3,32 @@ - - - + + + - - - + + + - - - - + + + + - - + + - - - - - + + + + + - + diff --git a/sw/airborne/arch/sim/sim_ap.c b/sw/airborne/arch/sim/sim_ap.c index a86fec963c..9b0f3553b2 100644 --- a/sw/airborne/arch/sim/sim_ap.c +++ b/sw/airborne/arch/sim/sim_ap.c @@ -109,7 +109,7 @@ value sim_init(value unit) value update_bat(value bat) { - electrical.vsupply = Int_val(bat); + electrical.vsupply = (float)Int_val(bat) / 10.; return Val_unit; } diff --git a/sw/airborne/boards/bebop/actuators.c b/sw/airborne/boards/bebop/actuators.c index 0f98deeb76..eda11de8cb 100644 --- a/sw/airborne/boards/bebop/actuators.c +++ b/sw/airborne/boards/bebop/actuators.c @@ -77,7 +77,7 @@ void actuators_bebop_commit(void) i2c_blocking_transceive(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 1, 13); // Update status - electrical.vsupply = (actuators_bebop.i2c_trans.buf[9] + (actuators_bebop.i2c_trans.buf[8] << 8)) / 100; + electrical.vsupply = (float)(actuators_bebop.i2c_trans.buf[9] + (actuators_bebop.i2c_trans.buf[8] << 8)) / 1000.f; // The 15th bit contains saturation information, so it needs to be removed to get the rpm actuators_bebop.rpm_obs[0] = (actuators_bebop.i2c_trans.buf[1] + (actuators_bebop.i2c_trans.buf[0] << 8)) & ~(1<<15); actuators_bebop.rpm_obs[1] = (actuators_bebop.i2c_trans.buf[3] + (actuators_bebop.i2c_trans.buf[2] << 8)) & ~(1<<15); diff --git a/sw/airborne/boards/disco/actuators.c b/sw/airborne/boards/disco/actuators.c index 6e74f6bae7..28eca5a5ef 100644 --- a/sw/airborne/boards/disco/actuators.c +++ b/sw/airborne/boards/disco/actuators.c @@ -122,7 +122,7 @@ void actuators_disco_commit(void) #pragma GCC diagnostic pop // Update status - electrical.vsupply = be16toh(obs_data.batt_mv) / 100; + electrical.vsupply = (float)(be16toh(obs_data.batt_mv)) / 1000.f; // extract 'rpm saturation bit' actuators_disco.rpm_saturated = (obs_data.rpm & (1 << 7)) ? true : false; // clear 'rpm saturation bit' and fix endianess diff --git a/sw/airborne/boards/swing/board.c b/sw/airborne/boards/swing/board.c index f89139459c..098c2e62bc 100644 --- a/sw/airborne/boards/swing/board.c +++ b/sw/airborne/boards/swing/board.c @@ -53,9 +53,8 @@ static void *bat_read(void *data __attribute__((unused))) /* Read the output a line at a time - output it. */ while (fgets(path, sizeof(path) - 1, fp) != NULL) { int raw_bat = atoi(path); - // convert to decivolt // from /bin/mcu_vbat.sh: MILLIVOLTS_VALUE=$(( ($RAW_VALUE * 4250) / 1023 )) - electrical.vsupply = ((raw_bat * 4250) / 1023) / 100; + electrical.vsupply = (float)((raw_bat * 4250) / 1023) / 1000.f; } /* close */ pclose(fp); diff --git a/sw/airborne/firmwares/fixedwing/autopilot_firmware.c b/sw/airborne/firmwares/fixedwing/autopilot_firmware.c index 01b1b740e6..5e08fba9ae 100644 --- a/sw/airborne/firmwares/fixedwing/autopilot_firmware.c +++ b/sw/airborne/firmwares/fixedwing/autopilot_firmware.c @@ -32,9 +32,8 @@ #include "firmwares/fixedwing/nav.h" #include -uint16_t vsupply; -int32_t current; -float energy; +// ap copy of fbw readings +struct Electrical ap_electrical; uint8_t lateral_mode; uint8_t mcu1_status; @@ -67,30 +66,12 @@ static void send_estimator(struct transport_tx *trans, struct link_device *dev) &(stateGetPositionUtm_f()->alt), &(stateGetSpeedEnu_f()->z)); } -static void send_bat(struct transport_tx *trans, struct link_device *dev) -{ - int16_t amps = (int16_t)(current / 10); - int16_t e = energy; - // prevent overflow - if (fabs(energy) >= INT16_MAX) { - e = INT16_MAX; - } - pprz_msg_send_BAT(trans, dev, AC_ID, - &v_ctl_throttle_slewed, &vsupply, &s, - &autopilot.flight_time, (uint8_t *)(&autopilot.kill_throttle), - &block_time, &stage_time, &e); -} - static void send_energy(struct transport_tx *trans, struct link_device *dev) { - uint16_t e = energy; - if (fabs(energy) >= INT16_MAX) { - e = INT16_MAX; - } - float vsup = ((float)vsupply) / 10.0f; - float curs = ((float)current) / 1000.0f; - float power = vsup * curs; - pprz_msg_send_ENERGY(trans, dev, AC_ID, &vsup, &curs, &e, &power); + uint8_t throttle = 100 * v_ctl_throttle_slewed / MAX_PPRZ; + float power = ap_electrical.vsupply * ap_electrical.current; + pprz_msg_send_ENERGY(trans, dev, AC_ID, + &throttle, &ap_electrical.vsupply, &ap_electrical.current, &power, &ap_electrical.charge, &ap_electrical.energy); } // FIXME not the best place @@ -132,16 +113,19 @@ void autopilot_send_mode(void) void autopilot_firmware_init(void) { - vsupply = 0; - current = 0; - energy = 0.f; + ap_electrical.vsupply = 0.f; + ap_electrical.current = 0.f; + ap_electrical.charge = 0.f; + ap_electrical.energy = 0.f; + + ap_electrical.bat_critical = false; + ap_electrical.bat_low = false; #if PERIODIC_TELEMETRY /* register some periodic message */ register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_PPRZ_MODE, send_mode); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ESTIMATOR, send_estimator); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AIRSPEED, send_airspeed); - register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_BAT, send_bat); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ENERGY, send_energy); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_DESIRED, send_desired); #if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS diff --git a/sw/airborne/firmwares/fixedwing/autopilot_firmware.h b/sw/airborne/firmwares/fixedwing/autopilot_firmware.h index c5ec6df54f..94a1c1c7b7 100644 --- a/sw/airborne/firmwares/fixedwing/autopilot_firmware.h +++ b/sw/airborne/firmwares/fixedwing/autopilot_firmware.h @@ -30,6 +30,7 @@ #include "std.h" #include "autopilot.h" +#include "subsystems/electrical.h" // FIXME, move to control #define LATERAL_MODE_MANUAL 0 @@ -39,23 +40,8 @@ #define LATERAL_MODE_NB 4 extern uint8_t lateral_mode; -/** Supply voltage in deciVolt. - * This the ap copy of the measurement from fbw - * FIXME use electrical module ? - */ -extern uint16_t vsupply; - -/** Supply current in milliAmpere. - * This the ap copy of the measurement from fbw - * FIXME use electrical module ? - */ -extern int32_t current; // milliAmpere - -/** Energy consumption (mAh) - * This is the ap copy of the measurement from fbw - * FIXME use electrical module ? - */ -extern float energy; +// ap copy of fbw readings +extern struct Electrical ap_electrical; /** Second MCU status (FBW part) */ diff --git a/sw/airborne/firmwares/fixedwing/autopilot_generated.c b/sw/airborne/firmwares/fixedwing/autopilot_generated.c index 19736c4bc1..c22a646b74 100644 --- a/sw/airborne/firmwares/fixedwing/autopilot_generated.c +++ b/sw/airborne/firmwares/fixedwing/autopilot_generated.c @@ -104,7 +104,7 @@ void autopilot_generated_on_rc_frame(void) { // update electrical from FBW - imcu_get_electrical(&vsupply, ¤t, &energy); + imcu_get_electrical(&ap_electrical); // FIXME what to do here ? copy_from_to_fbw(); diff --git a/sw/airborne/firmwares/fixedwing/autopilot_static.c b/sw/airborne/firmwares/fixedwing/autopilot_static.c index f040027417..c1d4722e16 100644 --- a/sw/airborne/firmwares/fixedwing/autopilot_static.c +++ b/sw/airborne/firmwares/fixedwing/autopilot_static.c @@ -171,7 +171,7 @@ void autopilot_static_on_rc_frame(void) #endif // RADIO_CONTROL // update electrical from FBW - imcu_get_electrical(&vsupply, ¤t, &energy); + imcu_get_electrical(&ap_electrical); #ifdef RADIO_CONTROL /* the SITL check is a hack to prevent "automatic" launch in NPS */ @@ -299,8 +299,8 @@ void attitude_loop(void) #endif #ifdef V_CTL_POWER_CTL_BAT_NOMINAL - if (vsupply > 0.) { - v_ctl_throttle_setpoint *= 10. * V_CTL_POWER_CTL_BAT_NOMINAL / (float)vsupply; + if (ap_electrical.vsupply > 0.) { + v_ctl_throttle_setpoint *= V_CTL_POWER_CTL_BAT_NOMINAL / ap_electrical.vsupply; v_ctl_throttle_setpoint = TRIM_UPPRZ(v_ctl_throttle_setpoint); } #endif diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c b/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c index c9f054d05c..2ad247ab91 100644 --- a/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c +++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c @@ -289,8 +289,8 @@ void v_ctl_guidance_loop(void) #endif #ifdef V_CTL_POWER_CTL_BAT_NOMINAL - if (vsupply > 0.) { - v_ctl_throttle_setpoint *= 10. * V_CTL_POWER_CTL_BAT_NOMINAL / (float)vsupply; + if (ap_electrical.vsupply > 0.) { + v_ctl_throttle_setpoint *= V_CTL_POWER_CTL_BAT_NOMINAL / ap_electrical.vsupply; v_ctl_throttle_setpoint = TRIM_UPPRZ(v_ctl_throttle_setpoint); } #endif diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c index 850faf3688..5169104bbe 100644 --- a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c +++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c @@ -206,8 +206,8 @@ void v_ctl_guidance_loop(void) #endif #ifdef V_CTL_POWER_CTL_BAT_NOMINAL - if (vsupply > 0.) { - v_ctl_throttle_setpoint *= 10. * V_CTL_POWER_CTL_BAT_NOMINAL / (float)vsupply; + if (ap_electrical.vsupply > 0.) { + v_ctl_throttle_setpoint *= V_CTL_POWER_CTL_BAT_NOMINAL / ap_electrical.vsupply; v_ctl_throttle_setpoint = TRIM_UPPRZ(v_ctl_throttle_setpoint); } #endif diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index a68a26a31f..2474c1c109 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ b/sw/airborne/firmwares/fixedwing/main_ap.c @@ -341,7 +341,7 @@ void monitor_task(void) #endif static uint8_t t = 0; - if (vsupply < CATASTROPHIC_BAT_LEVEL * 10) { + if (ap_electrical.vsupply < CATASTROPHIC_BAT_LEVEL) { t++; } else { t = 0; diff --git a/sw/airborne/firmwares/fixedwing/nav.h b/sw/airborne/firmwares/fixedwing/nav.h index eaae4da154..d5bd10fed4 100644 --- a/sw/airborne/firmwares/fixedwing/nav.h +++ b/sw/airborne/firmwares/fixedwing/nav.h @@ -45,7 +45,7 @@ #define Square(_x) ((_x)*(_x)) #define DistanceSquare(p1_x, p1_y, p2_x, p2_y) (Square(p1_x-p2_x)+Square(p1_y-p2_y)) -#define PowerVoltage() (vsupply/10.) +#define PowerVoltage() (ap_electrical.vsupply) #define RcRoll(travel) (imcu_get_radio(RADIO_ROLL) * (float)travel /(float)MAX_PPRZ) diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_firmware.c b/sw/airborne/firmwares/rotorcraft/autopilot_firmware.c index 3713c6fe61..a254ef2d3f 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot_firmware.c +++ b/sw/airborne/firmwares/rotorcraft/autopilot_firmware.c @@ -97,26 +97,21 @@ static void send_status(struct transport_tx *trans, struct link_device *dev) #endif uint8_t in_flight = autopilot.in_flight; uint8_t motors_on = autopilot.motors_on; - uint8_t arming_status = autopilot.arming_status; uint16_t time_sec = sys_time.nb_sec; pprz_msg_send_ROTORCRAFT_STATUS(trans, dev, AC_ID, &imu_nb_err, &_motor_nb_err, &radio_control.status, &radio_control.frame_rate, &fix, &autopilot.mode, &in_flight, &motors_on, - &arming_status, &guidance_h.mode, &guidance_v_mode, - &electrical.vsupply, &time_sec); + &autopilot.arming_status, &guidance_h.mode, &guidance_v_mode, + &time_sec, &electrical.vsupply); } static void send_energy(struct transport_tx *trans, struct link_device *dev) { - uint16_t e = electrical.energy; - if (fabs(electrical.energy) >= INT16_MAX) { - e = INT16_MAX; - } - float vsup = ((float)electrical.vsupply) / 10.0f; - float curs = ((float)electrical.current) / 1000.0f; - float power = vsup * curs; - pprz_msg_send_ENERGY(trans, dev, AC_ID, &vsup, &curs, &e, &power); + uint8_t throttle = 100 * stabilization_cmd[COMMAND_THRUST] / MAX_PPRZ; + float power = electrical.vsupply * electrical.current; + pprz_msg_send_ENERGY(trans, dev, AC_ID, + &throttle, &electrical.vsupply, &electrical.current, &power, &electrical.charge, &electrical.energy); } static void send_fp(struct transport_tx *trans, struct link_device *dev) diff --git a/sw/airborne/firmwares/rover/autopilot_firmware.c b/sw/airborne/firmwares/rover/autopilot_firmware.c index a48d2800d0..77fd7def5a 100644 --- a/sw/airborne/firmwares/rover/autopilot_firmware.c +++ b/sw/airborne/firmwares/rover/autopilot_firmware.c @@ -64,19 +64,15 @@ static void send_status(struct transport_tx *trans, struct link_device *dev) pprz_msg_send_ROVER_STATUS(trans, dev, AC_ID, &radio_control.status, &radio_control.frame_rate, &fix, &autopilot.mode, &motors_on, - &electrical.vsupply, &time_sec); + &time_sec, &electrical.vsupply); } static void send_energy(struct transport_tx *trans, struct link_device *dev) { - uint16_t e = electrical.energy; - if (fabs(electrical.energy) >= INT16_MAX) { - e = INT16_MAX; - } - float vsup = ((float)electrical.vsupply) / 10.0f; - float curs = ((float)electrical.current) / 1000.0f; - float power = vsup * curs; - pprz_msg_send_ENERGY(trans, dev, AC_ID, &vsup, &curs, &e, &power); + uint8_t throttle = 100 * cmd / MAX_PPRZ; + float power = electrical.vsupply * electrical.current; + pprz_msg_send_ENERGY(trans, dev, AC_ID, + &throttle, &electrical.vsupply, &electrical.current, &power, &electrical.charge, &electrical.energy); } static void send_fp(struct transport_tx *trans, struct link_device *dev) diff --git a/sw/airborne/inter_mcu.h b/sw/airborne/inter_mcu.h index 99cc435394..4703f4712b 100644 --- a/sw/airborne/inter_mcu.h +++ b/sw/airborne/inter_mcu.h @@ -55,9 +55,7 @@ struct fbw_state { #endif uint8_t status; uint8_t nb_err; - uint16_t vsupply; ///< 1e-1 V - int32_t current; ///< milliAmps - float energy; ///< mAh + struct Electrical electrical; }; struct ap_state { @@ -243,30 +241,22 @@ static inline void imcu_set_status(uint8_t status) } /** get electrical parameters - * @param vsupply pointer to return voltage - * @param current pointer to return current - * @param energy pointer to return cumulated energy + * @param _electrical pointer to electrical data to fill */ -static inline void imcu_get_electrical(uint16_t *_vsupply, int32_t *_current, float *_energy) +static inline void imcu_get_electrical(struct Electrical *_electrical) { PPRZ_MUTEX_LOCK(fbw_state_mtx); - *_vsupply = fbw_state->vsupply; - *_current = fbw_state->current; - *_energy = fbw_state->energy; + *_electrical = fbw_state->electrical; PPRZ_MUTEX_UNLOCK(fbw_state_mtx); } /** set electrical parameters - * @param vsupply new voltage (1e-1 V) - * @param current new current (mA) - * @param energy new cumulated energy (mAh) + * @param _electrical pointer to electrical data to use */ -static inline void imcu_set_electrical(uint16_t _vsupply, int32_t _current, float _energy) +static inline void imcu_set_electrical(struct Electrical *_electrical) { PPRZ_MUTEX_LOCK(fbw_state_mtx); - fbw_state->vsupply = _vsupply; - fbw_state->current = _current; - fbw_state->energy = _energy; + fbw_state->electrical = *_electrical; PPRZ_MUTEX_UNLOCK(fbw_state_mtx); } @@ -314,9 +304,7 @@ static inline void inter_mcu_fill_fbw_state(void) status |= (fbw_mode == FBW_MODE_FAILSAFE ? _BV(STATUS_MODE_FAILSAFE) : 0); fbw_state->status = status; - fbw_state->vsupply = electrical.vsupply; - fbw_state->current = electrical.current; - fbw_state->energy = electrical.energy; + fbw_state->electrical = electrical; #if defined SINGLE_MCU /**Directly set the flag indicating to AP that shared buffer is available*/ inter_mcu_received_fbw = true; diff --git a/sw/airborne/link_mcu_can.c b/sw/airborne/link_mcu_can.c index bad02f9edc..14a1487372 100644 --- a/sw/airborne/link_mcu_can.c +++ b/sw/airborne/link_mcu_can.c @@ -102,8 +102,8 @@ void link_mcu_on_can_msg(uint32_t id, uint8_t *data, int len) fbw_state->ppm_cpt = data[0]; fbw_state->status = data[1]; fbw_state->nb_err = data[2]; - fbw_state->vsupply = data[3] + (data[4] << 8); - fbw_state->current = 0; + fbw_state->electrical.vsupply = (float)(data[3] + (data[4] << 8))/10.f; + fbw_state->electrical.current = 0; #ifdef LINK_MCU_LED LED_TOGGLE(LINK_MCU_LED); @@ -147,8 +147,9 @@ void link_mcu_periodic_task(void) intermcu_tx_buff[0] = fbw_state->ppm_cpt; intermcu_tx_buff[1] = fbw_state->status; intermcu_tx_buff[2] = fbw_state->nb_err; - intermcu_tx_buff[3] = (uint8_t) fbw_state->vsupply; - intermcu_tx_buff[4] = (uint8_t)((fbw_state->vsupply & 0xff00) >> 8); + uint16_t vsupply = fbw_state->electrical.vsupply * 10; + intermcu_tx_buff[3] = (uint8_t) vsupply; + intermcu_tx_buff[4] = (uint8_t)(vsupply & 0xff00) >> 8); ppz_can_transmit(MSG_INTERMCU_FBW_STATUS_ID, intermcu_tx_buff, 5); #if defined RADIO_CONTROL || RADIO_CONTROL_AUTO1 @@ -207,7 +208,8 @@ static void send_fbw_status(struct transport_tx *trans, struct link_device *dev) rc_status = RC_LOST; } pprz_msg_send_FBW_STATUS(trans, dev, AC_ID, - &(rc_status), &(fbw_state->ppm_cpt), &(fbw_status), &(fbw_state->vsupply), &(fbw_state->current)); + &(rc_status), &(fbw_state->ppm_cpt), &(fbw_status), + &(fbw_state->electrical.vsupply), &(fbw_state->electrical.current)); } #endif diff --git a/sw/airborne/link_mcu_usart.c b/sw/airborne/link_mcu_usart.c index 41705d0fa7..d85ce2cc17 100644 --- a/sw/airborne/link_mcu_usart.c +++ b/sw/airborne/link_mcu_usart.c @@ -276,7 +276,7 @@ static void send_fbw_status(struct transport_tx *trans, struct link_device *dev) rc_status = RC_LOST; } pprz_msg_send_FBW_STATUS(trans, dev, AC_ID, - &(rc_status), &(fbw_state->ppm_cpt), &(fbw_status), &(fbw_state->vsupply), &(fbw_state->current)); + &(rc_status), &(fbw_state->ppm_cpt), &(fbw_status), &(fbw_state->electrical.vsupply), &(fbw_state->electrical.current)); } #endif @@ -325,8 +325,8 @@ void parse_mavpilot_msg(void) fbw_state->ppm_cpt = MSG_INTERMCU_FBW_MOD(intermcu_data.msg_buf); fbw_state->status = MSG_INTERMCU_FBW_STAT(intermcu_data.msg_buf); fbw_state->nb_err = MSG_INTERMCU_FBW_ERR(intermcu_data.msg_buf); - fbw_state->vsupply = MSG_INTERMCU_FBW_VOLT(intermcu_data.msg_buf); - fbw_state->current = MSG_INTERMCU_FBW_CURRENT(intermcu_data.msg_buf); + fbw_state->electrical.vsupply = (float)(MSG_INTERMCU_FBW_VOLT(intermcu_data.msg_buf)) / 10.f; + fbw_state->electrical.current = (float)(MSG_INTERMCU_FBW_CURRENT(intermcu_data.msg_buf)) / 10.f; #ifdef LINK_MCU_LED LED_TOGGLE(LINK_MCU_LED); @@ -361,8 +361,8 @@ void link_mcu_periodic_task(void) fbw_state->ppm_cpt, fbw_state->status, fbw_state->nb_err, - fbw_state->vsupply, - fbw_state->current); + fbw_state->electrical.vsupply * 10.f, + fbw_state->electrical.current * 10.f); #if defined RADIO_CONTROL || RADIO_CONTROL_AUTO1 InterMcuSend_INTERMCU_RADIO(fbw_state->channels); #endif diff --git a/sw/airborne/modules/adcs/battery_monitor.c b/sw/airborne/modules/adcs/battery_monitor.c index 010fbde9d9..640a13297e 100644 --- a/sw/airborne/modules/adcs/battery_monitor.c +++ b/sw/airborne/modules/adcs/battery_monitor.c @@ -230,8 +230,8 @@ void battery_monitor_read_bus(void){ // convert to [A] batmonbus.bus_current = ((float)batmonbus.bus_current_mvolts + (float)batmon_current_offset) / batmon_current_sensitivity; - //update electrical subsystem, current in mAs - electrical.current = (int32_t)(batmonbus.bus_current*1000); + //update electrical subsystem + electrical.current = batmonbus.bus_current; // increment status batmonbus.bus_status = BATTERY_MONITOR_BUS_VOLTAGE_REQ; @@ -271,12 +271,12 @@ void battery_monitor_read_bus(void){ batmonbus.bus_voltage_mvolts = (uint16_t)( (float)batmonbus.bus_voltage_mvolts * BatmonVbusGain); - //update electrical subsystem, voltage in decivolts + //update electrical subsystem if (batmonbus.bus_voltage_mvolts != 0) { - electrical.vsupply = (uint16_t)(batmonbus.bus_voltage_mvolts/100); + electrical.vsupply = (float)(batmonbus.bus_voltage_mvolts) / 1000.f; } else { - electrical.vsupply = 0; + electrical.vsupply = 0.f; } // update status diff --git a/sw/airborne/modules/com/generic_com.c b/sw/airborne/modules/com/generic_com.c index 6271ca0adf..c122a05d41 100644 --- a/sw/airborne/modules/com/generic_com.c +++ b/sw/airborne/modules/com/generic_com.c @@ -79,14 +79,15 @@ void generic_com_periodic(void) FillBufWith16bit(com_trans.buf, 11, gps.gspeed); // ground speed (cm/s) FillBufWith16bit(com_trans.buf, 13, (int16_t)(gps.course / 1e4)); // course (1e3rad) FillBufWith16bit(com_trans.buf, 15, (uint16_t)(stateGetAirspeed_f() * 100)); // TAS (cm/s) - com_trans.buf[17] = electrical.vsupply; // decivolts - com_trans.buf[18] = (uint8_t)(energy / 100); // deciAh + uint8_t vsupply = Min(electrical.vsupply * 10.f, 255); // deciVolt + uint8_t charge = Min(electrical.vsupply * 10.f, 255); // deciAh + com_trans.buf[17] = vsupply; + com_trans.buf[18] = charge; com_trans.buf[19] = (uint8_t)(imcu_get_command(COMMAND_THROTTLE) * 100 / MAX_PPRZ); com_trans.buf[20] = autopilot_get_mode(); com_trans.buf[21] = nav_block; FillBufWith16bit(com_trans.buf, 22, autopilot.flight_time); i2c_transmit(&GENERIC_COM_I2C_DEV, &com_trans, GENERIC_COM_SLAVE_ADDR, NB_DATA); - } void generic_com_event(void) diff --git a/sw/airborne/modules/datalink/mavlink.c b/sw/airborne/modules/datalink/mavlink.c index 5ea5948d9f..eb0c4fbb40 100644 --- a/sw/airborne/modules/datalink/mavlink.c +++ b/sw/airborne/modules/datalink/mavlink.c @@ -517,8 +517,8 @@ static void mavlink_send_sys_status(struct transport_tx *trans, struct link_devi UAV_SENSORS, // On-board sensors: active (bitmap) UAV_SENSORS, // On-board sensors: state (bitmap) -1,//10*sys_mon.cpu_load, // System loadof main-loop time (0=0% to 1000=100%) - 100 * electrical.vsupply, // Battery voltage (milivolts) - electrical.current / 10, // Battery current (10x miliampere) + electrical.vsupply * 1000.f, // Battery voltage (milivolts) + electrical.current * 100.f, // Battery current (10x miliampere) -1, // Battery remaining (0-100 in %) 0, // Communication packet drops (0=0% to 10000=100%) 0, // Communication error(per packet) (0=0% to 10000=100%) @@ -771,7 +771,7 @@ static void mavlink_send_battery_status(struct transport_tx *trans, struct link_ { static uint16_t voltages[10]; // we simply only set one cell for now - voltages[0] = electrical.vsupply * 100; + voltages[0] = electrical.vsupply * 1000.f; // convert to mV /// TODO: check what all these fields are supposed to represent mavlink_msg_battery_status_send(MAVLINK_COMM_0, 0, // id @@ -779,9 +779,9 @@ static void mavlink_send_battery_status(struct transport_tx *trans, struct link_ 0, // type INT16_MAX, // unknown temperature voltages, // cell voltages - electrical.current / 10, - electrical.consumed, - electrical.energy, // check scaling + electrical.current * 100.f, // convert to deciA + electrical.charge * 1000.f, // convert to mAh + electrical.energy * 36, // convert to hecto Joule -1); // remaining percentage not estimated MAVLinkSendMessage(); } diff --git a/sw/airborne/modules/display/max7456.c b/sw/airborne/modules/display/max7456.c index cf04562a04..6ca07fd405 100644 --- a/sw/airborne/modules/display/max7456.c +++ b/sw/airborne/modules/display/max7456.c @@ -156,9 +156,8 @@ void max7456_periodic(void) step = 10; break; case (10): - temp = ((float)electrical.vsupply) / 10; - osd_sprintf(osd_string, "%.1fV", temp); - if (temp > LOW_BAT_LEVEL) { + osd_sprintf(osd_string, "%.2fV", electrical.vsupply); + if (electrical.vsupply > LOW_BAT_LEVEL) { osd_put_s(osd_string, FALSE, 8, 0, 2); } else { osd_put_s(osd_string, BLINK | INVERT, 8, 0, 2); diff --git a/sw/airborne/modules/gsm/gsm.c b/sw/airborne/modules/gsm/gsm.c index 212b198a48..bcd0fe39f4 100644 --- a/sw/airborne/modules/gsm/gsm.c +++ b/sw/airborne/modules/gsm/gsm.c @@ -39,7 +39,7 @@ Reporting: In: OK Out: AT+CMGS=\"GCS_NUMBER\" In: > - Out: gps.utm_pos.east, gps.utm_pos.north, gps.course, gps.hmsl, gps.gspeed, -gps.ned_vel.z, vsupply, autopilot.flight_time, rssi CTRLZ + Out: gps.utm_pos.east, gps.utm_pos.north, gps.course, gps.hmsl, gps.gspeed, -gps.ned_vel.z, electrical.vsupply, autopilot.flight_time, rssi CTRLZ Receiving: In: +CMTI: ..., @@ -61,6 +61,7 @@ Receiving: #include "subsystems/datalink/downlink.h" #include "subsystems/gps.h" #include "autopilot.h" +#include "subsystems/electrical.h" //#include "subsystems/navigation/common_nav.h" //why is should this be needed? #include "generated/settings.h" @@ -402,7 +403,7 @@ void gsm_send_report_continue(void) // Donnees batterie (seuls vsupply et autopilot.flight_time sont envoyes) // concatenation de toutes les infos en un seul message à transmettre sprintf(data_to_send, "%ld %ld %d %ld %d %d %d %d %d", gps.utm_pos.east, gps.utm_pos.north, gps_course, gps.hmsl, - gps.gspeed, -gps.ned_vel.z, vsupply, autopilot.flight_time, rssi); + gps.gspeed, -gps.ned_vel.z, electrical.vsupply, autopilot.flight_time, rssi); // send the number and wait for the prompt char buf[32]; diff --git a/sw/airborne/modules/hott/hott_eam.h b/sw/airborne/modules/hott/hott_eam.h index fd7b476bb5..5d7e7e2ffb 100644 --- a/sw/airborne/modules/hott/hott_eam.h +++ b/sw/airborne/modules/hott/hott_eam.h @@ -139,14 +139,13 @@ static void hott_init_eam_msg(struct HOTT_EAM_MSG *hott_eam_msg) static void hott_update_eam_msg(struct HOTT_EAM_MSG *hott_eam_msg) { - - hott_eam_msg->batt1_voltage = electrical.vsupply; - hott_eam_msg->batt2_voltage = electrical.vsupply; + hott_eam_msg->batt1_voltage = electrical.vsupply * 5; // convert from V to steps of 0.2V + hott_eam_msg->batt2_voltage = electrical.vsupply * 5; // convert from V to steps of 0.2V //hott_eam_msg->temp1 = 20 + imu.temperature / 10; //hott_eam_msg->temp2 = 20 + imu.temperature / 10; hott_eam_msg->altitude = (uint16_t)(500 + (stateGetPositionEnu_i()->z) / (1 << INT32_POS_FRAC)); - hott_eam_msg->current = electrical.current / 100; - hott_eam_msg->main_voltage = electrical.vsupply; + hott_eam_msg->current = electrical.current * 10; // convert to deciA + hott_eam_msg->main_voltage = electrical.vsupply * 10; // convert to deciV hott_eam_msg->batt_cap = 0; uint16_t speed_buf = (uint16_t)(stateGetHorizontalSpeedNorm_i() * 36 / 10 / (1 << INT32_SPEED_FRAC)); hott_eam_msg->speed_L = speed_buf & 0xFF; diff --git a/sw/airborne/modules/light/led_safety_status.c b/sw/airborne/modules/light/led_safety_status.c index 2876f3ab40..37f8665b76 100644 --- a/sw/airborne/modules/light/led_safety_status.c +++ b/sw/airborne/modules/light/led_safety_status.c @@ -67,9 +67,9 @@ void led_safety_status_periodic(void) RunXTimesEvery(0, 240, 40, 5, {LED_OFF(SAFETY_WARNING_LED);}); } #ifdef LOW_BAT_LEVEL - else if (electrical.vsupply < (LOW_BAT_LEVEL * 10)) { + else if (electrical.vsupply < LOW_BAT_LEVEL) { RunOnceEvery(20, {LED_TOGGLE(SAFETY_WARNING_LED);}); - } else if (electrical.vsupply < ((LOW_BAT_LEVEL + 0.5) * 10)) { + } else if (electrical.vsupply < (LOW_BAT_LEVEL + 0.5)) { RunXTimesEvery(0, 300, 10, 10, {LED_TOGGLE(SAFETY_WARNING_LED);}); } #endif diff --git a/sw/airborne/modules/sensors/bat_voltage_ardrone2.c b/sw/airborne/modules/sensors/bat_voltage_ardrone2.c index 012ee678c5..d70b1a98ec 100644 --- a/sw/airborne/modules/sensors/bat_voltage_ardrone2.c +++ b/sw/airborne/modules/sensors/bat_voltage_ardrone2.c @@ -41,22 +41,25 @@ #include "subsystems/electrical.h" - - void electrical_ardrone2_setup(void); -int fd; +static int fd; void bat_voltage_ardrone2_init(void) { // Initialize 12c device for power fd = open("/dev/i2c-1", O_RDWR); + if (fd < 0){ + fprintf(stderr, "Failed to open i2c-1: %m\n"); + return; + } + if (ioctl(fd, I2C_SLAVE_FORCE, 0x4a) < 0) { fprintf(stderr, "Failed to set slave address: %m\n"); + return; } electrical_ardrone2_setup(); - } void electrical_ardrone2_setup(void) @@ -85,7 +88,6 @@ void electrical_ardrone2_setup(void) void bat_voltage_ardrone2_periodic(void) { - electrical_ardrone2_setup(); unsigned char lsb, msb; @@ -96,11 +98,9 @@ void bat_voltage_ardrone2_periodic(void) // we know from spec sheet that ADCIN0 has no prescaler // so that the max voltage range is 1.5 volt - // multiply by ten to get decivolts - //from raw measurement we got quite a lineair response + //from raw measurement we got quite a linear response //9.0V=662, 9.5V=698, 10.0V=737,10.5V=774, 11.0V=811, 11.5V=848, 12.0V=886, 12.5V=923 - //leading to our 0.13595166 magic number for decivolts conversion - electrical.vsupply = raw_voltage * 0.13595166; - + //leading to our 0.013595166 magic number for volts conversion + electrical.vsupply = raw_voltage * 0.013595166f; } diff --git a/sw/airborne/modules/sensors/ezcurrent.c b/sw/airborne/modules/sensors/ezcurrent.c index 9f44650343..d47454db5b 100644 --- a/sw/airborne/modules/sensors/ezcurrent.c +++ b/sw/airborne/modules/sensors/ezcurrent.c @@ -49,9 +49,6 @@ struct i2c_transaction ezcurrent_i2c_trans; void ezcurrent_init(void) { - electrical.vsupply = 0; - electrical.current = 0; - ezcurrent_i2c_trans.status = I2CTransDone; ezcurrent_i2c_trans.slave_addr = EZCURRENT_ADDR; } @@ -69,12 +66,12 @@ void ezcurrent_read_periodic(void) void ezcurrent_read_event(void) { if (ezcurrent_i2c_trans.status == I2CTransSuccess) { - /* voltage of EzOSD sensor is provided in mV, convert to deciVolt */ - electrical.vsupply = Uint16FromBuf(ezcurrent_i2c_trans.buf, 2) / 100; - /* consumed ? in mAh */ - electrical.consumed = Int16FromBuf(ezcurrent_i2c_trans.buf, 6); - /* sensor provides current in 1e-1 Ampere, convert to mA */ - electrical.current = Int16FromBuf(ezcurrent_i2c_trans.buf, 8) * 100; + /* voltage of EzOSD sensor is provided in mV */ + electrical.vsupply = (float)(Uint16FromBuf(ezcurrent_i2c_trans.buf, 2)) / 1000.f; + /* consumed electric charge in mAh */ + electrical.charge = (float)(Int16FromBuf(ezcurrent_i2c_trans.buf, 6)) / 1000.f; + /* sensor provides current in 1e-1 Ampere */ + electrical.current = (float)(Int16FromBuf(ezcurrent_i2c_trans.buf, 8)) / 10.f; // Transaction has been read ezcurrent_i2c_trans.status = I2CTransDone; diff --git a/sw/airborne/subsystems/electrical.c b/sw/airborne/subsystems/electrical.c index 2f2399f52b..24a22c4136 100644 --- a/sw/airborne/subsystems/electrical.c +++ b/sw/airborne/subsystems/electrical.c @@ -53,6 +53,7 @@ #endif #define ELECTRICAL_PERIODIC_FREQ 10 +static float period_to_hour = 1 / 3600.f / ELECTRICAL_PERIODIC_FREQ; #ifndef MIN_BAT_LEVEL #define MIN_BAT_LEVEL 3 @@ -98,9 +99,10 @@ PRINT_CONFIG_VAR(MILLIAMP_AT_IDLE_THROTTLE) void electrical_init(void) { - electrical.vsupply = 0; - electrical.current = 0; - electrical.energy = 0; + electrical.vsupply = 0.f; + electrical.current = 0.f; + electrical.charge = 0.f; + electrical.energy = 0.f; electrical.bat_low = false; electrical.bat_critical = false; @@ -125,16 +127,14 @@ void electrical_periodic(void) static bool vsupply_check_started = false; #if defined(ADC_CHANNEL_VSUPPLY) && !defined(SITL) && !USE_BATTERY_MONITOR - electrical.vsupply = 10 * VoltageOfAdc((electrical_priv.vsupply_adc_buf.sum / + electrical.vsupply = VoltageOfAdc((electrical_priv.vsupply_adc_buf.sum / electrical_priv.vsupply_adc_buf.av_nb_sample)); #endif #ifdef ADC_CHANNEL_CURRENT #ifndef SITL int32_t current_adc = electrical_priv.current_adc_buf.sum / electrical_priv.current_adc_buf.av_nb_sample; - electrical.current = MilliAmpereOfAdc(current_adc); - /* Prevent an overflow on high current spikes when using the motor brake */ - BoundAbs(electrical.current, 65000); + electrical.current = MilliAmpereOfAdc(current_adc) / 1000.f; #endif #elif defined MILLIAMP_AT_FULL_THROTTLE && defined COMMAND_CURRENT_ESTIMATION /* @@ -147,18 +147,14 @@ void electrical_periodic(void) * * define CURRENT_ESTIMATION_NONLINEARITY in your airframe file to change the default nonlinearity factor of 1.2 */ - float full_current = (float)MILLIAMP_AT_FULL_THROTTLE; - float idle_current = (float)MILLIAMP_AT_IDLE_THROTTLE; + static float full_current = (float)MILLIAMP_AT_FULL_THROTTLE / 1000.f; + static float idle_current = (float)MILLIAMP_AT_IDLE_THROTTLE / 1000.f; float x = ((float)commands[COMMAND_CURRENT_ESTIMATION]) / ((float)MAX_PPRZ); /* Boundary check for x to prevent math errors due to negative numbers in * pow() */ - if(x > 1.0f) { - x = 1.0f; - } else if(x < 0.0f) { - x = 0.0f; - } + Bound(x, 0.f, 1.f); /* electrical.current y = ( b^n - (b* x/a)^n )^1/n * a=1, n = electrical_priv.nonlin_factor @@ -170,24 +166,26 @@ void electrical_periodic(void) } else { #endif electrical.current = full_current - - pow((pow(full_current - idle_current, electrical_priv.nonlin_factor) - - pow(((full_current - idle_current) * x), electrical_priv.nonlin_factor)), - (1. / electrical_priv.nonlin_factor)); + powf((powf(full_current - idle_current, electrical_priv.nonlin_factor) - + powf(((full_current - idle_current) * x), electrical_priv.nonlin_factor)), + (1.f / electrical_priv.nonlin_factor)); #ifndef FBW } #endif #endif /* ADC_CHANNEL_CURRENT */ - // mAh = mA * dt (10Hz -> hours) - electrical.energy += ((float)electrical.current) / 3600.0f / ELECTRICAL_PERIODIC_FREQ; + float consumed_since_last = electrical.current * period_to_hour; + + electrical.charge += consumed_since_last; + electrical.energy += consumed_since_last * electrical.vsupply; /*if valid voltage is seen then start checking. Set min level to 0 to always start*/ - if (electrical.vsupply >= MIN_BAT_LEVEL * 10) { + if (electrical.vsupply >= MIN_BAT_LEVEL) { vsupply_check_started = true; } if (vsupply_check_started) { - if (electrical.vsupply < LOW_BAT_LEVEL * 10) { + if (electrical.vsupply < LOW_BAT_LEVEL) { if (bat_low_counter > 0) { bat_low_counter--; } @@ -200,7 +198,7 @@ void electrical_periodic(void) electrical.bat_low = false; } - if (electrical.vsupply < CRITIC_BAT_LEVEL * 10) { + if (electrical.vsupply < CRITIC_BAT_LEVEL) { if (bat_critical_counter > 0) { bat_critical_counter--; } diff --git a/sw/airborne/subsystems/electrical.h b/sw/airborne/subsystems/electrical.h index 0580a182fc..23f657597b 100644 --- a/sw/airborne/subsystems/electrical.h +++ b/sw/airborne/subsystems/electrical.h @@ -36,22 +36,18 @@ #define LOW_BAT_LEVEL 10.5 #endif - /** critical battery level in Volts (for 3S LiPo) */ #ifndef CRITIC_BAT_LEVEL #define CRITIC_BAT_LEVEL 9.8 #endif - struct Electrical { - - uint16_t vsupply; ///< supply voltage in decivolts - int32_t current; ///< current in milliamps - int32_t consumed; ///< consumption in mAh - float energy; ///< consumed energy in mAh - bool bat_low; ///< battery low status - bool bat_critical; ///< battery critical status - + float vsupply; ///< supply voltage in V + float current; ///< current in A + float charge; ///< consumed electric charge in Ah + float energy; ///< consumed energy in Wh + bool bat_low; ///< battery low status + bool bat_critical; ///< battery critical status }; extern struct Electrical electrical; diff --git a/sw/airborne/subsystems/intermcu/intermcu_ap.c b/sw/airborne/subsystems/intermcu/intermcu_ap.c index 0cf4830a78..0420522ed9 100644 --- a/sw/airborne/subsystems/intermcu/intermcu_ap.c +++ b/sw/airborne/subsystems/intermcu/intermcu_ap.c @@ -62,8 +62,8 @@ static struct GpsState gps_imcu; static void send_status(struct transport_tx *trans, struct link_device *dev) { pprz_msg_send_FBW_STATUS(trans, dev, AC_ID, - &fbw_status.rc_status, &fbw_status.frame_rate, &fbw_status.mode, &fbw_status.vsupply, - &fbw_status.current); + &fbw_status.rc_status, &fbw_status.frame_rate, &fbw_status.mode, + &fbw_status.electrical.vsupply, &fbw_status.electrical.current); } #endif @@ -163,8 +163,8 @@ static inline void intermcu_parse_msg(void (*rc_frame_handler)(void)) fbw_status.rc_status = DL_IMCU_FBW_STATUS_rc_status(imcu_msg_buf); fbw_status.frame_rate = DL_IMCU_FBW_STATUS_frame_rate(imcu_msg_buf); fbw_status.mode = DL_IMCU_FBW_STATUS_mode(imcu_msg_buf); - fbw_status.vsupply = DL_IMCU_FBW_STATUS_vsupply(imcu_msg_buf); - fbw_status.current = DL_IMCU_FBW_STATUS_current(imcu_msg_buf); + fbw_status.electrical.vsupply = DL_IMCU_FBW_STATUS_vsupply(imcu_msg_buf); + fbw_status.electrical.current = DL_IMCU_FBW_STATUS_current(imcu_msg_buf); break; } diff --git a/sw/airborne/subsystems/intermcu/intermcu_ap.h b/sw/airborne/subsystems/intermcu/intermcu_ap.h index d3eb036ccf..50aead74b8 100644 --- a/sw/airborne/subsystems/intermcu/intermcu_ap.h +++ b/sw/airborne/subsystems/intermcu/intermcu_ap.h @@ -29,6 +29,7 @@ #include "subsystems/intermcu.h" #include "generated/airframe.h" +#include "subsystems/electrical.h" void intermcu_set_actuators(pprz_t *command_values, uint8_t ap_mode); void RadioControlEvent(void (*frame_handler)(void)); @@ -52,8 +53,7 @@ struct fbw_status_t { uint8_t rc_status; uint8_t frame_rate; uint8_t mode; - uint16_t vsupply; - int32_t current; + struct Electrical electrical; }; #endif /* INTERMCU_AP_ROTORCRAFT_H */ diff --git a/sw/ext/pprzlink b/sw/ext/pprzlink index f57b5b16e5..0309572e58 160000 --- a/sw/ext/pprzlink +++ b/sw/ext/pprzlink @@ -1 +1 @@ -Subproject commit f57b5b16e5d3d582ea2bfe1266b3e9434904541f +Subproject commit 0309572e584c594a6e7ef39ee7b2511f2fa14dfb diff --git a/sw/ground_segment/tmtc/aircraft.ml b/sw/ground_segment/tmtc/aircraft.ml index c5823cd98a..7a95527179 100644 --- a/sw/ground_segment/tmtc/aircraft.ml +++ b/sw/ground_segment/tmtc/aircraft.ml @@ -179,7 +179,9 @@ type aircraft = { mutable temp : float; mutable bat : float; mutable amp : float; - mutable energy : int; + mutable power : float; + mutable charge : float; + mutable energy : float; mutable ap_mode : int; mutable gaz_mode : int; mutable lateral_mode : int; @@ -223,7 +225,7 @@ let new_aircraft = fun id name fp airframe -> desired_course = 0.; desired_altitude = 0.; desired_climb = 0.; cur_block=0; cur_stage=0; flight_time = 0; stage_time = 0; block_time = 0; - throttle = 0.; throttle_accu = 0.; rpm = 0.; temp = 0.; bat = 0.; amp = 0.; energy = 0; ap_mode= -1; + throttle = 0.; throttle_accu = 0.; rpm = 0.; temp = 0.; bat = 0.; amp = 0.; power = 0.; charge = 0.; energy = 0.; ap_mode= -1; kill_mode = false; gaz_mode= -1; lateral_mode= -1; gps_mode = 0; gps_Pacc = 0; periodic_callbacks = []; diff --git a/sw/ground_segment/tmtc/aircraft.mli b/sw/ground_segment/tmtc/aircraft.mli index fd7db35704..08e54bafa1 100644 --- a/sw/ground_segment/tmtc/aircraft.mli +++ b/sw/ground_segment/tmtc/aircraft.mli @@ -120,7 +120,9 @@ type aircraft = { mutable temp : float; mutable bat : float; mutable amp : float; - mutable energy : int; + mutable power : float; + mutable charge : float; + mutable energy : float; mutable ap_mode : int; mutable gaz_mode : int; mutable lateral_mode : int; diff --git a/sw/ground_segment/tmtc/ivy_serial_bridge.c b/sw/ground_segment/tmtc/ivy_serial_bridge.c index ccc100dfc8..db71934816 100644 --- a/sw/ground_segment/tmtc/ivy_serial_bridge.c +++ b/sw/ground_segment/tmtc/ivy_serial_bridge.c @@ -270,21 +270,6 @@ void send_ivy(void) // IvySendMsg("%d NAVIGATION %d 0 0 0 0 0 0 0 \n", remote_uav.ac_id, remote_uav.block); -/* - - - - - - - - - - -*/ - - // IvySendMsg("%d BAT 0 81 0 %ld 0 0 0 0\n", remote_uav.ac_id, count_serial); - /* @@ -314,9 +299,6 @@ void send_ivy(void) delayer = 0; } - - - count_serial++; sprintf(status_serial_str, "Read %d from '%s': forwarding to IVY [%ld] {Rx=%ld} {Err=%ld}", remote_uav.ac_id, port, count_serial, rx_bytes, rx_error); diff --git a/sw/ground_segment/tmtc/parse_messages_v1.ml b/sw/ground_segment/tmtc/parse_messages_v1.ml index 5e1071347f..fd6f89626f 100644 --- a/sw/ground_segment/tmtc/parse_messages_v1.ml +++ b/sw/ground_segment/tmtc/parse_messages_v1.ml @@ -302,17 +302,15 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values -> a.cur_stage <- ivalue "cur_stage"; a.horizontal_mode <- check_index (ivalue "horizontal_mode") horiz_modes "AP_HORIZ"; a.dist_to_wp <- (try fvalue "dist_wp" with _ -> 0.); - | "BAT" -> - a.throttle <- fvalue "throttle" /. 9600. *. 100.; - a.kill_mode <- ivalue "kill_auto_throttle" <> 0; - a.flight_time <- ivalue "flight_time"; - a.rpm <- a.throttle *. 100.; - a.bat <- fvalue "voltage" /. 10.; - a.stage_time <- ivalue "stage_time"; - a.block_time <- ivalue "block_time"; - a.energy <- ivalue "energy" + | "ENERGY" -> + a.throttle <- fvalue "throttle"; + a.bat <- fvalue "voltage"; + a.amp <- fvalue "current"; + a.power <- fvalue "power"; + a.charge <- fvalue "charge"; + a.energy <- fvalue "energy"; | "FBW_STATUS" -> - a.fbw.fbw_bat <- fvalue "vsupply" /. 10.; + a.fbw.fbw_bat <- fvalue "vsupply"; a.fbw.pprz_mode_msgs_since_last_fbw_status_msg <- 0; a.fbw.rc_rate <- ivalue "frame_rate"; let fbw_rc_mode = ivalue "rc_status" in @@ -363,7 +361,7 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values -> a.gps_mode <- check_index (ivalue "gps_status") gps_modes "GPS_MODE"; a.ap_mode <- check_index (ivalue "ap_mode") (modes_of_aircraft a) "ROTORCRAFT_AP_MODE"; a.kill_mode <- ivalue "ap_motors_on" == 0; - a.bat <- fvalue "vsupply" /. 10. + a.bat <- fvalue "vsupply" | "ROVER_STATUS" -> a.vehicle_type <- Rover; a.fbw.rc_status <- get_rc_status (ivalue "rc_status"); @@ -371,7 +369,7 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values -> a.gps_mode <- check_index (ivalue "gps_status") gps_modes "GPS_MODE"; a.ap_mode <- check_index (ivalue "ap_mode") (modes_of_aircraft a) "ROVER_AP_MODE"; a.kill_mode <- ivalue "ap_motors_on" == 0; - a.bat <- fvalue "vsupply" /. 10. + a.bat <- fvalue "vsupply" | "STATE_FILTER_STATUS" -> a.state_filter_mode <- check_index (ivalue "state_filter_mode") state_filter_modes "STATE_FILTER_MODES" | "DATALINK_REPORT" -> @@ -478,7 +476,7 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values -> a.heading <- a.course; a.agl <- a.alt -. (try float (Srtm.of_wgs84 a.pos) with _ -> a.ground_alt); a.bat <- fvalue "vsupply" /. 10.; - a.energy <- ivalue "energy" * 100; + a.charge <- fvalue "charge" /. 10.; a.throttle <- fvalue "throttle"; a.ap_mode <- check_index (ivalue "ap_mode") (modes_of_aircraft a) "AP_MODE"; a.cur_block <- ivalue "nav_block"; diff --git a/sw/ground_segment/tmtc/server.ml b/sw/ground_segment/tmtc/server.ml index d589b32f9f..10ade8716f 100644 --- a/sw/ground_segment/tmtc/server.ml +++ b/sw/ground_segment/tmtc/server.ml @@ -355,9 +355,6 @@ let send_moved_waypoints = fun a -> a.waypoints - - - let send_aircraft_msg = fun ac -> try let a = Hashtbl.find aircrafts ac in @@ -438,7 +435,7 @@ let send_aircraft_msg = fun ac -> "temp", f a.temp; "bat", f a.bat; "amp", f a.amp; - "energy", PprzLink.Int a.energy] in + "charge", f a.charge] in Ground_Pprz.message_send my_id "ENGINE_STATUS" values; let ap_mode = get_indexed_value ~text:(if a.ap_mode = -2 then "FAIL" else "UNK") (modes_of_aircraft a) a.ap_mode in diff --git a/sw/misc/satcom/tcp2ivy_generic.c b/sw/misc/satcom/tcp2ivy_generic.c index 5f6a389b2a..1ffb7f2275 100644 --- a/sw/misc/satcom/tcp2ivy_generic.c +++ b/sw/misc/satcom/tcp2ivy_generic.c @@ -62,28 +62,6 @@ static const char usage_str[] = "options:\n" " -s \n"; -unsigned char gps_mode; -unsigned short gps_week; -unsigned int gps_itow; -unsigned int gps_alt; -unsigned short gps_gspeed; -short gps_climb; -short gps_course; -int gps_utm_east, gps_utm_north; -unsigned char gps_utm_zone; -int gps_lat, gps_lon; /* 1e7 deg */ -int gps_hmsl; -short estimator_airspeed; -unsigned short electrical_vsupply; -unsigned char nav_block; -unsigned short energy; -unsigned char throttle; -unsigned short autopilot_flight_time; -unsigned char nav_utm_zone0; -float latlong_utm_x, latlong_utm_y; -unsigned char pprz_mode; - - GMainLoop *ml; int sock, length; struct sockaddr_in addr; @@ -104,11 +82,23 @@ unsigned short buf2ushort(char* dat) } static gboolean read_data(GIOChannel *chan, GIOCondition cond, gpointer data) { - int count; + uint8_t active; + int32_t gps_lat, gps_lon; // 1e7 deg + int16_t gps_alt; // m + uint16_t gps_gspeed; // cm/s + int16_t gps_course; // decideg + uint16_t airspeed; // cm/s + uint8_t vsupply; // deciV + uint8_t charge; //deciA + uint8_t throttle; // % + uint8_t nav_block; + uint8_t pprz_mode; + uint8_t autopilot_flight_time; + char buf[BUFSIZE]; /* receive data packet containing formatted data */ - count = recv(sock, buf, sizeof(buf), 0); + int count = recv(sock, buf, sizeof(buf), 0); if (count > 0) { if (count == 23) { // FillBufWith32bit(com_trans.buf, 1, gps_lat); @@ -122,12 +112,11 @@ static gboolean read_data(GIOChannel *chan, GIOCondition cond, gpointer data) { // FillBufWith16bit(com_trans.buf, 13, gps_course); // course gps_course = buf2ushort(&buf[12]); // FillBufWith16bit(com_trans.buf, 15, (uint16_t)(estimator_airspeed*100)); // TAS (cm/s) - estimator_airspeed = buf2ushort(&buf[14]); - // com_trans.buf[17] = electrical.vsupply; // decivolt - //FIXME: electrical.vsupply is now a uint16 - electrical_vsupply = buf[16]; - // com_trans.buf[18] = (uint8_t)(energy / 100); // deciAh - energy = buf[17]; + airspeed = buf2ushort(&buf[14]); + // com_trans.buf[17] = (uint8_t)(electrical.vsupply * 10.f); // deciVolt + vsupply = buf[16]; + // com_trans.buf[18] = (uint8_t)(electrical.charge * 10.f); // deciAh + charge = buf[17]; // com_trans.buf[19] = (uint8_t)(ap_state->commands[COMMAND_THROTTLE]*100/MAX_PPRZ); throttle = buf[18]; // com_trans.buf[20] = pprz_mode; @@ -137,30 +126,15 @@ static gboolean read_data(GIOChannel *chan, GIOCondition cond, gpointer data) { // FillBufWith16bit(com_trans.buf, 22, autopilot_flight_time); autopilot_flight_time = buf2ushort(&buf[21]); -#if 0 - gps_lat = 52.2648312 * 1e7; - gps_lon = 9.9939456 * 1e7; - gps_alt = 169 * 1000; - gps_gspeed = 13 * 100; - gps_course = 60 * 10; - estimator_airspeed = 15 * 100; - electrical_vsupply = 126; - energy = 9; - throttle = 51; - pprz_mode = 2; - nav_block = 1; - autopilot_flight_time = 123; -#endif - printf("**** message received from iridium module ****\n"); printf("gps_lat %f\n", DegOfRad(gps_lat/1e7)); printf("gps_lon %f\n", DegOfRad(gps_lon/1e7)); printf("gps_alt %d\n", gps_alt); printf("gps_gspeed %d\n", gps_gspeed); printf("gps_course %d\n", gps_course); - printf("estimator_airspeed %d\n", estimator_airspeed); - printf("electrical_vsupply %d\n", electrical_vsupply); - printf("energy %d\n", energy); + printf("estimator_airspeed %d\n", airspeed); + printf("electrical_vsupply %d\n", vsupply); + printf("charge %d\n", charge); printf("throttle %d\n", throttle); printf("pprz_mode %d\n", pprz_mode); printf("nav_block %d\n", nav_block); @@ -174,16 +148,14 @@ static gboolean read_data(GIOChannel *chan, GIOCondition cond, gpointer data) { gps_alt, gps_gspeed, gps_course, - estimator_airspeed, - electrical_vsupply, - energy, + airspeed, + vsupply, + charge, throttle, pprz_mode, nav_block, autopilot_flight_time); - - } - else { + } else { int i = 0; // Raw print printf("**** Raw message ****\n"); @@ -193,8 +165,7 @@ static gboolean read_data(GIOChannel *chan, GIOCondition cond, gpointer data) { for (i = 0; i < count; i++) printf("%x ",buf[i]); printf("\n"); } - } - else { + } else { printf("disconnect\n"); close(sock); g_main_loop_quit(ml); diff --git a/sw/simulator/nps/nps_electrical.c b/sw/simulator/nps/nps_electrical.c index f0a6363fc1..ba10db32be 100644 --- a/sw/simulator/nps/nps_electrical.c +++ b/sw/simulator/nps/nps_electrical.c @@ -44,5 +44,5 @@ void nps_electrical_init(void) void nps_electrical_run_step(double time __attribute__((unused))) { // todo: auto-decrease bat voltage - electrical.vsupply = nps_electrical.supply_voltage * 10; + electrical.vsupply = nps_electrical.supply_voltage; }