summaryrefslogtreecommitdiff
path: root/n/es-2006/src/sensor_rvb.c
diff options
context:
space:
mode:
Diffstat (limited to 'n/es-2006/src/sensor_rvb.c')
-rw-r--r--n/es-2006/src/sensor_rvb.c71
1 files changed, 45 insertions, 26 deletions
diff --git a/n/es-2006/src/sensor_rvb.c b/n/es-2006/src/sensor_rvb.c
index 5a4de2a..76e956a 100644
--- a/n/es-2006/src/sensor_rvb.c
+++ b/n/es-2006/src/sensor_rvb.c
@@ -112,22 +112,32 @@ sensor_rvb_color_select (uint8_t sensor_rvb_color)
inline uint8_t
sensor_rvb_sensor_select (uint8_t sensor_rvb_num)
{
+ static uint8_t sensor_cur = 0;
switch (sensor_rvb_num)
{
case 1:
case 2:
case 3:
case 4:
- case 5:
- case 6:
- case 7:
PORTC = ~_BV(sensor_rvb_num);
break;
- case 8:
- PORTD &= ~_BV(6);
- break;
- case 9:
- PORTC = ~_BV(0);
+ case 5:
+ switch (sensor_cur)
+ {
+ case 0:
+ case 1:
+ case 2:
+ PORTC = ~_BV(sensor_rvb_num + sensor_cur);
+ break;
+ case 3:
+ PORTD &= ~_BV(6);
+ break;
+ case 4:
+ PORTC = ~_BV(0);
+ break;
+ }
+ if (++sensor_cur == 5)
+ sensor_cur = 0;
break;
default:
/* High for disable */
@@ -182,6 +192,9 @@ sensor_rvb_start_capture (void)
sensor_rvb_number_ = 0;
/* Select it and launch capture */
sensor_rvb_update ();
+ /* Ensure we have no pending interrupt for IC1. */
+ TIFR = _BV (ICF1);
+ TIFR = _BV (TOV1);
/* Enable interrupt for IC1 and counter overflow */
TIMSK |= _BV (TICIE1);
TIMSK |= _BV (TOIE1);
@@ -189,8 +202,7 @@ sensor_rvb_start_capture (void)
}
/** Update everything for you. */
-// TODO, common part, clean TIFR, remove inline ?
-inline void
+void
sensor_rvb_update (void)
{
if (sensor_rvb_state_ == RVB_STATE_NEXT_COLOR)
@@ -205,7 +217,7 @@ sensor_rvb_update (void)
{
sensor_rvb_state_ = RVB_STATE_WAIT_IC;
/* Ensure we have no pending interrupt for IC1. */
-// TIFR = _BV (ICF1);
+ TIFR = _BV (ICF1);
/* Enable interrupt for IC1 */
TIMSK |= _BV (TICIE1);
}
@@ -220,8 +232,6 @@ sensor_rvb_update (void)
/* Disable IC1 and TC1 overflow interrupts */
TIMSK &= ~_BV (TICIE1);
TIMSK &= ~_BV (TOIE1);
- /* Ensure we have no pending interrupt for IC1. */
-// TIFR = _BV (ICF1);
/* Finish ! Go to sleep */
sensor_rvb_state_ = RVB_STATE_SLEEP;
return;
@@ -230,8 +240,8 @@ sensor_rvb_update (void)
sensor_rvb_color_select (sensor_rvb_color_ = 0);
sensor_rvb_state_ = RVB_STATE_WAIT_IC;
sensor_rvb_overflow_count_ = 0;
-// /* Ensure we have no pending interrupt for IC1. */
-// TIFR = _BV (ICF1);
+ /* Ensure we have no pending interrupt for IC1. */
+ TIFR = _BV (ICF1);
/* Enable interrupt for IC1 */
TIMSK |= _BV (TICIE1);
}
@@ -252,7 +262,7 @@ SIGNAL (SIG_OVERFLOW1)
if (++sensor_rvb_overflow_count_ >= sensor_rvb_conf_max_ov_)
{
/* Invalidate the sensor */
- sensor_rvb_values[sensor_rvb_number_ - 1][0] = 0;
+ sensor_rvb_values[sensor_rvb_number_ - 1][0] = RVB_INVALID_CAPTURE;
/* Disable IC interrupt */
TIMSK &= ~_BV (TICIE1);
/* Ask for next sensor */
@@ -266,7 +276,8 @@ SIGNAL (SIG_OVERFLOW1)
/** Interrupt on falling edge for Input Capture 1. */
SIGNAL (SIG_INPUT_CAPTURE1)
{
- uint16_t temp;
+ /* Save it earlier ! */
+ uint16_t ic1_cache = ICR1;
switch (sensor_rvb_state_)
{
case RVB_STATE_WAIT_IC:
@@ -279,19 +290,22 @@ SIGNAL (SIG_INPUT_CAPTURE1)
sensor_rvb_state_ = RVB_STATE_START_CAPTURE;
break;
case RVB_STATE_START_CAPTURE:
+ /* Manage case when overflow occured before IC but we are called first
+ * */
+ if (!(ic1_cache & (TC1_TOP & ~(TC1_TOP >> 1))))
+ TIFR = _BV (TOV1);
/* Save capture start */
- sensor_rvb_value_ = ICR1;
+ sensor_rvb_value_ = ic1_cache;
/* Reset overflow_count */
sensor_rvb_overflow_count_ = 0;
/* New capture */
sensor_rvb_state_ = RVB_STATE_STOP_CAPTURE;
break;
case RVB_STATE_STOP_CAPTURE:
- /* Save it earlier ! */
- temp = ICR1;
/* Compute value */
- sensor_rvb_values[sensor_rvb_number_ - 1][sensor_rvb_color_] = temp +
- sensor_rvb_overflow_count_ * TC1_TOP - sensor_rvb_value_;
+ sensor_rvb_values[sensor_rvb_number_ - 1][sensor_rvb_color_] =
+ ic1_cache + sensor_rvb_overflow_count_ * (TC1_TOP + 1) -
+ sensor_rvb_value_;
/* Disable IC interrupt */
TIMSK &= ~_BV (TICIE1);
sensor_rvb_state_ = RVB_STATE_NEXT_COLOR;
@@ -310,10 +324,15 @@ sensor_rvb_update_asserv_pins (void)
for (compt = 0; compt < 4; compt++)
{
- if (sniff_rvb_analysis_color (compt, 0))
- PORTA |= _BV (compt + 4);
- else
- PORTA &= ~_BV (compt + 4);
+ /* Is sensor capture valid */
+ if (sensor_rvb_values[compt][0] < RVB_INVALID_CAPTURE)
+ {
+ if (sniff_rvb_analysis_color (compt, RVB_SNIFF_ONLY_GREEN)
+ != RVB_SNIFF_GREEN)
+ PORTA |= _BV (compt + 4);
+ else
+ PORTA &= ~_BV (compt + 4);
+ }
}
// TODO: